一、主函数
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 cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
findStartEndAngle();
// 3. Range image projection
projectPointCloud();
// 4. Mark ground points
groundRemoval();
// 5. Point cloud segmentation
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}可以看到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 line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// have "ring" channel in the cloud
if (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 cloud
segMsg.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 projection
float 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 point
if (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。
