自动驾驶算法---传感器数据融合（1）--激光雷达与点云（Linux 下的c++工程）
今天又研究下了<传感器数据融合>课程中的激光雷达和点云的小项目，项目的源码在https://github.com/JackJu-HIT/Lidar-and-point-cloud-1-。
在博客上写这个也是为了以后方便自己查看。我先说一下他的整个工程的思路：
他利用PCL库建立了一个路面、绿色的自动驾驶汽车以及周围交通道路中蓝色的汽车模型，同时，它还拥有一个激光雷达模型，它被安置在汽车顶部。下图是我电脑跑完整个项目的所生成的点云。
那么个人认为最复杂的部分就是激光雷达的建模，这里它没有使用真实的激光雷达，而是用算法生成了一个激光雷达，你需要设置最大距离、位置、最小探测距离、等参数，同时你还要设置从射出得光束遇到障碍物就不要再往前走了，要符合物理约束，好处是你没有64线激光雷达也能进行后续的算法学习。
这里面用的·最多的就是Point cloud library即PCL点云模块，从场景的创建以及激光束。点云的生成都是依赖于这个库，如果说opencv是处理二维信息的工具，那么PCL就是要比他高一维度处理三维信息。
这个就是汽车上的激光雷达，这个坐标系和小车的坐标系是类似的，所以这里我们有个自上而下的视野，x轴被定义指向小车的前面，y轴指向汽车的左侧，z轴指向垂直方向。现在我们看他侧视图了，我们能看见x的指指向那个z州的指向垂直，然后我们知道这会给我们一些很好地细节关于如何将这些不同层的组织起来。这个顶层，与x轴成24.8度的倾斜，然后他覆盖了26.8度的范围，然后这个64层中的每一层都会均匀分布在26.8度层范围内。所以这就是我们如何定义xyz坐标系的点。
下面分析一下源码：这个是主函数，调用initCamera(setAngle, viewer)和 simpleHighway(viewer)函数，CameraAngle setAngle = XY;//XY;是用来设置相机的角度
int main (int argc, char** argv)
{
std::cout << "starting enviroment" << std::endl;
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D 图像Viewer"));//这将创建查看器对象，
//并为其提供一个漂亮的名称以显示在标题栏中。我们仅将其存储在智能指针中，以便可以在演示程序中传递它。通常，您不需要这样做。
CameraAngle setAngle = XY;//XY;设置相机的角度
initCamera(setAngle, viewer);
simpleHighway(viewer);
while (!viewer->wasStopped ())
{
viewer->spinOnce ();
}
}相机的初始化函数，来设置显示器的背景、摄像机相对于物体的位置也就是视角等。
void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr& viewer)
{
//PCL可视化是一个引用传进来的，所以后续对他更改都想存在
viewer->setBackgroundColor (0, 0, 0);//查看器的背景色可以设置为您喜欢的任何RGB颜色。在这种情况下，我们将其设置为黑色
// set camera position and angle
viewer->initCameraParameters();
// distance away in meters
int distance = 16;
switch(setAngle)
{
case XY : viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0); break;
case TopDown : viewer->setCameraPosition(0, 0, distance, 1, 0, 1); break;//101
case Side : viewer->setCameraPosition(0, -distance, 0, 0, 0, 1); break;
case FPS : viewer->setCameraPosition(-10, 0, 0, 0, 0, 1);
}
if(setAngle!=FPS)
viewer->addCoordinateSystem (1.0);//查看复杂的点云通常会令人迷惑。为了使自己与世界保持一致，可以显示轴。
//它们将沿X（红色），Y（绿色）和Z（蓝色）轴显示为三个圆柱
//体。可以使用scale参数控制气缸的尺寸。在这种情况下，我们将其设置为1.0（如果未提供任何值，则它也是默认值）
//。此方法的替代版本可用于将轴放置在世界上的任何位置。
}
std::vector initHighway(bool renderScene, pcl::visualization::PCLVisualizer::Ptr& viewer)
{
Car egoCar( Vect3(0,0,0), Vect3(4,2,2), Color(0,1,0), "egoCar");
Car car1( Vect3(15,0,0), Vect3(4,2,2), Color(0,0,1), "car1");
Car car2( Vect3(8,-4,0), Vect3(4,2,2), Color(0,0,1), "car2");
Car car3( Vect3(-12,4,0), Vect3(4,2,2), Color(0,0,1), "car3");
std::vector cars;
cars.push_back(egoCar);
cars.push_back(car1);
cars.push_back(car2);
cars.push_back(car3);
if(renderScene)
{
renderHighway(viewer);//高穗公路显示
egoCar.render(viewer);//显示汽车1
car1.render(viewer);//显示汽车2
car2.render(viewer);//传送视觉显示显示汽车3
car3.render(viewer);//显示汽车4
}
return cars;
}
initHighway函数，生成了并显示了高速公路、4辆汽车，关于具体怎么产生的，还需要关注car这个结构体如下所示：
具体是关于如何生成指定的汽车模型，包括长宽高以及位置坐标，这里面还有个碰撞检测函数，它是用来到后面模拟激光发出的光束特性，遇到障碍物时，逛就停在这里。
struct Car
{
// units in meters以米为单位
Vect3 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)
{
// render bottom of car 渲染汽车底部
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);//addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,double r = 1.0, double g = 1.0, double b = 1.0, const std::string &id = "cube", int viewport = 0);
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);
// render top of car
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");//不透名度100%
}
// collision helper function
bool inbetween(double point, double center, double range)
{
return (center-range <= point) && (center+range >= point);
}
bool checkCollision(Vect3 point)
{
return (inbetween(point.x,position.x,dimensions.x/2)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z/3,dimensions.z/3))||
(inbetween(point.x,position.x,dimensions.x/4)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z*5/6,dimensions.z/6));
}
};高速公路的模型void renderHighway是在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, 0.2, 0.2, 0.2, "highwayPavement");//0.2 0.2 0.2
viewer->setShapeRenderingProperties(pcl::visualization::PCLISUALIZER_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");
}
环境到此已经建立好了，我们需要对激光雷达以及其发出的激光进行建模。它是1个激光雷达的建模的结构体，里面有很多参数尅设置，激光雷达的位置，最大距离最小距离等，同时还人为的加入了噪音，这是因为真正的激光雷达一定会有测量的不缺定性存在。
struct Lidar
{
std::vector rays;
pcl::PointCloud::Ptr cloud;
std::vector cars;
Vect3 position;//这就是激光雷达的位置
double groundSlope;
double minDistance;
double maxDistance;
double resoultion;
double sderr;
Lidar(std::vector setCars, double setGroundSlope)
: cloud(new pcl::PointCloud()), position(0,0,2.6)
{
// TODO:: set minDistance to 5 to remove points from roof of ego car将minDistance设置为5以删除自我车顶的点
minDistance = 5;//单位米
maxDistance = 50;
resoultion = 0.2;
// TODO:: set sderr to 0.2 to get more interesting pcd files将sderr设置为0.2以获取更多有趣的pcd文件
sderr = 0.2;//加入噪音模拟真实场景
cars = setCars;
groundSlope = setGroundSlope;
// TODO:: increase number of layers to 8 to get higher resoultion pcd 将层数增加到8以获得更高的分辨率pcd
int numLayers = 8;
// the steepest vertical angle 最陡的垂直角
double steepestAngle = 30.0*(-pi/180);
double angleRange = 26.0*(pi/180);
// TODO:: set to pi/64 to get higher resoultion pcd设置为pi / 64以获取更高分辨率的pcd
double 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::Ptr scan()
{
cloud->points.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(endTime - startTime);
cout << "ray casting took " << elapsedTime.count() << " milliseconds" << endl;
cloud->width = cloud->points.size();
cloud->height = 1; // one dimensional unorganized point cloud dataset
return cloud;
}
}; 这里面我想重点解释一下，发出的光束遇到障碍物即停止的那段程序，如下所示。定义了一个光束的结构体。
struct Ray
{
Vect3 origin;
double resolution;
Vect3 direction;
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& cars, double minDistance, double maxDistance, pcl::PointCloud::Ptr& cloud, double slopeAngle, double sderr)
{
// reset ray
castPosition = 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
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));//多次循环得到很多光束
}
}
}; 这里我摘录碰撞检测部分：这里他考虑两种碰撞检测的情形，一是激光雷达的光束照到了地面上，产生了碰撞检测，2是光束照射到了周围的汽车上，也是产生了碰撞检测。
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;
}
}
}关于是否碰到地面， collision = (castPosition.z <= castPosition.x * tan(slopeAngle));//从垂直三角形这段代码的理解，我画一个图，希望有助于理解。
下述代码部分就是添加测量的不确定性了。
if((castDistance >= minDistance)&&(castDistance<=maxDistance))
{
// add noise based on standard deviation error
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));//多次循环得到很多光束
}
这部分是与周围的汽车的碰撞检测，如果碰撞了就是True。他的思路就是光束当前到达的位置、周围车辆的中心位置、以及车长、车宽，很容易计算当前的光束位置是否碰撞到了周围的汽车。
// collision helper function
bool inbetween(double point, double center, double range)
{
return (center-range <= point) && (center+range >= point);
}
bool checkCollision(Vect3 point)
{
return (inbetween(point.x,position.x,dimensions.x/2)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z/3,dimensions.z/3))||
(inbetween(point.x,position.x,dimensions.x/4)&&inbetween(point.y,position.y,dimensions.y/2)&&inbetween(point.z,position.z+dimensions.z*5/6,dimensions.z/6));
}最后就是使用激光雷达数据并可视化的地方，这里有2种可视化命令：
renderRays(viewer,lidar->position,inputCloud);它是以激光束显示，你会看到很多光束。
或者
renderPointCloud(viewer,inputCloud,"inputcloud");它是以点云显示，就是我一开始那张运行结果图。
void simpleHighway(pcl::visualization::PCLVisualizer::Ptr& viewer)
{
// ----------------------------------------------------
// -----Open 3D viewer and display simple highway -----
// ----------------------------------------------------
// RENDER OPTIONS
bool renderScene = false;
std::vector cars = initHighway(renderScene, viewer);
// TODO:: Create lidar sensor
Lidar *lidar =new Lidar(cars,0);//我们在堆上实例化，所以我们有这个激光雷达的指针类型，我们使用新的关键词，输入是汽车，汽车实际上来自于高速公路的函数。
//这就是我们模拟环境中的汽车列表。我们用零表示与地面是0斜率的，所以这是水平面，
pcl::PointCloud::Ptr inputCloud=lidar->scan();//使用激光雷达，因为我们的指针在这，所以右箭头指向扫描scan函数（没有输入）这里将会是产生pcl点云 pclxyz指针，所以我们能做的是调用这个输入点云，我们称为光束，
//renderRays(viewer,lidar->position,inputCloud);//但是我们如果想要显示他，我们称之为光束函数，我们会给他viewer和激光雷达。
/**但是我们如果想要显示他，我们称之为光束函数，我们会给他viewer*
* lidar->position: 其中position代表着激光雷达的位置（你可以在lidar里面结构提找到），所以他能告诉我们所产生的光束到底来自于那个位置。
* 然后我们继续，并且给他传递一个我之前产生的点云
* *
* 所以现在我们有雷达模块在我们的车顶上，这里就是激光雷达射出不同的光束的位置
* 我们将研究如何提高这个人分辨率，现在看起来非常稀疏，在我们讨论之前我们从没见过相同的定义
* 找一个高分辨率的激光雷达，因为他现在一些。*/
renderPointCloud(viewer,inputCloud,"inputcloud");
// TODO:: Create point processor
}
这块东西比较多，可能会比较复杂，由于时间原因我没有很详细展开讲，今天先这样，后续有空再补充，下一次会根据这次的点云数据如何提取出周围车辆的信息。
20200706 22:39
鞠春宇
于 研究室