muti_LIDAR_calibration解析

主要就是这4个可执行文件

LaserOdometry

laserMapping

scanRegistration

multi_map_fusion

通过它们之间的一个订阅和发布，完成激光雷达点云的处理，融合，标定。

首先是scanRegistration 订阅得到雷达点云并进行处理（scanRegistration.cpp）

main函数里首先初始化节点句柄
并解析出相应的配置参数 比如雷达线数，最小距离
nh.param<int>("scan_line", N_SCANS, 16);nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);然后订阅雷达话题，将其和laserCloudHandler（）函数绑定，将得到的点云进行处理
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/velodyne_points", 100, laserCloudHandler);在laserCloudHandler（）中
首先将点云转换为pcl格式
计算第一个点后最后一个点的弧度差，按作者的意思角度应该是在一个范围内，暂时没有深纠
按照雷达线数 比如16线就应该是
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS)
遍历点云里的每个点，计算出该点应该在雷达的哪根线的量程角内
然后把点push进去代码片段
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;int scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}
...
float relTime = (ori - startOri) / (endOri - startOri);point.intensity = scanID + scanPeriod * relTime;laserCloudScans[scanID].push_back(point);
按从0到15遍历每个量程角内的点，并将其累加到 laserCloud变量
for (int i = 0; i < N_SCANS; i++){scanStartInd[i] = laserCloud->size() + 5;  但是这个后边的数不知道为什么这样加减*laserCloud += laserCloudScans[i];         scanEndInd[i] = laserCloud->size() - 6;     应该就是线与线之间留得空白有些点太接近，为了区分是哪一层的}下边是比较重要的 变量
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;点云经过排序处理，依据曲率划分，成为上边四种点：
代码中使用的pcl::PointCloud类型的四个容器cornerPointsSharp、cornerPointsLessSharp、surfPointsFlat和surfPointsLessFlat分别用于存储提取得到的角点和平面点。1. 首先，通过循环遍历每个激光雷达扫描线，其中N_SCANS表示扫描线的数量。通过扫描起始和结束索引scanStartInd和scanEndInd确定每条扫描线的范围。2. 在每条扫描线内，将扫描范围等分为六个子区间，并在每个子区间内进行点云处理。3. 在每个子区间内，首先对点云按照曲率进行排序（使用std::sort函数），以便后续的点云筛选操作。                                                  对排序后的点云进行遍历，根据曲率和邻域信息对点进行分类和筛选。如果点的曲率大于阈值（0.1）且邻域内的点未被选取过（cloudNeighborPicked为0），则将该点标记为角点。如果已选取的角点数量不超过2个，则将该点添加到cornerPointsSharp和cornerPointsLessSharp容器中。如果已选取的角点数量不超过20个，则将该点添加到cornerPointsLessSharp容器中。否则，结束当前子区间的遍历。如果点的曲率小于阈值，将该点标记为平面点，并将其添加到surfPointsFlat容器中。对于未被选取的点，将其添加到临时的点云容器surfPointsLessFlatScan中。4. 对于每个子区间提取得到的surfPointsLessFlatScan点云，使用体素滤波器（pcl::VoxelGrid）进行下采样，并将结果存储在surfPointsLessFlat容器中。通过以上步骤，我们成功提取出具有显著特征的角点和平面点，并将它们存储在相应的容器中。这些角点和平面点在激光SLAM、点云配准、特征提取等任务中起到重要的作用，可以帮助我们更好地理解环境、定位机器人或构建精确的地图。将这些点按种类划分好后，就会发布对应的话题，以便给到下一个步骤进行处理
其实就是 Odometry 来进行下一阶段的操作

2. 经过上述处理，在 Odometry.cpp里会接收话题

