个人网站模板大全百度文档怎么免费下vvv
后端在 mapOptmization 文件中，主要功能是：
- 接收特征点云的匹配信息进行位姿估计
- 以一定频率进行回环检测并优化全局位姿
优化器的创建
创建优化器是借助 gtsam 库，LIO-SAM 创建优化器借助了以下几个部分：
// gtsam 因子图库相关变量
NonlinearFactorGraph gtSAMgraph; // 非线性因子图类
Values initialEstimate; // Values 是管理节点与节点之间的边的变量
Values optimizedEstimate; // Values 是管理节点与节点之间的边的变量
ISAM2 *isam; // gtsam 中用于求解非线性优化问题的类，可以理解为求解器类型
在上述的相关变量中，指针 *isam 在后续还需要进行创建对象，因此在构造函数创建对象时，就可以指定一些优化要用的参数：
// 创建 isam 求解器
ISAM2Params parameters; // 因子图的相关参数配置
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
回环检测与优化
回环检测会订阅相关的主题，在回调函数中将回环信息存储在队列中，但只会存储 5 个回环信息
subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());
void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
{std::lock_guard<std::mutex> lock(mtxLoopInfo); // 添加线程锁if (loopMsg->data.size() != 2)return;loopInfoVec.push_back(*loopMsg);// 只保留 5 个最新的回环信息while (loopInfoVec.size() > 5)loopInfoVec.pop_front();
}
std::lock_guard<std::mutex> 线程锁会在作用域结束后自动释放
回环检测的原理
判断是否能构成回环
用距离以及时间进行判断，是否能构成回环。
构建 kd 树，在树中寻找距离比较近，但时间比较远的帧，并将此帧的标号记录下来：
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
// 寻找空间距离相近的关键帧
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
// 确保空间距离相近的帧是较久前采集的，排除是前面几个关键帧
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{int id = pointSearchIndLoop[i];if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff){loopKeyPre = id;break;}
}
// 如果没有找到位置关系，时间关系都符合要求的关键帧，则返回false
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;ICP 配准
在找到可能构成回环的帧后，使用 pcl 库中的 ICP 配准来确定是否能真的构成回环，并且将当前帧与回环帧的位姿偏差计算出来：
// 7. 调用ICP降当前帧匹配到局部地图，得到当前帧位姿的偏差，将偏差应用到当前帧的位姿，得到修正后的当前帧位姿。
static pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// Align clouds
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);// 将回环帧与当前帧的位姿存储下来，后续构成一个回环因子
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);// 9. 将回环索引、回环间相对位姿、回环噪声模型加入全局变量
mtx.lock();
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();初始位姿计算
这部分是 mapOptmization 最主要的内容，订阅 cloud_info 这个自定义主题的消息(这个主题的消息包含后端优化所需要的所有信息，角点，平面点等，可以参考前端的内容)。
首先就是取出 cloud_info 中的角点与平面点：
cloudInfo = *msgIn;pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLa之后就是优化位姿的流程，与 LOAM 类似：
- 利用 imu 信息计算当前帧的位姿，为后续的因子图优化提供一个初始估计位姿
如果是第一帧点云，则使用最近的 imu 数据作为初始位姿；
// 第一帧点云，使用最近的imu数据来作为初始位姿
if (cloudKeyPoses3D->points.empty())
{transformTobeMapped[0] = cloudInfo.imuRollInit;transformTobeMapped[1] = cloudInfo.imuPitchInit;transformTobeMapped[2] = cloudInfo.imuYawInit;if (!useImuHeadingInitialization)transformTobeMapped[2] = 0;lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;return;
}如果不是第一帧点云，则区分两种情况：
1）一种是 cloudInfo.odomAvai 为 ture，表示有 imu 里程计(与雷达优化融合后)消息，其步骤也比较容易理解，计算 imu 预积分数据计算上一帧点云到当前点云数据的位姿变化量，再读取上一帧点云的位姿，就可以计算出当前帧的位姿：
// use imu pre-integration estimation for pose guess
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTra