Udacity 传感器融合笔记（一）lidar

udacity的传感器融合课程整体上分为四大部分，分别为lidar、radar、camera、以及传感器的融合。此篇记录下第一部分lidar学习过程中的一些笔记与心得，便于自己和广大开发者日后查看。博客内容为自己对课程的理解，如有问题，欢迎留言或私信讨论。

课程对lidar的介绍分为四部分：

1.对lidar传感器及其点云数据的介绍
激光雷达感测通过发送数千个激光信号为我们提供高分辨率数据。这些激光从物体上反弹出来，返回到传感器，然后我们可以通过定时返回信号所需的时间来确定物体的距离。我们还可以通过测量返回信号的强度来告诉一些有关撞击物体的信息。每束激光都在红外光谱范围内，并以许多不同的角度发出，通常在360度范围内。激光雷达传感器可为我们提供3D周围世界的高精度模型。
（1）激光雷达以不同角度发送数千条激光。
（2）激光被发射出来，被障碍物反射，然后使用接收器进行检测。
（3）根据发射和接收激光之间的时间差，计算距离。
（4）还接收激光强度值，并且可以将其用于评估激光反射离开的对象的材料属性。

lidar的数据是以点云的形式被记录的，所谓点云，就是每个测量点的激光信号的集合。通常以PCD文件格式记录，具体的数据格式为（X,Y,Z,I），其中（X,Y,Z)表示测量点的三维空间坐标。I表示接收激光的强度值。

可视化后的PCD，不同的颜色代表不同的激光强度。
为了方便处理点云数据，我们需要使用PCL点云处理的库，需自行下载安装

2.课程代码结构介绍
udacitiy课程的是提供代码的。其代码结构和功能如下：

Top-level CMakelists.txt 配置文件
readMe
src：
render
box.h 定义了目标方形的结构体
render.h
render.cpp 定义了目标渲染如车道线等的函数
lidar.h 定义了传感器的相关功能以及形成pcd文件
environment.cpp 主函数、创建了pcl的Viewer窗口、进程和读取PCD文件
processPointClouds.h
processPointClouds.cpp 点云滤波、点云分割、点云聚类、box功能、读取保存pcd功能的实现。

接下来，将逐一功能进行记录，介绍。

3.代码解读
3.1 box.h
box.h文件定义了框定目标物体的长方体的长宽高等结构体，其结构体定义如下：

struct BoxQ
{Eigen::Vector3f bboxTransform;    //旋转向量的定义Eigen::Quaternionf bboxQuaternion; //四元数的定义float cube_length;  //长float cube_width;  //宽float cube_height;  //高
};//这个结构体是目标物体长方体的结构定义
struct Box
{float x_min;    //所属一类点云中X方向最小的点坐标，以下六个量可以确定长方体的长宽高float y_min;float z_min;float x_max;float y_max;float z_max;
};

此图片中的红色长方体就是在box.h文件中所定义的结构体。

3.2 render.h
render用于pcd文件显示的渲染，render.h文件中定义了渲染相关的结构体和相关成员函数声明。

颜色的渲染

//自定义颜色结构体
struct Color
{float r, g, b;  //使用RGB颜色通道Color(float setR, float setG, float setB): r(setR), g(setG), b(setB){}
};

位置的定义

//自定义位置结构
struct Vect3
{double x, y, z;  //xyz三维空间坐标//构造函数Vect3(double setX, double setY, double setZ): x(setX), y(setY), z(setZ){}//重载Vect3类型的 + 运算并返回结果Vect3 operator+(const Vect3& vec){Vect3 result(x+vec.x,y+vec.y,z+vec.z);return result;}
};

枚举类型，视角的定义

enum CameraAngle
{XY, TopDown, Side, FPS  //XY为 轴测图  TopDown 为俯视  side 为侧视图 Fps为前视图
};

汽车的定义

