# LIO-SAM学习总结目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档
前言
提示：这里可以添加本文要记录的大概内容：
最近在学习LIO-SAM这个紧耦合的激光惯导框架，由于涉及到的内容较多，参考的资料较多，索性就写一个博客记录一下学到的知识和参考的资料，以便以后回顾和交流。内容较多，可通过目录快速跳转至各部分。
提示：以下是本篇文章正文内容，下面案例可供参考
一、参考资料
提示：本人刚入门slam不久，学习该框架参考了网上许多大佬的资料和博客，因此在这里也进行一个汇总，方便以后需要学习的同学查阅和学习。
**项目网址：
https://github.com/TixiaoShan/LIO-SAM**
1、论文
论文：T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, D. Rus. LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2020.
【论文阅读38】LIO-SAM
论文学习可以直接参考上面链接中翻译后的中文版，对比LOAM对应的论文，该篇论文中别没有特别多的学习内容，主要是在Lego-LOAM的基础上加入了IMU预积分和采用因子图优化融合了激光、IMU、GPS、回环因子等。
2、知识点
（1）数学基础
预积分
预积分是LIO和VIO的核心部分。下面介绍一下SLAM中预积分的作用和推导思路。
1、为什么要使用预积分？
IMU测量的角速度和加速度进行积分可得到旋转角度和位移。如果对每个IMU数据进行积分，计算量将非常大，通常的做法是对两个激光帧(或图像帧)之间的IMU数据进行积分，从而构建相邻两帧之间的相对位姿约束。这就出现一个问题，两个激光帧之间的IMU积分需要给定第一个帧的状态估计量作为积分的初始条件。而每次迭代优化，这些状态估计量都会更新，这就需要不断重复地进行所有帧之间的IMU积分。IMU预积分就是为了解决这个问题提出的，避免了重复积分，减少了计算量。
2、IMU推导思路
首先推导得到IMU角速度和加速度的模型。由于加速度积分得到速度，速度积分得到位置。根据IMU模型推导得到R(旋转)、v(速度)、p(位置)与与角速度、加速度、IMU偏置bias、高斯白噪声之间的公式。
对第i时刻到j时刻直接的IMU数据进行积分，直接积分虽然简单，但是有一个致命的缺点：如果第i时刻的状态量经过优化更新了，则从第i+1时刻到j-1时刻的状态也要跟着更新，这时需要重新计算积分，需要耗费巨大的计算量和时间。于是使用一段时间的相对量来代替某个时刻的绝对量。因此，引入旋转、速度、位置的相对状态量，称为预积分。
后续推导过程主要分为以下几步：
一、假设零偏已知，然后将噪声项分离出来。
二、推导出预积分中的噪声递推模型。
三、当更新零偏后，推导预积分的更新的更新方式，避免重复积分。
四、求预积分中残差对状态增量的雅可比矩阵。
因子图
因子图是一种用于处理大量变量和约束关系的图形模型，被广泛应用于机器人等领域的状态估计和感知问题。在SLAM中，因子图被用来描述机器人在未知环境中的位置和地图信息。SLAM问题中，机器人需要同时进行定位和建图，因此需要在不确定性和噪声的条件下处理大量的测量和状态变量。因此，SLAM问题可以描述为一个大规模的非线性最小化问题，其中目标是最小化所有测量和状态变量的误差，从而获得最准确的地图和机器人位置。因子图可以很好地解决SLAM问题中的大规模非线性优化问题。当测量数据增加时，因子图提供了一种自适应建图的方法，能够快速完成数据的处理。此外，因子图还可以帮助机器人对测量数据的不确定性进行建模和处理，使机器人能够更准确地估计自身位置和地图信息。
因子图详细学习可参考以下链接：
1、因子图的理论基础与在机器人中的应用
2、【泡泡机器人公开课】第五十六课：gtsam_tutorial-董靖
根据上述的内容学习之后，我自己也总结了一下：
因子图学习总结
（2）ROS基础
1、ROS中的多线程使用方法
代码中使用到的ROS多线程，可参考上述链接。
2、ROS中nav_msgs消息
（3）PCL
PCL库学习笔记——入门、基本的数据结构
代码出现is_dense这个数据不太理解。查看源码：
/** \brief True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields). */
bool is_dense = true;
上述注释代表意思：指定点云中的所有数据是有效的则为true，否则为false（True if no points are invalid (e.g., have NaN or Inf values).）
即： 点云数据中所有点均有值为true，有inf/NaN为false。通过该值可去除无效点云数据
（4）GTSAM
GTSAM是一个用于解决大规模非线性优化问题的开源C++库。它提供了处理因子图的方法，能够用于多种机器人感知和估计问题。
GTSAM官方文档链接：链接
下面介绍一下使用GTSAM构建因子图优化的大致步骤：
- 设置传感器噪声模型
- 添加因子factor
- 设置初始值
- 选择优化器进行优化
下面介绍代码中使用到GTSAM的一些函数：
1. 设置传感器噪声模型
gtsam::PreintegrationParams
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
imuGravity为重力加速度，在参数服务器中设置的参数，用于计算相对于重力的线加速度。
其中MakeSharedU就是设置重力方向的，其中的设置和IMU的坐标轴建模方向有关
- MakeSharedU 是ENU，就是东北天，内部把imuGravity的值进行了取负。
- MakeSharedD 是NED，就是北东地，内部不改变变量的正负号。
gtsam::Matrix33::Identity(3, 3)
p->accelerometerCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuAccNoise, 2); // acc white noise in continuous
p->gyroscopeCovariance = gtsam::Matrix33::Identity(3, 3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
p->integrationCovariance = gtsam::Matrix33::Identity(3, 3) * pow(1e-4, 2); // error committed in integrating position from velocities
该函数是设置一个33单位阵。
由于需要设置传感器噪声模型，因此需要添加IMU噪声模型，角速度噪声和加速度噪声。这里的协方差矩阵使用33的单位矩阵乘以高斯白噪声imuAccNoise和imuGyrNoise的平方。
gtsam::imuBias::ConstantBias
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());
gtsam::imuBias::ConstantBias()是gtsam做好的一个imu零偏，其中都是0。
gtsam::noiseModel::Diagonal::Sigmas
gtsam::noiseModel::Diagonal是gtsam中的一个噪声模型类，表示噪声协方差矩阵是一个对角矩阵的噪声模型。gtsam::noiseModel::Diagonal::Sigmas()函数是该类中的一个方法，用来获取噪声模型中对角线元素的标准差。
在这里插入代码片
2. 添加因子
3. 设置初始值
4. 选择优化器
（5）C++基础
std::move:
详解C++移动语义std::move()
参考以上博客，简单点理解，就是将对象的状态或者所有权从一个对象转移到另一个对象，只是转移，没有内存的搬迁或者内存拷贝。
std::lock_guard：
C++11中std::lock_guard的使用
简单理解，std::lock_guard的作用就是更加方便对mutex的上锁和解锁
map
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
这段代码定义了一个名为laserCloudMapContainer的变量，它是一个
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>>类型的容器。
map是C++标准库中的关联容器，它提供了一种以键值对形式存储和访问元素的方式。在这里，map的键类型是int，值类型是pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>>。
pair是C++标准库中的一个模板类，它可以存储两个不同类型的对象。在这个代码中，pair存储了两个pcl::PointCloud<PointType>类型的对象，用于表示某个整数键对应的两个点云数据。
通过使用laserCloudMapContainer[key]的形式，可以访问和操作map中特定键所对应的值。在这里，key是一个整数，可以作为索引来获取对应的点云数据对。
例如，可以通过laserCloudMapContainer[key].first和laserCloudMapContainer[key].second来分别访问两个点云数据对象。
通过使用map容器，可以方便地将整数键与两个点云数据对象进行关联和管理。
3、标定
标定是重中之中，在本系统中，主要涉及到IMU自身的内参标定和LIDAR与IMU直接的联合外参标定，在实车运行时，标定结果的好坏直接决定了最后地图会不会跑飞。
在LIO-SAM代码中的doc文件夹下有一个params.yaml文件，在该文件中可以修改IMU的bias和噪声等数据，以及IMU和LIDAR之间的旋转矩阵。
下面代码就是之前标定好的数据。
# IMU Settings IMU内参 bias和白噪声
imuAccNoise: 3.9939570888238808e-03
imuGyrNoise: 1.5636343949698187e-03
imuAccBiasN: 6.4356659353532566e-05
imuGyrBiasN: 3.5640318696367613e-05
imuGravity: 9.80511
imuRPYWeight: 0.01
# Extrinsics: T_lb (lidar -> imu) 这里应该是 imu->lidar
# lidar和imu外参标定数据 即转移矩阵
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [-1, 0, 0,
0, 1, 0,
0, 0, -1]
extrinsicRPY: [0, -1, 0,
1, 0, 0,
0, 0, 1]
根据上述标定结果进行室内实车测试，发现建图结果是一团点云，并且小车移动的同时地图跟着旋转。如下图所示：
参考以下博客：Lio_sam运行测试环节遇到的问题以及实测总结
发现本人实验所使用的小车结构和传感器安装位置和上述博客中博主的情况很类似，如下图所示：
且本人实验室的小车的地盘由于是两轮差速运动，前后两个扭角轮距地面有缝隙，因此小车在运行过程中前后老是晃动，特别是在转弯和中途停下的过程中，小车晃动特别厉害，加之雷达安装得很高，经过较长的铝型材，尾端雷达的晃动又被放大，雷达数据和IMU数据时间同步也是一个问题。因此，使用上述标定结果室内实车测试如下图：
基于上面糟糕的建图效果，下面准备采取以下几种方法来进行改进：
- 降低雷达安装高度
- 更换更合适的两个从动轮，解决小车晃动问题。
- 重新在室外低速情况下进行标定。
- 标定完成后在室外大范围场景下进行测试。
最终建图效果如下图所示：
4、代码
1、LIO-SAM代码阅读
该博客大概讲解了代码中每个函数的大概功能。
2、LIO-SAM超级详细源码流程图
该博客主要详细总结了代码流程图，学习代码时可对照着学习。
3、LIO-SAM流程及代码详解（最全）
该博客详细介绍了所有代码的注释，适合了解完算法原理和代码框架后，比对查阅学习。此外，该博主SLAM学习笔记的其他博客也是很好的学习资料。
二、论文和框架
上图展示了LIO-SAM的因子图。图中主要包含四种因子。第一种是IMU预积分因子（橙色），由两个相邻关键帧之间的IMU测量积分得到。第二种是激光里程计因子（绿色），由每个关键帧和之前n个关键帧之间的帧图匹配结果得到。第三种是GPS因子（黄色），由每个关键帧的GPS测量得到。第四种是回环因子（黑色），由每个关键帧和候选回环关键帧的时序相邻的2m+1个关键帧之间的帧图匹配得到。此外，因子图优化通过iSAM2完成。
三、传感器标定
1、IMU内参标定
首先需要拿到imu的内参，使用港科大的imu_utils标定工具，使用Allan方差来标定高斯白噪声和随机游走噪声。
参考博客：
https://blog.csdn.net/weixin_53073284/article/details/123341141?spm=1001.2014.3001.5502
2、IMU和激光雷达外参标定
联合标定lidar和imu的外参，使用瑞士苏黎世联邦理工大学自动驾驶实验室开发的lidar_align工具标定雷达和imu外参过程。
参考博客：
https://blog.csdn.net/weixin_53073284/article/details/123337885?spm=1001.2014.3001.5502
四、代码介绍
学习具体代码前，最好保证理解了文章的大致思想，对代码框架在脑子中有一个大体的结构，区别于Cartographer代码的工程风格(代码较难学习和理解)，LOAM系列代码偏向学院风，更容易大家理解，主要部分都在src文件夹下的几个.cpp中。
代码整体框架
代码主要上述图片中的各个文件组成。其中：
config文件夹：主要存放params.yaml文件，用于调参（即设置代码中一些关键参数）
include文件夹：存放utility.h头文件，主要包括一个读取参数和初始化参数的类ParamServer
launch文件夹：存放launch文件，其中最重要的是run.launch文件，用于启动该框架
msg文件夹：存放作者自定义的cloud_info.msg文件（作者自定义的消息格式）
src文件夹：代码的核心，存放4个核心cpp文件
srv文件夹：存放ROS的srv文件
CMakeList.txt：cmake文件
下面来具体介绍每个文件的代码和实现的功能：
0、params.yaml
待补充…
1、utility.h
待补充…
2、imageProjection.cpp
本程序文件主要工作分为两大块，一个是类似于Lego-loam里的将点云投射到距离图像，另一个是去除运动畸变。
（1）输入(订阅)：
- imu原始数据 (imuTopic)
- 来自IMUPreintegration,imu预积分数据(odomTopic+“_incremental”)
- 激光点云原始数据(pointCloudTopic)
（2）输出(发布)：
- 完成去畸变的点云数据(“lio_sam/deskew/cloud_deskewed”)
- cloudinfo数据(“lio_sam/deskew/cloud_info”)
（3）具体函数：
-
主函数
主函数肯定是最重要的，看代码一般也从主函数开始看起。不过这个.cpp文件的主函数较简单，主要是初始化ros节点和实例化一个ImageProjection对象，具体的功能程序在ImageProjection类中进行封装。 -
ImageProjection类
ImageProjection是该模块的核心。 主要是封装了程序执行过程中的一系列变量和函数，这里不展开具体介绍了，详细代码注释会放在该部分最后，请自行查看。
下面开始介绍ImageProjection类中的每个函数：
各函数功能介绍
1. ImageProjection()构造函数
ImageProjection()构造函数主要用于初始化。初始化ROS订阅的话题和发布的话题。其中allocateMemory()函数用于给一些变量分配内存。resetParameters函数用于重置部分参数数据。这两个函数就不做具体介绍了，主要涉及智能指针。
2. imuHandler回调函数
imuHandler回调函数主要功能是接收订阅的IMU数据，并利用标定好的外参数据（IMU到Lidar的旋转矩阵）将IMU数据从IMU坐标系转化到Lidar坐标系。然后将数据存储到imuQueue队列中。 注意：deskewFlag(0)应该是初始化deskewFlag这个变量为0的意思。
3. cloudHandler回调函数
cloudHandler这个函数应该是类中最重要的函数。
cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
该函数主要用于根据激光雷达类型，转化数据格式，判断数据是否有效，去除无效点云数据，缓存点云数据
deskewInfo()
该函数作用是确保当前获取的IMU数据在时间上处在激光雷达数据起始点云和结束点云之间。并且将初始时刻的IMU姿态角和IMU里程计设为当前激光帧的初始姿态角和初始位姿。主要是为了后续去运动畸变，获取IMU数据转化得到的去运动畸变的参数。该函数中主要包含以下两个函数：
——imuDeskewInfo
a、选取出IMU数据定义为有效当前激光数据帧对应的IMU数据(当前帧IMU数据中时间大于最早激光点云数据时间-0.01s的IMU数据)
b、遍历筛选出来的有效数据，获取这些IMU数据转化得到的姿态角数据 Roll Pitch Yaw
c、对这些IMU数据进行积分，获得积分后的数据imuRotX imuRotY imuRotZ
——odomDeskewInfo
a、选取出时间上大于当前激光帧起始时间的最早的里程计数据作为startOdom，并获取该startOdom的姿态角
b、用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization
c、
if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
这句代码意思是// 如果起止两个时刻对应imu里程计的方差不等，返回
为啥呢？？？
d、通过imu里程计的位姿xyzrpy得到imu里程计初始坐标系转换到大地坐标系的旋转矩阵transBegin。
通过imu里程计的位姿xyzrpy得到imu里程计终止坐标系转换到大地坐标系的旋转矩阵transEnd
通过这两个矩阵transBegin.inverse() * transEnd; 得到起始时刻里程计的相对变换矩阵transBt
由transBt 提取XYZ以及欧拉角 用于后续去运动畸变
projectPointCloud()
该函数主要的作用是遍历激光点云数据，并使用IMU数据去激光点云数据的运动畸变。该函数中还有一个deskewPoint函数，主要用于根据IMU数据去激光雷达数据的运动畸变。
——deskewPoint
cloudExtraction()
该函数的作用主要是保存有效激光点云数据，并且由于特征提取的时候不考虑每一行的前5个和最后5个，因此设置startRingIndex和endRingIndex这两个索引从第五个和倒数第五个。
publishClouds()
发布用户自定义的cloudInfo激光数据(经过数据预处理的激光点云数据)
resetParameters()
该函数用于重置部分存储数据的变量，方便下一次处理数据
4. odometryHandler回调函数
里程计回调函数 存储数据到队列中
具体注释：
#include "utility.h"
#include "lio_sam/cloud_info.h"
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring) (float, time, time)
)
struct OusterPointXYZIRT {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t noise;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
(uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
)
// Use the Velodyne point format as a common representation
using PointXYZIRT = VelodynePointXYZIRT;
const int queueLength = 2000;
class ImageProjection : public ParamServer //继承ParamServer
{
private:
std::mutex imuLock; // 多线程编程上锁和解锁
std::mutex odoLock;
ros::Subscriber subLaserCloud; // 激光数据订阅和发布对象
ros::Publisher pubLaserCloud;
ros::Publisher pubExtractedCloud;
ros::Publisher pubLaserCloudInfo;
ros::Subscriber subImu;
std::deque<sensor_msgs::Imu> imuQueue;
ros::Subscriber subOdom;
std::deque<nav_msgs::Odometry> odomQueue;
std::deque<sensor_msgs::PointCloud2> cloudQueue;
sensor_msgs::PointCloud2 currentCloudMsg;
double *imuTime = new double[queueLength];
double *imuRotX = new double[queueLength];
double *imuRotY = new double[queueLength];
double *imuRotZ = new double[queueLength];
int imuPointerCur;
bool firstPointFlag;
Eigen::Affine3f transStartInverse;
pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
pcl::PointCloud<PointType>::Ptr fullCloud;
pcl::PointCloud<PointType>::Ptr extractedCloud;
int deskewFlag;
cv::Mat rangeMat;
bool odomDeskewFlag;
float odomIncreX;
float odomIncreY;
float odomIncreZ;
lio_sam::cloud_info cloudInfo;
double timeScanCur;
double timeScanEnd;
std_msgs::Header cloudHeader;
vector<int> columnIdnCountVec;
public:
ImageProjection():
deskewFlag(0)
{
//订阅话题进入回调函数 imu数据 激光点云.
// imuTopic:topic name;
// 2000:queue size;
// &ImageProjection::imuHandler:callback function 回调函数
// this: 调用这个class里的返回函数，可以使用第四个参数，例如有个对象叫listener，
// 调用该对象内部的回调函数，则传入&listener，这里调用自己的，则传入this指针
// ros::TransportHints().tcpNoDelay() :被用于指定hints，确定传输层的作用话题的方式:无延迟的TCP传输方式
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅/odometry_incremental 来自IMUPreintegration,imu预积分 进入回调函数处理数据
subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅激光雷达数据 并进入回调函数
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
// 发布去畸变的点云
pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
// 发布激光点云数据
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
// 分配内存
allocateMemory();
// 重置部分参数数据
resetParameters();
//setVerbosityLevel用于设置控制台输出的信息。 L_DEBUG会输出DEBUG信息;
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}
/* 给变量分配内存 */
void allocateMemory()
{
//根据params.yaml中给出的N_SCAN Horizon_SCAN参数值分配内存
//用智能指针的reset方法在构造函数里面进行初始化
laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
fullCloud.reset(new pcl::PointCloud<PointType>());
extractedCloud.reset(new pcl::PointCloud<PointType>());
fullCloud->points.resize(N_SCAN*Horizon_SCAN);
cloudInfo.startRingIndex.assign(N_SCAN, 0);
cloudInfo.endRingIndex.assign(N_SCAN, 0);
cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);
resetParameters();//重置部分参数数据
}
/* 重置部分参数数据 */
void resetParameters()
{
laserCloudIn->clear();
extractedCloud->clear();
// reset range matrix for range image projection
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
imuPointerCur = 0;
firstPointFlag = true;
odomDeskewFlag = false;
for (int i = 0; i < queueLength; ++i)
{
imuTime[i] = 0;
imuRotX[i] = 0;
imuRotY[i] = 0;
imuRotZ[i] = 0;
}
columnIdnCountVec.assign(N_SCAN, 0);
}
~ImageProjection(){}
/* IMU回调函数 先通过标定的外参旋转矩阵R将IMU数据从IMU坐标系转到Lidar坐标系 然后存储IMU数据到队列 */
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
// IMU数据转到Lidar坐标系下
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
// 存储IMU数据到队列imuQueue
std::lock_guard<std::mutex> lock1(imuLock);
imuQueue.push_back(thisImu);
// debug IMU data
// cout << std::setprecision(6);
// cout << "IMU acc: " << endl;
// cout << "x: " << thisImu.linear_acceleration.x <<
// ", y: " << thisImu.linear_acceleration.y <<
// ", z: " << thisImu.linear_acceleration.z << endl;
// cout << "IMU gyro: " << endl;
// cout << "x: " << thisImu.angular_velocity.x <<
// ", y: " << thisImu.angular_velocity.y <<
// ", z: " << thisImu.angular_velocity.z << endl;
// double imuRoll, imuPitch, imuYaw;
// tf::Quaternion orientation;
// tf::quaternionMsgToTF(thisImu.orientation, orientation);
// tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
// cout << "IMU roll pitch yaw: " << endl;
// cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
}
/* 里程计回调函数 存储数据到队列中 */
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
std::lock_guard<std::mutex> lock2(odoLock);
odomQueue.push_back(*odometryMsg);
}
/* 接收激光雷达点云数据回调函数 */
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// 根据激光雷达类型，转化数据格式，去除无效点云数据，缓存点云数据
if (!cachePointCloud(laserCloudMsg))
return;
// 将两帧lidar数据之间的IMU数据用于获取对应的IMU姿态角 并且将IMU里程计作为当前两帧之间的初始位姿
if (!deskewInfo())
return;
// 遍历激光点云数据，并使用IMU数据去激光点云数据的运动畸变 存储点云的深度
projectPointCloud();
// 该函数的作用主要是保存有效激光点云数据，
// 并且由于特征提取的时候不考虑每一行的前5个和最后5个，因此设置startRingIndex和endRingIndex这两个索引从第五个和倒数第五个
cloudExtraction();
// 发布用户自定义的cloudInfo激光数据(经过数据预处理的)
publishClouds();
/* 重置部分参数数据 */
resetParameters();
}
/* 根据激光雷达类型，转化数据格式，去除无效点云数据，缓存点云数据 */
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// cache point cloud
cloudQueue.push_back(*laserCloudMsg);// 添加当前激光数据帧到队列
if (cloudQueue.size() <= 2)
return false;
// convert cloud
currentCloudMsg = std::move(cloudQueue.front());// 从队列中获取激光点云数据
cloudQueue.pop_front(); // 获取点云数据后，将队列最前面的数据剔除
if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)// 判断激光雷达类型(VELODYNE、LIVOX)
{
// 如果是VELODYNE、LIVOX类型的激光雷达 将点云数据从ROS数据转化为PCL数据格式
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);//
}
else if (sensor == SensorType::OUSTER)// 如果是 OUSTER激光雷达
{
// Convert to Velodyne format
pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);// 转化数据格式
laserCloudIn->points.resize(tmpOusterCloudIn->size()); // 重置数据数量
laserCloudIn->is_dense = tmpOusterCloudIn->is_dense; // is_dense用于去除无效点云
// 遍历一帧有效激光数据中的所有点云 将OUSTER雷达点云数据存到laserCloudIn中
for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
{
auto &src = tmpOusterCloudIn->points[i];
auto &dst = laserCloudIn->points[i];
dst.x = src.x;
dst.y = src.y;
dst.z = src.z;
dst.intensity = src.intensity; // 激光雷达的反射强度
dst.ring = src.ring; // 通道
dst.time = src.t * 1e-9f; // 时间戳
}
}
else // 如果均不是枚举的激光雷达数据类型 则报错
{
ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
ros::shutdown();
}
// get timestamp 获取时间戳
cloudHeader = currentCloudMsg.header;
timeScanCur = cloudHeader.stamp.toSec(); // 当前激光点云的时间戳
//可以看出lasercloudin中存储的time是一帧中距离起始点的相对时间,timeScanEnd是该帧点云的结尾时间
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// check dense flag 根据is_dense去除无效点云
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
// check ring channel
//由于static关键字，只检查一次，检查ring这个field是否存在. veloodyne和ouster都有;
//ring代表线数，0是最下面那条
static int ringFlag = 0;
if (ringFlag == 0)
{
ringFlag = -1;
for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
{
if (currentCloudMsg.fields[i].name == "ring")
{
ringFlag = 1;
break;
}
}
if (ringFlag == -1)
{
ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
ros::shutdown();
}
}
// check point time
// 检查是否存在time通道
if (deskewFlag == 0)
{
deskewFlag = -1;
// 这里的fields代表每个点云数据的成员变量
for (auto &field : currentCloudMsg.fields)
{
if (field.name == "time" || field.name == "t")
{
deskewFlag = 1;
break;
}
}
if (deskewFlag == -1)
ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
}
return true; // 上述步骤都通过，代表该帧点云数据有效
}
bool deskewInfo()
{
std::lock_guard<std::mutex> lock1(imuLock);
std::lock_guard<std::mutex> lock2(odoLock);
// make sure IMU data available for the scan
// 确保IMU数据在时间上处于激光数据之间，否则直接退出
// 即IMU数据需要再两帧激光数据之间
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{
ROS_DEBUG("Waiting for IMU data ...");
return false;
}
// 当前帧对应imu数据处理
// 1、遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
// 2、用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
// 注：imu数据都已经转换到lidar系下了
// imu去畸变参数计算
imuDeskewInfo();
// 当前帧对应imu里程计处理
// 1、遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
// 2、用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
// 注：imu数据都已经转换到lidar系下了
// 里程计去畸变参数计算
odomDeskewInfo();
return true;
}
void imuDeskewInfo()
{
cloudInfo.imuAvailable = false; // imuAvailable为IMU数据有效标志位
while (!imuQueue.empty()) // IMU数据队列不为空
{
// 判断IMU数据时间戳是否大于当前帧激光点云数据的时间戳timeScanCur
// 如果IMU数据时间小于激光雷达时间-0.01 删除该imu数据
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
// 如果IMU队列没有数据 直接退出
if (imuQueue.empty())
return;
imuPointerCur = 0;
// 遍历当前IMU队列 将有效IMU数据积分
for (int i = 0; i < (int)imuQueue.size(); ++i)
{
sensor_msgs::Imu thisImuMsg = imuQueue[i]; // 获取IMU数据
double currentImuTime = thisImuMsg.header.stamp.toSec();// 获取IMU数据时间戳
// get roll, pitch, and yaw estimation for this scan
// 从IMU数据中得到姿态角 Roll Pitch Yaw三个数据
if (currentImuTime <= timeScanCur) //
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
// 如果当前IMU数据时间戳 > 激光数据最后一帧的时间戳则剔除
if (currentImuTime > timeScanEnd + 0.01)
break;
// 如果是第一个IMU有效数据 初始化旋转角
if (imuPointerCur == 0){
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuTime[0] = currentImuTime;
++imuPointerCur;
continue;
}
// get angular velocity
// 获取IMU加速度数据
double angular_x, angular_y, angular_z;
imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
// integrate rotation
// IMU数据积分
// 当前IMU数据与上一帧IMU数据之间的时间间隔 = 当前IMU时间戳 - 上一帧IMU时间戳
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff; // X(t) = X_(t-1) + x * △t
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff; // y z 同理
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime; // 存储当前IMU数据的时间戳 用于下一个IMU数据积分
++imuPointerCur; // IMU有效数据计数位+1
}
--imuPointerCur;// ?
// 没有合规的imu数据
if (imuPointerCur <= 0)
return;
cloudInfo.imuAvailable = true; // 处理完IMU数据 将标志位设置为true
}
void odomDeskewInfo()
{
// 同理 odomAvailable为odom数据有效标志位
cloudInfo.odomAvailable = false;
// 和IMU数据一样 只有lidar数据帧起止之间的odom数据才是有效的
while (!odomQueue.empty())
{
if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
odomQueue.pop_front();
else
break;
}
// 无数据 退出
if (odomQueue.empty())
return;
if (odomQueue.front().header.stamp.toSec() > timeScanCur)
return;
// get start odometry at the beinning of the scan
// 获取当前lidar数据起止间隔之中最早的IMU odom数据
nav_msgs::Odometry startOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
startOdomMsg = odomQueue[i];
if (ROS_TIME(&startOdomMsg) < timeScanCur)
continue;
else
break;
}
// 提取imu里程计姿态角
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// Initial guess used in mapOptimization
// 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization
cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
cloudInfo.initialGuessRoll = roll;
cloudInfo.initialGuessPitch = pitch;
cloudInfo.initialGuessYaw = yaw;
cloudInfo.odomAvailable = true;
// get end odometry at the end of the scan
odomDeskewFlag = false;
// 如果当前激光帧结束时刻之后没有imu里程计数据，返回
if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
return;
// 提取当前激光帧结束时刻的imu里程计
nav_msgs::Odometry endOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
if (ROS_TIME(&endOdomMsg) < timeScanEnd)
continue;
else
break;
}
// 如果起止两个时刻对应imu里程计的方差不等，返回
// round函数：四舍五入
if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
return;
// Eigen::Affine3f为仿射变换矩阵 即旋转+平移
//通过imu里程计的位姿xyzrpy得到imu里程计初始坐标系转换到大地坐标系的旋转矩阵transBegin
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
//通过imu里程计的位姿xyzrpy得到imu里程计终止坐标系转换到大地坐标系的旋转矩阵transEnd
tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
// 起止时刻imu里程计的相对变换
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
// 给定的转换中，提取XYZ以及欧拉角,通过tranBt 获得增量值 后续去畸变用到 ？？？？？
float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
odomDeskewFlag = true;
}
/* 通过IMU数据差值计算当前激光点云时刻对应的旋转量 */
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
int imuPointerFront = 0;
// 寻找大于当前激光点云时间的最前面的IMU数据
while (imuPointerFront < imuPointerCur)
{
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
//如果计数为0或该次imu时间戳小于了当前时间戳(异常退出)
if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
{
*rotXCur = imuRotX[imuPointerFront];
*rotYCur = imuRotY[imuPointerFront];
*rotZCur = imuRotZ[imuPointerFront];
} else {
// 前后时刻插值计算当前时刻的旋转增量
//此时front的时间是大于当前pointTime时间，back=front-1刚好小于当前pointTime时间，前后时刻插值计算
int imuPointerBack = imuPointerFront - 1;
//算一下该点时间戳在本组imu中的位置
double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
//这三项作为函数返回值，以形参指针的方式返回
//按前后百分比赋予旋转量
*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
}
}
void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
*posXCur = 0; *posYCur = 0; *posZCur = 0;
// If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
// if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
// return;
// float ratio = relTime / (timeScanEnd - timeScanCur);
// *posXCur = ratio * odomIncreX;
// *posYCur = ratio * odomIncreY;
// *posZCur = ratio * odomIncreZ;
}
/* 去运动畸变函数
利用当前帧起止时刻之间的imu数据计算旋转增量，
imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
*/
PointType deskewPoint(PointType *point, double relTime)
{
//这个来源于上文的时间戳通道和imu可用判断，没有或是不可用则返回点
if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
return *point;
// relTime=points[i].time 即激光点云时间戳数据
// lasercloudin中存储的time是一帧中距离起始点的 相对时间
// 在cloudHandler的cachePointCloud函数中，timeScanCur = cloudHeader.stamp.toSec();，即当前帧点云的初始时刻
//二者相加即可得到当前点的准确时刻
double pointTime = timeScanCur + relTime;
//根据时间戳插值获取imu计算的旋转量与位置量（注意imu计算的相对于起始时刻的旋转增量）
float rotXCur, rotYCur, rotZCur;
findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
// ????
float posXCur, posYCur, posZCur;
findPosition(relTime, &posXCur, &posYCur, &posZCur);
//这里的firstPointFlag来源于resetParameters函数，而resetParameters函数每次ros调用cloudHandler都会启动
if (firstPointFlag == true)
{
// 第一个点的位姿增量（0），求逆 ??? 为啥要这么搞
transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
firstPointFlag = false;//改成false以后，同一帧激光数据的下一次就不执行了
}
// transform points to start
//扫描当前点时lidar的世界坐标系下变换矩阵
Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
//扫描该点相对扫描本次scan第一个点的lidar变换矩阵=
//第一个点时lidar世界坐标系下变换矩阵的逆×当前点时lidar世界坐标系下变换矩阵
//Tij=Twi^-1 * Twj
//注：这里准确的来说，不是世界坐标系,
//根据代码来看，是把imu积分:
//从imuDeskewInfo函数中，在当前激光帧开始的前0.01秒的imu数据开始积分，
//把它作为原点，然后获取当前激光帧第一个点时刻的位姿增量transStartInverse，
//和当前点时刻的位姿增量transFinal，根据逆矩阵计算二者变换transBt。
//因此相对的不是“世界坐标系”，
//而是“当前激光帧开始前的0.01秒的雷达坐标系（在imucallback函数中已经把imu转换到了雷达坐标系了）
Eigen::Affine3f transBt = transStartInverse * transFinal;
PointType newPoint;
//根据lidar位姿变换 Tij，修正点云位置: Tij * Pj ？？？？
newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
newPoint.intensity = point->intensity;
return newPoint;
}
void projectPointCloud()
{
int cloudSize = laserCloudIn->points.size();// 获取输入点云数量
// range image projection
// 遍历激光点云
for (int i = 0; i < cloudSize; ++i)
{
// 获取激光点云数据
PointType thisPoint;
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
thisPoint.intensity = laserCloudIn->points[i].intensity;
// 计算点云的距离并根据该距离判断是否在激光点云最小范围和最大范围之间
float range = pointDistance(thisPoint);
if (range < lidarMinRange || range > lidarMaxRange)
continue;
// 获取激光点云的通道数 并判断该通道数是否小于激光雷达总线数N_SCAN
int rowIdn = laserCloudIn->points[i].ring;
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
if (rowIdn % downsampleRate != 0)
continue;
int columnIdn = -1;
if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
{
//水平角分辨率
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
//Horizon_SCAN=1800,每格0.2度
static float ang_res_x = 360.0/float(Horizon_SCAN);
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
}
else if (sensor == SensorType::LIVOX)
{
columnIdn = columnIdnCountVec[rowIdn];
columnIdnCountVec[rowIdn] += 1;
}
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
continue;
//去畸变 运动补偿 这里需要用到雷达信息中的time 这个field
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
//图像中填入欧几里得深度
rangeMat.at<float>(rowIdn, columnIdn) = range;
// 转换成一维索引，存校正之后的激光点
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
}
}
void cloudExtraction()
{
// count为有效激光点云数据
int count = 0;
// extract segmented cloud for lidar odometry
for (int i = 0; i < N_SCAN; ++i)
{
//提取特征的时候，每一行的前5个和最后5个不考虑
// 将startRingIndex(记录每根扫描线起始第5个激光点在一维数组中的索引)设为5
cloudInfo.startRingIndex[i] = count - 1 + 5;
for (int j = 0; j < Horizon_SCAN; ++j)
{
if (rangeMat.at<float>(i,j) != FLT_MAX)
{
// mark the points' column index for marking occlusion later
// 记录激光点云对应的Horizon_SCAN方向上的索引
cloudInfo.pointColInd[count] = j;
// save range info
// 保存激光点云距离(range)数据
cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
// save extracted cloud
// 保存有效激光点云到extractedCloud
extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of extracted cloud
// 有效激光点云数量++
++count;
}
}
// 提取特征的时候，每一行的最后5个不考虑
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
/* 发布用户自定义的cloudInfo激光数据(经过数据预处理的) */
void publishClouds()
{
cloudInfo.header = cloudHeader;
cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam"); //初始化ROS节点 lio_sam
ImageProjection IP; //创建对象IP
ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
ros::MultiThreadedSpinner spinner(3); // 使用3个线程
spinner.spin(); // 等待 直到节点shutdowmn
return 0;
}
3、featureExtraction.cpp
该文件的程序较为简单，主要作用就是特征点云的提取(角点和平面点)。
（1）输入(订阅)：
- 已经去除运动畸变的自定义激光数据cloud_info
（2）输出(发布)：
- 提取的角点数据
- 提取的平面点数据
- 自定义激光数据cloud_info
（3）具体函数：
- 主函数
初始化ros节点和实例化一个featureExtraction对象，具体的功能程序在featureExtraction类中进行封装。
各函数功能介绍
1. FeatureExtraction()构造函数
该构造函数主要是初始化ros的订阅和发布，以及完成其他一些变量的初始化工作。
2. laserCloudInfoHandler
这个回调函数是该文件的核心。特征提取的主要步骤都在回调函数中完成，ros订阅到激光数据cloud_info之后，进入回调函数，在回调函数中先转化点云数据格式，然后计算点云的曲率并排序，最后根据曲率提取角点和平面点。
calculateSmoothness()
该函数作用是计算激光点云的曲率。关键就是下面的代码：
// 曲率= 前五个点云和后五个点云的实际距离分别都减去当前点云的实际距离之和
float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
+ cloudInfo.pointRange[i+5];
markOccludedPoints()
该函数作用是标记被遮挡的点和与激光束平行的面上的点。主要是根据激光点云的实际距离和pointColInd来去除与激光束平行的面上的点云和前后遮挡的点云
extractFeatures()
该函数是核心，根据曲率大小提取角点和平面点。将一条扫描线扫描一周的点云数据，划分为6段，每段分开提取有限数量的特征，保证特征均匀分。每段只取20个角点，如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点。曲率小的点云和未处理的点云都被当做平面点处理。
publishFeatureCloud()
该函数的作用是发布提取到的角点和平面点。
具体注释：
#include "utility.h"
#include "lio_sam/cloud_info.h"
struct smoothness_t{
float value;
size_t ind;
};
struct by_value{
bool operator()(smoothness_t const &left, smoothness_t const &right) {
return left.value < right.value;
}
};
class FeatureExtraction : public ParamServer
{
public:
ros::Subscriber subLaserCloudInfo;
ros::Publisher pubLaserCloudInfo;
ros::Publisher pubCornerPoints;
ros::Publisher pubSurfacePoints;
pcl::PointCloud<PointType>::Ptr extractedCloud;
pcl::PointCloud<PointType>::Ptr cornerCloud;
pcl::PointCloud<PointType>::Ptr surfaceCloud;
pcl::VoxelGrid<PointType> downSizeFilter;
lio_sam::cloud_info cloudInfo;
std_msgs::Header cloudHeader;
std::vector<smoothness_t> cloudSmoothness;
float *cloudCurvature;
int *cloudNeighborPicked;
int *cloudLabel;
/* 构造函数 */
FeatureExtraction()
{
// 订阅自定义激光数据cloud_info
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布cloudinfo数据
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);
// 发布角点点云数据
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
// 发布平面点点云数据
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);
// 初始化变量数据
initializationValue();
}
/* 变量初始化 */
void initializationValue()
{
cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
extractedCloud.reset(new pcl::PointCloud<PointType>());
cornerCloud.reset(new pcl::PointCloud<PointType>());
surfaceCloud.reset(new pcl::PointCloud<PointType>());
cloudCurvature = new float[N_SCAN*Horizon_SCAN];
cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
cloudLabel = new int[N_SCAN*Horizon_SCAN];
}
/* 激光数据回调函数 */
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
//msgIn即为回调函数获取的去畸变点云信息
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
// 点云数据格式转换
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
/* 计算当前激光帧中每个点的曲率 */
calculateSmoothness();
/* 标记被遮挡的点和与激光束平行的面上的点 */
markOccludedPoints();
/* 特征提取 提取角点和平面点 */
extractFeatures();
/* 发布提取到的特征点云数据 */
publishFeatureCloud();
}
/* 计算当前激光帧中每个点的曲率 */
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();// 有效激光点云数量
// 遍历当前帧有效激光点云(前五个和最后五个不提取特征，排查在外)
for (int i = 5; i < cloudSize - 5; i++)
{
// 计算曲率
// pointRange = sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
// 曲率= 前五个点云和后五个点云的实际距离分别都减去当前点云的实际距离之和
float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
+ cloudInfo.pointRange[i+5];
// 平方
cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// cloudSmoothness for sorting
// 存储曲率的平方
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
/* 标记被遮挡的点和与激光束平行的面上的点 */
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// 1、occluded points 被遮挡的点
// 当前点和下一个点的range(实际深度)值
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
// 计算当前点云和下一个点云之间的pointColInd数据差值
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
// 两个点在同一扫描线上，且距离相差大于0.3，认为存在遮挡关系
//（也就是这两个点不在同一平面上，如果在同一平面上，距离相差不会太大，即一个在前一个在后，存在遮挡关系）
// 远处的点会被遮挡，标记一下该点以及相邻的5个点，后面不再进行特征提取
if (columnDiff < 10){
// 10 pixel diff in range image
if (depth1 - depth2 > 0.3){ // 当前点比下一个点云更远 则前五个点云剔除
cloudNeighborPicked[i - 5] = 1;// 设为1 表示该点云被标记为遮挡的点
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){ // 下一个点云比当前点云更远 则后五个点云剔除
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// 2、parallel beam 平行于激光束
// 用前后相邻点判断当前点所在平面是否与激光束方向平行
// diff1和diff2是当前点距离前后两个点的距离
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
// 如果当前点距离左右邻点都过远，则视其为瑕点，因为入射角可能太大导致误差较大
// 选择距离变化较大的点，并将他们标记为1
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
}
}
/* 特征提取 提取角点和平面点 */
void extractFeatures()
{
cornerCloud->clear(); // 清空角点数据
surfaceCloud->clear(); // 清空平面点数据
// 重新分配内存
pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());
// 一条一条线遍历
for (int i = 0; i < N_SCAN; i++)
{
surfaceCloudScan->clear(); // 清空数据
// 将一条扫描线扫描一周的点云数据，划分为6段，每段分开提取有限数量的特征，保证特征均匀分布
for (int j = 0; j < 6; j++)
{
// 每段点云的起始、结束索引；startRingIndex为扫描线起始第5个激光点在一维数组中的索引
//注意：所有的点云在这里都是以"一维数组"的形式保存
//startRingIndex和 endRingIndex 在imageProjection.cpp中的 cloudExtraction函数里被填入
//假设 当前ring在一维数组中起始点是m，结尾点为n（不包括n），那么6段的起始点分别为：
// m + [(n-m)/6]*j j从0～5
// 化简为 [（6-j)*m + nj ]/6
// 6段的终止点分别为：
// m + (n-m)/6 + [(n-m)/6]*j -1 j从0～5,-1是因为最后一个,减去1
// 化简为 [（5-j)*m + (j+1)*n ]/6 -1
//这块不必细究边缘值到底是不是划分的准（例如考虑前五个点是不是都不要，还是说只不要前四个点），
//只是尽可能的分开成六段，首位相接的地方不要。因为庞大的点云中，一两个点其实无关紧要。
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
// 根据曲率大小对选取出来的点从小到大进行排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
int largestPickedNum = 0;
// 按照曲率从大到小遍历 寻找角点
for (int k = ep; k >= sp; k--)
{
int ind = cloudSmoothness[k].ind;
// 如果该点没有被标记为无效点云 且该点云的曲率大于阈值edgeThreshold 则认为是角点cornerCloud
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
largestPickedNum++; // 计数增加
// 每段只取20个角点，如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
// 大于20个点云则跳出for循环
if (largestPickedNum <= 20){
cloudLabel[ind] = 1;
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
cloudNeighborPicked[ind] = 1;// 被标记为角点
// 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集 ？？？？
for (int l = 1; l <= 5; l++)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
//大于10，说明距离远，则不作标记 ？？？
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
// 同理
for (int l = -1; l >= -5; l--)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 按照曲率从小到大遍历 寻找平面点 过程和寻找角点的过程一样
for (int k = sp; k <= ep; k++)
{
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
{
cloudLabel[ind] = -1;
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 平面点和未被处理的点(<=0)，都认为是平面点，加入平面点云集合
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0){ // <=0代表平面点和未处理的点云
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
//用surfaceCloudScan来装数据，然后放到downSizeFilter里，
//再用downSizeFilter进行.filter()操作，把结果输出到*surfaceCloudScanDS里。
//最后把DS装到surfaceCloud中。DS指的是DownSample。
//同样角点（边缘点）则没有这样的操作，因为角点数量较少，直接就用cornerCloud来装点云。
surfaceCloudScanDS->clear();// 清空数据
// 平面点数量较多 需要降采样
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 存储降采样后的点云 加入平面点云集合
*surfaceCloud += *surfaceCloudScanDS;
}
}
/* 清楚变量数据 */
void freeCloudInfoMemory()
{
cloudInfo.startRingIndex.clear();
cloudInfo.endRingIndex.clear();
cloudInfo.pointColInd.clear();
cloudInfo.pointRange.clear();
}
/* 发布提取到的特征点云数据 */
void publishFeatureCloud()
{
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features
cloudInfo.cloud_corner = publishCloud(pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}
};
int main(int argc, char** argv)
{
// 创建ros节点lio_sam
ros::init(argc, argv, "lio_sam");
// 实例化FeatureExtraction对象
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");// ROS打印消息
ros::spin();// 循环等待程序执行
return 0;
}
4、imuPreintergration.cpp
本程序主要封装了两个类。一个类是TransformFusion，主要作用是根据前一时刻激光里程计数据和该激光里程计时刻到现在时刻间的IMU数据来计算当前时刻IMU里程计。另一个类是IMUPreintegration，是本程序的核心，主要作用就是预积分。
先介绍简单一点的TransformFusion类。
TransformFusion类
该类的具体作用是根据上一帧得到的激光里程计和上一帧到当前帧之间的IMU数据得到当前时刻IMU里程计
（1）输入(订阅)：
- 激光里程计 lio_sam/mapping/odometry。来自mapOptimization.cpp
- IMU里程计 odometry/imu_incremental 来自同个cpp文件的另一个IMUPreintegration类
（2）输出(发布)：
- IMU里程计话题odomTopic
- imu里程计轨迹话题pubImuPath
（3）具体函数：
- TransformFusion构造函数
该构造函数作用主要有两点。一是将激光坐标系转到baselink坐标系。一个是创建订阅和发布变量。 - odom2affine函数
该函数的作用是将激光里程计数据转化为对应的变换矩阵Affine3f。 - lidarOdometryHandler函数
该回调函数的作用是保存最近一个激光雷达里程计的变换矩阵和时间戳。注意：这里不再和之前的回调函数一样将数据保存在一个队列中，而是保存一个数据。 - imuOdometryHandler函数
- 订阅imu里程计数据，来自IMUPreintegration
- 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
- 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段。
IMUPreintegration类
（1）输入(订阅)：
- 订阅IMU原始数据imuTopic
- 激光里程计数据，来自mapOptimization，用两帧之间的imu预积分量构建因子图
（2）输出(发布)：
- IMU里程计 odomTopic+“_incremental”
（3）具体函数：
-
IMUPreintegration构造函数
该构造函数作用主要有两点。一是将激光坐标系转到baselink坐标系。一个是创建订阅和发布变量。 -
odom2affine函数
该函数的作用是将激光里程计数据转化为对应的变换矩阵Affine3f。 -
lidarOdometryHandler函数
该回调函数的作用是保存最近一个激光雷达里程计的变换矩阵和时间戳。注意：这里不再和之前的回调函数一样将数据保存在一个队列中，而是保存一个数据。 -
imuHandler函数
将接收到的IMU原始数据转到LIDAR坐标系下，转换后，会存入两个队列，一个imuQueOpt队列，一个imuQueImu队列。这两队列有什么区别和联系呢？这个主要在另一个回调函数odometryHandler会被处理，在那里会说明。这里我们可以先理解为，imuQueImu是真正我们要用的数据，imuQueOpt是一个中间缓存的数据结构，用来优化imu的bias之类的东西。
该函数的主要作用是上一帧（最近一帧）的激光里程计为起始，通过不断订阅到的IMU数据，通过预积分得到当前时刻激光里程计
主要步骤是：
1、将IMU数据从imu坐标系转到LIDAR坐标系下。
2、以上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态currentState
3、从currentState中获取到当前时刻imu的位姿等数据，再经过坐标变换得到激光的位姿的数据
4、将得到的激光里程计数据发布出去 -
odometryHandler函数
1、获取激光里程计数据（xyz和四元数） 将来自里程计（odometry）的位姿数据转换为GTSAM中的Pose3对象，以便在后面进行因子图优化
2、初始化
a、重置ISAM2优化器
b、删除保存imu数据的队列中旧的IMU数据，即保证IMU队列中的IMU数据都是最近接收到的激光里程计之后的数据
c、添加机器人priorPose、priorVel、priorBias先验因子到graphFactors因子图模型中，并且将当前机器人的状态prevPose_、prevVel_、prevBias_插入到graphValues因子图优化的状态变量中
d、利用定义的因子图优化器optimizer进行一次优化操作，对变量的最优估计进行计算。
3、取两帧激光里程计帧之间的IMU数据，然后往imu预积分器imuIntegratorOpt_添加IMU数据
4、利用两帧之间的IMU数据完成了预积分后，增加imu因子preint_imu到因子图模型中
5、向图中添加一个表示两个节点之间约束关系的因子imu偏置Bias因子，同时指定了初始状态和噪声模型
6、添加激光里程计位姿因子
7、插入预测变量，利用定义的因子图优化器optimizer进行一次优化操作，对变量的最优估计进行计算
8、优化后 用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
5、mapOptimization.cpp
（1）输入(订阅)：
- 订阅当前激光帧点云cloud_info
- 订阅GPS里程计gpsTopic
- 订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上
（2）输出(发布)：
-
发布历史关键帧里程计(组成轨迹)
-
发布局部关键帧map的特征点云
-
发布激光里程计
-
发布激光里程计，它与上面的激光里程计基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制
-
发布激光里程计路径
-
发布地图保存服务
-
发布闭环匹配关键帧局部map
-
发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
-
发布闭环边，rviz中表现为闭环帧之间的连线
-
发布局部map的降采样平面点集合
-
发布历史帧（累加的）的角点、平面点降采样集合
-
发布当前帧原始点云配准之后的点云
（3）具体函数：
该文件主要由mapOptimization类和main函数组成，mapOptimization在main函数中被实例化。
主函数main：
初始化ros节点lio_sam，实例化mapOptimization对象。mapOptimization类
该类主要的内容就是一个初始化构造函数和三个回调函数 -
构造函数：
该构造函数的作用是以下几点：
1.配置ISAM2的参数
2.创建订阅和发布
3.配置降采样滤波器
4.变量分配内存
laserCloudInfoHandler
该激光点云cloud_info的回调函数。该函数主要作用如下：
订阅当前激光帧点云信息，来自featureExtraction
-
1、当前帧位姿初始化
1) 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分） 2) 后续帧，用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、发布里程计、点云、轨迹
updateInitialGuess函数：
该函数的作用是以下几点：
1.如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
2.如果是后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
extractSurroundingKeyFrames函数：
该函数的作用是以下几点：
1.提取当前关键帧的相邻关键帧集合
2.对提取到的关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
downsampleCurrentScan函数：
该函数的作用是以下几点：
1.降采样当前帧激光点云数据中的角点、平面点
scan2MapOptimization函数：
该函数的作用是根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图，主要分为以下几点：
1.角点优化cornerOptimization函数
2.平面点优化surfOptimization函数
3.组合优化多项式系数 将角点优化和面点优化的结果合并为优化系数combineOptimizationCoeffs函数
4.Levenberg-Marquardt优化LMOptimization函数
5.对位姿进行更新transformUpdate函数
gpsHandler函数：
该回调函数作用是将gps数据存入队列。
具体注释：
#include "utility.h"
#include "lio_sam/cloud_info.h"
#include "lio_sam/save_map.h"
#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) IMU偏置
using symbol_shorthand::G; // GPS pose GPS位姿
/*
* 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
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;
class mapOptimization : public ParamServer
{
public:
// gtsam
NonlinearFactorGraph gtSAMgraph;
Values initialEstimate;
Values optimizedEstimate;
ISAM2 *isam;
Values isamCurrentEstimate;
Eigen::MatrixXd poseCovariance;
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubLaserOdometryGlobal;
ros::Publisher pubLaserOdometryIncremental;
ros::Publisher pubKeyPoses;
ros::Publisher pubPath;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRecentKeyFrame;
ros::Publisher pubCloudRegisteredRaw;
ros::Publisher pubLoopConstraintEdge;
ros::Publisher pubSLAMInfo;
ros::Subscriber subCloud;
ros::Subscriber subGPS;
ros::Subscriber subLoop;
ros::ServiceServer srvSaveMap;
// std::deque为双端队列
std::deque<nav_msgs::Odometry> gpsQueue;
lio_sam::cloud_info cloudInfo;
// 关键帧角点和平面点集合
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames;
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames;
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D;
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D;
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 feature set from odoOptimization
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf feature set from odoOptimization
// 当前帧与局部map匹配上了的角点、平面点，加入同一集合；后面是对应点的参数
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
// 当前帧与局部map匹配上了的角点、参数、标记
std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation
std::vector<PointType> coeffSelCornerVec;
std::vector<bool> laserCloudOriCornerFlag;
// 当前帧与局部map匹配上了的平面点、参数、标记
std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation
std::vector<PointType> coeffSelSurfVec;
std::vector<bool> laserCloudOriSurfFlag;
map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer;
// 局部map的角点和平面点
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
// 局部map的角点和平面点(经过降采样的)
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS;
// 局部关键帧构建的map点云，对应kdtree，用于scan-to-map找相邻点
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses;
// 降采样
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterICP;
pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // for surrounding key poses of scan-to-map optimization
// 时间戳
ros::Time timeLaserInfoStamp;
double timeLaserInfoCur; // 当前时间
float transformTobeMapped[6];
std::mutex mtx;
std::mutex mtxLoopInfo;
bool isDegenerate = false;
cv::Mat matP;
// 局部map角点、平面点数量
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()
{
// 配置ISAM2的参数 调整iSAM2的重线性化阈值和重线性化跳跃
// relinearizeThreshold设置为0.1，表示只有当状态变化超过0.1时，才会触发重新线性化操作。
// 这个阈值可以控制iSAM2何时决定使用线性化近似而不是精确的非线性因子。较小的阈值可以提供更精确的估计，但计算成本也会增加。
// relinearizeSkip设置为1，表示每个时间步都进行重新线性化。
// 通过设置此参数，可以调整重线性化的间隔，以平衡计算成本和估计精度。较大的值会降低计算成本，但可能导致估计不准确。
ISAM2Params parameters;
parameters.relinearizeThreshold = 0.1;
parameters.relinearizeSkip = 1;
isam = new ISAM2(parameters);
// 发布历史关键帧里程计(组成轨迹)
pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1);
// 发布局部关键帧map的特征点云
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1);
// 发布激光里程计
pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1);
// 发布激光里程计，它与上面的激光里程计基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制
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());
// 订阅gps里程计
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);
pubSLAMInfo = nh.advertise<lio_sam::cloud_info>("lio_sam/mapping/slam_info", 1);
// 设置了downSizeFilterCorner滤波器的叶子大小为mappingCornerLeafSize，并应用于点云数据的角点部分。
// mappingCornerLeafSize是一个具体的数值，它决定了点云在每个维度上的降采样程度，从而影响最终得到的点云分辨率。
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();
}
/* 分配内存 */
void allocateMemory()
{
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>());
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>());
kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>());
kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>());
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里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
* 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 提取时间戳数据
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
// extract info and feature cloud 提取信息和特征点云
cloudInfo = *msgIn;
// 点云格式由ROS转化为pcl
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; // 记录当前帧点云时间作为上一帧点云时间
// 激光位姿初始化
// 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
// 如果是后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
updateInitialGuess();
// 提取当前关键帧的相邻关键帧集合
// 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
extractSurroundingKeyFrames();
// 降采样当前帧激光点云数据中的角点、平面点
downsampleCurrentScan();
// 根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图，
// 它分为角点优化、平面点优化、配准与更新等部分。
// 优化的过程与里程计的计算类似，是通过计算点到直线或平面的距离，构建优化公式再用LM法求解。
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishOdometry();
publishFrames();
}
}
void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg)
{
gpsQueue.push_back(*gpsMsg);
}
void pointAssociateToMap(PointType const * const pi, PointType * const po)
{
// 使用仿射矩阵中的旋转矩阵与输入点云位姿相乘 得到 旋转之后的点云位姿
po->x = transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z + transPointAssociateToMap(0,3);
po->y = transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z + transPointAssociateToMap(1,3);
po->z = transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z + transPointAssociateToMap(2,3);
po->intensity = pi->intensity;// intensity不变
}
pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
int cloudSize = cloudIn->size();
cloudOut->resize(cloudSize);
Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < cloudSize; ++i)
{
const auto &pointFrom = cloudIn->points[i];
cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);
cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);
cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);
cloudOut->points[i].intensity = pointFrom.intensity;
}
return cloudOut;
}
gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint)
{
return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
}
gtsam::Pose3 trans2gtsamPose(float transformIn[])
{
return gtsam::Pose3(gtsam::Rot3::RzRyRx(transformIn[0], transformIn[1], transformIn[2]),
gtsam::Point3(transformIn[3], transformIn[4], transformIn[5]));
}
Eigen::Affine3f pclPointToAffine3f(PointTypePose thisPoint)
{
return pcl::getTransformation(thisPoint.x, thisPoint.y, thisPoint.z, thisPoint.roll, thisPoint.pitch, thisPoint.yaw);
}
/* 返回一个变换矩阵，包括旋转+平移
transformIn[3]、transformIn[4]和transformIn[5]分别表示x、y和z方向的 平移，
transformIn[0]、transformIn[1]和transformIn[2]分别表示绕x、y和z轴的 旋转。
*/
Eigen::Affine3f trans2Affine3f(float transformIn[])
{
return pcl::getTransformation(transformIn[3], transformIn[4], transformIn[5], transformIn[0], transformIn[1], transformIn[2]);
}
PointTypePose trans2PointTypePose(float transformIn[])
{
PointTypePose thisPose6D;
thisPose6D.x = transformIn[3];
thisPose6D.y = transformIn[4];
thisPose6D.z = transformIn[5];
thisPose6D.roll = transformIn[0];
thisPose6D.pitch = transformIn[1];
thisPose6D.yaw = transformIn[2];
return thisPose6D;
}
bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res)
{
string saveMapDirectory;
cout << "****************************************************" << endl;
cout << "Saving map to pcd files ..." << endl;
if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
else saveMapDirectory = std::getenv("HOME") + req.destination;
cout << "Save destination: " << saveMapDirectory << endl;
// create directory and remove old files;
int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
// save key frame transformations
pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
// extract global point cloud map
pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]);
cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
}
if(req.resolution != 0)
{
cout << "\n\nSave resolution: " << req.resolution << endl;
// down-sample and save corner cloud
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
downSizeFilterCorner.filter(*globalCornerCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
// down-sample and save surf cloud
downSizeFilterSurf.setInputCloud(globalSurfCloud);
downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);
downSizeFilterSurf.filter(*globalSurfCloudDS);
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
}
else
{
// save corner cloud
pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
// save surf cloud
pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
}
// save global point cloud map
*globalMapCloud += *globalCornerCloud;
*globalMapCloud += *globalSurfCloud;
int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
res.success = ret == 0;
downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
cout << "****************************************************" << endl;
cout << "Saving map to pcd files completed\n" << endl;
return true;
}
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;
}
}
void publishGlobalMap()
{
if (pubLaserCloudSurround.getNumSubscribers() == 0)
return;
if (cloudKeyPoses3D->points.empty() == true)
return;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap(new pcl::KdTreeFLANN<PointType>());;
pcl::PointCloud<PointType>::Ptr globalMapKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyFrames(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS(new pcl::PointCloud<PointType>());
// kd-tree to find near key frames to visualize
std::vector<int> pointSearchIndGlobalMap;
std::vector<float> pointSearchSqDisGlobalMap;
// search near key frames to visualize
mtx.lock();
kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
kdtreeGlobalMap->radiusSearch(cloudKeyPoses3D->back(), globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
mtx.unlock();
for (int i = 0; i < (int)pointSearchIndGlobalMap.size(); ++i)
globalMapKeyPoses->push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);
// downsample near selected key frames
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses; // for global map visualization
downSizeFilterGlobalMapKeyPoses.setLeafSize(globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity, globalMapVisualizationPoseDensity); // for global map visualization
downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);
for(auto& pt : globalMapKeyPosesDS->points)
{
kdtreeGlobalMap->nearestKSearch(pt, 1, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap);
pt.intensity = cloudKeyPoses3D->points[pointSearchIndGlobalMap[0]].intensity;
}
// extract visualized and downsampled key frames
for (int i = 0; i < (int)globalMapKeyPosesDS->size(); ++i){
if (pointDistance(globalMapKeyPosesDS->points[i], cloudKeyPoses3D->back()) > globalMapVisualizationSearchRadius)
continue;
int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
}
// downsample visualized points
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames; // for global map visualization
downSizeFilterGlobalMapKeyFrames.setLeafSize(globalMapVisualizationLeafSize, globalMapVisualizationLeafSize, globalMapVisualizationLeafSize); // for global map visualization
downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);
publishCloud(pubLaserCloudSurround, globalMapKeyFramesDS, timeLaserInfoStamp, odometryFrame);
}
void loopClosureThread()
{
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(loopClosureFrequency);
while (ros::ok())
{
rate.sleep();
performLoopClosure();
visualizeLoopClosure();
}
}
void loopInfoHandler(const std_msgs::Float64MultiArray::ConstPtr& loopMsg)
{
std::lock_guard<std::mutex> lock(mtxLoopInfo);
if (loopMsg->data.size() != 2)
return;
loopInfoVec.push_back(*loopMsg);
while (loopInfoVec.size() > 5)
loopInfoVec.pop_front();
}
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;
}
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;
}
bool detectLoopClosureExternal(int *latestID, int *closestID)
{
// this function is not used yet, please ignore it
int loopKeyCur = -1;
int loopKeyPre = -1;
std::lock_guard<std::mutex> lock(mtxLoopInfo);
if (loopInfoVec.empty())
return false;
double loopTimeCur = loopInfoVec.front().data[0];
double loopTimePre = loopInfoVec.front().data[1];
loopInfoVec.pop_front();
if (abs(loopTimeCur - loopTimePre) < historyKeyframeSearchTimeDiff)
return false;
int cloudSize = copy_cloudKeyPoses6D->size();
if (cloudSize < 2)
return false;
// latest key
loopKeyCur = cloudSize - 1;
for (int i = cloudSize - 1; i >= 0; --i)
{
if (copy_cloudKeyPoses6D->points[i].time >= loopTimeCur)
loopKeyCur = round(copy_cloudKeyPoses6D->points[i].intensity);
else
break;
}
// previous key
loopKeyPre = 0;
for (int i = 0; i < cloudSize; ++i)
{
if (copy_cloudKeyPoses6D->points[i].time <= loopTimePre)
loopKeyPre = round(copy_cloudKeyPoses6D->points[i].intensity);
else
break;
}
if (loopKeyCur == loopKeyPre)
return false;
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
// extract near keyframes
nearKeyframes->clear();
int cloudSize = copy_cloudKeyPoses6D->size();
for (int i = -searchNum; i <= searchNum; ++i)
{
int keyNear = key + i;
if (keyNear < 0 || keyNear >= cloudSize )
continue;
*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
}
if (nearKeyframes->empty())
return;
// downsample near keyframes
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterICP.setInputCloud(nearKeyframes);
downSizeFilterICP.filter(*cloud_temp);
*nearKeyframes = *cloud_temp;
}
void visualizeLoopClosure()
{
if (loopIndexContainer.empty())
return;
visualization_msgs::MarkerArray markerArray;
// loop nodes
visualization_msgs::Marker markerNode;
markerNode.header.frame_id = odometryFrame;
markerNode.header.stamp = timeLaserInfoStamp;
markerNode.action = visualization_msgs::Marker::ADD;
markerNode.type = visualization_msgs::Marker::SPHERE_LIST;
markerNode.ns = "loop_nodes";
markerNode.id = 0;
markerNode.pose.orientation.w = 1;
markerNode.scale.x = 0.3; markerNode.scale.y = 0.3; markerNode.scale.z = 0.3;
markerNode.color.r = 0; markerNode.color.g = 0.8; markerNode.color.b = 1;
markerNode.color.a = 1;
// loop edges
visualization_msgs::Marker markerEdge;
markerEdge.header.frame_id = odometryFrame;
markerEdge.header.stamp = timeLaserInfoStamp;
markerEdge.action = visualization_msgs::Marker::ADD;
markerEdge.type = visualization_msgs::Marker::LINE_LIST;
markerEdge.ns = "loop_edges";
markerEdge.id = 1;
markerEdge.pose.orientation.w = 1;
markerEdge.scale.x = 0.1;
markerEdge.color.r = 0.9; markerEdge.color.g = 0.9; markerEdge.color.b = 0;
markerEdge.color.a = 1;
for (auto it = loopIndexContainer.begin(); it != loopIndexContainer.end(); ++it)
{
int key_cur = it->first;
int key_pre = it->second;
geometry_msgs::Point p;
p.x = copy_cloudKeyPoses6D->points[key_cur].x;
p.y = copy_cloudKeyPoses6D->points[key_cur].y;
p.z = copy_cloudKeyPoses6D->points[key_cur].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
p.x = copy_cloudKeyPoses6D->points[key_pre].x;
p.y = copy_cloudKeyPoses6D->points[key_pre].y;
p.z = copy_cloudKeyPoses6D->points[key_pre].z;
markerNode.points.push_back(p);
markerEdge.points.push_back(p);
}
markerArray.markers.push_back(markerNode);
markerArray.markers.push_back(markerEdge);
pubLoopConstraintEdge.publish(markerArray);
}
/*
如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
如果是后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
*/
void updateInitialGuess()
{
// save current transformation before any processing
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
static Eigen::Affine3f lastImuTransformation;
// initialization 初始化
// 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
if (cloudKeyPoses3D->points.empty())
{
// 数组前三个数代表旋转
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
// 如果没有使用IMU的航向信息（useImuHeadingInitialization为假），则将transformTobeMapped[2]（航向）设置为0
if (!useImuHeadingInitialization)
transformTobeMapped[2] = 0;
// 根据imu姿态信息创建一个变换矩阵lastImuTransformation
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预积分估计位姿
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
//odomAvailable和imuAvailable均来源于imageProjection.cpp中赋值，
//imuAvailable是遍历激光帧前后起止时刻0.01s之内的imu数据，
//如果都没有那就是false，因为imu频率一般比激光帧快，因此这里应该是都有的。
//odomAvailable同理，是监听imu里程计的位姿，如果没有紧挨着激光帧的imu里程计数据，那么就是false；
//这俩应该一般都有
if (cloudInfo.odomAvailable == true)
{
//cloudInfo来自featureExtraction.cpp发布的lio_sam/feature/cloud_info,
//而其中的initialGuessX等信息本质上来源于ImageProjection.cpp发布的deskew/cloud_info信息，
//而deskew/cloud_info中的initialGuessX则来源于ImageProjection.cpp中的回调函数odometryHandler，
//odometryHandler订阅的是imuPreintegration.cpp发布的odometry/imu_incremental话题，
//该话题发布的xyz是imu在前一帧雷达基础上的增量位姿
//纠正一个观点：增量位姿，指的绝不是预积分位姿！！是在前一帧雷达的基础上(包括该基础!!)的（基础不是0）的位姿
//当前帧的初始估计位姿（来自imu里程计），后面用来计算增量位姿变换
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
//lastImuPreTransAvailable是一个静态变量，初始被设置为false,之后就变成了true
//也就是说这段只调用一次，就是初始时，把imu位姿赋值给lastImuPreTransformation
if (lastImuPreTransAvailable == false)
{
lastImuPreTransformation = transBack; // 把imu位姿赋值给lastImuPreTransformation
lastImuPreTransAvailable = true;
} else {
//lastImuPreTransformation就是上一帧激光时刻的imu位姿,transBack是这一帧时刻的imu位姿
//求完逆相乘以后才是增量，绝不可把imu_incremental发布的当成是两激光间的增量
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;// 位姿增量
// 前一帧的位姿
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
// 当前帧的位姿
Eigen::Affine3f transFinal = transTobe * transIncre;
//将transFinal传入，结果输出至transformTobeMapped中
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)
// 只在第一帧调用（注意上面的return），用imu数据初始化当前帧位姿，仅初始化旋转部分
if (cloudInfo.imuAvailable == true)
{
//注：这一时刻的transBack和之前if (cloudInfo.odomAvailable == true)内部的transBack不同，
//之前获得的是initialGuessRoll等，但是在这里是imuRollInit，它来源于imageProjection中的imuQueue，直接存储原始imu数据的。
//那么对于第一帧数据，目前的lastImuTransformation是initialGuessX等，即imu里程计的数据；
//而transBack是imuRollInit是imu的瞬时原始数据roll、pitch和yaw三个角。
//那么imuRollInit和initialGuessRoll这两者有啥区别呢？
//imuRollInit是imu姿态角，在imageProjection中一收到，就马上赋值给它要发布的cloud_info，
//而initialGuessRoll是imu里程计发布的姿态角。
//直观上来说，imu原始数据收到速度是应该快于imu里程计的数据的，因此感觉二者之间应该有一个增量，
//那么lastImuTransformation.inverse() * transBack算出增量，增量再和原先的transformTobeMapped计算,
//结果依旧以transformTobeMapped来保存
//感觉这里写的非常奇怪
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;
}
}
void extractForLoopClosure()
{
pcl::PointCloud<PointType>::Ptr cloudToExtract(new pcl::PointCloud<PointType>());
int numPoses = cloudKeyPoses3D->size();
for (int i = numPoses-1; i >= 0; --i)
{
if ((int)cloudToExtract->size() <= surroundingKeyframeSize)
cloudToExtract->push_back(cloudKeyPoses3D->points[i]);
else
break;
}
extractCloud(cloudToExtract);
}
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
// 提取当前激光帧邻近的关键帧 并 降采样
// 使用kd-tree搜索当前关键帧邻近的关键帧
//创建Kd树然后搜索 半径在配置文件中
//指定半径范围查找近邻
//球状固定距离半径近邻搜索
//surroundingKeyframeSearchRadius是搜索半径，pointSearchInd应该是返回的index，pointSearchSqDis应该是依次距离中心点的距离
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree kdtree的输入，全局关键帧位姿集合（历史所有关键帧集合）
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)
{
//k近邻搜索,找出最近的k个节点（这里是1）
kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
//把强度替换掉，注意是从原始关键帧数据中替换的,相当于是把下采样以后的点的强度，换成是原始点强度
//注意，这里的intensity应该不是强度，因为在函数saveKeyFramesAndFactor中:
//thisPose3D.intensity = cloudKeyPoses3D->size();
//就是索引，只不过这里借用intensity结构来存放
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;
}
// 对降采样后的点云进行提取出边缘点和平面点对应的localmap
extractCloud(surroundingKeyPosesDS);
}
/*
* 将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
*/
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;
// 根据该索引检查是否在laserCloudMapContainer中存在对应的点云数据
if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
{
// transformed cloud available
// 如果存在，则将其中的点云数据添加到laserCloudCornerFromMap和laserCloudSurfFromMap中
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first; // 角点
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second; // 平面点
} else {
// transformed cloud not available
// 如果不存在，则会将关键帧的点云数据进行变换，并添加到laserCloudCornerFromMap和laserCloudSurfFromMap中，
// 这里对点云数据进行变换指的是将这些点云转换到地图的坐标系中，以方便后续的配准或地图构建任务
// 同时将该点云数据存储在laserCloudMapContainer中以备后续使用。
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)
// 对laserCloudCornerFromMap进行降采样
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
// 如果laserCloudMapContainer中存储的关键帧点云数据数量超过1000，函数会将其进行清空，以释放内存空间
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}
/**
* 提取局部角点、平面点云集合，加入局部map
* 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
* 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
*/
void extractSurroundingKeyFrames()
{
// 如果点云为空直接返回
if (cloudKeyPoses3D->points.empty() == true)
return;
// if (loopClosureEnableFlag == true)
// {
// extractForLoopClosure();
// } else {
// extractNearby();
// }
extractNearby();
}
/* 降采样当前帧激光点云数据中的角点、平面点 */
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();
}
/* 得到上一帧变换矩阵（旋转+平移） */
void updatePointAssociateToMap()
{
transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
}
/* 角点优化 */
void cornerOptimization()
{
// 得到上一帧位姿的变换矩阵transPointAssociateToMap
updatePointAssociateToMap();
// 表示使用OpenMP并行来执行一个for循环，并设置并行线程数为numberOfCores。
//这样可以利用多个处理器核心来同时执行循环的不同迭代，以加速程序的执行。
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudCornerLastDS->points[i]; // 得到当前帧点云的角点
pointAssociateToMap(&pointOri, &pointSel); // 经过仿射变换 将该角点旋转到kdtreeCornerFromMap中点云相同坐标系下
// 最近邻搜索 在局部地图角点集合中搜索与当前帧点云中角点相邻的点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 初始化3个3*3的零矩阵 CV_32F数据类型为单精度浮点数
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)); // 特征向量
// 只有所有搜索到的5个邻近点与当前角点距离均小于1(即最远的点pointSearchSqDis[4] < 1)才进行下面的优化
if (pointSearchSqDis[4] < 1.0) {
// 将搜索到的5个点数据进行累加然后取平均
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;// 取平均
// 下面求矩阵matA1=[ax,ay,az]^t*[ax,ay,az]
// 更准确地说应该是在求协方差matA1
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;
// 求正交阵的特征值和特征向量
// 特征值：matD1，特征向量：matV1中 对应于LOAM论文里雷达建图 特征值与特征向量那块
cv::eigen(matA1, matD1, matV1);
// at用于访问mat中特定位置的元素
// 边缘：与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大方向）
// 以下这一大块是在计算点到边缘的距离，最后通过系数s来判断是否距离很近
// 如果距离很近就认为这个点在边缘上，需要放到laserCloudOri中
// 如果最大的特征值相比次大特征值，大很多，认为构成了线，角点是合格的
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;
// 局部map对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
// 根据特征向量的方向，构造了两个沿着该方向前后偏移的点 x1, y1, z1 和 x2, y2, z2
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);
// 这边是在求[(x0-x1),(y0-y1),(z0-z1)]与[(x0-x2),(y0-y2),(z0-z2)]叉乘得到的向量的模长
// 这个模长是由0.2*V1[0]和点[x0,y0,z0]构成的平行四边形的面积
// 因为[(x0-x1),(y0-y1),(z0-z1)]x[(x0-x2),(y0-y2),(z0-z2)]=[XXX,YYY,ZZZ],
// 叉乘[XXX,YYY,ZZZ]=[(y0-y1)(z0-z2)-(y0-y2)(z0-z1),-(x0-x1)(z0-z2)+(x0-x2)(z0-z1),(x0-x1)(y0-y2)-(x0-x2)(y0-y1)]
// 再求a012 = [XXX,YYY,ZZZ]的模
// area_012，也就是三个点组成的三角形面积*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)));
// line_12，底边边长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 两次叉积，得到点到直线的垂线段单位向量，x分量，下面同理
// 求叉乘结果[la',lb',lc']=[(x1-x2),(y1-y2),(z1-z2)]x[XXX,YYY,ZZZ]
// [la,lb,lc]=[la',lb',lc']/a012/l12
// 得到底边上的高的方向向量[la,lb,lc]
// LLL=[la,lb,lc]是V1[0]这条高上的单位法向量。||LLL||=1；
//如不理解则看图：
// A
// B C
// 这里ABxAC，代表垂直于ABC面的法向量，其模长为平行四边形面积
//因此BCx(ABxAC),代表了BC和（ABC平面的法向量）的叉乘，那么其实这个向量就是A到BC的垂线的方向向量
//那么(ABxAC)/|ABxAC|,代表着ABC平面的单位法向量
//BCxABC平面单位法向量，即为一个长度为|BC|的（A到BC垂线的方向向量），因此再除以|BC|，得到A到BC垂线的单位方向向量
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// ld2为三角形ABC中BC边上的高
// 计算点pointSel到直线的距离
// 这里需要特别说明的是ld2代表的是点pointSel到过点[cx,cy,cz]的方向向量直线的距离
float ld2 = a012 / l12;
// 下面涉及到一个鲁棒核函数，作者简单地设计了这个核函数。
// 距离越大，s越小，是个距离惩罚因子（权重）
float s = 1 - 0.9 * fabs(ld2);
// coeff代表系数的意思
// coff用于保存距离的方向向量
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
// intensity本质上构成了一个核函数，ld2越接近于1，增长越慢
// intensity=(1-0.9*ld2)*ld2=ld2-0.9*ld2*ld2
coeff.intensity = s * ld2;
// 程序末尾根据s的值来判断是否将点云点放入点云集合laserCloudOri以及coeffSel中。
// 所以就应该认为这个点是边缘点
// s>0.1 也就是要求点到直线的距离ld2要小于1m
// s越大说明ld2越小(离边缘线越近)，这样就说明点pointOri在直线上
if (s > 0.1) {
laserCloudOriCornerVec[i] = pointOri;
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
/**平面点优化
* 当前激光帧平面点寻找局部map匹配点
* 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
*/
void surfOptimization()
{
updatePointAssociateToMap();
// 遍历当前帧平面点集合
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 寻找5个紧邻点, 计算其特征值和特征向量
// 平面点（坐标还是lidar系
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
// 在局部平面点map中查找当前平面点相邻的5个平面点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
matA0.setZero(); // 5*3 存储5个紧邻点
matB0.fill(-1); // matB0是-1
matX0.setZero();
// 只考虑附近1.0m内
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;
}
// 求maxA0中点构成的平面法向量
// matB0是-1，这个函数用来求解AX=B的X，
// 也就是AX+BY+CZ+1=0
matX0 = matA0.colPivHouseholderQr().solve(matB0);
// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
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);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
// 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
bool planeValid = true;
for (int j = 0; j < 5; j++) {
// 点到平面距离公式
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) {
// 当前激光帧点到平面距离
//点(x0,y0,z0)到了平面Ax+By+Cz+D=0的距离为：d=|Ax0+By0+Cz0+D|/√(A^2+B^2+C^2)
//但是会发现下面的分母开了两次方，不知道为什么，分母多开一次方会更小，这因此求出的距离会更大
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
+ pointOri.y * pointOri.y + pointOri.z * pointOri.z));
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
/* 组合优化多项式系数 将角点优化和面点优化的结果合并为优化系数
* 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
*/
void combineOptimizationCoeffs()
{
// combine corner coeffs
// 遍历当前帧角点集合，提取出与局部map匹配上了的角点
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
if (laserCloudOriCornerFlag[i] == true){
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
}
}
// combine surf coeffs
// 遍历当前帧平面点集合，提取出与局部map匹配上了的平面点
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);
}
/**进行Levenberg-Marquardt优化，通过迭代计算出最佳的优化位姿
* scan-to-map优化
* 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* 公式推导：todo
*/
bool LMOptimization(int iterCount)
{
// 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
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]);
// 对laserCloudOri中的点云数量进行了检查。如果点云数量小于50个，则返回false表示处理失败
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
// 初始化矩阵
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
PointType pointOri, coeff;
// 遍历匹配特征点，构建Jacobian矩阵
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
//https://wykxwyc.github.io/2019/08/01/The-Math-Formula-in-LeGO-LOAM/
// 求雅克比矩阵中的元素，距离d对roll角度的偏导量即d(d)/d(roll)
//各种cos sin的是旋转矩阵对roll求导，pointOri.x是点的坐标，coeff.x等是距离到局部点的偏导，也就是法向量（建议看链接）
//注意：链接当中的R0-5公式中，ex和ey是反的
//另一个链接https://blog.csdn.net/weixin_37835423/article/details/111587379#commentBox当中写的更好
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;
// camera -> lidar
// matA为J matB为−J^T*f
// 构造J^T*J*ΔT=−J^T*f
//matA就是误差对旋转和平移变量的雅克比矩阵
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;
}
// 通过QR分解计算公式(3)来求得位姿增量delta_x
cv::transpose(matA, matAt); // 将矩阵matA转置得到矩阵matAt 即J^T
matAtA = matAt * matA; // J^T*J
matAtB = matAt * matB; //
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// iterCount==0 说明是第一次迭代，需要初始化
if (iterCount == 0) {
// 对近似的Hessian矩阵求特征值和特征向量
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));
//matE特征值,matV是特征向量
//退化方向只与原始的约束方向 A有关，与原始约束的位置 b 无关
//算这个的目的是要判断退化，即约束中较小的偏移会导致解所在的局部区域发生较大的变化
cv::eigen(matAtA, matE, matV); // 计算特征值和特征向量
matV.copyTo(matV2);
//初次优化时，特征值门限设置为100，小于这个值认为是退化了
//系统退化与否和系统是否存在解没有必然联系，即使系统出现退化，系统也是可能存在解的，
//因此需要将系统的解进行调整，一个策略就是将解进行投影，
//对于退化方向，使用优化的状态估计值，对于非退化方向，依然使用方程的解。
//另一个策略就是直接抛弃解在退化方向的分量。
//对于退化方向，我们不考虑，直接丢弃，只考虑非退化方向解的增量。
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]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
// 防止非线性优化解退化
// 以下这步原理可以参考链接：
// https://blog.csdn.net/i_robots/article/details/108724606
// 以及https://zhuanlan.zhihu.com/p/258159552
matP = matV.inv() * matV2;
}
if (isDegenerate)
{
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
// 更新当前位姿 x = x + delta_x
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);
// 计算了位姿增量的旋转和平移部分的大小
// pcl::rad2deg函数将弧度转换为角度 pow(xxx,2)为平方
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
}
/* scan - map */
void scan2MapOptimization()
{
// 判断是否有关键帧位姿
if (cloudKeyPoses3D->points.empty())
return;
// 满足设定参数
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
// 构建kd-tree
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 30; iterCount++)
{
laserCloudOri->clear();
coeffSel->clear();
// 角点优化
cornerOptimization();
// 平面点优化
surfOptimization();
// 组合优化多项式系数 将角点优化和面点优化的结果合并为优化系数。
combineOptimizationCoeffs();
// 调用 LMOptimization 函数进行Levenberg-Marquardt优化，通过迭代计算出最佳的优化位姿。
// 如果优化成功（返回值为 true），则跳出循环，结束优化过程
if (LMOptimization(iterCount) == true)
break;
}
// 调用 transformUpdate 函数，对位姿进行更新，将优化后的位姿应用于点云数
transformUpdate();
} else { // 没有足够的角点和平面点特征
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}
/* 对位姿进行更新，将优化后的位姿应用于点云数 */
void transformUpdate()
{
if (cloudInfo.imuAvailable == true)
{
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = imuRPYWeight;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[0] = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[1] = pitchMid;
}
}
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
}
float constraintTransformation(float value, float limit)
{
if (value < -limit)
value = -limit;
if (value > limit)
value = limit;
return value;
}
bool saveFrame()
{
if (cloudKeyPoses3D->points.empty())
return true;
if (sensor == SensorType::LIVOX)
{
if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0)
return true;
}
Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);
if (abs(roll) < surroundingkeyframeAddingAngleThreshold &&
abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
abs(yaw) < surroundingkeyframeAddingAngleThreshold &&
sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
return false;
return true;
}
void addOdomFactor()
{
if (cloudKeyPoses3D->points.empty())
{
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
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
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::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
}
}
void addGPSFactor()
{
if (gpsQueue.empty())
return;
// wait for system initialized and settles down
if (cloudKeyPoses3D->points.empty())
return;
else
{
if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
return;
}
// pose covariance small, no need to correct
if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
return;
// last gps position
static PointType lastGPSPoint;
while (!gpsQueue.empty())
{
if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
{
// message too old
gpsQueue.pop_front();
}
else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2)
{
// message too new
break;
}
else
{
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];
if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
continue;
float gps_x = thisGPS.pose.pose.position.x;
float gps_y = thisGPS.pose.pose.position.y;
float gps_z = thisGPS.pose.pose.position.z;
if (!useGpsElevation)
{
gps_z = transformTobeMapped[5];
noise_z = 0.01;
}
// GPS not properly initialized (0,0,0)
if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
continue;
// Add GPS every a few meters
PointType curGPSPoint;
curGPSPoint.x = gps_x;
curGPSPoint.y = gps_y;
curGPSPoint.z = gps_z;
if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
continue;
else
lastGPSPoint = curGPSPoint;
gtsam::Vector Vector3(3);
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::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
gtSAMgraph.add(gps_factor);
aLoopIsClosed = true;
break;
}
}
}
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;
gtsam::Pose3 poseBetween = loopPoseQueue[i];
gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
}
loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
aLoopIsClosed = true;
}
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);
}
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;
}
}
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);
}
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]);
pubLaserOdometryGlobal.publish(laserOdometryROS);
// Publish TF
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)
static bool lastIncreOdomPubFlag = false;
static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
if (lastIncreOdomPubFlag == false)
{
lastIncreOdomPubFlag = true;
laserOdomIncremental = laserOdometryROS;
increOdomAffine = trans2Affine3f(transformTobeMapped);
} else {
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);
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
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
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}
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);
if (isDegenerate)
laserOdomIncremental.pose.covariance[0] = 1;
else
laserOdomIncremental.pose.covariance[0] = 0;
}
pubLaserOdometryIncremental.publish(laserOdomIncremental);
}
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();
}
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam"); // 初始化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;
}
四、下载安装LIO-SAM
1、下载
2、编译
五、数据集测试、实车测试
1、跑数据集
首先下载论文中的数据集，这里以park_dataset数据集为例。由于数据集文件较大，一般有好几个G，建议科学上网或者寻找别人已经从官网下载好保存在网盘里的。
下载完数据集后，将其放在一个文件夹内，然后运行LIO-SAM：
roslaunch lio_sam run.launch
启动LIO-SAM后，cd进入到存放数据集的文件夹中，播放数据集bag（根据电脑性能自行选择频率）：
rosbag play park_dataset.bag -r 3
park_dataset数据集建图效果如下：
2、实车测试
驱动安装
在进行实车测试之前，首先需要安装一些必备的驱动。如IMU驱动、雷达驱动、机器人驱动。首先介绍一下我们实验室使用的机器人。目前测试使用：
- 机器人是EAI的Dashgo D1，需要安装机器人底盘的驱动。
- IMU使用的是LPMS-IG1九轴惯性测量模块(注意：LIO-SAM必须使用九轴IMU，否则无法正常运行)，需要安装IMU驱动。
- 激光雷达使用的是速腾聚创的16线激光雷达RS-LIDAR-16，需要安装雷达的驱动。
下面介绍三个驱动的安装方式：
1、EAI的Dashgo D1
安装EAI驱动可参考CSDN上EAI官方提供的文档：https://blog.csdn.net/eaibot/category_6256249.html
官方文档描述的很清楚如何安装驱动和使用驱动，下面的命令是如何调用键盘去控制机器人运动。
cd catkin_ws
source devel/setup.bash
roslaunch dashgo_driver demo.launch
rosrun dashgo_tools teleop_twist_keyboard.py
将机器人和电脑通过连接线连接后，运行上述命令，启动了机器人驱动，并且可以通过键盘控制机器人运动。
此外，由于电脑需要放在机器人上跟着机器人一起运动，因此也可以通过ssh的方式，远程控制机器人进行移动和建图。ssh的安装此处不再赘述，可自行上网查阅教程。
可通过以下命令使用另一台电脑连接到该电脑，在另一台电脑的ubuntu系统中输入以下命令，输入正确的密码后即可远程控制和建图。
ssh xxx@ip_address
这里的xxx是电脑ubuntu的设置的名称，ip_address可通过
ifconfig
命令查询本机ip地址
2、激光雷达驱动
由于LIO-SAM代码中需要使用到激光雷达数据中的ring(通道，即点云位于哪条扫描线上)和timestamp(时间戳)。但是旧版的雷达驱动中没有这两个数据，因此需要安装新版的驱动。z
-1 XYZI - x, y, z, intensity
-2 XYZIRT - x, y, z, intensity, ring, timestamp
上面1为老版数据参数，2为新版数据参数。
注意：如果要输出新版点云数据，还需要根据**切换点类型**进行配置重新编译
下面介绍如何安装新版雷达驱动，主要是下载官方github上的驱动包并按照官方的文档进行安装。
下载地址：https://github.com/RoboSense-LiDAR/rslidar_sdk
中文安装文档：https://github.com/RoboSense-LiDAR/rslidar_sdk/blob/main/README_CN.md
根据官方提供的中文文档安装驱动后，使用网线将电脑的以太网口连接上雷达，启动launch文件：
cd catkin_ws
source devel/setup.bash
roslaunch rslidar_sdk start.launch
正常运行结果如下图：
3、IMU驱动安装
LPMG IG1的驱动可参考以下博客进行安装：
LPMS-IG1 IMU内参标定
同时，可根据上述博客完成IMU内参标定。
然后就是通过命令行启动IMU：
cd catkin_ws
source devel/setup.bash
rosrun openzen_sensor openzen_sensor_node
然后使用rostopic命令查看IMU数据：
rostopic echo /imu/data
正常运行结果如下图：
运行LIO-SAM
完成上述步骤后，运行以下命令启动lio-sam
cd lio_sam/
source devel/setup.bash
roslaunch lio_sam run.launch
这里出现一个小bug，运行lio-sam后，代码中数据预处理环节有个地方报错：
// check dense flag 根据is_dense去除无效点云
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
目前原因还不清楚，目前采取的方法是先注释掉这个if语句运行LIO-SAM。
总结
未完待续......
910
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
