前言
LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。
LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。
其中建图优化节点整体如下图
本篇主要分析： 点云匹配前戏之初值计算及局部地图构建
建图优化代码
首先来看 mapOptmization.cpp的 主函数部分
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
节点初始化
mapOptimization MO;
地图优化对象
其中点云匹配及因子图优化的内容都在这个类里面
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
定义两个线程
回环检测线程
可视化线程 rviz可视化接口的发布
下面来看mapOptimization的构造函数
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());
定义订阅消息的句柄
- cloudinfo 包括 角点、面点、初值等
- gps
- 回环
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();
预先分配内存
下面来看 laserCloudInfoHandler 这个点云回调函数，主要内容都在这个函数中
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
提取当前时间戳
cloudInfo = *msgIn;
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
提取cloudinfo中的角点和面点
static double timeLastProcessing = -1;
上次处理的时间 初始化为-1 第一帧肯定进不去
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
{
控制后端频率，两帧处理一帧
mappingProcessInterval 在配置文件（params.yaml）中是0.15s 一帧是0.1s 这个可以根据算力进行调整
timeLastProcessing = timeLaserInfoCur;
更新上次的处理时间
updateInitialGuess();
更新当前匹配结果的初始位姿
点云匹配前戏之初值计算
终于到了本节内容的地方
作为基于优化方案的点云匹配，初始值是非常重要的，一个好的初始值会帮助优化问题快速收敛且避免局部最优解的情况
点云匹配前的初值计算 updateInitialGuess() 这个函数
进到内部
void updateInitialGuess()
{
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
transformTobeMapped 是上一帧优化后的最佳位姿 是一个 6维数组 [x y z roll pitch yaw] 该框架下是用欧拉角来描述姿态的 最好用四元数
if (cloudKeyPoses3D->points.empty())
{
没有关键帧，也就是系统刚刚初始化完成 cloudKeyPoses3D是存的xyz 关键帧的位置
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
初始的位姿由磁力计提供
if (!useImuHeadingInitialization)
transformTobeMapped[2] = 0;
这里虽然有磁力计将yaw对齐，但是也可以考虑不使用yaw
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
保存磁力计得到的位姿，平移置0
if (cloudInfo.odomAvailable == true)
{
如果有预积分节点提供的里程计
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
将提供的初值转成eigen的数据结构保存下来
if (lastImuPreTransAvailable == false)
{
lastImuPreTransformation = transBack;
lastImuPreTransAvailable = true;
}
这个标志位表示到第一帧预积分里程计信息
将当前里程计结构记录下来
收到第一个里程计数据以后这个标志位就是true
else {
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
计算上一个里程计的结果和当前里程计结果之间的 delta pose
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
把上一帧的最优位姿估计 转成eigen的格式
Eigen::Affine3f transFinal = transTobe * transIncre;
将这个增量加到上一帧最佳位姿上去，就是当前帧位姿的一个先验估计位姿
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
将eigen变量转成欧垃角和平移形式 此时transformTobeMapped变成了当前帧的 先验 位姿估计，之前是上一帧的
lastImuPreTransformation = transBack;
同理，把当前帧的值保存下来
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
虽然有里程计信息，仍然需要把imu磁力计得到的旋转记录下来
return;
注意这里有return 如果有里程计信息，则不用下面的imu信息了
if (cloudInfo.imuAvailable == true)
{
如果没有里程计信息 ， 就是 用 imu 的旋转信息来更新，因为单纯使用imu无法得到靠谱的平移信息，因此，平移直接置0
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的格式
Eigen::Affine3f transFinal = transTobe * transIncre;
将这个增量加到上一帧最佳位姿上去，就是当前帧位姿的一个先验估计位姿
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
将eigen变量转成欧垃角和平移形式 此时transformTobeMapped变成了当前帧的 先验 位姿估计，之前是上一帧的
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
return;
更新上一次的 imu 姿态
总结
点云匹配之前戏局部地图构建
上一节解读完初值计算后，回到 laserCloudInfoHandler 函数中
则到了这个函数
extractSurroundingKeyFrames();
提取当前帧相关的关键帧并且构建点云局部地图
void extractSurroundingKeyFrames()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
如果当前没有关键帧 ，就直接return 了
extractNearby();
有关键帧则进行局部地图构造
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
cloudKeyPoses3D 存储 关键帧的3d信息 lidar是360的，所以没保存姿态
以关键帧位置形成的点云建立kd-tree
kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
根据最后一个KF的位姿，进行最近邻搜索，半径是50m，在param中设置，提取一定距离内的关键帧
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;
}
确认每个下采样后的点的索引
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)
{
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())
{
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
}
如果这个关键帧对应的点云信息已经存储在一个地图容器里
直接从容器中取出来加到局部地图中
else {
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);
把转换后的面点和角点存进这个容器中，方便后续直接加入点云地图，避免点云转换的操作，节约时间
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
将提取的关键帧的点云转到世界坐标系下后，避免点云过度密集，因此对面点和角点的局部地图做一个采样的过程
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
如果这个局部地图容器过大，就clear一下，避免占用内存过大
总结
评论（0）
您还未登录，请登录后发表或查看评论