LeGO-LOAM代码和原理解析（1）-ImageProjection ⽂章⽬录
前⾔
imageProjecion.cpp进⾏的数据处理是图像映射，将得到的激光数据分割，并在得到的激光数据上进⾏坐标变换。
⼀、ImageProjection成员变量
pcl::PointCloud<PointType>::Ptr larCloudIn;//接受到的来⾃激光Msg的原始点云数据
pcl::PointCloud<PointXYZIR>::Ptr larCloudInRing;//⽤ larCloudInRing 存储含有具有通道R的原始点云数据
//深度图点云：以⼀维形式存储与深度图像素⼀⼀对应的点云数据
pcl::PointCloud<PointType>::Ptr fullCloud;// projected velodyne raw cloud, but saved in the form of 1-D matrix
//带距离值的深度图点云：与深度图点云存储⼀致的数据，但是其属性intensity记录的是距离值
pcl::PointCloud<PointType>::Ptr fullInfoCloud;// same as fullCloud, but with intensity - range
//注：所有点分为被分割点、未被分割点、地⾯点、⽆效点。
pcl::PointCloud<PointType>::Ptr groundCloud;//地⾯点点云
pcl::PointCloud<PointType>::Ptr gmentedCloud;//gMsg 点云数据：包含被分割点和经过降采样的地⾯点优美作文开头结尾摘抄
pcl::PointCloud<PointType>::Ptr gmentedCloudPure;//存储被分割点点云，且每个点的i值为分割标志值
pcl::PointCloud<PointType>::Ptr outlierCloud;//经过降采样的未分割点
另外使⽤⾃创的rosmsg来表⽰点云信息
// gMsg点云信息(存储分割结果并⽤于发送)
cloud_msgs::cloud_info gMsg;
具体定义如下：
Header header
int32[] startRingIndex // 长度：N_SCAN
int32[] endRingIndex // 长度：N_SCAN
float32 startOrientation
float32 endOrientation
float32 orientationDiff
// 以下长度都是 N_SCAN*Horizon_SCAN
bool[] gmentedCloudGroundFlag # true - ground point, fal - other points
uint32[] gmentedCloudColInd # point column index in range image
float32[] gmentedCloudRange # point range
⼆、构造函数ImageProjection()
main()函数通过构造函数订阅和发布消息。代码如下：
subLarCloud = nh.subscribe<nsor_msgs::PointCloud2>
(pointCloudTopic,1,&ImageProjection::cloudHandler, this);
pubFullCloud = nh.adverti<nsor_msgs::PointCloud2>("/full_cloud_projected",1);
pubFullInfoCloud = nh.adverti<nsor_msgs::PointCloud2>("/full_cloud_info",1);
pubGroundCloud = nh.adverti<nsor_msgs::PointCloud2>("/ground_cloud",1);
pubSegmentedCloud = nh.adverti<nsor_msgs::PointCloud2>("/gmented_cloud",1);
pubSegmentedCloudPure = nh.adverti<nsor_msgs::PointCloud2>("/gmented_cloud_pure",1);
pubSegmentedCloudInfo = nh.adverti<cloud_msgs::cloud_info>("/gmented_cloud_info",1);
pubOutlierCloud = nh.adverti<nsor_msgs::PointCloud2>("/outlier_cloud",1);
...
allocateMemory();//为指针分配内存，否则出现野指针错误，初始化⼀次
retParameters();//容器清空，参数初始化化，为下⼀次循环做准备，回调函数中循环美林谷滑雪场
其中pointCloudTopic就是输⼊点云的topic，本代码中为/os1_points 。
1.分配内存allocateMemory
初始化：为成员变量指针分配内存，否则出现野指针错误，在构造函数中初始化⼀次。
代码如下：
void allocateMemory(){
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointXYZIR>());
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointType>());
<(new pcl::PointCloud<PointType>());
fullCloud-&size(N_SCAN*Horizon_SCAN);
fullInfoCloud-&size(N_SCAN*Horizon_SCAN);
gMsg.startRingIndex.assign(N_SCAN,0);
mm照片
std::pair<int8_t, int8_t> neighbor;//上下左右偏移
neighbor.first =-1; d =0; neighborIterator.push_back(neighbor);
neighbor.first =0; d =1; neighborIterator.push_back(neighbor);
neighbor.first =0; d =-1; neighborIterator.push_back(neighbor);
neighbor.first =1; d =0; neighborIterator.push_back(neighbor);
allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];
queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
}
三、回调函数cloudHandler
代码会在回调函数内进⾏循环，需注意成员变量容器的清空，否则会内存泄露，本代码由构造函数中retParameters()函数进⾏clear 由它调⽤其他的函数。代码如下：
void cloudHandler(const nsor_msgs::PointCloud2ConstPtr& larCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(larCloudMsg);//点云的复制
// 2. Start and end angle of a scan
findStartEndAngle();//寻找始末⾓度
// 3. Range image projection
projectPointCloud();//点云投影
// 4. Mark ground points
groundRemoval();//地⾯检测
// 5. Point cloud gmentation
cloudSegmentation();//点云分割
// 6. Publish all clouds
publishCloud();
// 7. Ret parameters for next iteration
retParameters();//容器清空，参数初始化化，为下⼀次循环做准备
｝
本函数将ROS中的nsor_msgs::PointCloud2ConstPtr类型转换到pcl点云库指针。代码如下：
void copyPointCloud(const nsor_msgs::PointCloud2ConstPtr& larCloudMsg){
cloudHeader = larCloudMsg->header;
cloudHeader.stamp = ros::Time::now();// Ouster lidar urs may need to uncomment this line
pcl::fromROSMsg(*larCloudMsg,*larCloudIn);
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*larCloudIn,*larCloudIn, indices);
// have "ring" channel in the cloud
if(uCloudRing == true){
pcl::fromROSMsg(*larCloudMsg,*larCloudInRing);
if(larCloudInRing->is_den == fal)// 指定points中的数据是否全部是有限的，此时为true；当数据集中包含有Inf/NaN等值时，此时为fal。
{
ROS_ERROR("Point cloud is not in den format, plea remove NaN points first!");
ros::shutdown();
}
}
}
2.findStartEndAngle
本函数将起始点与最末点进⾏⾓度的转换
并保存到gMsg：
1）计算开始⾓度（gMsg.startOrientation）；
2）计算结束⾓度（dOrientation）；
3）计算雷达转过的⾓度（开始和结束的⾓度差，ientationDiff）。
代码如下（⽰例）：
//将起始点与最末点进⾏⾓度的转换
void findStartEndAngle(){
//计算⾓度时以x轴负轴为基准(-pi~-0~0~pi)
gMsg.startOrientation =-atan2(larCloudIn->points[0].y, larCloudIn->points[0].x);
larCloudIn->points[larCloudIn->points.size()-1].x)+2* M_PI;
dOrientation - gMsg.startOrientation >3* M_PI){
}el dOrientation - gMsg.startOrientation < M_PI)
}
下图依次是atan2和-atan的取值范围，以及正常和特殊情况下开始⾓度和结束⾓度
3.projectPointCloud
本函数逐⼀计算点的深度，将具有深度的点云保存⾄fullInfoCloud中。
以16为例，将激光雷达得到的数据看成⼀个16x1800的点云阵列。然后根据每个点云返回的XYZ数据将他们对应到这个阵列⾥去。
1）计算竖直⾓度，⽤atan2函数进⾏计算。
2）通过计算的竖直⾓度得到对应的⾏的序号rowIdn，rowIdn计算出该点激光雷达是⽔平⽅向上第⼏线的。从下往上计数，-15度记为初始线，第0线，⼀共16线(N_SCAN=16)。
3）求⽔平⽅向上的⾓度horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI。
4）根据⽔平⽅向上的⾓度计算列向columnIdn。
5. 接着在thisPoint.intensity中保存⼀个点的位置信息rowIdn+columnIdn / 10000.0，存⼊fullInfoCloud
点云。
对视恐惧症代码如下：
//逐⼀计算点云深度，并具有深度的点云保存⾄fullInfoCloud中；本函数将整个点云数据投影成⼀个16*1800的⼆维平⾯图
void projectPointCloud(){
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;
cloudSize = larCloudIn->points.size();
for(size_t i =0; i < cloudSize;++i){杜甫的诗风
thisPoint.x = larCloudIn->points[i].x;
thisPoint.y = larCloudIn->points[i].y;
thisPoint.z = larCloudIn->points[i].z;
// find the row and column index in the iamge for this point
if(uCloudRing == true){
rowIdn = larCloudInRing->points[i].ring;
}
el{
//计算竖直⽅向上的点的⾓度以及在整个雷达点云中的哪⼀条⽔平线上
verticalAngle =atan2(thisPoint.z,sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y))*180/ M_PI;
rowIdn =(verticalAngle + ang_bottom)/ ang_res_y;
}
/
/出现异常⾓度则⽆视
if(rowIdn <0|| rowIdn >= N_SCAN)
continue;
//计算⽔平⽅向上点的⾓度与所在线数
horizonAngle =atan2(thisPoint.x, thisPoint.y)*180/ M_PI;
徐晨妍
//round是四舍五⼊取偶
columnIdn =-round((horizonAngle-90.0)/ang_res_x)+ Horizon_SCAN/2;
if(columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if(columnIdn <0|| columnIdn >= Horizon_SCAN)
continue;
/说大话使小钱
/当前点与雷达的深度
range =sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if(range < nsorMinimumRange)
continue;
rangeMat.at<float>(rowIdn, columnIdn)= range;
// 每个点的强度值，整数部分代表⾏号，⼩数部分代表列号。后⾯会通过intensity来恢复这个点
thisPoint.intensity =(float)rowIdn +(float)columnIdn /10000.0;
index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index]= thisPoint;
fullInfoCloud->points[index]= thisPoint;
fullInfoCloud->points[index].intensity = range;// the corresponding range of a point is saved as "intensity"
}
本函数利⽤不同的扫描圈来表⽰地⾯，进⽽检测地⾯是否⽔平。例如在源码中的七个扫描圈，每两个圈之间进⾏⼀次⽐较，⾓度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加⼊到groundCloud点云。
代码如下：
//利⽤不同的扫描圈来表⽰地⾯，进⽽检测地⾯是否⽔平。例如在源码中的七个扫描圈，每两个圈之间
//进⾏⼀次⽐较，⾓度相差10°以内的我们可以看做是平地。并且将扫描圈中的点加⼊到groundCloud点云void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
for(size_t j =0; j < Horizon_SCAN;++j){
for(size_t i =0; i < groundScanInd;++i){
lowerInd = j +( i )*Horizon_SCAN;
upperInd = j +(i+1)*Horizon_SCAN;
if(fullCloud->points[lowerInd].intensity ==-1||
fullCloud->points[upperInd].intensity ==-1){
// no info to check, invalid points其实很简单
groundMat.at<int8_t>(i,j)=-1;
continue;
}
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle =atan2(diffZ,sqrt(diffX*diffX + diffY*diffY))*180/ M_PI;
if(abs(angle - nsorMountAngle)<=10){
groundMat.at<int8_t>(i,j)=1;
groundMat.at<int8_t>(i+1,j)=1;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for gmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan for(size_t i =0; i < N_SCAN;++i){
for(size_t j =0; j < Horizon_SCAN;++j){
if(groundMat.at<int8_t>(i,j)==1|| rangeMat.at<float>(i,j)== FLT_MAX){
labelMat.at<int>(i,j)=-1;
}
}
}
NumSubscribers()!=0){
for(size_t i =0; i <= groundScanInd;++i){
for(size_t j =0; j < Horizon_SCAN;++j){
if(groundMat.at<int8_t>(i,j)==1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
}
}
}
}
各种标记的含义