odometry.cppros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/laser_cloud_sharp", 100, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/laser_cloud_flat", 100, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>(ns_ + "/velodyne_cloud_2", 100, laserCloudFullResHandler);可以看到为了避免数据冲突，进行加锁处理，每个话题订阅后都有一个函数来放置 比如
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{mBuf.lock();cornerSharpBuf.push(cornerPointsSharp2);mBuf.unlock();
}
应该是一个数据缓存区，每个话题都有这个文件的基本内容都在main函数里边写的
订阅到上边的话题后，会将相应数据放进各自的缓存区
cornerSharpBuf.push(cornerPointsSharp2);
cornerLessSharpBuf.push(cornerPointsLessSharp2);
surfFlatBuf.push(surfPointsFlat2);
surfLessFlatBuf.push(surfPointsLessFlat2);然后从缓存区将数据取出到对应变量首先是看systemInited 是否初始化过了
初始化其实就是 1.将第一帧作为世界坐标系
2.拿到该帧的点云并放进kdtreeCornerLast，后边来第二帧的时候才能搜索
（每次处理好的点云会放进对应的位置）
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
pubLaserCloudFullRes.publish(laserCloudFullRes3);
当初始化完成 后续进来的点云就会进行一些操作
1.ceres迭代设置两次
2.遍历角点，将当前帧点云处理成和上一帧相同坐标系，搜索上一帧最近点，及其点的索引
（好像是找了两个点，最近点和次近点）
3.遍历平面点，流程接近（不过好像是找了3个点）   具体根他们构建损失函数有关系经过迭代后优化旋转（q_w_curr）和平移（t_w_curr）参数，使得给定的点云特征在当前帧和上一帧之间的匹配误差最小化然后一样的将优化后的数据给到Odometry，进行发布我一直在想明明只用了 cornerPointsSharp  surfPointsFlat
为什么还要有 cornerPointsLessSharp  surfPointsLessFlat
后来发现是为了更新到上一帧 因为cornerPointsLessSharp  surfPointsLessFlat 里边的的点包含 cornerPointsSharp  surfPointsFlat 里边的点，而且提取条件相对宽松。使用他们更新上一帧的点云，会得到更多更好的匹配需要注意的是 每处理一帧点云都会发布，并没有前后关系，我只解释是如何处理的
当我们每处理完一帧点云更新到 laserCloudCornerLast 都会有个publish()操作
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
然后对应发布者就会将其内容发布出去 我们发布的其实是 cornerPointsLessSharp surfPointsLessFlat里的点云
但是Odometry里主要是对 cornerPointsSharp surfPointsFlat 处理的，可能是为了计算效率考虑
计算位姿也不需要那么多的点吧？由此可知该节点会发布话题如下
pubLaserCloudCornerLast->/G()/laser_cloud_corner_last
pubLaserCloudSurfLast->/G()/laser_cloud_surf_last
pubLaserCloudFullRes->/G()/velodyne_cloud_3
pubLaserOdometry->/G()/laser_odom_to_init
pubLaserPath->/G()/laser_odom_path

3. laserMapping.cpp 里主要功能实现是在 process（）函数里的

std::thread mapping_process{process}; 主函数里开了个线程

首先订阅了Odometry发布的话题，并进行处理
放置到对应的缓存区
cornerLastBuf --上帧角点 点云
surfLastBuf   --上帧平面点 点云
fullResBuf    --所有的点 点云
odometryBuf  --里程计信息 位姿只要缓存区有数据，process()函数就在不停地处理数据while (centerCubeI < 3)
while (centerCubeI >= laserCloudWidth - 3)
这是一对，一共三对
内部处理理解起来有点绕，功能就是
这段代码的作用是对点云地图进行裁剪和调整，以保持当前位置周围的点云数据处于地图中心，并获取有效点云数据和周围点云数据的索引，用于后续的处理和计算，主要是处理边缘的cube块将地图分成很多个cube块
进来的点云按照距离会分到各自的cube里边
边缘的cube会进行那几个while循环的操作
靠近中心的认为比较准，拿来迭代优化 q_w_curr t_w_curr
用优化后的更新 transformUpdate();每一块的点云做个降采样
for (int i = 0; i < laserCloudValidNum; i++){int ind = laserCloudValidInd[i];pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);downSizeFilterCorner.filter(*tmpCorner);laserCloudCornerArray[ind] = tmpCorner;pcl::PointCloud<PointType>::Ptr tmpSurf(new                                                                                                                                                          pcl::PointCloud<PointType>());downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);downSizeFilterSurf.filter(*tmpSurf);laserCloudSurfArray[ind] = tmpSurf;}基本流程就是这样
最后 判断一下 frameCount变量
有个条件限制 每5帧 发点云 /laser_cloud_surround 每20帧 发map /laser_cloud_map
还有一些别的话题发布 /velodyne_cloud_registered/aft_mapped_to_init  /aft_mapped_path
应该会在地图融合中用到，下边应该发送到multi_map_fusion  进行地图融合，并完成标定

