本文详细介绍了LIO-SAM激光SLAM系统中的关键步骤，包括点云预处理、特征点提取、IMU预积分、关键帧选择、因子图构建与优化、回环检测等。通过优化当前帧与局部地图的匹配，结合GPS因子和回环约束，实现高精度的定位与建图。文章还展示了如何处理回环，更新地图，并发布里程计和轨迹信息。
我们在前述的博客中介绍了LIO-SAM的一些准备工作、点云预处理、特征点提取、IMU预积分等内容。之后，我们来到了最核心也是最重要的一部分工作，把（3）中提取的特征点进行匹配，获取激光里程计，并添加回环检测因子、GPS因子等进行后端优化。
详细参考：
LIO-SAM代码逐行解读（1）-准备工作
LIO-SAM代码逐行解读（2）-点云预处理
LIO-SAM代码逐行解读（3）-特征点提取
LIO-SAM代码逐行解读（4）-IMU预积分
LIO-SAM回环检测模块代码解析
准备工作
- 引用头文件
// 引用自定义的函数
#include "utility.h"
// cloud_info自定义数据结构，save_map自定义的服务
#include "lio_sam/cloud_info.h"
#include "lio_sam/save_map.h"
// GTSAM相关头文件
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
// 使用命名空间以及缩写
using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::G; // GPS pose
- 自定义数据结构，存储激光雷达位姿6D pose
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
自定义一个点云数据结构（6D位姿点云结构）
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
// 注册自定义的PointXYZIRPYT到PCL 配合自定义的PointXYZIRPYT结构体使用
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
(float, x, x) (float, y, y)
(float, z, z) (float, intensity, intensity)
(float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
(double, time, time))
// 别名
typedef PointXYZIRPYT PointTypePose;
- 定义mapOptimization类所使用的各种变量
class mapOptimization : public ParamServer
{
public:
// gtsam因子图
gtsam::NonlinearFactorGraph gtSAMgraph;
// gtsamValues()graphValues
gtsam::Values initialEstimate;
gtsam::Values optimizedEstimate;
// 定义一个isam2优化器
gtsam::ISAM2 *isam;
// 存储图中当前的估计值（包括很多因子）
gtsam::Values isamCurrentEstimate;
// LiDAR位姿协方差(在进行GTSAM优化过程中输出的一个值)
// 若该值很大，则说明LiDAR位姿的精度不高，需要添加GPS因子修正，反之，则不添加GPS因子
Eigen::MatrixXd poseCovariance;
// 发布各种消息
ros::Publisher pubLaserCloudSurround; // 发布局部关键帧map的特征点云 "lio_sam/mapping/map_global"
ros::Publisher pubLaserOdometryGlobal;// 发布激光里程计，rviz中表现为坐标轴 "lio_sam/mapping/odometry"
ros::Publisher pubLaserOdometryIncremental;// 发布激光里程计，它与"lio_sam/mapping/odometry"基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制
ros::Publisher pubKeyPoses; // 发布历史关键帧里程计 "lio_sam/mapping/trajectory"
ros::Publisher pubPath; // 发布激光里程计路径，rviz中表现为载体的运行轨迹 "lio_sam/mapping/path"
// 发布回环相关点云
ros::Publisher pubHistoryKeyFrames; // 发布闭环匹配关键帧局部map "lio_sam/mapping/icp_loop_closure_history_cloud"
ros::Publisher pubIcpKeyFrames; // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云 "lio_sam/mapping/icp_loop_closure_corrected_cloud"
// 发布局部地图 实时点云
ros::Publisher pubRecentKeyFrames; // 发布局部map的降采样平面点集合 "lio_sam/mapping/map_local"
ros::Publisher pubRecentKeyFrame; // 发布历史帧（累加的）的角点、平面点降采样集合 "lio_sam/mapping/cloud_registered"
// 发布原始点云配准处理后的点云
ros::Publisher pubCloudRegisteredRaw; // 发布当前帧原始点云配准之后的点云"lio_sam/mapping/cloud_registered_raw"
// 发布回环约束边
ros::Publisher pubLoopConstraintEdge; // 发布闭环边，rviz中表现为闭环帧之间的连线 "/lio_sam/mapping/loop_closure_constraints"
ros::Subscriber subCloud; // 订阅当前激光帧点云信息，来自featureExtraction "lio_sam/feature/cloud_info"
// 实际使用中都没有使用
ros::Subscriber subGPS; // 订阅GPS里程计 gpsTopic默认为"odometry/gpsz"
ros::Subscriber subLoop; // 订阅回环 来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
// 保存地图数据服务
ros::ServiceServer srvSaveMap;
// GPS数据队列
std::deque<nav_msgs::Odometry> gpsQueue;
// 从特征点提取节点传出的cloudinfo信息
lio_sam::cloud_info cloudInfo;
// 角点关键帧，平面点关键帧 （typedef pcl::PointXYZI PointType）;
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D; // 存储关键帧的位置信息的点云
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D; // 存储关键帧的6D位姿信息的点云
pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D;
pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D;
// 用于里程计优化的角点与平面点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
// 当前帧与局部map匹配上了的角点、平面点，加入同一集合
pcl::PointCloud<PointType>::Ptr laserCloudOri;
// 后面是对应点的参数（直线方程与平面方程系数）
pcl::PointCloud<PointType>::Ptr coeffSel;
std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
std::vector<PointType> coeffSelCornerVec;
std::vector<bool> laserCloudOriCornerFlag;
std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
std::vector<PointType> coeffSelSurfVec;
std::vector<bool> laserCloudOriSurfFlag;
// map与pair的作用
// 每个pair 可以存储两个值，这两种值的类型没有限制。
// map类似于一个容器,可以存储关键值索引、pair。
// 这里使用map容器分别存储两种特征点类型
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer; // 局部地图的一个容器
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap; // 局部地图中的角点集合
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap; // 局部地图中的面片点集合
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS; // 角点局部地图的下采样后的点云
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS; // 面点局部地图的下采样后的点云
// 局部地图 建立kdtree用于找相邻点
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap; // 角点局部地图的kdtree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap; // 面点局部地图的kdtree
// 对关键帧位置建立kdtree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
// 下采样
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
// 回环进行ICP匹配时使用的降采样滤波器
pcl::VoxelGrid<PointType> downSizeFilterICP;
// 设置关键帧周围的相关关键帧数量不会太密（参数文件中设置为2m）
// for surrounding key poses of scan-to-map optimization
pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses;
// 存储当前cloudinfo的时间戳信息
ros::Time timeLaserInfoStamp;
double timeLaserInfoCur;
// 最优变换
float transformTobeMapped[6];
std::mutex mtx;
std::mutex mtxLoopInfo;
// LM第一次进行优化是否发生退化
// 如果发生退化，则在退化方向不更新
bool isDegenerate = false;
cv::Mat matP; // 定义退化方向，若没有退化则为单位向量
int laserCloudCornerFromMapDSNum = 0; // 当前局部地图下采样后的角点数目
int laserCloudSurfFromMapDSNum = 0; // 当前局部地图下采样后的面点数目
int laserCloudCornerLastDSNum = 0; // 当前帧下采样后的角点的数目
int laserCloudSurfLastDSNum = 0; // 当前帧下采样后的面点数目
// 回环检测相关
bool aLoopIsClosed = false;
// 回环索引，前一个为新的关键帧索引，后一个为旧的关键帧索引
map<int, int> loopIndexContainer; // from new to old
// 回环索引队列
vector<pair<int, int>> loopIndexQueue;
// 回环对的相对位姿
vector<gtsam::Pose3> loopPoseQueue;
// 回环对应的噪声项
vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue;
// 存储外部回环检测模块发送的回环信息
deque<std_msgs::Float64MultiArray> loopInfoVec;
// 行驶路径
nav_msgs::Path globalPath;
// 当前帧位姿
Eigen::Affine3f transPointAssociateToMap;
// 前一帧位姿
Eigen::Affine3f incrementalOdometryAffineFront;
// 当前帧位姿
Eigen::Affine3f incrementalOdometryAffineBack;
- 构造函数
// 构造函数
mapOptimization()
{
// gtsam初始化
gtsam::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);
// 发布激光里程计，rviz中表现为坐标轴
pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
// 发布激光里程计，它与上面的激光里程计基本一样，只是增量式的累加位姿变化，roll、pitch用imu数据加权平均了一下
pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1);
pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
// 订阅来自特征提取节点的点云信息 gpsTopic 来自外部闭环检测程序提供的闭环数据(本程序没有提供，这里实际没用上)
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);
// 发布闭环匹配关键帧局部map
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);
// 发布闭环边，rviz中表现为闭环帧之间的连线
pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1);
// 发布局部map的降采样平面点集合
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);
// 体素滤波设置珊格大小
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
// 设置关键帧周围的相关关键帧数量不会太密（参数文件中设置为2m）
downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization
// 各种变量预先分配内存
allocateMemory();
}
- 分配内存空间
// 各种变量预先分配内存
void allocateMemory()
{
// 存储点云关键帧位姿 包括 3D与6d版本
cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>());
copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>());
// 构建关键帧周边关键位姿的kdtree 历史位姿的kdtree
kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>());
// 角点、平面点特征
laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization
laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization
laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization
laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization
//
laserCloudOri.reset(new pcl::PointCloud<PointType>());
coeffSel.reset(new pcl::PointCloud<PointType>());
//
laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN);
coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN);
coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN);
laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN);
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>
扫一扫
5126
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
