mapOptimization函数是LEGO-LOAM算法中的一个关键函数,用于进行建图和位姿优化的迭代优化过程。下面详细解释mapOptimization函数的主要步骤: 1.匹配点云:首先,输入的点云数据将分为地图点云和当前帧点云。mapOptimization函数将对当前帧点云进行特征提取,并与地图点云进行匹配,以估计当前帧的位姿。 2.匹配点云滤波:为...
mapOptimization节点一方面负责利用\mathbf{T}_{k-1}^k 预测LiDAR位姿 \mathbf{T}_k^w ,由scan-to-map的特征点云匹配对预测的 \mathbf{T}_k^w 进一步优化;另一方面,在并行线程中根据关键帧位置点间的距离检测回环,并采用ICP算法匹配构成回环的关键帧点云,最后进行全局位姿图优化。 transformFusion节点主要负责...
mapOptimization节点一方面负责利用 Tk−1k 预测LiDAR在世界坐标系下的位姿TkW,由scan-to-map的特征点云匹配对预测的进一步优化;另一方面,在并行线程中根据关键帧位置点间的距离检测回环,并采用ICP算法匹配构成回环的关键帧点云,最后进行全局位姿图优化。 transformFusion节点主要负责将高频运行的featureAssociation节点和低频...
intmain(intargc,char** argv){ros::init(argc, argv,"lego_loam");ROS_INFO("\033[1;32m--->\033[0m Map Optimization Started.");mapOptimization MO;// 1.进行闭环检测与闭环的功能std::threadloopthread(&mapOptimization::loopClosur...
不同的是,LeGO-LOAM将LOAM中负责点云特征提取的 scanRegistration 节点和负责scan-to-scan匹配的 laserOdometry 节点整合为 featureAssociation 节点,增加 imageProjection 节点,同时在 mapOptimization 节点中开辟闭环检测与全局优化线程。各节点的具体功能如下:
transformFusion节点负责将高频运行的featureAssociation节点和低频运行的mapOptimization节点估计的位姿进行整合输出,保证高频的位姿输出。 2.代码 相比于Aloam,Lego-Loam的输入会多一个imu数据,这里首先介绍imu数据预处理 imu数据输入及预处理 imu数据主要用于做lidar点云去畸变以及odometry位姿估计; 首先对输入的imu数据去重...
void mapOptimization::performLoopClosure(void) { if (cloudKeyPoses3D->points.empty() == true) return; // try to find close key frame if there are any if (potentialLoopFlag == false) { if (detectLoopClosure() == true) { std::cout << std::endl; ...
根据闭环条件更新2d map 闭环条件由lego-loam 3d slam 触发和后端优化,根据3d位置更新 2d投影位置。 // 若存在闭环处理,则需要对位姿进行修正,将历史的的位姿用优化后的数据进行更新 void MapOptimization::correctPoses() { if (aLoopIsClosed == true) { ...
*/if(!systemInitedLM){checkSystemInitialization();return;}updateInitialGuess();updateTransformation();//帧间里程计核心部分integrateTransformation();publishOdometry();publishCloudsLast();// cloud to mapOptimization 在这个函数中进行了 去除畸变
下面是LeGO LOAM的三个主要线程的处理时间的分布曲线。分别是image projection线程(将扫描帧投影到二维矩阵,并做一系列的预处理),feature association线程(与LOAM大概一致,特征点的提取和匹配),以及map optimization线程(低频激发的全局优化线程)。 LEGO LOAM - image projection ...