4. 地图融合 multi_map_fusion.cpp

基本上就是一些参数配置的代码
主要是订阅了两个雷达得到的点云数据
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_map_sub1(nh, "/G6/laser_cloud_map", 1);
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_map_sub2(nh, "/G0/laser_cloud_map", 1);typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> MySyncPolicy_map;message_filters::Synchronizer<MySyncPolicy_map> sync_map(MySyncPolicy_map(3), cloud_map_sub1, cloud_map_sub2);
这两行代码好像是做时间同步的作用，我也不是很清楚首先，通过message_filters::Subscriber创建了两个消息订阅器对象cloud_map_sub1和cloud_map_sub2，分别用于订阅两个话题/G6/laser_cloud_map和/G0/laser_cloud_map。这两个话题分别对应着两个点云地图。接下来，定义了一个同步策略MySyncPolicy_map，使用ApproximateTime来进行近似时间同步。这意味着它将在两个输入消息的时间戳相近的情况下进行同步。在这个例子中，同步策略期望同时接收两个类型为sensor_msgs::PointCloud2的消息。然后，通过message_filters::Synchronizer创建了一个同步器对象sync_map，它采用之前定义的同步策略和订阅器作为参数。在这里，同步器的缓冲区大小设置为3，表示最多缓存3个最近的消息进行同步。最后，通过调用sync_map.registerCallback()方法，将同步器与回调函数cloud_map_cb绑定起来。这意味着每当两个输入消息被同步时，回调函数cloud_map_cb将被调用，同时传递两个消息作为参数。整个代码的作用是实现对两个话题的消息同步，并在同步时调用cloud_map_cb函数进行处理。 --gptcloud_map_cb()函数主要就是 将消息转成pcl格式，然后在进行icp匹配
当匹配的得到的成绩小于1时就可以输出 标定出来的外参矩阵了

muti_LIDAR_calibration解析相关推荐

golang通过RSA算法生成token，go从配置文件中注入密钥文件，go从文件中读取密钥文件，go RSA算法下token生成与解析；go java token共用
RSA算法 token生成与解析本文演示两种方式,一种是把密钥文件放在配置文件中,一种是把密钥文件本身放入项目或者容器中. 下面两种的区别在于私钥公钥的初始化, init方法,需要哪种取哪种. 通过 ...
List元素互换，List元素转换下标，Java Collections.swap()方法实例解析
Java Collections.swap()方法解析 jdk源码: public static void swap(List<?> list, int i, int j) {// ins ...
条形码？二维码？生成、解析都在这里！
二维码生成与解析一.生成二维码二.解析二维码三.生成一维码四.全部的代码五.pom依赖直接上代码: 一.生成二维码 public class demo {private static fi ...
Go 学习笔记（82）— Go 第三方库之 viper（解析配置文件、热更新配置文件）
1. viper 特点 viper 是一个完整的 Go应用程序的配置解决方案,它被设计为在应用程序中工作,并能处理所有类型的配置需求和格式.支持特性功能如下: 设置默认值读取 JSON.TOML.Y ...
Go 学习笔记（77）— Go 第三方库之 cronexpr（解析 crontab 表达式，定时任务）
cronexpr 支持的比 Linux 自身的 crontab 更详细,可以精确到秒级别. 1. 实现方式 cronexpr 表达式从前到后的顺序如下所示: 字段类型是否为必须字段允许的值允 ...
mybatis配置文件解析
mybatis配置文件解析 mybatis核心配置文件`mybatis-config.xml文件. mybatis的配置文件包含了会深深影响mybatis行为的设置和属性信息. 能配置的内容: con ...
谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...
Python 标准库之 xml.etree.ElementTree xml解析
Python 标准库之 xml.etree.ElementTree Python中有多种xml处理API,常用的有xml.dom.*模块.xml.sax.*模块.xml.parser.expat模块和 ...
谷歌BERT预训练源码解析（三）：训练过程
目录前言源码解析主函数自定义模型遮蔽词预测下一句预测规范化数据集前言本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...

muti_LIDAR_calibration解析

muti_LIDAR_calibration解析相关推荐

最新文章

热门文章