节点数据传递
从上图可以看出，地图优化节点(红色)接收话题(绿色)/odometry/gps, /lio_sam/feature/cloud_info，并发 /lio_sam/mapping/odometry, /lio_sam/mapping/odometry_incremental 话题，发布的被rviz节点订阅的话题只是为了显示用，忽略讨论．
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;
}
从上面代码可以看出该节点总共3个线程，一个用于回环检测，一个用于显示，最后一个用于数据的回调处理，下面对回调处理线程和回环线程进行解析
回调处理线程
void gpsHandler(const nav_msgs::Odometry::ConstPtr &gpsMsg)
{
gpsQueue.push_back(*gpsMsg);
}
上面是GPS话题 /odometry/gps 回调处理函数，主要是对GPS数据进行缓存，用于后面添加进因子图的需要．
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();
}
}
上面是点云信息话题 /lio_sam/feature/cloud_info 回调处理函数，由多个处理函数组成，下面分析几个重要的处理函数．
更新当前数据帧对应的位姿的初始估计updateInitialGuess()
void updateInitialGuess()
{
// save current transformation before any processing
// 进行处理前保存上一帧的转换(也即上一关键帧的位姿)
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
static Eigen::Affine3f lastImuTransformation;
// initialization
// 如果还没有关键帧位置信息，则利用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;
}
// use imu pre-integration estimation for pose guess
// 当imu里程计信息状态是可得到的，则使用imu里程计结果进行位姿估计
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
if (cloudInfo.odomAvailable == true)
{
// 获得当前数据帧对应的imu里程计位姿
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
{
// 获得上个数据帧对应的imu里程计位姿相对与当前数据帧对应的imu里程计位姿的增量
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]);
// 更新上个数据帧对应的imu里程计位姿信息
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)
// 当imu增量信息状态是可得到的，则使用imu积分获得姿态增量(在imageProjection中进行点云偏斜矫正时计算的)进行位姿估计
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;
}
}
该部分的处理过程主要是利用图像投影节点进行点云偏斜矫正时积分的IMU姿态增量信息和计算的IMU里程计位姿增量信息，叠加到上一关键帧位姿之上，得到对当前数据帧位姿的估计．
提取附近关键帧的角点和平面点点云extractSurroundingKeyFrames()
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
// 利用关键帧位姿建立kdtree，并利用半径搜索的方式搜索最后一个关键帧附近的关键帧
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);
// also extract some latest key frames in case the robot rotates in one position
// 将早于当前帧10秒的关键帧位姿添加到附近帧中，为的是防止机器人原地旋转，导致滤波后在附近帧里面只有一个关键帧
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);
}
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
// 如果检索关键帧索引存在于雷达点云map容器中，则直接提取对应的角点和平面点点云
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
}
else
{
// transformed cloud not available
// 如果不存在，则将点云转换到地图坐标系下，并添加到雷达点云map容器中
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
// 当雷达点云map容器太大时，直接清空
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}
该部分内容对应论文中3C部分的子关键帧概念，其主要思路是，在进行当前数据帧与已有数据帧进行匹配时，采取的是滑窗策略，也就是将当前数据帧前面一定数量的关键帧点云信息累加起来构成一个局部地图，然后再将当前数据帧点云跟该局部地图进行匹配来获得当前数据帧的位姿，这样做可以有效的提高匹配效率．此部分代码主要是生成该策略所需的局部地图．
匹配当前特征点与地图特征点，并进行优化求解最优位姿scan2MapOptimization()
void scan2MapOptimization()
{
// 无关键帧存在时，直接跳过
if (cloudKeyPoses3D->points.empty())
return;
// 当特征数超过给定值时才能进行有效的scan2map优化
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
// 建立基于附近关键帧特征点云的kdtree
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);
}
}
该部分主要对应论文中的公式(10)(11)．计算边特征点到局部地图中的边特征的距离和计算面特征点到局部地图中的面特征的距离，并将这两个距离作为残差函数进行LM(Levenberg-Marquardt)优化，从而获得优化后的当前数据帧位姿．
保存当前关键帧并更新因子图和优化saveKeyFramesAndFactor()
void saveKeyFramesAndFactor()
{
// 判断是否将当前帧添加为关键帧
if (saveFrame() == false)
return;
// odom factor
// 添加优化后的雷达里程计因子到因子图中
addOdomFactor();
// gps factor
// 添加gps因子到因子图中
addGPSFactor();
// loop factor
// 添加回环因子到因子图中
addLoopFactor();
// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");
// update iSAM
// 将新的因子图更新到isam2优化器中
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);
}
该部分算是一个多传感器融合优化，将雷达里程计约束、回环约束、GPS约束全部更新到因子图中，并通过iSAM2优化器进行优化，可获得最优的当前关键帧的位姿估计．这也是论文中提到的这个算法框架的优势之一是能够轻松融合其它传感器，只需要在这个函数内添加对应传感器引入的约束就可以实现对关键帧位姿的矫正．
回环线程
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
// 进行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);
// 当匹配未收敛或匹配得分较低时，则认为匹配失败，也即回环检测失败
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);
// 将icp匹配得分作为回环约束的协方差
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;
}
此部分代码对应论文中3E部分"回环约束"．其包括使用基于欧拉距离的方法检测可能的回环帧、使用icp匹配算法确定是否存在回环及回环约束．具体的检查方法可按下面"注释代码路径"部分提供的链接查看详细的注释．
3248
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