struct Car
{// units in metersVect3 position, dimensions;  //位置、尺寸（长宽高）std::string name;Color color;//构造函数Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, std::string setName): position(setPosition), dimensions(setDimensions), color(setColor), name(setName){}//渲染函数 ，用于汽车的渲染void render(pcl::visualization::PCLVisualizer::Ptr& viewer){// 汽车底部的渲染  viewer->addCube(position.x-dimensions.x/2, position.x+dimensions.x/2, position.y-dimensions.y/2, position.y+dimensions.y/2, position.z, position.z+dimensions.z*2/3, color.r, color.g, color.b, name); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name);// 汽车顶部的渲染viewer->addCube(position.x-dimensions.x/4, position.x+dimensions.x/4, position.y-dimensions.y/2, position.y+dimensions.y/2, position.z+dimensions.z*2/3, position.z+dimensions.z, color.r, color.g, color.b, name+"Top"); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name+"Top"); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name+"Top");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, name+"Top");}

其他函数的声明，其函数定义在render.cpp中

//车道线定义及渲染函数
void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer);
//lidar光束的渲染
void renderRays(pcl::visualization::PCLVisualizer::Ptr& viewer, const Vect3& origin, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
void clearRays(pcl::visualization::PCLVisualizer::Ptr& viewer);
//点云渲染
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color = Color(1,1,1));
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color = Color(-1,-1,-1));
//目标物立方体的渲染
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, Box box, int id, Color color = Color(1,0,0), float opacity=1);
void renderBox(pcl::visualization::PCLVisualizer::Ptr& viewer, BoxQ box, int id, Color color = Color(1,0,0), float opacity=1);

3.3 render.cpp

车道线渲染

void renderHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
{// units in meters  长宽高double roadLength = 50.0;double roadWidth = 12.0;double roadHeight = 0.2;//渲染viewer->addCube(-roadLength/2, roadLength/2, -roadWidth/2, roadWidth/2, -roadHeight, 0, .2, .2, .2, "highwayPavement"); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, "highwayPavement"); viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, .2, .2, .2, "highwayPavement");viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0, "highwayPavement");//添加车道线viewer->addLine(pcl::PointXYZ(-roadLength/2,-roadWidth/6, 0.01),pcl::PointXYZ(roadLength/2, -roadWidth/6, 0.01),1,1,0,"line1");viewer->addLine(pcl::PointXYZ(-roadLength/2, roadWidth/6, 0.01),pcl::PointXYZ(roadLength/2, roadWidth/6, 0.01),1,1,0,"line2");
}

其他各个函数定义类似，就不过多介绍。需要注意的是addCube、addLine、addPointCloud等都是PCL库自带的模型建立及渲染功能。详细功能可学习PCL点云库。

3.4lidar.h
lidar.h中定义了lidar传感器的相关结构体

激光线束及测量返回值的定义

