LIO-SAM代码逐行解读（5）-点云匹配及后端优化模块
我们在前述的博客中介绍了LIO-SAM的一些准备工作、点云预处理、特征点提取、IMU预积分等内容。之后，我们来到了最核心也是最重要的一部分工作，把（3）中提取的特征点进行匹配，获取激光里程计，并添加回环检测因子、GPS因子等进行后端优化。
详细参考：
LIO-SAM代码逐行解读（1）-准备工作
LIO-SAM代码逐行解读（2）-点云预处理
LIO-SAM代码逐行解读（3）-特征点提取
LIO-SAM代码逐行解读（4）-IMU预积分
LIO-SAM回环检测模块代码解析
目录
- 准备工作
- 激光点云处理
-
- 选取关键帧，添加各种因子并进行优化
- 回环检测
- Main函数
准备工作
- 引用头文件
// 引用自定义的函数
#include "utility.h"
// cloud_info自定义数据结构，save_map自定义的服务
#include "lio_sam/cloud_info.h"
#include "lio_sam/save_map.h"
// GTSAM相关头文件
#include - 自定义数据结构，存储激光雷达位姿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>());
laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>());
laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>());
// 对局部地图的特征点构建KDtree
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
// 变换数组初始化为0
for (int i = 0; i < 6; ++i){
transformTobeMapped[i] = 0;
}
matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0));
}
激光点云处理
订阅原始的激光点云数据并进行处理，主要流程如下：
/**
* 订阅当前激光帧点云信息，来自featureExtraction
* 1、当前帧位姿初始化
* 1) 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
* 2) 后续帧，用imu预积分里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光预测位姿
* 3）如果不存在IMU预积分数据，则使用IMU磁力计的旋转部分
* 2、提取局部角点、平面点云集合，加入局部map
* 1) 对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
* 2) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
* 3、当前激光帧角点、平面点集合降采样
* 4、scan-to-map优化当前帧位姿
* (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
* (2) 迭代30次（上限）优化
* 1) 当前激光帧角点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
* 2) 当前激光帧平面点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
* 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
* 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
* 5、设置当前帧为关键帧并执行因子图优化
* 1) 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
* 2) 添加激光里程计因子、GPS因子、闭环因子
* 3) 执行因子图优化
* 4) 得到当前帧优化后位姿，位姿协方差
* 5) 添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
* 6、更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
* 7、发布激光里程计
* 8、发布里程计、点云、轨迹
*/
// 接收处理来自特征提取节点的点云信息
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
// extract time stamp
// 提取当前时间戳（一个为ROS Time格式，另外一个为double格式）
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
// extract info and feature cloud
// 提取cloudinfo中的角点和面点（最新的角点与面片点）
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;
// 控制后端频率，两帧处理一帧（mappingProcessInterval在参数文件中设置为0.15s）
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
{
timeLastProcessing = timeLaserInfoCur;
/**
* 作为基于优化方式的点云匹配，初始值是非常重要的，一个好的初始值会帮助优化问题快速收敛且避免局部最优解的情况
* @brief 当前帧位姿初始化
* 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
* 2、后续帧，用imu预积分里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光帧预测位姿
* 3、如果不存在IMU预积分里程计数据，则仍使用IMU磁力计中的旋转部分进行初始化预测位姿
* （得到了front的转换）
*/
updateInitialGuess();
/**
* @brief 提取当前帧相关的关键帧并且构建点云局部地图
* 2、提取局部角点、平面点云集合，加入局部map
* 1) 对最新的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
* 2) 此外，对最新的一帧关键帧，添加一些时间上比较近的关键帧。以防机器人在原地转圈时，导致出现局部地图为空的情况
* 3) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
*/
extractSurroundingKeyFrames();
/**
* @brief 对当前帧进行下采样
* 分别对当前的扫描 角点 与 面片点 进行下采样
*/
downsampleCurrentScan();
/**
* @brief 对点云配准进行优化问题构建求解
* scan-to-map优化当前帧位姿
* (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
* (2) 迭代30次（上限）优化
* 1) 当前激光帧角点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
* 2) 当前激光帧平面点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
* 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
* 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
* （得到了end的转换）
*/
scan2MapOptimization();
/**
* @brief 设置当前帧为关键帧并执行因子图优化
* 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
* 2、添加激光里程计因子、GPS因子、闭环因子
* 3、执行因子图优化
* 4、得到当前帧优化后位姿，位姿协方差
* 5、添加到cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
*/
saveKeyFramesAndFactor();
/**
* @brief 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
* 1、判断是否添加了回环因子（或者GPS因子）进行优化
* 2、若使用了，则清空局部地图map容器中的关键帧，清除全局路径
* 3、遍历因子图中所有变量节点的位姿，更新关键帧变量中的位姿（cloudKeyPoses3D与cloudKeyPoses6D），更新全局路径
* 使用优化后的位姿更新已有的关键帧位姿
*/
correctPoses();
/**
* @brief 发布里程计
* 1、发布当前位姿transformTobeMapped， "lio_sam/mapping/odometry"
* 2、发布增量式位姿“lio_sam/mapping/odometry_incremental”， 与上一话题基本相同
* 3、发送lidar在odom坐标系下（odometryFrame("odom")与 "lidar_link"）的tf（其值本质上也是transformTobeMapped）
*/
publishOdometry();
/**
* @brief 发布各种信息
* 1、发布关键帧位置"lio_sam/mapping/trajectory"
* 2、发布降采样后的局部地图面片点 "lio_sam/mapping/map_local"
* 3、发布实时显示的点云，"lio_sam/mapping/cloud_registered"
* 4、发布原始配准后的点云"lio_sam/mapping/cloud_registered_raw"
* 5、发布载体的运行轨迹 "lio_sam/mapping/path"
*/
publishFrames();
}
}
- 角点、面片点匹配
/**
* @brief 角点优化，当前帧角点寻找局部map对应的角点
* 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
*/
void cornerOptimization()
{
// 获取Affine类型的变换，即更新transPointAssociateToMap变量
updatePointAssociateToMap();
// 使用openmp并行加速
#pragma omp parallel for num_threads(numberOfCores)
// 遍历当前帧的角点
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
// 定义原始点，配对的点，系数（最多存取4个参数）
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 选择第i个点作为当前点
pointOri = laserCloudCornerLastDS->points[i];
// 将该点从当前帧通过初始的位姿转换到地图坐标系下去，使用了transPointAssociateToMap变量
// 从ori换成sel
pointAssociateToMap(&pointOri, &pointSel);
// 在角点地图里寻找距离当前点比较近的5个点,索引存在pointSearchInd中，对应的距离存在pointSearchSqDis变量中
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 协方差矩阵
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
// 计算找到的点中距离当前点最远的点，如果距离太大那说明这个约束不可信，就跳过
if (pointSearchSqDis[4] < 1.0) {
// 五个点的坐标均值
float cx = 0, cy = 0, cz = 0;
// 计算协方差矩阵
// 首先计算均值
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
// 计算五个点的协方差矩阵（3*3），仅计算矩阵的上半部分
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
// 协方差矩阵赋值，对称阵
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
// 对matA1进行特征值分解
cv::eigen(matA1, matD1, matV1);
// 这是线特征性，要求最大特征值大于3倍的次大特征值（即λ1 >> λ2≈λ3）
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
// 当前点在map坐标系下的坐标
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
// 特征向量对应的就是直线的方向向量
// 通过点的均值沿着特征向量的方向（直线）两边拓展，构成一个线的两个端点
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
// 下面是计算点到线的残差和垂线方向（及雅克比方向）
// area_012，也就是三个点（x0,x1,x2）组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta)
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
// x1,x2两点之间的距离
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 两次叉积，得到点到直线的垂线段单位向量（x分量）
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
// (y分量)
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// (z分量)
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// x0点到x1x2两点连接的直线之间的距离，面积/底边长
float ld2 = a012 / l12;
// 一个简单的核函数，残差越大权重降低
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
// 如果残差小于1m，就认为是一个有效的约束
if (s > 0.1) {
// 当前激光帧角点，加入匹配集合中（注意：加入的点坐标是ori，即未进行转换的点）
laserCloudOriCornerVec[i] = pointOri;
// 当前角点对应的系数
coeffSelCornerVec[i] = coeff;
// 标记，代表此点有效，可以用于匹配优化
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
/**
* @brief 当前激光帧平面点寻找局部map匹配点
* 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
*/
void surfOptimization()
{
// 获取当前Affine类型的变换
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
// 定义当前原始点，转换到世界坐标系下的点，对应的系数
PointType pointOri, pointSel, coeff;
// 用于kdtree搜索的结果（索引与距离）
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 同样找5个面点
pointOri = laserCloudSurfLastDS->points[i];
// 转换到世界坐标系
pointAssociateToMap(&pointOri, &pointSel);
// 查找当前点（已经转换到世界坐标系）在局部地图中的最近的5个最近邻点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 最近邻的五个点，集合成一个5*3的矩阵
Eigen::Matrix<float, 5, 3> matA0;
// 定义平面方程时使用，方程的常数项，设置为1
Eigen::Matrix<float, 5, 1> matB0;
// 存储平面方程的系数
Eigen::Vector3f matX0;
// 平面方程Ax + By + Cz + 1 = 0
matA0.setZero();
matB0.fill(-1);
matX0.setZero();
// 同样最大距离不能超过1m
if (pointSearchSqDis[4] < 1.0) {
// 最近邻点集合为矩阵形式
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
matX0 = matA0.colPivHouseholderQr().solve(matB0);
// 求出来x的就是这个平面的法向量，也就是系数abc
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;// 常数项，任意定义即可
// 单位法向量
float ps = sqrt(pa * pa + pb * pb + pc * pc);
// 归一化，将法向量模长统一为1
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
// 判断选出的5个点所拟合平面是否有效
bool planeValid = true;
for (int j = 0; j < 5; j++) {
// 每个点代入平面方程，计算点到平面的距离，如果距离大于0.2m认为这个平面曲率偏大，就是无效的平面
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
// 如果通过了平面的校验
if (planeValid) {
// 计算当前点到平面的距离
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// 同样做个加权，与点到平面的距离成反比，若距离较远则权重小
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
// 平面的系数（经过权重调整）
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
// 如果S大于阈值，则认为当前点有效
if (s > 0.1) {
// 添加到待匹配的面片点集合中（注意：添加的是原始坐标）
laserCloudOriSurfVec[i] = pointOri;
// 添加对应的系数（平面方程）
coeffSelSurfVec[i] = coeff;
// 标记 当前面片点有效，可以用于优化匹配
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
- 两种类型特征点融合、LM优化
/**
* @brief 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
*/
void combineOptimizationCoeffs()
{
// combine corner coeffs
// 遍历当前帧所有的角点
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
// 只有标志位为true的时候才是有效约束 添加有效的角点与对应参数到统一集合中
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
// 标志位清零，重设为false
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}
/**
* @brief scan-to-map优化
* 1、对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，
* 2、迭代优化当前位姿，更新transformTobeMapped
* 3、判断是否收敛,变化量小于阈值。
* @param iterCount
* @return true
* @return false
*/
bool LMOptimization(int iterCount)
{
// 原始的loam代码是将lidar坐标系转到相机坐标系，这里把原先loam中的代码拷贝了过来，但是为了坐标系的统一，就先转到相机系优化，然后结果转回lidar系
// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
// 将lidar系转到相机系（更换坐标轴）
float srx = sin(transformTobeMapped[1]);
float crx = cos(transformTobeMapped[1]);
float sry = sin(transformTobeMapped[2]);
float cry = cos(transformTobeMapped[2]);
float srz = sin(transformTobeMapped[0]);
float crz = cos(transformTobeMapped[0]);
// 选取点的数量 如果配对点的数量过少，则直接返回
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
// 创建一个 配对点云数量×6大小的矩阵 及 其转置
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
// At*A
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
// B
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
// A*B
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
// 优化求解的x
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
// 临时变量，代表原始点与对应系数
PointType pointOri, coeff;
for (int i = 0; i < laserCloudSelNum; i++) {
// 首先将当前点以及点到线（面）的单位向量转到相机系
// lidar -> camera
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// lidar -> camera
// 点到线的垂线单位向量，平面的法向量 更改坐标轴
coeff.x = coeffSel->points[i].y;
coeff.y = coeffSel->points[i].z;
coeff.z = coeffSel->points[i].x;
coeff.intensity = coeffSel->points[i].intensity;
// in camera
// 相机系下的旋转顺序是Y - X - Z对应lidar系下Z -Y -X
// 这是求导？ 构建雅克比矩阵？
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
// lidar -> camera
// 这里就是把camera转到lidar了
matA.at<float>(i, 0) = arz;
matA.at<float>(i, 1) = arx;
matA.at<float>(i, 2) = ary;
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;
// 点到直线距离、平面距离，作为观测值
matB.at<float>(i, 0) = -coeff.intensity;
}
// 构造JTJ以及-JTe矩阵
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// 求解增量
// J^T·J·delta_x = -J^T·B
// (J^T·J等同与matAtA，delta_x为要求的变化量对应于matX，J为雅克比矩阵，B为残差，matAtB)
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// 如果是第一次迭代优化
// 检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0
if (iterCount == 0) {
// 检查一下是否有退化的情况
cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
// 对JTJ进行特征值分解，分解成E和V
cv::eigen(matAtA, matE, matV);
// 复制V到V2
matV.copyTo(matV2);
// 是否退化了？
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
// 特征值从小到大遍历，如果小于阈值就认为退化
if (matE.at<float>(0, i) < eignThre[i]) {
// 退化方向对应的特征向量全部置0
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
// 表征退化的方向
// 如果没有退化，matP应该是单位向量
matP = matV.inv() * matV2;
}
// 如果发生退化，就对增量进行修改，退化方向不更新
if (isDegenerate)
{
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
// 增量更新
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
// 计算更新的旋转和平移大小
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(3, 0) * 100, 2) +
pow(matX.at<float>(4, 0) * 100, 2) +
pow(matX.at<float>(5, 0) * 100, 2));
// 旋转和平移增量足够小，认为优化问题收敛了
if (deltaR < 0.05 && deltaT < 0.05) {
return true; // converged
}
// 否则继续优化
return false; // keep optimizing
}
选取关键帧，添加各种因子并进行优化
/************************************************************************************/
/**********************关键帧、添加各种因子、矫正轨迹并发布*******************************/
/************************************************************************************/
// 根据旋转与平移量判断当前帧是否为关键帧
bool saveFrame()
{
// 如果没有关键帧，那直接认为是关键帧(第一帧直接为关键帧)
if (cloudKeyPoses3D->points.empty())
return true;
// 取出上一个关键帧的位姿
Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
// 当前帧的位姿转成eigen形式
Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
// 计算两个位姿之间的delta pose
Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
float x, y, z, roll, pitch, yaw;
// 两帧之间的相对变换 转成 平移+欧拉角的形式
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
// 任何一个旋转大于给定阈值或者平移大于给定阈值就认为是关键帧
// 旋转大于0.2弧度，平移大于1m
if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
return false;
return true;
}
/**
* @brief 添加激光里程计因子
* 第一个关键帧添加的是PriorFactor
* 后续添加的是BetweenFactor
*/
void addOdomFactor()
{
// 如果是第一帧关键帧
if (cloudKeyPoses3D->points.empty())
{
// 置信度就设置差一点，尤其是不可观的平移和yaw角
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
// 增加先验约束，对第0个节点增加约束
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
// 加入节点信息（索引 gtsam位姿）
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
}else{
// 如果不是第一帧，就增加帧间约束
// 这时帧间约束置信度就设置高一些
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
// 转成gtsam的格式
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
// 这是一个帧间约束，分别输入两个节点的id，帧间约束大小以及置信度
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
// 加入节点信息
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
}
}
/**
* @brief 添加GPS因子
* 找到对应时间的GPS数据
* Z坐标不使用GPS数据，仍使用LiDAR匹配的结果
* 构建一个GPSFactor，添加到GTSAM图中，作用类似于回环
*/
void addGPSFactor()
{
// 如果没有gps信息就算了
if (gpsQueue.empty())
return;
// wait for system initialized and settles down
// 如果有gps但是没有关键帧信息也算了
if (cloudKeyPoses3D->points.empty())
return;
else
{
// 第一个关键帧和最后一个关键帧相差很近则不添加GPS
if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
return;
}
// pose covariance small, no need to correct
// gtsam反馈的当前x，y的置信度，如果置信度比较高也不需要gps来修正(阈值设置为25)
if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
return;
// last gps position
static PointType lastGPSPoint;
// 当还有GPS数据
while (!gpsQueue.empty())
{
// 把距离当前帧比较早的帧都抛弃
if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
{
// message too old
gpsQueue.pop_front();
}
// 比较晚就则等待lidar匹配计算
else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
{
// message too new
break;
}
else
{
// 说明这个gps时间上距离当前帧已经比较近了,那就把这个数据取出来
nav_msgs::Odometry thisGPS = gpsQueue.front();
gpsQueue.pop_front();
// GPS too noisy, skip
float noise_x = thisGPS.pose.covariance[0];
float noise_y = thisGPS.pose.covariance[7];
float noise_z = thisGPS.pose.covariance[14];
// 如果gps的置信度不高，也没有必要使用了（阈值设置为2）
if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
continue;
// 取出gps的位置
float gps_x = thisGPS.pose.pose.position.x;
float gps_y = thisGPS.pose.pose.position.y;
float gps_z = thisGPS.pose.pose.position.z;
// 通常gps的z没有x y准，因此这里可以不使用z值（参数文件中默认为false）
if (!useGpsElevation)
{
// GPS坐标Z轴信息恢复成Lidar匹配的结果，对应的噪声
gps_z = transformTobeMapped[5];
noise_z = 0.01;
}
// GPS not properly initialized (0,0,0)
// gps的x或者y太小说明还没有初始化好
if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
continue;
// Add GPS every a few meters
// 整理当前的GPS点坐标
PointType curGPSPoint;
curGPSPoint.x = gps_x;
curGPSPoint.y = gps_y;
curGPSPoint.z = gps_z;
// 加入gps观测不宜太频繁，相邻不小于5m
if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
continue;
else
lastGPSPoint = curGPSPoint;
// 添加GPS因子
gtsam::Vector Vector3(3);
// gps的置信度，标准差设置成最小1m，也就是不会特别信任gps信号
Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f);
noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3);
// 调用gtsam中集成的gps的约束
gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
gtSAMgraph.add(gps_factor);
// 加入gps之后等同于回环，需要触发较多的isam update
aLoopIsClosed = true;
break;
}
}
}
/**
* @brief 添加一个回环因子
* 1、遍历回环队列中的每个数据
* 2、添加相应的 回环中两帧索引 帧间位姿 噪声
*/
void addLoopFactor()
{
// 有一个专门的回环检测线程会检测回环，检测到就会给这个队列塞入回环结果
if (loopIndexQueue.empty())
return;
// 把队列里所有的回环约束都添加进来
for (int i = 0; i < (int)loopIndexQueue.size(); ++i)
{
int indexFrom = loopIndexQueue[i].first;
int indexTo = loopIndexQueue[i].second;
// 队列中第i回环的帧间约束
gtsam::Pose3 poseBetween = loopPoseQueue[i];
// 使用icp的得分作为回环的置信度
gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
// 加入回环约束
gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
}
// 清空回环相关队列
loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
// 标志位置true（是否添加了一个回环）
aLoopIsClosed = true;
}
/**
* @brief 设置当前帧为关键帧并执行因子图优化
* 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
* 2、添加激光里程计因子、GPS因子、闭环因子
* 3、执行因子图优化
* 4、得到当前帧优化后位姿，位姿协方差
* 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
*/
void saveKeyFramesAndFactor()
{
// 通过旋转和平移的增量来判断是否是关键帧
// 如果saveFrame函数返回false，则认为不是关键帧
// 直接返回
if (saveFrame() == false)
return;
// 如果是关键帧就给isam增加因子
// odom factor
// 增加odom的因子
addOdomFactor();
// gps的因子
// gps factor
addGPSFactor();
// 回环的因子
// loop factor
addLoopFactor();
// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");
// 所有因子加完了，就调用isam接口更新图模型
// update iSAM
// initialEstimate为位姿的初始估计，在添加odomfactor过程中同时添加
isam->update(gtSAMgraph, initialEstimate);
isam->update();
// 如果加入了gps的约束或者回环约束，isam需要进行更多次的优化
if (aLoopIsClosed == true)
{
// 为什么需要多次？
isam->update();
isam->update();
isam->update();
isam->update();
isam->update();
}
// 将约束和节点信息清空，他们已经被加入到isam中去了，因此这里清空不会影响整个优化
gtSAMgraph.resize(0);
initialEstimate.clear();
// 下面保存关键帧信息
//save key poses
PointType thisPose3D; // PointXYZI xyz存储平移量，I存储索引
PointTypePose thisPose6D; //PointXYZIRPYT 多个属性，用来存储平移、旋转
Pose3 latestEstimate;
// 计算当前的估计值
isamCurrentEstimate = isam->calculateEstimate();
// 取出优化后的最新关键帧位姿
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
// cout << "****************************************************" << endl;
// isamCurrentEstimate.print("Current estimate: ");
// 平移信息取出来保存进cloudKeyPoses3D这个结构中，其中索引作为intensity值
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);
// 6D姿态同样保留下来
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity ; // 同thisPose3D中的索引
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;
// 保存当前LiDAR位姿优化后的置信度
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);
// save updated transform
// 将优化后的位姿更新到transformTobeMapped数组中，作为当前最佳估计值
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
// 根据当前最新位姿更新rviz可视化
updatePath(thisPose6D);
}
/**
* @brief 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
* 1、判断是否添加了回环因子（或者GPS因子）进行优化
* 2、若使用了，则清空局部地图map容器中的关键帧，清除全局路径
* 3、遍历因子图中所有变量节点的位姿，更新关键帧变量中的位姿（cloudKeyPoses3D与cloudKeyPoses6D），更新全局路径
* 使用优化后的位姿更新已有的关键帧位姿
*/
void correctPoses()
{
// 没有关键帧，自然也没有什么意义
if (cloudKeyPoses3D->points.empty())
return;
// 只有回环以及gps信息这些会触发全局调整信息才会触发
if (aLoopIsClosed == true)
{
// clear map cache
// 很多位姿会变化，因子之前的容器内转到世界坐标系下的很多点云就需要调整，因此这里索性清空
laserCloudMapContainer.clear(); // 清空点云地图容器中的点云
// clear path
// 清空path
globalPath.poses.clear();
// update key poses
// 然后更新所有的位姿
int numPoses = isamCurrentEstimate.size(); // 当前所有位姿的数量
for (int i = 0; i < numPoses; ++i)
{
// 更新所有关键帧的位姿（3D格式的关键帧位姿）
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();
// 更新（6D格式的关键帧位姿）
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();
// 同时更新path
updatePath(cloudKeyPoses6D->points[i]);
}
// 标志位复位
aLoopIsClosed = false;
}
}
/**
* @brief 更新路径
* 把矫正后的位姿添加到globalPath中
* @param pose_in
*/
void updatePath(const PointTypePose& pose_in)
{
// 定义一个位姿
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
pose_stamped.header.frame_id = odometryFrame;
pose_stamped.pose.position.x = pose_in.x;
pose_stamped.pose.position.y = pose_in.y;
pose_stamped.pose.position.z = pose_in.z;
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
pose_stamped.pose.orientation.x = q.x();
pose_stamped.pose.orientation.y = q.y();
pose_stamped.pose.orientation.z = q.z();
pose_stamped.pose.orientation.w = q.w();
// 添加到路径中
globalPath.poses.push_back(pose_stamped);
}
/**
* @brief 发布里程计
* 1、发布当前位姿transformTobeMapped， "lio_sam/mapping/odometry"
* 2、发布增量式位姿“lio_sam/mapping/odometry_incremental”， 与上一话题基本相同，是一个累计相对变换，不断增加的里程计（平滑，不会突变）
* 3、发送lidar在odom坐标系下（odometryFrame("odom")与 "lidar_link"）的tf（其值本质上也是transformTobeMapped）
*/
void publishOdometry()
{
// Publish odometry for ROS (global)
// 发送当前帧的位姿
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = timeLaserInfoStamp;
laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
// 发布当前位姿"lio_sam/mapping/odometry"
pubLaserOdometryGlobal.publish(laserOdometryROS);
// Publish TF
// 发送lidar在odom坐标系下的tf（其值本质上也是transformTobeMapped）
static tf::TransformBroadcaster br;
tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
br.sendTransform(trans_odom_to_lidar);
// Publish odometry for ROS (incremental)
// 发送增量位姿变换
// 这里主要用于给imu预积分模块使用，需要里程计是平滑的
static bool lastIncreOdomPubFlag = false;
// 定义一个里程计变量（存储增量式的值）
// 这个增量式的里程计不会由于优化而突然改变值，不会优化之后进行纠正，只是不断累计增量
static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
// 该标志位处理一次后始终为true
if (lastIncreOdomPubFlag == false)
{
lastIncreOdomPubFlag = true;
// 记录当前位姿（第一帧直接在这里转换）
laserOdomIncremental = laserOdometryROS;
increOdomAffine = trans2Affine3f(transformTobeMapped);
} else {
// 上一帧的最佳位姿和当前帧最佳位姿（scan matching之后，而不是根据回环或者gps调整之后的位姿）之间的位姿增量
Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
// 位姿增量叠加到上一帧位姿上（增量累计的里程计）
increOdomAffine = increOdomAffine * affineIncre;
float x, y, z, roll, pitch, yaw;
// 分解成欧拉角+平移向量
pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);
// 如果有imu数据，同样对roll和pitch做插值
if (cloudInfo.imuAvailable == true)
{
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = 0.1;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
// 对roll进行插值（实质上IMU发挥的作用很小）
transformQuaternion.setRPY(roll, 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
roll = rollMid;
// slerp pitch
// 对pitch进行插值（实质上IMU发挥的作用很小）
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}
// 增量式的lidar里程计
laserOdomIncremental.header.stamp = timeLaserInfoStamp;
laserOdomIncremental.header.frame_id = odometryFrame;
laserOdomIncremental.child_frame_id = "odom_mapping";
laserOdomIncremental.pose.pose.position.x = x;
laserOdomIncremental.pose.pose.position.y = y;
laserOdomIncremental.pose.pose.position.z = z;
laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
// 当前帧与map进行匹配，LM优化过程中，第一次迭代矩阵分解判断是否发生了退化
// 协方差这一属性作为是否退化的标志位
if (isDegenerate)
laserOdomIncremental.pose.covariance[0] = 1;
else
laserOdomIncremental.pose.covariance[0] = 0;
}
// 发布增量式的里程计
pubLaserOdometryIncremental.publish(laserOdomIncremental);
}
/**
* @brief 发布各种信息
* 1、发布关键帧位置"lio_sam/mapping/trajectory"
* 2、发布降采样后的局部地图面片点 "lio_sam/mapping/map_local"
* 3、发布实时显示的点云，"lio_sam/mapping/cloud_registered"
* 4、发布原始配准后的点云"lio_sam/mapping/cloud_registered_raw"
* 5、发布载体的运行轨迹 "lio_sam/mapping/path"
*/
void publishFrames()
{
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
// 发送关键帧位置（关键帧位置已经被存储为pointtype类型，所以发布成pointcloud2类型的点云）
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
// 发送周围局部地图点云信息（odometryFrame的值为“odom”）
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// publish registered key frame
// 发布配准后的关键帧，也就是当前点云（转换到世界坐标系下）
// 话题名称 "lio_sam/mapping/cloud_registered"
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
// 发布高分辨率的配准后点云（cloud_deskewed是在点云去畸变处理节点中提取的有效点云）
// 变换到世界坐标系并发布
// 话题名称 "lio_sam/mapping/cloud_registered_raw"
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
// 发送path
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = odometryFrame;
pubPath.publish(globalPath);
}
}
/************************************************************************************/
/**********************关键帧、添加各种因子、矫正轨迹并发布*******************************/
/************************************************************************************/
回环检测
详细参考：LIO-SAM回环检测模块代码解析
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;
}