代码注释
变量注释：
class ImageProjection{
private:
ros::NodeHandle nh; //ROS句柄
// 一个订阅者
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; //接受到的来自激光Msg的原始点云数据
pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing; //用 laserCloudInRing 存储含有具有通道R的原始点云数据
pcl::PointCloud<PointType>::Ptr fullCloud; //深度图点云：以一维形式存储与深度图像素一一对应的点云数据
pcl::PointCloud<PointType>::Ptr fullInfoCloud; //带距离值的深度图点云:与深度图点云存储一致的数据，但是其属性intensity记录的是距离值
//注：所有点分为被分割点、未被分割点、地面点、无效点。
pcl::PointCloud<PointType>::Ptr groundCloud; //地面点点云
pcl::PointCloud<PointType>::Ptr segmentedCloud; //segMsg 点云数据:包含被分割点和经过降采样的地面点
pcl::PointCloud<PointType>::Ptr segmentedCloudPure; //存储被分割点点云，且每个点的i值为分割标志值
pcl::PointCloud<PointType>::Ptr outlierCloud; //经过降采样的未分割点
cv::Mat rangeMat; //由激光点云数据投影出来的深度图像
cv::Mat labelMat; //分割的标志矩阵：每一个数字代表一个分割的类别,大值表示未分割点,-1表示不需要被分割(地面点和无效点);
cv::Mat groundMat; //地面点的标志矩阵:1表示为地面点;0表示不是;-1表示无法判断
int labelCount; //成功分割的簇的数量
//一些临时使用的变量
PointType nanPoint;
float startOrientation;
float endOrientation;
std_msgs::Header cloudHeader;
cloud_msgs::cloud_info segMsg; // segMsg点云信息(存储分割结果并用于发送)
// segMsg{
// Header header //与接收到的点云数据header一致
// int32[] startRingIndex //segMsg点云中,每一行点云的起始和结束索引
// int32[] endRingIndex
// float32 startOrientation // 起始点与结束点的水平角度(atan(y,x))
// float32 endOrientation
// float32 orientationDiff //以上两者的差
// bool[] segmentedCloudGroundFlag //segMsg中点云的地面点标志序列(true:ground point)
// uint32[] segmentedCloudColInd// segMsg中点云的cols序列
// float32[] segmentedCloudRange // segMsg中点云的range
// }
std::vector<std::pair<int8_t, int8_t> > neighborIterator; // 四个pair集合(1,0)(-1,0)(0,1)(0,-1),用于在分割过程中检索点的深度图邻域
// 分割过程中的临时变量
uint16_t *allPushedIndX; // 用来记录一次聚类的中取得的点的像素位置(x,y)
uint16_t *allPushedIndY; //
uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
uint16_t *queueIndY;
public:
...
函数注释
class ImageProjection{
private:
...
public:
//构造函数
ImageProjection():
nh("~"){
// 订阅主题 pointCloudTopic , 接收到点云信息后，调用函数 cloudHandler.
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
// 多个发布者的声明
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
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;
// 相关变量的初始化与赋予空间
allocateMemory();
// 相关变量的初始化.
resetParameters();
}
//回调函数，对点云数据处理的主要部分
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
// std_msgs::Header cloudHeader 获取消息的header
// laserCloudIn(原始点云) 获得点云消息中去除无效点后的点云
// useCloudRing 存储点云消息中的点云
// 2. Start and end angle of a scan
findStartEndAngle();
// segMsg.startOrientation 记录起始点的角度(atan(y,x))
// segMsg.endOrientation 记录结束点的角度(atan(y,x))
// segMsg.orientationDiff 记录上面两者的差值
// 3. Range image projection
projectPointCloud();
// a. 获取深度图 rangeMat
// b. fullCloud 记录深度图点云
// c. fullInfoCloud 记录深度图点云以及对应每个点的距离属性。
// 4. Mark ground points
groundRemoval();
// a. 为地面点标志矩阵 groundMat 赋值: 1表示为地面点;0表示不是;-1表示无法判断
// b. 为分割标志矩阵 labelMat 赋值: -1:不需要被分割的点(地面点和无效点);0:其他
// 5. Point cloud segmentation
cloudSegmentation();
// a. 在深度图上对每个点进行分割,分割结果存储在 labelMat(分割标签矩阵) 中,其中确定值表示点所属类别,大值表示点未被分割,-1值表示地面点和无效点
// b. 遍历每个点， outlierCloud 记录经过降采样的未被分割点
// segmentedCloud 记录seg点云数据:包含被分割点和经过降采样的地面点
// segMsg.segmentedCloudGroundFlag 记录seg中点云的地面点标志序列
// segMsg.segmentedCloudColInd 记录seg中点云的cols
// segMsg.segmentedCloudRange 记录seg中点云的range
// c. 为发布者 pubSegmentedCloudPure 准备数据 segmentedCloudPure
// segmentedCloudPure 存储被分割点点云，且每个点的i值为分割标志值。
// 6. Publish all clouds
publishCloud();
// pubSegmentedCloudInfo 发布 segMsg
// segMsg{
// Header header //与接收到的点云数据header一致
// int32[] startRingIndex //segMsg点云中,每一行点云的起始和结束索引
// int32[] endRingIndex
// float32 startOrientation// 起始点与结束点的水平角度(atan(y,x))
// float32 endOrientation
// float32 orientationDiff //以上两者的差
// bool[] segmentedCloudGroundFlag //segMsg中点云的地面点标志序列(true:ground point)
// uint32[] segmentedCloudColInd // segMsg中点云的cols序列
// float32[] segmentedCloudRange // segMsg中点云的range
// }
// pubOutlierCloud 发布 msg{ //降采样的未被分割点云
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// outlierCloud;
// }
// pubSegmentedCloud 发布 msg{ //seg点云数据:包含被分割点和经过降采样的地面点
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// segmentedCloud;
// }
// pubFullCloud 发布 msg{ //一维形式存储深度图对应的点云
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// fullCloud;
// }
// pubGroundCloud 发布 msg{ //地面点集合
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// groundCloud;
// }
// pubSegmentedCloudPure 发布 msg{ // 存储被分割点点云，且每个点的i值为分割标志值
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// segmentedCloudPure;
// }
// pubFullInfoCloud 发布 msg{ // 存储被分割点点云，且每个点的i值为分割标志值
// msg.header.stamp = cloudHeader.stamp;
// msg.header.frame_id = "base_link";
// fullInfoCloud; //带距离值的深度图点云:与深度图点云存储一致的数据，但是其属性intensity记录的是距离值
// }
// 7. Reset parameters for next iteration
resetParameters();
}
/*
功能:以输入(row,col)为起点,进行聚类分割
a. 以输入(row,col)为起点,获得聚类的所有点,存储在 allPushedIndSize(此次聚类的点的数量) (allPushedIndX,allPushedIndSize)(位置序列)
b. 进行有效簇判断:单簇超过30个点，或者单簇超过5个点且跨越3个ring,视为有效簇
c. 如果是有效簇: 为 labelMat 中的每个点赋予簇的标志值 labelCount,同时 labelCount++
d. 如果不是有效簇：为 labelMat 中该簇的每个点赋予 999999,即不在与用于以后的聚类
*/
void labelComponents(int row, int col){
...
}
/*
仅定义了ImageProjection IP,即调用构造函数 ImageProjection().
*/
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;
}
运行机理
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