const double pi = 3.1415;struct Ray  //lidar 线束
{Vect3 origin; //原点，光束的起始位置，也就是lidar位置double resolution; //射线分辨率的大小，Vect3 direction; //xy平面与射线之间的方向角，例如0弧度沿xy平面，而pi / 2弧度沿法线Vect3 castPosition; //返回测量点double castDistance;//返回测量距离// parameters:// setOrigin: the starting position from where the ray is cast// horizontalAngle: the angle of direction the ray travels on the xy plane// verticalAngle: the angle of direction between xy plane and ray //                  for example 0 radians is along xy plane and pi/2 radians is stright up// resoultion: the magnitude of the ray's step, used for ray casting, the smaller the more accurate but the more expensive//构造函数Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, double setResolution): origin(setOrigin), resolution(setResolution), direction(resolution*cos(verticalAngle)*cos(horizontalAngle), resolution*cos(verticalAngle)*sin(horizontalAngle),resolution*sin(verticalAngle)),castPosition(origin), castDistance(0){}void rayCast(const std::vector<Car>& cars, double minDistance, double maxDistance, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double slopeAngle, double sderr){// reset raycastPosition = origin;castDistance = 0;bool collision = false;while(!collision && castDistance < maxDistance){castPosition = castPosition + direction;castDistance += resolution;// check if there is any collisions with ground slope 检测是否有地面返回值collision = (castPosition.z <= castPosition.x * tan(slopeAngle));// check if there is any collisions with cars 检查是否有车辆检测返回值if(!collision && castDistance < maxDistance){for(Car car : cars){collision |= car.checkCollision(castPosition);if(collision)break;}}}if((castDistance >= minDistance)&&(castDistance<=maxDistance)){// add noise based on standard deviation error 添加噪声 并加入cloud向量中double rx = ((double) rand() / (RAND_MAX));double ry = ((double) rand() / (RAND_MAX));double rz = ((double) rand() / (RAND_MAX));cloud->points.push_back(pcl::PointXYZ(castPosition.x+rx*sderr, castPosition.y+ry*sderr, castPosition.z+rz*sderr));}}};

lidar结构体的定义

struct Lidar
{std::vector<Ray> rays; //激光线束pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; //点云储存向量std::vector<Car> cars; //车Vect3 position;  //位置double groundSlope; //地面坡度double minDistance; //最小距离double maxDistance; //最大距离double resoultion;  //分辨率double sderr; //标准误差 用于给测量点添加噪声//构造函数Lidar(std::vector<Car> setCars, double setGroundSlope): cloud(new pcl::PointCloud<pcl::PointXYZ>()), position(0,0,2.6){// TODO:: set minDistance to 5 to remove points from roof of ego car 将自小距离设置为5m 用于删除minDistance = 5;maxDistance = 50;resoultion = 0.2;// TODO:: set sderr to 0.2 to get more interesting pcd filessderr = 0.2;cars = setCars;groundSlope = setGroundSlope;// TODO:: increase number of layers to 8 to get higher resoultion pcd 将层数增加到8以获得更高的分辨率pcdint numLayers = 8;// the steepest vertical angledouble steepestAngle =  30.0*(-pi/180);double angleRange = 26.0*(pi/180);// TODO:: set to pi/64 to get higher resoultion pcd 设置为pi / 64以获取更高分辨率的pcddouble horizontalAngleInc = pi/64;double angleIncrement = angleRange/numLayers;for(double angleVertical = steepestAngle; angleVertical < steepestAngle+angleRange; angleVertical+=angleIncrement){for(double angle = 0; angle <= 2*pi; angle+=horizontalAngleInc){Ray ray(position,angle,angleVertical,resoultion);rays.push_back(ray);}}}//析构函数~Lidar(){// pcl uses boost smart pointers for cloud pointer so we don't have to worry about manually freeing the memory}pcl::PointCloud<pcl::PointXYZ>::Ptr scan(){cloud->points.clear(); //clear操作auto startTime = std::chrono::steady_clock::now();//开始时间设置for(Ray ray : rays)ray.rayCast(cars, minDistance, maxDistance, cloud, groundSlope, sderr);auto endTime = std::chrono::steady_clock::now();//结束时间设置auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);//飞行时间法，计算往返时间cout << "ray casting took " << elapsedTime.count() << " milliseconds" << endl;cloud->width = cloud->points.size();cloud->height = 1; // one dimensional unorganized point cloud datasetreturn cloud;}};

其程序运行结果可以用下图大致表示

红色为激光线束，蓝色为目标车辆渲染为蓝色，车道渲染为灰色，车道线渲染为黄色。

关于lidar如的得到测量点做如下简单说明:
lidar有其固有属性，激光雷达利用飞行时间计算物体的距离，发射激光脉冲时，发射时间和发射方向被记录，碰到障碍物返回的激光脉冲被接收时，记录采集和接收的时间。
由于激光雷达传感器的是一个特殊的球面坐标系值，让我们来复习该特殊的球面坐标系。球面坐标系在球面坐标系中，点由距离和两个角度定义。为了表示这两个角，我们使用方位角（Th）和极角（γ）约定。因此，点是由（r，τ，γ）定义的。

从上面的图解可以看出，方位角是在X轴上测量的X-Y平面，极坐标角是Z轴测量的Z-Y平面。

从上面的图，我们可以得到下列方程，将笛卡尔坐标转换为球面坐标。

可以使用下面的方程从球坐标导出笛卡尔坐标。
详细资料可转至：
lidar基础-坐标系及测量点

本篇记录了udacity传感器信息融合中的基础代码部分，主要包括，环境的渲染，lidar、car、position、ray等结构体的定义。

在接下来中会继续介绍lidar点云数据的一些处理，重点在
processPointClouds.h
processPointClouds.cpp
两个文件，包含了点云滤波、RANSAC的点云分割，聚类等核心部分。
文中有什么问题，欢迎大家评论区交流指正！