百度做网站要多长时间,appstore应用商店,wordpress删除登录,东昌网站建设前言#xff1a;
LIO-SAM后端优化部分写在了mapOptimization.cpp文件中#xff0c;本部分主要进行了激光帧的scan-to-map匹配#xff0c;回环检测以及关键帧的因子图优化。本部分主要有两个环节同步进行#xff0c;一个单独开辟了回环检测线程#xff0c;另外一个是lidar…前言
LIO-SAM后端优化部分写在了mapOptimization.cpp文件中本部分主要进行了激光帧的scan-to-map匹配回环检测以及关键帧的因子图优化。本部分主要有两个环节同步进行一个单独开辟了回环检测线程另外一个是lidar数据的回调函数。
功能简介:
1、scan-to-map匹配提取当前激光帧特征点角点、平面点局部关键帧map的特征点执行scan-to-map迭代优化更新当前帧位姿2、闭环检测在历史关键帧中找距离相近时间相隔较远的帧设为匹配帧匹配帧周围提取局部关键帧map同样执行scan-to-map匹配得到位姿变换构建闭环因子数据加入因子图优化。3、关键帧因子图优化关键帧加入因子图添加激光里程计因子、GPS因子、闭环因子执行因子图优化更新所有关键帧位姿
订阅
1、订阅当前激光帧点云信息来自FeatureExtraction2、订阅GPS里程计3、订阅来自外部闭环检测程序提供的闭环数据本程序没有提供这里实际没用上。
发布
1、发布历史关键帧里程计2、发布局部关键帧map的特征点云3、发布激光里程计rviz中表现为坐标轴4、发布激光里程计5、发布激光里程计路径rviz中表现为载体的运行轨迹6、发布地图保存服务7、发布闭环匹配关键帧局部map8、发布当前关键帧经过闭环优化后的位姿变换之后的特征点云9、发布闭环边rviz中表现为闭环帧之间的连线10、发布局部map的降采样平面点集合11、发布历史帧累加的的角点、平面点降采样集合12、发布当前帧原始点云配准之后的点云。
一scan-to-map匹配
scan-to-map匹配出现在雷达数据的回调函数laserCloudInfoHandler中其是激光里程计最基本的位姿估计方法其进行的操作有当前帧位姿的初始化、组建局部地图、进行当前帧到局部地图的匹配和位姿优化、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合。
1.位姿初始化
1如果是第一帧用原始imu数据的RPY初始化当前帧位姿旋转部分
2后续帧用imu里程计计算两帧之间的增量位姿变换作用于前一帧的激光位姿是因子图联合优化后的最优位姿得到当前帧激光位姿 // note:利用imu里程计信息设置优化的初值
注意此处的当前帧和上一帧之间的位姿变换 由上一帧的imu里程计位姿的逆*当前帧的imu里程计位姿。
2.局部地图map
1对最近的一帧关键帧搜索时空维度上相邻的关键帧集合降采样一下。这里采用的kdtree进行搜索是将历史所有关键帧集合存入kdtree中进行搜索。
2对关键帧集合中的每一帧提取对应的角点、平面点加入局部map中。局部地图map中的点云超过一定数量时就会清空下一帧的点云进行scan-to-map时进行重新创建。因此局部地图并不是每一帧点云到来时都进行重新创建一次只需要往点云地图中添加新的点云即可。
3.scan-to-map位姿优化
前面位姿初始化时我们设置的当前帧的位姿主要是通过imu里程计得到这里对上面的初始化位姿进行优化。
3.1 当前激光帧角点寻找局部map匹配点
1更新当前帧位姿将当前帧角点坐标变换到map系下在局部map中查找5个最近点距离小于1m且5个点构成直线用距离中心点的协方差矩阵特征值进行判断则认为匹配上了。
2计算当前帧角点到直线的距离、垂线的单位向量存储为角点参数。
3.2 当前激光帧平面点寻找局部map匹配点
1更新当前帧位姿将当前帧平面点坐标变换到map系下在局部map中查找5个最近点距离小于1m且5个点构成平面最小二乘拟合平面则认为匹配上了。
2计算当前帧平面点到平面的距离、垂线的单位向量存储为平面点参数。
3.3 scan-to-map优化
上面的角点到直线的距离以及平面点到平面的距离就是非线性优化中的约束项并以此构建残差。
对匹配特征点计算Jacobian矩阵观测值为特征点到直线、平面的距离构建高斯牛顿方程迭代优化当前位姿存transformTobeMapped
3.4 与imu原始RPY数据加权融合
此处imu使用的是一个九轴的imu相比与六轴imu多出一个全局的imu位姿此处imu原始RPY就是一个全局的imu位姿的roll、pitch和yaw角。
更新当前帧位姿的roll, pitch, z坐标因为是小车roll、pitch是相对稳定的不会有很大变动一定程度上可以信赖imu的数据z是进行高度约束。
二闭环检测线程
在此.cpp文件的节点中单独开辟了一个回环检测线程在检测到回环后在回环检测线程会对当前帧位姿进行一个icp的优化位姿。
1.选取候选闭环帧
在历史关键帧中查找与当前关键帧距离最近的关键帧集合选择时间相隔较远的一帧作为候选闭环帧此处同样使用的是kdtree的方法进行搜索。
这里时间的间隔设置的是30s必须满足时间上超过一定的阈值才认为是一个有效的回环。
2.提取当前帧特征点集合和组建候选帧区域map
提取当前关键帧特征点集合降采样提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合降采样目的是使用scan-to-map的方法进行优化位姿。
3.scan-to-map优化icp
执行scan-to-map优化调用icp方法得到优化后位姿构造闭环因子需要的数据在因子图优化中一并加入更新位姿。
注意
前面也有scan-to-map优化是基于非线性优化的方法来进行优化的此处采用的icp点云配准的方法这种ICP方法视为基于线性优化的变体。 闭环的时候没有立即更新当前帧的位姿而是添加闭环因子让图优化去更新位姿。
通过调用PCL的icp库可以得到闭环帧到当前帧较为准确的位姿变换用此位姿变换去矫正当前帧位姿并作为约束因子加入到gtsam中等待因子图优化环节一起优化。 三因子图优化
因子图优化也是写在了雷达数据的回调函数laserCloudInfoHandler中因子图优化联合了雷达里程计信息包含IMU信息、GPS信息和闭环信息一起进行优化。因子图优化是全局关键帧优化前面的优化都是对当前帧的局部优化。
1.添加关键帧
计算当前帧与前一帧位姿变换如果变化太小不设为关键帧反之设为关键帧非关键帧则跳出不进行因子图优化。
2.添加激光里程计因子
激光里程计因子是一个帧间约束前一帧和当前帧3.3中scan-to-map之后的位姿的位姿变换。
3. 添加GPS因子
gps的数据会先被一个成熟的节点在module_navsat.launch文件处理我们直接调用的gps的处理结果。
本项目中作者对gps的使用较为谨慎。
4.添加闭环因子
闭环因子就是就是闭环检测线程中计算的当前帧和闭环帧之间的位姿变换。
5.执行因子图优化
执行了因子图优化后优化器中所有的关键帧位姿都进行了优化。
执行完因子图优化后清空一下这一轮优化使用的因子图里面的优化因子历史数据不会清掉ISAM保存起来了只是清空因子图不是清空的优化器。
到此为之不管是全局优化还是局部优化都完成了有几个变量需要注意 // note当前位姿的一个最优估计(最准确的)通过以下两种方式更新 // 1.如果这帧是普通帧通过scan to map得到 // 2.若果这帧是关键帧先通过scan to map更新再通过因子图优化更新 transformTobeMapped[6] // 历史关键帧位姿 // note所有的雷达帧位姿会在这里更新 如果是普通帧则是因子图优化之前的san-map得到的位姿如果是关键帧则是因子图优化得到的位姿 pcl::PointCloudPointTypePose::Ptr cloudKeyPoses6D; pcl::PointCloudPointType::Ptr copy_cloudKeyPoses3D; pcl::PointCloudPointTypePose::Ptr copy_cloudKeyPoses6D; 四发布激光里程计
此处发布了一个增量式里程计信息用于IMU预计分时使用。此外还有一个优化后的最优位姿用于IMU里程计使用。
增量式里程计当前帧scan to map 后的位姿没经过回环因子图优化与前一帧因子图优化后的位姿之间的位姿变换 这是一个平滑的结果。
