好了，到目前为止已经探索完了IMU预积分、点云去畸变以及特征提取这三个功能的程序。小兵退散完该boss上场了，小两千行的建图优化程序来了。这个文件的代码量好像是前面的三个那么多。第一遍大概浏览的时候看到那些密密麻麻的定义真的是一个头两个大。但没办法，硬着头皮上吧，在梳理整个函数流程图的时候，结构实在是有点复杂，函数套函数的结构，再加上有某些函数特别相似的命名，所以在制作流程图的时候把一些不重要的数据转换类函数直接给忽略掉了，没有体现在流程图内。
话不多说，直接进入main函数：
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
mapOptimization MO;
ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::spin();
loopthread.join();
visualizeMapThread.join();
return 0;
}这里的main函数虽然与前面的main函数结构类似，但是多出了几行代码，除去常规的节点初始化、实例化类，还多出线程有关的代码。其中std::thread开头的两行代码分别定义了回环检测线程和可视化地图线程。照例我们先从构造函数开始：
mapOptimization()
{
//参数初始化
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
//发布
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
//订阅
subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay());
//地图保存服务
srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);
//一系列发布
pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1);
pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1);
pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);
pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1);
pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1);
pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1);
pubSLAMInfo = nh.advertise<lio_sam::cloud_info>("lio_sam/mapping/slam_info", 1);
//体素滤波设置（params.yaml）
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
//分配内存
allocateMemory();
}
构造函数整体分为三大部分，第一部分主要是初始化了部分参数，第二部分则是发布订阅了许多内容，第三部分为为体素滤波设置了参数，这部分的变量定义在头文件内，值在params文件内。最后为一个分配变量函数。
接下来我们看订阅部分的三个回调函数，分别是laserCloudInfoHandler、gpsHandler以及loopInfoHandler三个回调函数。
首先来看laserCloudInfoHandler回调函数：
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
// extract time stamp提取时间戳
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
// extract info and feature cloud提取特征信息（角点面点）
cloudInfo = *msgIn;
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
std::lock_guard<std::mutex> lock(mtx);
//频率控制
static double timeLastProcessing = -1;
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
{
timeLastProcessing = timeLaserInfoCur;
//当前帧位姿初始化
updateInitialGuess();
//提取局部特征点加入局部地图
extractSurroundingKeyFrames();
//降采样
downsampleCurrentScan();
//优化当前帧位姿
scan2MapOptimization();
//保存为关键帧并进行因子图优化
saveKeyFramesAndFactor();
//更新历史位姿
correctPoses();
//发布激光里程计
publishOdometry();
//发布里程计、点云、轨迹
publishFrames();
}
}回调函数首先提取了点云信息的时间戳，然后提取了特征信息，也就是之前处理过的角点面点，然后加锁，在经过频率控制后依次执行下面一系列功能函数。
频率控制通过一个if语句来实现，判断两帧间的时间差是否大于等于预设值mappingProcessInterval，该变量定义在头文件内，在params中给定数值大小。数据一帧为0.1秒，但参数预设值为0.15秒，也就是说考虑到算力进行隔帧处理，处理一帧放一帧。
第一行赋值语句就是为上面判断时间差的相关命令。
之后为一系列的函数，还是安按顺序来看，首先来看updateInitialGuess函数，这个函数主要功能是基于上一帧最优位姿求出当前帧的一个先验估计位姿：
void updateInitialGuess()
{
// save current transformation before any processing
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
static Eigen::Affine3f lastImuTransformation;
// initialization
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;
}
// use imu pre-integration estimation for pose guess
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
if (cloudInfo.odomAvailable == true)
{
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
if (lastImuPreTransAvailable == false)
{
lastImuPreTransformation = transBack;
lastImuPreTransAvailable = true;
} else {
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Eigen::Affine3f transFinal = transTobe * transIncre;
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuPreTransformation = transBack;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
}
// use imu incremental estimation for pose guess (only rotation)
if (cloudInfo.imuAvailable == true)
{
Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
Eigen::Affine3f transFinal = transTobe * transIncre;
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
}函数首行对前一帧的雷达位姿进行了一下数据格式转换，之后if判断关键帧集合，如果为空继续进行初始化，初始化后直接return掉了函数。
如果已经进行了初始化的函数，则进行下面的环节，进行当前帧的位姿估计，通过if语句判断有无可用的里程计数据，如果有的话，基于上一帧的最优位姿数据叠加位姿增量生成一个先验估计位姿，然后将结果保存起来，然后return掉。
如果没有进入上面两个if函数内部，则表示没有里程计数据，那么就需要使用imu来进行更新，但是imu没法提供靠谱的平移消息，所以在这里仅使用旋转信息进行更新，使用姿态角变化量加到上一针最优位姿上，同样的得到一个先验位姿估计。
求出这个估计位姿后，进入下一个函数extractSurroundingKeyFrames函数：
void extractSurroundingKeyFrames()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
extractNearby();
}函数体内部比较简单，只有一个判断队列是否为空的判断语句以及调用了一个extractNearby函数，继续跳转到extractNearby函数：
void extractNearby()
{
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// extract all the nearby key poses and downsample them
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
for (int i = 0; i < (int)pointSearchInd.size(); ++i)
{
int id = pointSearchInd[i];
surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
}
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
for(auto& pt : surroundingKeyPosesDS->points)
{
kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
}
// also extract some latest key frames in case the robot rotates in one position
int numPoses = cloudKeyPoses3D->size();
for (int i = numPoses-1; i >= 0; --i)
{
if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
else
break;
}
extractCloud(surroundingKeyPosesDS);
}在这函数里面，首先基于关键帧创建了一个kd树，这个kd树简单提一下，在这里假设有三个点，在计算距离的时候，如果A和B相距很近，但是A和C相距很远，那么B和C距离也很远，这样通过将距离信息储存在树里面可以节省后续的计算算力。kd树建立后然后搜索，半径信息在params文件中预设为了50米，这样就在以半径为50m的范围内搜索近邻，将这些符合要求的点储存进一个点云结构，同时为了减少点的密度，做了一个下采样，确认下采样后的点的索引。
做完这些后提取了一下最新的关键帧，防止机器人原地旋转，，前面已经对关键帧点的空间分布保存了下来，接下来对时间范围的点进行一个保存，将最近十秒的关键帧同样保存下来。
保存好后进入结尾的extractCloud函数：
void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
{
// fuse the map
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < (int)cloudToExtract->size(); ++i)
{
if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
continue;
int thisKeyInd = (int)cloudToExtract->points[i].intensity;
if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
{
// transformed cloud available
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
} else {
// transformed cloud not available
pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
*laserCloudCornerFromMap += laserCloudCornerTemp;
*laserCloudSurfFromMap += laserCloudSurfTemp;
laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
}
}
// Downsample the surrounding corner key frames (or map)
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
// Downsample the surrounding surf key frames (or map)
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
// clear map cache if too large
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}函数首先清空了临时角点和面点地图，然后循环处理所有点，超过距离的点丢弃，然后保存关键帧索引，然后进入if判断，如果这个帧对应点云已经储存在了地图容器中就取出来加入局部地图，如果没有储存就吧点云经过tf转换后加入到当前点云地图中，然后将转换后角点面点直接存进容器，方便后续操作。
然后对关键帧进行降采样操作。最后if判断一下如果地图容器太大了，就清除掉。
回到最开始的回调函数中，下面一个函数是downsampleCurrentScan函数：
void downsampleCurrentScan()
{
// Downsample cloud from current scan
laserCloudCornerLastDS->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerLastDS);
laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
laserCloudSurfLastDS->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfLastDS);
laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}函数比较短，主要功能是从当前帧将点云降采样。
接下来的函数是scan2MapOptimization：
void scan2MapOptimization()
{
if (cloudKeyPoses3D->points.empty())
return;
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 30; iterCount++)
{
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization();
surfOptimization();
combineOptimizationCoeffs();
if (LMOptimization(iterCount) == true)
break;
}
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}首先，通过if语句判断一下关键帧是否为空，空则直接return，接下来继续判断角点以及面点是否满足要求，params中设置默认角点要求10，面点要求100.如果不满足要求则会输出报错信息。符合要求则首先将角点面点局部地图构建了kd树，然后正式进入for循环。循环执行30次。
循环体内第一个函数cornerOptimization：
这个函数简单来说，就是遍历角点，在选中角点附近选五个最近点，用这五个最近点构造一个直线，求点到这个直线的距离和方向。在这个功能实现中使用了大量数学方法。
该函数内又嵌套着updatePointAssociateToMap函数，这里就不展开这个嵌套子函数了，这个update开头的函数以及其内部嵌套的函数整体完成一个功能，就是将回调函数内第一个函数中得到的先验位姿估计从欧拉角形式转至eigen形式。
接下来的for循环就是开始遍历每一个角点，对于每一个角点首先用pointAssociateToMap函数将该点从当前帧初始位姿变换到地图坐标系，然后在角点地图中寻找相距最近的五个点，点找好后通过if判断下五个点里最远的点如果距离该点太远，代表基于这组数据生成的信息不可靠，抛弃这组数据。没问题的话将计算五个点的均值，然后继续求协方差，用特征值分解判断一下五个点构不构成一条直线，如果构成一条直线，就求一下点到这个构造直线的残差和梯度（残差方向）。点到直线的距离可以根据三角形面积来求，使用核函数令残差越大的权重越低，而梯度就是点到直线最短距离的方向。最后依据残差大小来判断这个点是不是边缘点，s大于阈值0.1，也就是点与直线小于10cm的时候判断为边缘点，认作一个有效约束。
循环体内第二个函数surfOptimization：
角点残差梯度构建完接下来是面点的残差梯度构建，思路和角点的方式处理方式差不多，只是把构造之间的部分改为了构造面，在这个过程中用特征值分解判断是否构成直线改为了用超定方程来求解平面，求点到直线的距离改为了求点到面的距离。
循环体内第三个函数combineOptimizationCoeffs：
void combineOptimizationCoeffs()
{
// combine corner coeffs
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
if (laserCloudOriCornerFlag[i] == true){
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
}
}
// combine surf coeffs
for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
if (laserCloudOriSurfFlag[i] == true){
laserCloudOri->push_back(laserCloudOriSurfVec[i]);
coeffSel->push_back(coeffSelSurfVec[i]);
}
}
// reset flag for next iteration
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}这个函数的主要功能就是将前面两个函数所求的角点面点的残差和梯度统一到一起，取出与局部地图匹配上的角点面点（也就是标识位为true的点）加入集合，然后清空标识位。
最后执行优化，优化函数是在if语句内的LMOptimization函数，这个函数内主要是利用了高斯牛顿法进行了优化，基本上都是数学方法的代码实现。
至此scan2MapOptimization这个充满各种数学方法的函数结束，这里给出几个详解该函数内数学方法的参考链接：
https://blog.csdn.net/weixin_37835423/article/details/111587379
接下来进入回调函数中的saveKeyFramesAndFactor函数，该函数主要负责保存关键帧和进行因子图优化的功能，代码如下：
void saveKeyFramesAndFactor()
{
if (saveFrame() == false)
return;
// odom factor
addOdomFactor();
// gps factor
addGPSFactor();
// loop factor
addLoopFactor();
// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");
// update iSAM
isam->update(gtSAMgraph, initialEstimate);
isam->update();
if (aLoopIsClosed == true)
{
isam->update();
isam->update();
isam->update();
isam->update();
isam->update();
}
gtSAMgraph.resize(0);
initialEstimate.clear();
//save key poses
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
// cout << "****************************************************" << endl;
// isamCurrentEstimate.print("Current estimate: ");
thisPose3D.x = latestEstimate.translation().x();
thisPose3D.y = latestEstimate.translation().y();
thisPose3D.z = latestEstimate.translation().z();
thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().roll();
thisPose6D.pitch = latestEstimate.rotation().pitch();
thisPose6D.yaw = latestEstimate.rotation().yaw();
thisPose6D.time = timeLaserInfoCur;
cloudKeyPoses6D->push_back(thisPose6D);
// cout << "****************************************************" << endl;
// cout << "Pose covariance:" << endl;
// cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);
// save updated transform
transformTobeMapped[0] = latestEstimate.rotation().roll();
transformTobeMapped[1] = latestEstimate.rotation().pitch();
transformTobeMapped[2] = latestEstimate.rotation().yaw();
transformTobeMapped[3] = latestEstimate.translation().x();
transformTobeMapped[4] = latestEstimate.translation().y();
transformTobeMapped[5] = latestEstimate.translation().z();
// save all the received edge and surf points
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
// save key frame cloud
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
// save path for visualization
updatePath(thisPose6D);
}函数首先通过saveFrame函数判断一下是否为关键帧，如果不是则return掉不进入下面的添加因子环节。
saveFrame函数流程是计算当前帧与上一个关键帧的位姿变化情况，如果变化小于一定阈值则不认为当前帧为关键帧，反之则认为当前帧为关键帧，其中的阈值设置仍然是在params文件中给定。
判断结束后认定是关键帧后就要给优化器isam添加因子，三个添加因子的函数，分别是odom因子、gps因子、loop因子。
首先是odom约束，通过一个if-else语句进行一个分支执行，如果判断这是第一个关键帧，就将置信度设置的低一些并增加先验约束。如果不是第一个关键帧就增加帧间约束，相应的置信度也会高一点，将前一帧与当前帧转换为gtsam格式建立帧间约束。
第二个是gps约束，我们知道gps作为一个可选项是可以不使用的，在程序中体现在首先有一个检测gps信息的if语句，如果为空则直接return。如果有gps信息的话还需要进一步检测是否有关键帧消息，毕竟gps是为了约束关键帧的，如果没有关键帧信息同样也就作罢return掉。即使都满足，仍然要进行判断，判断最后一帧和初始帧空间距离是不是太近，太近的话考虑到回环或者刚起步也就不进行下一步操作了。再考虑帧间置信度，如果置信度很高也不需要使用gps约束。当这一系列判断都通过后，丢弃掉比较早的gps信息，与此对应，既然早于关键帧0.2秒的gps信息丢掉，那么如果gps信息太新了，比关键帧还要领先0.2秒同样需要等着。符合要求后取出gps队列里最前面这个数据判断置信度，置信度满足要求的情况下，取出信息内的位置信息，去掉高度部分的信息，直接采用里程计的高度信息。在判断一下gps的x、y是否太小，太小表示初始化还没进行好，然后限制一下gps观测不要小于5米，给予一个gps的置信度，这里置信度没有给的很高，也就是说信gps信息，但不完全信。然后调用gtsam生成gps约束。
接下来是loop约束，依然是开局先判断队列是否为空，在这里需要提一下这个回环队列，在main函数中单独有一个回环检测线程会检测回环，如果检测到了回环就会给这个队列里面添加信息。在进行回环约束时将队列里面的所有信息循环添加约束，其中置信度采用的是回环检测部分的icp设置中提供的值。添加约束后清空回环相关的队列，标识为置为true。
因子添加完后调用isam接口更新一下图模型，因为如果有gps和回环那么isam需要多次优化所以if内有多条update指令。
然后将约束和节点信息清空，由于已经加入到了isam中所以清空不影响功能，接下来取出优化后关键帧位姿，然后将不同信息分批保存起来便结束了saveKeyFramesAndFactor函数的功能。
回到回调函数，下一个函数时correctPoses函数，这是一个更新历史位姿的相关函数：
void correctPoses()
{
if (cloudKeyPoses3D->points.empty())
return;
if (aLoopIsClosed == true)
{
// clear map cache
laserCloudMapContainer.clear();
// clear path
globalPath.poses.clear();
// update key poses
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
updatePath(cloudKeyPoses6D->points[i]);
}
aLoopIsClosed = false;
}
}照例开头判断一下关键帧是否为空，非空才会进入下一环节，因为是对历史位姿进行调整更新的函数，所以通过if判断一下是否触发了回环或者gps这种需要全局调整修正的功能。接下来清空地图缓存以及路径信息，然后对关键帧位姿进行更新，更新完毕后标识位置false。如此便完成全局更新的功能。
回调函数的下一个函数是publishOdometry函数，这个函数发布激光里程计，内容方面主要分为三部分，首先是发布ros用里程计数据，也就是激光里程计，通过一系列赋值命令后发布；第二部分是发布坐标变换（odom2lidar）第三部分是发布增量式里程计数据，和以前处理数据方式类似，通过if-else语句判断如果是首帧就用全局里程计优化一下，如果不是就可以计算帧间位姿变化，算一个mid均值然后通过一系列赋值语句后发布出去。
回到回调函数，下一个函数是publishFrames，主要发布历史关键帧点云以及轨迹：
void publishFrames()
{
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
publishCloud(pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
publishCloud(pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// publish registered key frame
if (pubRecentKeyFrame.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
publishCloud(pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish registered high-res raw cloud
if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
publishCloud(pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// publish path
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = odometryFrame;
pubPath.publish(globalPath);
}
// publish SLAM infomation for 3rd-party usage
static int lastSLAMInfoPubSize = -1;
if (pubSLAMInfo.getNumSubscribers() != 0)
{
if (lastSLAMInfoPubSize != cloudKeyPoses6D->size())
{
lio_sam::cloud_info slamInfo;
slamInfo.header.stamp = timeLaserInfoStamp;
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
*cloudOut += *laserCloudCornerLastDS;
*cloudOut += *laserCloudSurfLastDS;
slamInfo.key_frame_cloud = publishCloud(ros::Publisher(), cloudOut, timeLaserInfoStamp, lidarFrame);
slamInfo.key_frame_poses = publishCloud(ros::Publisher(), cloudKeyPoses6D, timeLaserInfoStamp, odometryFrame);
pcl::PointCloud<PointType>::Ptr localMapOut(new pcl::PointCloud<PointType>());
*localMapOut += *laserCloudCornerFromMapDS;
*localMapOut += *laserCloudSurfFromMapDS;
slamInfo.key_frame_map = publishCloud(ros::Publisher(), localMapOut, timeLaserInfoStamp, odometryFrame);
pubSLAMInfo.publish(slamInfo);
lastSLAMInfoPubSize = cloudKeyPoses6D->size();
}
}
}
};依旧是先判断一下队列是否为空，接下来几乎全是关于发布的内容：发布历史关键帧的数据、局部地图降采样后数据、当前帧的角点面点降采样数据、发布当前帧去畸变后的点云数据以及发布里程计的轨迹数据。其中后三个加了if判断非零时才会处理发布。
到这里，终于，laserCloudInfoHandler这个回调函数结束了，可以回到mapOptimization这个构造函数内接着看其他内容了，这个构造函数内还有两个回调函数、地图保存服务以及一个内存分配函数。
剩下的部分内容不多，首先来到gps的回调函数gpsHandler来简单看一下，函数非常简单，只有一行把数据存入队列的代码。接下来是loopInfoHandler函数，这个函数接收处理来自外部闭环检测程序提供的闭环数据，如果没有额外接入外部的程序这部分的内容可以先行略过。
接下来是地图保存服务saveMapService函数，函数内包含了终端输出文本、文件操作、循环写入等内容。最终将关键帧特征点保存起来，生成CornerMap、GlobalMap、SurfMap、trajectory、transformations五个文件。特别需要注意的是代码中包含移除旧文件的内容，如果在程序运行结束要保存文件时，储存位置如果存在文件会将其删除，所以建图完成一定记得将文件及时拷贝出去，可不要随手测试了一下把之前建好的图给覆盖了。。。
回到构造函数内，函数内还剩下一些发布、降采样代码以及一个分配内存函数，这些功能都顾名思义就不再研究了，结束掉mapOptimization，回到main函数，这里还有两个线程，首先来看loopClosureThread，这是之前在添加回环约束时给开头回环队列添加数据的函数，代码如下：
void loopClosureThread()
{
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(loopClosureFrequency);
while (ros::ok())
{
rate.sleep();
performLoopClosure();
visualizeLoopClosure();
}
}函数首先检查一下标识位是否为false，如果为false直接退出回环检测，接下来设置一下检测的频率，防止占用过多的性能资源。接下来进入一个while死循环，通过sleep命令防止执行过于频繁，接下来使用两个函数来实现具体的功能。
performLoopClosure函数：
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
// find keys
int loopKeyCur;
int loopKeyPre;
if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
return;
// extract cloud
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
{
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
// ICP Settings
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);
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// publish corrected cloud
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// Get pose transformation
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
correctionLidarFrame = icp.getFinalTransformation();
// transform from world origin to wrong pose
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// transform from world origin to corrected pose
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
// Add pose constraint
mtx.lock();
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();
// add loop constriant
loopIndexContainer[loopKeyCur] = loopKeyPre;
}首先通过if保证关键帧是存在的，锁定一下线程，将位置位姿全部取出来后解锁。来到一个嵌套if语句，语句的判断条件都是函数，外层if中的函数位detectLoopClosureExternal函数，当我跳转到函数定义的时候，他告诉我
// this function is not used yet, please ignore it
好的听你的，来到内层判断语句的detectLoopClosureDistance函数：
bool detectLoopClosureDistance(int *latestID, int *closestID)
{
int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
int loopKeyPre = -1;
// check loop constraint added before
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
// find the closest history key frame
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
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;
}
}
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}检查一下之前添加的回环约束，然后把只包含位置信息的点云加入到kd树中，依据最后一帧寻找最近的关键帧。接下来循环检查这些关键帧，首先要满足时间间隔要在设定的阈值以上，找到的话赋值，找不到就break。最后还通过一个if语句，在回环检测找不到或者只找到当前帧的话，返回false。如果不触发上面的if语句，函数会在赋值后返回一个true。
回到performLoopClosure函数，假使找到了关键帧，接下来会计算找到的历史关键帧与当前关键帧的位姿变化，接下来处理点云信息，在作者给出注释提取点云的这段代码中，主要是为了获取了将历史帧和当前帧的点云信息投影到地图坐标系下的局部地图，同时发布了一个局部地图信息给rviz使用。
接下来有了当前帧和历史帧的这两个信息后，使用icp实现关键帧到局部地图的配准，对这两组点云进行一系列处理，会得到两个点云的位姿变化信息、icp的收敛结果以及得分等信息。这样就可以得到这两帧之间的一系列所需要的约束信息。大体的流程有作者的标注：ICP设置、配准点云、发布当前的点云、获取位姿变换、添加位姿约束、添加回环约束。
在最后还有一个visualizeGlobalMapThread函数：
void visualizeGlobalMapThread()
{
ros::Rate rate(0.2);
while (ros::ok()){
rate.sleep();
publishGlobalMap();
}
if (savePCD == false)
return;
lio_sam::save_mapRequest req;
lio_sam::save_mapResponse res;
if(!saveMapService(req, res)){
cout << "Fail to save map" << endl;
}
}它的主要功能是可视化地图发布线程，函数的结构是和回环检测部分类似的，功能是通过一个publishGlobalMap函数实现。
在负责功能实现的publishGlobalMap函数里作者给了实现流程：首先在kd树查找要可视化的关键帧，接着在关键帧附近搜索用以可视化，在选定的关键帧附近帧进行降采样，提取可视化和降采样的关键帧，最后降采样可视化的点。
最后，判断一下在params参数中设置的savePCD这个用于保存点云图的值是否为ture，如果为false，直接return。但如果置为true但未保存成功，则输出保存失败信息。
2705
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
