lego-loam imageProjection.cpp源码注释（一）

 一、主函数

int main(int argc, char** argv){ros::init(argc, argv, "lego_loam");ImageProjection IP;ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");ros::spin();return 0;
}

主函数很简单，常规ros初始化ros::init，ros::spin()进入循环，等待回调函数被调用。

在ROS（Robot Operating System）中，`ROS_INFO` 是一个宏，用于打印信息级别的日志消息。这些消息通常用于调试和监控ROS节点的行为。`ROS_INFO` 宏会将消息发送到ROS日志系统，可以通过 `rosconsole` 配置来控制其显示的详细程度。在你提供的代码行中：ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");这条消息使用了ANSI转义码来改变日志消息的颜色。ANSI转义码是一种特殊的字符序列，用于控制视频文本终端的样式，包括颜色、亮度等。在这个例子中：- `\033[1;32m` 是一个ANSI转义码，用于设置文本颜色为绿色（`32` 是绿色的代码）。`1` 是一个加粗的标志，但并不是所有的终端都支持。
- `\033[0m` 是另一个ANSI转义码，用于重置文本样式到默认值。因此，这条消息会在ROS日志中打印出绿色的“---->“和正常颜色的”Image Projection Started.”文本，并且在文本前后没有特殊格式。这可以帮助开发者在日志输出中快速识别重要的步骤或事件。请注意，ANSI转义码在所有终端和ROS日志查看器中可能不都支持。如果在不支持ANSI转义码的环境中运行，这些代码将不会被解释为颜色代码，而是作为普通文本输出。

二、ImageProjection结构体定义

 private:

class ImageProjection{

private:

ros::NodeHandle nh;

//常规ros初始化三板斧之一，nh非常有用，可以用来发布数据、接收数据、设置参数、获取参数。

ros::Subscriber subLaserCloud; //接收原始点云数据

ros::Publisher pubFullCloud;

ros::Publisher pubFullInfoCloud;

ros::Publisher pubGroundCloud;

ros::Publisher pubSegmentedCloud;

ros::Publisher pubSegmentedCloudPure;

ros::Publisher pubSegmentedCloudInfo;

ros::Publisher pubOutlierCloud;

//从发布的数据可以看出，做了哪些处理：提取地面点，分割点云（刨除聚类中比较零散的点，然后再对大的聚类做分割），刨除异常点（或者是不可视的点），保留有效点云。具体的内容看下面的代码。

pcl::PointCloud<PointType>::Ptr laserCloudIn;

//解决了上一篇中的一个问题！！！之所以没有定义PointXYZIR是因为pcl中有现成的定义！！！

pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;

pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix

pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range

//这两个1-D matrix，intensity - range没有看懂，后续具体看用法。

pcl::PointCloud<PointType>::Ptr groundCloud;

pcl::PointCloud<PointType>::Ptr segmentedCloud;

pcl::PointCloud<PointType>::Ptr segmentedCloudPure;

pcl::PointCloud<PointType>::Ptr outlierCloud;

PointType nanPoint; // fill in fullCloud at each iteration

cv::Mat rangeMat; // range matrix for range image 深度矩阵/存储距离的矩阵

cv::Mat labelMat; // label matrix for segmentaiton marking 用于标记分割后的点云，然后从地面点点中提取面特征，从非地面点中提取角点。

cv::Mat groundMat; // ground matrix for ground cloud marking

？？很奇怪labelMat和groundMat不可以合成一个吗？

int labelCount;

float startOrientation;

float endOrientation;

cloud_msgs::cloud_info segMsg; // info of segmented cloud

std_msgs::Header cloudHeader;//std_msgs::Header。这是一个标准的消息类型，通常用于存储关于消息的元数据，如时间戳（timestamp）和帧ID（frame_id）。

std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process 在点云分割过程中，需要找到每个点的邻居点。每个 std::pair 可以表示一个点的两个邻居点的索引或者标签。

uint16_t *allPushedIndX; // array for tracking points of a segmented object

uint16_t *allPushedIndY;

uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed

//"Breadth-first"（广度优先）是一种遍历数据结构的算法策略，通常用于图遍历或树遍历。

uint16_t *queueIndY;

public:

ImageProjection():

nh("~"){  //nh("~")：这是ros::NodeHandle的构造函数。

subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);

//extern const string pointCloudTopic = "/velodyne_points";

//它订阅了"/velodyne_points"，这个话题上传递的是sensor_msgs::PointCloud2类型的消息。&ImageProjection::cloudHandler是当接收到消息时回调的函数，this是指向当前对象的指针。

pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);

pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);

//1是发布队列的大小，它指定了ROS在发布者发布消息时可以存储在队列中的消息数量。如果发布者发布消息的速率超过了任何订阅者的消费速率，超出的消息将被丢弃。

pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);

pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);

pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("segmented_cloud_pure", 1);

pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);

pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);

nanPoint.x = std::numeric_limits<float>::quiet_NaN();

nanPoint.y = std::numeric_limits<float>::quiet_NaN();

nanPoint.z = std::numeric_limits<float>::quiet_NaN();

nanPoint.intensity = -1;

//std::numeric_limits<float>::quiet_NaN() 是一个标准库函数，它返回一个特殊的浮点数值，表示“安静”的 NaN（quiet NaN）。安静 NaN 的特点是，当它作为算术运算的操作数时，不会触发异常（例如，除以零）。与之相对的是“信号” NaN（signaling NaN），它在被使用时会触发异常。

allocateMemory();

resetParameters();

}

回调函数cloudHandler

咱们不按原代码顺序，直接去看处理点云的回调函数！！

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){// 1. Convert ros message to pcl point cloudcopyPointCloud(laserCloudMsg);// 2. Start and end angle of a scanfindStartEndAngle();// 3. Range image projectionprojectPointCloud();// 4. Mark ground pointsgroundRemoval();// 5. Point cloud segmentationcloudSegmentation();// 6. Publish all cloudspublishCloud();// 7. Reset parameters for next iterationresetParameters();}

 可以看到lego-loam将几个功能封装成了不同的函数。

1.将数据从ros消息转换为pcl点云。

2.找到一次扫描的起始角和终止角。

3.将扫描的点云投影到深度相片上。

4.提取地面点

5.点云分割

6.发布计算结果的点云

7.重置参数

copyPointCloud（laserCloudMsg）

~ImageProjection(){}

//在C++中，~ImageProjection() 表示 ImageProjection 类的析构函数。析构函数是一种特殊的成员函数，当一个对象的生命周期结束时，它会被自动调用。它的目的是执行任何必要的清理工作，例如释放资源、关闭文件、断开网络连接等。

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){cloudHeader = laserCloudMsg->header;// cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this linepcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);// Remove Nan pointsstd::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);// have "ring" channel in the cloudif (useCloudRing == true){pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);if (laserCloudInRing->is_dense == false) {ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");ros::shutdown();}  }}

//cloudHeader.stamp = ros::Time::now();奥斯特雷达的用户必须添加这一行。

//extern const bool useCloudRing = true; 默认useCloudRing是True

//对于没有ring类型的数据可以直接读到laserCloudIn中，而有ring类型的数据可以读到laserCloudInRing中。

//pcl::PointCloud<PointType>::Ptr laserCloudIn;  XYZI

//pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing; XYZIR，这个PointXYZIR在头文件中定义。

if (laserCloudInRing->is_dense == false)：这是一个条件语句，检查点云对象 laserCloudInRing 的 is_dense 成员变量是否为 false。在PCL（点云库）中，is_dense 标志指示点云是否为密集格式，即不包含任何无效点（NaN或无穷大）。

??为什么不延用removeNANFromPointCloud呢？？

findStartEndAngle()

 void findStartEndAngle(){// start and end orientation of this cloudsegMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {segMsg.endOrientation -= 2 * M_PI;} else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)segMsg.endOrientation += 2 * M_PI;segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;}

//cloud_msgs::cloud_info segMsg; // info of segmented cloud 存储点云信息。

//atan2 函数的取值范围是 [−π,π]。加2π后范围是π到3π。

1.因为激光雷达是顺时针转的，而坐标系是右手系，逆时针为正，顺时针为负，加一个负号与扫描方向一致，也就是顺时针为正。

2.为什么要给结束的点加上2π，因为结束的角度肯定比起始角度大，那么这样理解，假设起始角为0，那么当结束角过了半周为负，此时加2π，可以使得差值恢复正常。

 如图，当结束点过了起始点，此时，加多一圈2π,对应的就是实际转过的角度。（图片来自b站up主@lyxichigoichie）

这个图来自大佬的博客：LeGo-Loam代码解析ImageProjection节点（一）_start and end orientation of this cloud-CSDN博客

为什么在大于3π，小于π的情况要特殊考虑呢？

实际是将角度差控制在π到3π的范围之间了。

为什么要控制方位角在π到3π呢？

这一帧雷达转了多少，要限制在π到3π之间。

这是大于3π的情况，此时激光雷达转了一圈半还要多，显然不能差那么多。所以减去2π，认为它转了半圈多。

同理，对于小于π的情况，-140+360-140，也就是激光雷达只转了80度，也显然不太对，我们认为它应该是多转了一圈，也就是440度。即我们只允许激光雷达终止角相比于起始角多转半圈或者少转半圈。

 projectPointCloud()

生成距离图像，重投影之后，三维点云变为二维图像，以像素点到传感器之间的距离作为像素值。

 void projectPointCloud(){// range image projectionfloat verticalAngle, horizonAngle, range;size_t rowIdn, columnIdn, index, cloudSize; PointType thisPoint;cloudSize = laserCloudIn->points.size();for (size_t i = 0; i < cloudSize; ++i){thisPoint.x = laserCloudIn->points[i].x;thisPoint.y = laserCloudIn->points[i].y;thisPoint.z = laserCloudIn->points[i].z;// find the row and column index in the iamge for this pointif (useCloudRing == true){rowIdn = laserCloudInRing->points[i].ring;}else{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;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 < sensorMinimumRange)continue;rangeMat.at<float>(rowIdn, columnIdn) = range;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"}}

 PointType 是typedef pcl::PointXYZI PointType;这个类型。

 rowIdn = (verticalAngle + ang_bottom) / ang_res_y;//ang_bottom等于15.1,为什么是这个数呢？因为扫描仪向下扫描时角度为负，为了计算rowldn不能有负值，就加上最小扫描角的相反数，使竖直方向扫描角均为正。

size_t是无符号整数，在有小数时会向下取整！！

比较难理解的还是columnIdn的计算！！

 horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

 columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;

 雷达对应的坐标系如图：

雷达坐标系是以y轴正方向为起始，顺时针为正的坐标系（与算起始点·终止点夹角的坐标系区分开）。

注意！！这里就可以看到，雷达坐标系和计算角度的坐标系以逆时针为正不一样，是以顺时针为正的。

horizonAngle-90.0，减去90度对应的坐标系是以x轴正方向为0的坐标系，也就是x轴正对前方的坐标系，这将和计算角度的坐标系起始保持一致。-round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2将角度转换为列索引，此时对应的坐标系是以逆时针计算（因为乘了负号），并且以x轴负方向为起始（因为加了Horizon_SCAN/2，作用类似加了π）的坐标系，如下图。可以看到-x ,-y区域的值超过H，再单独处理。

处理之后对应的是，以x轴负方向为起始，逆时针计算的坐标系，取值范围是【0，H】。

 然后解答两个问题：

1.为什么要加Horizon_SCAN/2？

我认为这个的作用相当于加π，让columnIdn的值非负。

2.为什么要减90度？

atan2(thisPoint.x, thisPoint.y) 计算得到的，
这个角度是从正x轴开始逆时针测量的。因此，当 thisPoint 位于正x轴上时，
horizonAngle 为0度；当它位于正y轴上时，horizonAngle 为90度；在许多激光雷达系统中，点云的水平角度是从激光雷达的正前方（通常是设备安装的方向）开始测量的，
这个方向在水平面上的角度是0度。因此，为了将 horizonAngle 转换为这种坐标系统中的列索引，
需要将 horizonAngle 减去90度。这样，当点云中的点位于激光雷达正前方时，
其水平角度为0度，对应的列索引为0。

 rangeMat.at<float>(rowIdn, columnIdn) = range;将三维扫描的点云转换成了深度矩阵。

最后记录点云的索引和点云距离雷达的range。