0 整体框架分析
翻看 LEGO-Loam 的代码目录，首先进入到launch 文件中，看到：
<!--- LeGO-LOAM -->
<node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
<node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
<node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
<node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>
luanch 文件启动了四个 node ，所以整个程序执行了四个 node 的基础上完成了整体功能的实现。
然后再打开 CmakeList.txt 可以看到这些 node 的一些依赖
add_executable(imageProjection src/imageProjection.cpp)
add_dependencies(imageProjection ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(featureAssociation src/featureAssociation.cpp)
add_dependencies(featureAssociation ${catkin_EXPORTED_TARGETS} cloud_msgs_gencpp)
target_link_libraries(featureAssociation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(mapOptmization
src/mapOptmization.cpp
src/Scancontext.cpp
)
target_link_libraries(mapOptmization
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${OpenCV_LIBRARIES}
gtsam
Eigen3::Eigen
)
add_executable(transformFusion src/transformFusion.cpp)
target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
简单截取了以上部分，每一个节点之间链接了那些库可以看的很清楚。接下来的内容，按照这四个 node 的顺序 逐步对整个代码进行分析
1 ImageProjection – 将激光点云处理成图像
在这个node 中，cpp 文件实现了 激光点云特征提取，标号，分割等功能
第一步，先打开main 函数：
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;
}
在 main 函数中，只是实例化了一个对象 IP ，所以 一旦创建了一个对象，就进入到了 Class 中去执行构造函数。
ImageProjection() : nh("~")
{
// 订阅原始的激光点云
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();
}
class 中的构造函数中，订阅了原始的激光点云数据（subLaserCloud）然后就进入到了 回调函数（ImageProjection::cloudHandler）对点云数据进行处理。
// 订阅激光雷达点云信息之后的回调函数
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 1. 使用 pcl库
copyPointCloud(laserCloudMsg);
// 2. 找到开始时刻和结束时刻的方向角度
findStartEndAngle();
// 3.将点云信息投影到 16 * 1800 分辨率的图像上（点云阵列上）
projectPointCloud();
// 4. 根据上下线束 俯仰角 判断是否是 地面 （角度小于10度 为地面）
groundRemoval();
// 5. 点云分割 首先对点云进行聚类标记 然后通过聚类完成的标签 对点云分块存储
cloudSegmentation();
剥洋葱
// 6. Publish all clouds 发布各类型的点云
publishCloud();
// 7. 重启
resetParameters();
}
回调函数中 按照以上 7 个逻辑步骤 对点云进行处理 个人认为这个回调函数写的相当nice 主要的逻辑步骤通过一个个封装好的函数呈现出来。按照总分的结构下面将逐步对这几个步骤进行分析。
1.1 ROS 消息转PCL
由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息，所以在处理的过错中统一需要对消息类型转换，通常的办法就是使用 PCL 库
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 1. 读取ROS点云转换为PCL点云
cloudHeader = laserCloudMsg->header;
cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
//2.移除无效的点云 Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// 3. have "ring" channel in the cloud or not
// 如果点云有"ring"通过，则保存为laserCloudInRing
// 判断是不是使用了 velodyne 的激光雷达
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();
}
}
}
这个函数主体内容 分为三个步骤，其中的第三步 注意区分 自己使用的激光雷达是否存在 CloudRing 没有这个的激光雷达 接下来的步骤都不会去执行，所以在实际自己跑这个框架的时候一定要注意到这个地方
1.2 寻找方向角
void findStartEndAngle()
{
// 1.开始点和结束点的航向角 (负号表示顺时针旋转)
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
// 加 2 * M_PI 表示已经转转了一圈
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
laserCloudIn->points[laserCloudIn->points.size() - 1].x) +
2 * M_PI;
// 2.保证 所有角度 落在 [M_PI , 3M_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;
}
这个函数在 LOAM 代码里面也存在。首先通过一个反正切函数 找到一帧激光点云起始时刻和结束时刻的航向角 第二步的作用 我个人认为是将一帧激光点云的航向角度限制在【pi , 3pi 】 之间，方便后续计算出一帧激光点云的时间。
1.3 激光点云图片化
// 3.点云消息处理成图像方式的阵列 将一帧点云变成 16* 1800 的图片
// rangeMat.at<float>(rowIdn, columnIdn) = range; 关键的代码
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)
{
// 提取点云中 x y z 坐标数值
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
// 判断是不是使用了 velodyne 的雷达
if (useCloudRing == true)
{
// 提取激光雷达线束到 rowIdn
rowIdn = laserCloudInRing->points[i].ring;
}
// 是其他的雷达 就通过俯仰角 确定当前的激光点是来自哪个线束 index
else
{
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
// idx = (theta +15.1)/2
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
