关于安装配置，博客LIO_SAM实测运行，论文学习及代码注释[附对应google driver数据] 我觉得已经写的比较完善了。但是我觉得在注释方面，这位博主写的还不够完善，因此在学习以后，我想补充一些内容。
关于本身论文的原理，参见我的专栏内容：
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
上面这篇文章内容比较多，我放了目录，可以只看对应位置。
而现在这篇文章的主要目的是理清LIO-SAM的流程，对其中的数据处理的思路、程序设计的流程等各方面做一个总结。代码的注释有我自己写的内容，还有就是参考了一些别人的网络博客的内容。总之在总结的过程中，我发现有些东西其实最开始的时候自己理解的不够深刻。
因此，总结此文，一来为自己对代码更熟悉，便于之后回顾，二来也希望能帮到大家。如果这篇文章有幸被你读到，我希望读者可以自己总结一下流程，自己一行一行亲手把代码注释写进去。否则如果只是整篇复制下来读一遍的话，说实话没啥意义，如同过眼云烟，等过个两天脑子里什么都没了。
截至目前，我认为我这篇文章是对代码梳理的最详细的一篇文章。
目录
写在前面
utility.h
imageProjection.cpp
注释
总结
featureExtraction.cpp
注释
总结
imuPreintergration.cpp
注释
总结
mapOptimization.cpp
注释
总结
LIO-SAM说白了只有五个文件，一个头文件utility.h，还有四个cpp文件。我们从头文件开始介绍。
头文件里主要是放了一些通用的参数定义，比方说：
nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");这个意思就是我launch文件里面有这个"lio_sam/pointCloudTopic"（前面这个）的参数值的赋值，那么就赋值给pointCloudTopic（后面这个），后面的"/points_raw"就会忽略。那假如launch文件里面没有这个"lio_sam/pointCloudTopic"的定义，则就用"points_raw"。我们打开run.launch文件，可以看到是有加载参数的：
<rosparam file="$(find lio_sam)/config/params.yaml" command="load" />那么相关的参数就放在params.yaml文件中。
关于这个params.yaml，里面的东西乍一看很多，其实就几个比较重要：
第一：IMU Settings 来记录IMU的bias和噪声。当然IMU其实是加速度计和陀螺仪一共三个轴，这里却不分轴，用了一个平均数。如果没有转台之类的设备，就跑艾伦方差也可以标出各个轴的bias。作者的处理是：在imu预积分部分，三个轴都用了同样的bias方差（也就是写在配置文件里的这个），当然你要是有能力标的更准，那不妨这里把各个轴的bias都写进去，然后在imupreintegration.cpp文件里面改一改。
第二，IMU和雷达之间的外参。我不知道为什么作者TiXiaoshan很骚包的在这里写了一个Extrinsics (lidar -> IMU)，其实不能这样写，应该写成IMU->Lidar。因为它其实是，也就是说在IMU坐标系下的一个点，左乘
，就可以变换到lidar坐标系下。 而且作者用的IMU也不是正常IMU，我推测他用的应该是左手系IMU。对于正常人使用的话，就普通的IMU就行，假如你就是那个机器人，x射向正前方，y射向左边，z射向头顶。雷达和IMU都是这样摆放，所以extrinsicRot和extrinsicRPY全部弄成单位矩阵就行。差的不太远的话，extrinsicTrans弄成[0，0，0]就行。
不过有精力的话还是标定了一下，就用浙大在2020开源的标定工具，lidar_IMU_calib ，（这个博客讲怎么配置的，我用的是这个），我个人拿尺子量出来大致比一下，我觉得他们估的还挺准。顺便提一嘴，ETH还有一个标定雷达和IMU的，那个准确来说标的是雷达和里程计的，原理也比较简单，反正雷达运动算一个轨迹，IMU也来一个轨迹，两边用外参联系起来，构成一个环，来求解外参。但是这种是有问题的，因为IMU估计的轨迹本身也就不准，这样估出来的外参也就不对，所以人家是标雷达和里程计的，不能单估雷达和IMU。浙大的这个lidar_IMU_calib的文章我大致看了看，大致是用样条插值，相机去对齐IMU，通过这种方式来初始化，之后构建surfelsMap，迭代优化来精修。（细节以我的性格正常我会展开说的，但是我不喜欢他们这篇论文所以不讲，主要是因为他们在论文里表示坐标系变换，非要写成
，像这种反人类的写法我们应该坚决抵制 ^ _ ^）
第三，z_tollerance，可以给z一个比较大的限制，如果用的是无人车，那就不可能在z轴变化过大，这里就可以限制它，防止它飘走。
第四，voxel filter paprams，这个是一些体素滤波的下采样参数，注意室内和室外是有区别的。
现在回到这个头文件里，可以注意两个内容：
第一，imuConverter函数，这个函数之后会被频繁调用。它主要的作用，是把IMU的信息，从IMU坐标系，转换到雷达坐标系。（注意，这个函数只旋转，没有平移，和真正的雷达坐标系之间还是差了一个平移的。至于为什么没有平移，先提前剧透一下，在imuPreintegration.cpp文件中，还有两个imu2Lidar，lidar2imu变量，这俩变量只有平移，没有旋转。
事实上，作者后续是把imu数据先用imuConverter旋转到雷达系下（但其实还差了个平移）。然后他把雷达数据又根据lidar2Imu反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐，之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。
第二，publishCloud函数，这个函数传入句柄，然后发布形参里的内容，在cpp文件涉及到话题发布的地方，都会调用它。
其他的函数都是一些数据转换函数，没什么可说的。
#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
{
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;
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());
//订阅话题进入回调函数
//订阅imu里程计: 来自IMUPreintegration(IMUPreintegration.cpp中的类IMUPreintegration)发布的里程计话题（增量式）
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());
//发布去畸变的点云,"lio_sam/deskew/cloud_deskewed":topic name; 1:queue_size
pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
//发布激光点云信息 这里建议看一下自定义的lio_sam::cloud_info的msg文件 里面包含了较多信息
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
//分配内存
allocateMemory();
//重置部分参数
resetParameters();
//setVerbosityLevel用于设置控制台输出的信息。(pcl::console::L_ALWAYS)不会输出任何信息;L_DEBUG会输出DEBUG信息;
//L_ERROR会输出ERROR信息
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是msg文件下自定义的cloud_info消息，对其中的变量进行赋值操作
//(int size, int value):size-要分配的值数,value-要分配给向量名称的值
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,
//初始全部用FLT_MAX 填充，
//因此后文函数projectPointCloud中有一句if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX) continue;
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;
}
}
~ImageProjection(){}
/**
* 订阅原始imu数据
* 1、imu原始测量数据转换到lidar系，加速度、角速度、RPY
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
//imuConverter在头文件utility.h中，作用是把imu数据转换到lidar坐标系
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
// 上锁，添加数据的时候队列不可用
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;
}
/**
* 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿.(地图优化程序中发布的)
*/
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;
// 当前帧起止时刻对应的imu数据、imu里程计数据处理
if (!deskewInfo())
return;
// 当前帧激光点云运动畸变校正
// 1、检查激光点距离、扫描线是否合规
// 2、激光运动畸变校正，保存激光点
projectPointCloud();
// 提取有效激光点，存extractedCloud
cloudExtraction();
// 发布当前帧校正后点云，有效点
publishClouds();
// 重置参数，接收每帧lidar数据都要重置这些参数
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)
{
// 转换成pcl点云格式 形参: (in,out)
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
}
else if (sensor == SensorType::OUSTER)
{
// Convert to Velodyne format
pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
laserCloudIn->points.resize(tmpOusterCloudIn->size());
laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
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中，函数deskewPoint中会被加上laserCloudIn->points[i].time
timeScanCur = cloudHeader.stamp.toSec();
//可以看出lasercloudin中存储的time是一帧中距离起始点的相对时间,timeScanEnd是该帧点云的结尾时间
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// check dense flag
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;
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数据时间上包含激光数据，否则不往下处理了
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;
// 从imu队列中删除当前激光帧0.01s前面时刻的imu数据
while (!imuQueue.empty())
{
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
if (imuQueue.empty())
return;
imuPointerCur = 0;
// 遍历当前激光帧起止时刻（前后扩展0.01s）之间的imu数据
for (int i = 0; i < (int)imuQueue.size(); ++i)
{
sensor_msgs::Imu thisImuMsg = imuQueue[i];
double currentImuTime = thisImuMsg.header.stamp.toSec();
// get roll, pitch, and yaw estimation for this scan
// 提取imu姿态角RPY，作为当前lidar帧初始姿态角
if (currentImuTime <= timeScanCur)
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
// 超过当前激光帧结束时刻0.01s，结束
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
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;
}
--imuPointerCur;
// 没有合规的imu数据
if (imuPointerCur <= 0)
return;
cloudInfo.imuAvailable = true;
}
//初始pose信息保存在cloudInfo里
void odomDeskewInfo()
{
cloudInfo.odomAvailable = false;
// 从imu里程计队列中删除当前激光帧0.01s前面时刻的imu数据
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(地图优化程序中发布的)
nav_msgs::Odometry startOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
startOdomMsg = odomQueue[i];
// 在cloudHandler的cachePointCloud函数中，timeScanCur = cloudHeader.stamp.toSec();，即当前帧点云的初始时刻
//找到第一个大于初始时刻的odom
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;
nav_msgs::Odometry endOdomMsg;
// 提取当前激光帧结束时刻的imu里程计
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
// 在cloudHandler的cachePointCloud函数中， timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// 找到第一个大于一帧激光结束时刻的odom
if (ROS_TIME(&endOdomMsg) < timeScanEnd)
continue;
else
break;
}
// 如果起止时刻对应imu里程计的方差不等，返回
if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
return;
//感觉之后计算的信息并没有被用到
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
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;
// 相对变换，提取增量平移、旋转（欧拉角）
float rollIncre, pitchIncre, yawIncre;
// 给定的转换中，提取XYZ以及欧拉角,通过tranBt 获得增量值 后续去畸变用到
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
odomDeskewFlag = true;
}
/**
* 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）
*/
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
// 查找当前时刻在imuTime下的索引
int imuPointerFront = 0;
//imuDeskewInfo中，对imuPointerCur进行计数(计数到超过当前激光帧结束时刻0.01s)
while (imuPointerFront < imuPointerCur)
{
//imuTime在imuDeskewInfo（deskewInfo中调用，deskewInfo在cloudHandler中调用）被赋值，从imuQueue中取值
//pointTime为当前时刻，由此函数的函数形参传入,要找到imu积分列表里第一个大于当前时间的索引
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
// 设为离当前时刻最近的旋转增量
//如果计数为0或该次imu时间戳小于了当前时间戳(异常退出)
if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
{
//未找到大于当前时刻的imu积分索引
//imuRotX等为之前积分出的内容.(imuDeskewInfo中)
*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]);
//这三项作为函数返回值，以形参指针的方式返回
//按前后百分比赋予旋转量
*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里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
*/
//relTime:laserCloudIn->points[i].time
PointType deskewPoint(PointType *point, double relTime)
{
//这个来源于上文的时间戳通道和imu可用判断，没有或是不可用则返回点
if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
return *point;
//点的时间等于scan时间加relTime（后文的laserCloudIn->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都会启动
第一个点的位姿增量（0），求逆
if (firstPointFlag == true)
{
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;
//laserCloudIn就是原始的点云话题中的数据
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;
//距离图像的行 与点云中ring对应,
//rowIdn计算出该点激光雷达是水平方向上第几线的。从下往上计数，-15度记为初始线，第0线，一共16线(N_SCAN=16
int rowIdn = laserCloudIn->points[i].ring;
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
if (rowIdn % downsampleRate != 0)
continue;
//水平角分辨率
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);
//horizonAngle 为[-180,180],horizonAngle -90 为[-270,90],-round 为[-90,270], /ang_res_x 为[-450,1350]
//+Horizon_SCAN/2为[450,2250]
// 即把horizonAngle从[-180,180]映射到[450,2250]
int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
//大于1800，则减去1800，相当于把1801～2250映射到1～450
//先把columnIdn从horizonAngle:(-PI,PI]转换到columnIdn:[H/4,5H/4],
//然后判断columnIdn大小，把H到5H/4的部分切下来，补到0～H/4的部分。
//将它的范围转换到了[0,H] (H:Horizon_SCAN)。
//这样就把扫描开始的地方角度为0与角度为360的连在了一起，非常巧妙。
//如果前方是x，左侧是y，那么正后方左边是180，右边是-180。这里的操作就是，把它展开成一幅图:
// 0
// 90 -90
// 180 || (-180)
// (-180) ----- (-90) ------ 0 ------ 90 -------180
//变为: 90 ----180(-180) ---- (-90) ----- (0) ----- 90
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
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()
{
// 有效激光点数量
int count = 0;
// extract segmented cloud for lidar odometry
for (int i = 0; i < N_SCAN; ++i)
{
//提取特征的时候，每一行的前5个和最后5个不考虑
//记录每根扫描线起始第5个激光点在一维数组中的索引
//cloudInfo为自定义的msg
// 记录每根扫描线起始第5个激光点在一维数组中的索引
cloudInfo.startRingIndex[i] = count - 1 + 5;
///Horizon_SCAN=1800
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激光点距离
cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
// save extracted cloud
// 加入有效激光点
extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of extracted cloud
++count;
}
}
// 记录每根扫描线倒数第5个激光点在一维数组中的索引
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
/**
* 发布当前帧校正后点云，有效点
*/
void publishClouds()
{
cloudInfo.header = cloudHeader;
//publishCloud在utility.h头文件中,需要传入发布句柄pubExtractedCloud，提取出的有效点云，该帧时间戳，
//pubExtractedCloud定义在构造函数中，用来发布去畸变的点云.
//extractedCloud主要在cloudExtraction中被提取，点云被去除了畸变，
//另外每行头五个和后五个不要(（仍然被保存，但是之后在提取特征时不要,因为要根据前后五个点算曲率）
//cloudHeader.stamp 来源于currentCloudMsg,cloudHeader在cachePointCloud中被赋值currentCloudMsg.header
//而currentCloudMsg是点云队列cloudQueue中提取的
//lidarFrame:在utility.h中被赋为base_link,
//在publishCloud函数中，tempCloud.header.frame_id="base_link"(lidarFrame)
//之后用发布句柄pubExtractedCloud来发布去畸变的点云
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
//发布自定义cloud_info信息
pubLaserCloudInfo.publish(cloudInfo);
//pubExtractedCloud发布的只有点云信息，而pubLaserCloudInfo发布的为自定义的很多信息
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
ImageProjection IP;
ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
//对于一些只订阅一个话题的简单节点来说，我们使用ros::spin()进入接收循环，
//每当有订阅的话题发布时，进入回调函数接收和处理消息数据。
//但是更多的时候，一个节点往往要接收和处理不同来源的数据，并且这些数据的产生频率也各不相同，
//当我们在一个回调函数里耗费太多时间时，会导致其他回调函数被阻塞，导致数据丢失。
//这种场合需要给一个节点开辟多个线程，保证数据流的畅通。
ros::MultiThreadedSpinner spinner(3);
spinner.spin();
return 0;
}
我会依次把下面的内容标号，请注意我的标号。
和大多数的SLAM算法一样，程序就在回调函数里处理算法。
1：imu原始数据送入imuhandle，放入imuqueue中；
2.odometry/imu_incremental送入odomhandle，放入odomqueue中。
这里要说一下，注意两点：
第一，这个话题是imu里程计，是来自IMUPreintegration(imuPreintegration.cpp中的类IMUPreintegration)发布的里程计话题。既然叫里程计，就不是两帧之间的预积分数据。不要被它名字里的incremental误导了，以为它好像是一个增加的量，以为是一段中间时刻内的数据。这种理解是不对的。它就是一个里程计数据，通过imu预积分计算优化来得到的任意时刻在世界坐标系下的位姿。
第二，mapOptimization.cpp文件还会发布一个叫"lio_sam/mapping/odometry_incremental"，它代表的是激光里程计，不要和这里的odometry/imu_incremental混起来。
3.原始点云数据送入cloudhandle：
3.1 点云加入cloudQueue，检查数据有效性（格式上，主要因为雷达款式不同）
3.2 deskewinfo：
3.2.1 imuDeskewInfo：遍历激光帧，从imuqueue中找当前激光帧前0.01s开始，当前帧后0.01s结束，找到最近的一个imu数据，把它的原始角度数据，作为cloudInfo.imuRollInit等变量。cloudInfo是自定义的一个消息类型，用来之后发布去畸变的点云，具体可以看cloud_info.msg里面的定义。与此同时，要根据角速度信息做一个积分，保存到imuRotX等数据结构中，之后用来去畸变。
3.2.2 odomDeskewInfo：遍历imu里程计的odomqueue队列，剔除当前0.01s之前的数据，找到第一个大于当前激光帧的数据，然后用里程计数据来初始化雷达，也是填充角度，不过这次填充的是cloudInfo.initialGuessRoll等变量，位置也会被填充到initialGuessX里。
注意：cloudInfo.initialGuessRoll和cloudInfo.imuRollInit，这些东西都是角度数据，带Guess的为imu里程计提供的数据，imuRollInit这种为imu原始数据。如果有合适的，分别会填充cloudInfo.odomAvailable和cloudInfo.imuAvailable变量为true，代表这块的数据可用。这些数据到底用在哪里呢？
这也需要剧透：会被用在mapOptimization.cpp的updateInitGuess函数，给激光里程计做一个初始化，然后在这个初始化的基础上进行非线性优化。
3.3 projectPointCloud：
3.3.1 获取点云中的每一个点，投影图像。
3.3.2 去畸变。假设一帧激光数据，是在一个很小的时间段内获取的，但是由于这段时间内，激光雷达可能处于运动状态，那么就可能导致运动畸变。这里就要用到了3.2.1中提到的imuRotX等玩意，根据imu的运动信息，把这段时间内依次接收的每个点统一投影到初始时刻，去除运动畸变。
3.4 CloudExtraction：
这个是想把特征依次装到一个一维数组中，然后发布到别的进程里处理。
既然是一维数组，16线的点都弄到一维数组里，那就比较麻烦。虽然说这里要弄一个深度投影图像，但是其实进程之间传输的并不是这个深度投影图像，而是自定义的cloudinfo形式的数据。
实际上是想把有效的数据记录进来，例如空值不要，每根线前5个和后5个也不要……那这样16线的数据混到一维组里就分不清了，所以要记录下每根扫瞄线起始点在1维数组中的索引startRingIndex，结束点的索引endRingIndex，在原始图像中的列数pointColInd，距离值pointRange。然后把有效激光点，放到extractedCloud中。
3.5 publishClouds：
把extractedCloud发布出去，/lio_sam/deskew/cloud_deskewed，这个只有点云信息。
然后再把这些点，赋值到cloudInfo.cloud_deskewed中，把整个的cloudInfo发布出去，名字就叫“lio_sam/deskew/cloud_info"，这个就是作者自己定义的特殊的msg。
#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;
// 特征提取标记，1表示遮挡、平行，或者已经进行特征提取的点，0表示还未进行特征提取处理
int *cloudNeighborPicked;
// 1表示角点，-1表示平面点
int *cloudLabel;
FeatureExtraction()
{
// 订阅当前激光帧运动畸变校正后的点云信息
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息
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];
}
//接收imageProjection.cpp中发布的去畸变的点云，实时处理的回调函数
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();
// 点云角点、平面点特征提取
// 1、遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合
// 2、认为非角点的点都是平面点，加入平面点云集合，最后降采样
extractFeatures();
// 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
publishFeatureCloud();
}
void calculateSmoothness()
{
// 遍历当前激光帧运动畸变校正后的有效点云
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
// 用当前激光点前后5个点计算当前点的曲率
//注意，这里把前后五个点共10个点加起来，还减去了10倍的当前点
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;
//0表示还未进行特征提取处理,1表示遮挡、平行，或者已经进行特征提取的点
cloudNeighborPicked[i] = 0;
//1表示角点，-1表示平面点
cloudLabel[i] = 0;
// 存储该点曲率值、激光点一维索引
//之所以可以这样操作，是因为在initializationValue部分，对cloudSmoothness进行过初始化，
//否则直接对cloudSmoothness[i]赋值，一定会报段错误
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)
{
// 当前点和下一个点的range值
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
// 两个激光点之间的一维索引差值，如果在一条扫描线上，那么值为1；
//如果两个点之间有一些无效点被剔除了，可能会比1大，但不会特别大
// 如果恰好前一个点在扫描一周的结束时刻，下一个点是另一条扫描线的起始时刻，那么值会很大
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;
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;
}
}
// 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;
// 按照曲率从小到大排序点云
//可以看出之前的byvalue在这里被当成了判断函数来用
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;
// 当前激光点还未被处理，且曲率大于阈值，则认为是角点
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
// 每段只取20个角点，如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
largestPickedNum++;
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;
}
// 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
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;
// 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
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;
}
// 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
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){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
// 平面点云降采样
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 加入平面点云集合
*surfaceCloud += *surfaceCloudScanDS;
//用surfaceCloudScan来装数据，然后放到downSizeFilter里，
//再用downSizeFilter进行.filter()操作，把结果输出到*surfaceCloudScanDS里。
//最后把DS装到surfaceCloud中。DS指的是DownSample。
//同样角点（边缘点）则没有这样的操作，直接就用cornerCloud来装点云。
}
}
/**
* 清理
*/
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
// 发布角点、面点点云，用于rviz展示
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
// 发布当前激光帧点云信息，加入了角点、面点点云数据，发布给mapOptimization
// 和imageProjection.cpp发布的不是同一个话题，
// image发布的是"lio_sam/deskew/cloud_info"，
// 这里发布的是"lio_sam/feature/cloud_info"，
// 因此不用担心地图优化部分的冲突
pubLaserCloudInfo.publish(cloudInfo);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
ros::spin();
return 0;
}这个cpp原理比较简单，就是订阅刚刚在imageProjection.cpp总结中的3.5部分中发布的lio_sam/deskew/cloud_info消息，然后提取边缘点（角点），平面点，然后填充cloud_info的cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式发布出去。
1.calculateSmoothness，计算当前激光帧点云中每个点的曲率，比较简单不详细说了。
2.markOccludedPoints，标记被遮挡的点。
这点还是可以说一下，第一个，在cloudinfo中，pointCollnd记录的是当前点在原始扫瞄线图像中的列值，列值在10以内，即2度范围内（10*360/1800），两点之间深度差0.3m，就不考虑这种点；第二，当前点左右的有效点的距离和它比都太大了，那么不要这个点。
3.extractFeatures：
对每条线上大曲率进行排序，每条线分为6段，每段最多提取20个。大于阈值就是角点（默认0.1），并且标记该点周围其他点已经被提取了。小于阈值同理，当成平面点。不过平面点要经过一个降采样过程（角点不用，可能是平面点比较多）。角点以"lio_sam/feature/cloud_corner"发布，平面点以"lio_sam/feature/cloud_surface"发布，然后把二者合在一起，在lio_sam/deskew/cloud_info的基础上，填充cloud_corner和cloud_surface字段，重新以"lio_sam/feature/cloud_info"的形式发布出去。（注意名字不一样了，同样是cloud_info，只不过deskew/cloud_info是imageProjection.cpp发布的，feature/cloud_info是featureExtraction.cpp发布的，后者比前者多了角点和平面点字段。deskew/cloud_info在此之后就算寿终正寝了，没有其他的进程需要监听它了。）
#include "utility.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>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
// 订阅激光里程计（来自MapOptimization）和IMU里程计，
//根据前一时刻激光里程计，和该时刻到当前时刻的IMU里程计变换增量，
//计算当前时刻IMU里程计；
//rviz展示IMU里程计轨迹（局部）。
class TransformFusion : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImuOdometry;
ros::Subscriber subLaserOdometry;
ros::Publisher pubImuOdometry;
ros::Publisher pubImuPath;
Eigen::Affine3f lidarOdomAffine;
Eigen::Affine3f imuOdomAffineFront;
Eigen::Affine3f imuOdomAffineBack;
tf::TransformListener tfListener;
tf::StampedTransform lidar2Baselink;
double lidarOdomTime = -1;
deque<nav_msgs::Odometry> imuOdomQueue;
TransformFusion()
{
// 如果lidar系与baselink系不同（激光系和载体系），需要外部提供二者之间的变换关系
if(lidarFrame != baselinkFrame)
{
try
{
// 等待3s
tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
// lidar系到baselink系的变换
tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
}
// 订阅激光里程计，来自mapOptimization
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅imu里程计，来自IMUPreintegration(IMUPreintegration.cpp中的类IMUPreintegration)
//topic name: odometry/imu_incremental
//注意区分lio_sam/mapping/odometry_incremental
//目前可以明确的一点，odometry/imu_incremental是增量内容，即两帧激光里程计之间的预积分内容,（加上开始的激光里程计本身有的位姿）
//imuIntegratorImu_本身是个积分器，只有两帧之间的预积分，但是发布的时候发布的实际是结合了前述里程计本身有的位姿
//如这个predict里的prevStateOdom:
//currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
subImuOdometry = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
// 发布imu里程计，用于rviz展示
pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);
// 发布imu里程计轨迹
pubImuPath = nh.advertise<nav_msgs::Path> ("lio_sam/imu/path", 1);
}
/**
* 里程计对应变换矩阵
*/
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
double x, y, z, roll, pitch, yaw;
x = odom.pose.pose.position.x;
y = odom.pose.pose.position.y;
z = odom.pose.pose.position.z;
tf::Quaternion orientation;
tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}
/**
* 订阅激光里程计的回调函数，来自mapOptimization
*/
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// 激光里程计对应变换矩阵
lidarOdomAffine = odom2affine(*odomMsg);
// 激光里程计时间戳
lidarOdomTime = odomMsg->header.stamp.toSec();
//这二者里面保存的都是最近的一个雷达激光里程计的变换和时间戳（不再是用一个vector之类的东西保存起来）
}
/**
* 订阅imu里程计，来自IMUPreintegration
* 1、以最近一帧激光里程计位姿为基础，计算该时刻与当前时刻间imu里程计增量位姿变换，相乘得到当前时刻imu里程计位姿
* 2、发布当前时刻里程计位姿，用于rviz展示；发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
*/
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
// 发布tf，map与odom系设为同一个系
static tf::TransformBroadcaster tfMap2Odom;
static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
std::lock_guard<std::mutex> lock(mtx);
// 添加imu里程计到队列，注：imu里程计由本cpp中的另一个类imuPreintegration来发布
imuOdomQueue.push_back(*odomMsg);
// get latest odometry (at current IMU stamp)
// 从imu里程计队列中删除当前（最近的一帧）激光里程计时刻之前的数据
// lidarOdomTime初始化为-1，在收到lidar里程计数据后，在回调函数lidarOdometryHandler中被赋值时间戳
if (lidarOdomTime == -1)
return;
while (!imuOdomQueue.empty())
{
if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
imuOdomQueue.pop_front();
else
break;
}
// 最近的一帧激光里程计时刻对应imu里程计位姿
Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
// 当前时刻imu里程计位姿
Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
// imu里程计增量位姿变换
Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
// 当前时刻imu里程计位姿=最近的一帧激光里程计位姿 * imu里程计增量位姿变换
//lidarOdomAffine在本类的lidarOdometryHandler回调函数中被赋值，消息来源于mapOptimization.cpp发布,是激光里程计
Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
// publish latest odometry
//发布名为"odometry/imu"的话题
//也就是说，这个函数先监听了类IMUPreintegration中发布的odometry/imu_incremental，
//然后计算imu二者之间的增量，然后在激光的基础上加上增量，重新发布
//可以看出发布的并非odometry/imu_incremental的一部分子数据，而是把x，y，z，roll，pitch，yaw都经过了激光里程计的修正，才发布
//可以看出这次发布的内容，是当前时刻里程计位姿.发布名称为"odometry/imu"
nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
laserOdometry.pose.pose.position.x = x; //赋上新的值
laserOdometry.pose.pose.position.y = y;
laserOdometry.pose.pose.position.z = z;
laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
pubImuOdometry.publish(laserOdometry);
// 发布tf，当前时刻odom与baselink系变换关系
//由于之前把map和odom坐标系固定了，因此这里我认为发布的就是真正的最终位姿关系
//map优化提供激光，预积分提供imu，imu之间变换再乘以激光里程计得到各个时刻精确位姿
static tf::TransformBroadcaster tfOdom2BaseLink;
tf::Transform tCur;
tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
if(lidarFrame != baselinkFrame)
tCur = tCur * lidar2Baselink;
tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
tfOdom2BaseLink.sendTransform(odom_2_baselink);
// publish IMU path
// 发布imu里程计路径，注：只是最近一帧激光里程计时刻与当前时刻之间的一段
static nav_msgs::Path imuPath;
static double last_path_time = -1;
double imuTime = imuOdomQueue.back().header.stamp.toSec();
// 每隔0.1s添加一个
if (imuTime - last_path_time > 0.1)
{
last_path_time = imuTime;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
pose_stamped.header.frame_id = odometryFrame; //就叫"odom"
pose_stamped.pose = laserOdometry.pose.pose;
imuPath.poses.push_back(pose_stamped);
// 删除最近一帧激光里程计时刻之前的imu里程计
while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
imuPath.poses.erase(imuPath.poses.begin());
if (pubImuPath.getNumSubscribers() != 0)
{
imuPath.header.stamp = imuOdomQueue.back().header.stamp;
imuPath.header.frame_id = odometryFrame;
pubImuPath.publish(imuPath);
}
}
}
};
class IMUPreintegration : public ParamServer
{
public:
std::mutex mtx;
ros::Subscriber subImu;
ros::Subscriber subOdometry;
ros::Publisher pubImuOdometry;
bool systemInitialized = false;
// 噪声协方差
gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
gtsam::Vector noiseModelBetweenBias;
// imu预积分器
//imuIntegratorOpt_负责预积分两个激光里程计之间的imu数据，作为约束加入因子图，并且优化出bias
gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
//imuIntegratorImu_用来根据新的激光里程计到达后已经优化好的bias，预测从当前帧开始，下一帧激光里程计到达之前的imu里程计增量
gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
// imu数据队列
//imuQueOpt用来给imuIntegratorOpt_提供数据来源，不要的就弹出(从队头开始出发，比当前激光里程计数据早的imu通通积分，用一个扔一个)；
std::deque<sensor_msgs::Imu> imuQueOpt;
//imuQueImu用来给imuIntegratorImu_提供数据来源，不要的就弹出(弹出当前激光里程计之前的imu数据,预积分用完一个弹一个)；
std::deque<sensor_msgs::Imu> imuQueImu;
// imu因子图优化过程中的状态变量
gtsam::Pose3 prevPose_;
gtsam::Vector3 prevVel_;
gtsam::NavState prevState_;
gtsam::imuBias::ConstantBias prevBias_;
// imu状态
gtsam::NavState prevStateOdom;
gtsam::imuBias::ConstantBias prevBiasOdom;
bool doneFirstOpt = false;
double lastImuT_imu = -1;
double lastImuT_opt = -1;
// ISAM2优化器
gtsam::ISAM2 optimizer;
gtsam::NonlinearFactorGraph graphFactors; //总的因子图模型
gtsam::Values graphValues; //因子图模型中的值
const double delta_t = 0;
int key = 1;
// imu-lidar位姿变换
//这点要注意，tixiaoshan这里命名的很垃圾，这只是一个平移变换，
//同样头文件的imuConverter中，也只有一个旋转变换。这里绝对不可以理解为把imu数据转到lidar下的变换矩阵。
//事实上，作者后续是把imu数据先用imuConverter旋转到雷达系下（但其实还差了个平移）。
//作者真正是把雷达数据又根据lidar2Imu反向平移了一下，和转换以后差了个平移的imu数据在“中间系”对齐，
//之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。
gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
IMUPreintegration()
{
//订阅imu原始数据，用下面因子图优化的结果，施加两帧之间的imu预计分量，预测每一时刻（imu频率）的imu里程计
//imuTopic name: "imu_correct"
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅激光里程计，来自mapOptimization，用两帧之间的imu预计分量构建因子图，
// 优化当前帧位姿（这个位姿仅用于更新每时刻的imu里程计，以及下一次因子图优化）
subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
//发布imu里程计: odometry/imu_incremental
pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);
// imu预积分的噪声协方差
boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
//imuAccNoise和imuGyrNoise都是定义在头文件中的高斯白噪声，由配置文件中写入
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
//假设没有初始的bias
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
// 噪声先验
//Diagonal对角线矩阵
//发现diagonal型一般调用.finished(),注释中说finished()意为设置完所有系数后返回构建的矩阵
priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
// 激光里程计scan-to-map优化过程中发生退化，则选择一个较大的协方差
correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
//imu预积分器，用于预测每一时刻（imu频率）的imu里程计（转到lidar系了，与激光里程计同一个系）
imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
//imu预积分器，用于因子图优化
imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
}
void resetOptimization()
{
gtsam::ISAM2Params optParameters;
optParameters.relinearizeThreshold = 0.1;
optParameters.relinearizeSkip = 1;
optimizer = gtsam::ISAM2(optParameters);
gtsam::NonlinearFactorGraph newGraphFactors;
graphFactors = newGraphFactors;
gtsam::Values NewGraphValues;
graphValues = NewGraphValues;
}
void resetParams()
{
lastImuT_imu = -1;
doneFirstOpt = false;
systemInitialized = false;
}
// 订阅的是激光里程计,"lio_sam/mapping/odometry_incremental"
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
// 当前帧激光里程计时间戳
double currentCorrectionTime = ROS_TIME(odomMsg);
// 确保imu优化队列中有imu数据进行预积分
if (imuQueOpt.empty())
return;
// 当前帧激光位姿，来自scan-to-map匹配、因子图优化后的位姿
float p_x = odomMsg->pose.pose.position.x;
float p_y = odomMsg->pose.pose.position.y;
float p_z = odomMsg->pose.pose.position.z;
float r_x = odomMsg->pose.pose.orientation.x;
float r_y = odomMsg->pose.pose.orientation.y;
float r_z = odomMsg->pose.pose.orientation.z;
float r_w = odomMsg->pose.pose.orientation.w;
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
// 0. initialize system
// 0. 系统初始化，第一帧
if (systemInitialized == false)
{
// 重置ISAM2优化器
resetOptimization();
// pop old IMU message
// 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据,delta_t=0
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}
else
break;
}
// initial pose
// 添加里程计位姿先验因子
//lidarPose 为本回调函数收到的激光里程计数据，重组成gtsam的pose格式
//并转到imu坐标系下,我猜测compose可能类似于左乘之类的含义吧
prevPose_ = lidarPose.compose(lidar2Imu);
//X可能是固定搭配（当使用Pose时），如果是速度则是V，bias则是B
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
//通过调用总的因子图模型的add方式，添加第一个因子
//PriorFactor 概念可看gtsam 包括了位姿 速度 bias
//加入PriorFactor在图优化中基本都是必需的前提
//各种noise都定义在构造函数当中
graphFactors.add(priorPose);
// initial velocity
prevVel_ = gtsam::Vector3(0, 0, 0);
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
graphFactors.add(priorVel);
// initial bias
prevBias_ = gtsam::imuBias::ConstantBias();
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
graphFactors.add(priorBias);
// add values
// 变量节点赋初值
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
// 优化一次
optimizer.update(graphFactors, graphValues);
//图和节点均清零 为什么要清零不能继续用吗?
//是因为节点信息保存在gtsam::ISAM2 optimizer，所以要清理后才能继续使用
graphFactors.resize(0);
graphValues.clear();
//积分器重置,重置优化之后的偏置
imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
key = 1;
systemInitialized = true;
return;
}
// reset graph for speed
// 每隔100帧激光里程计，重置ISAM2优化器，保证优化效率
if (key == 100)
{
// get updated noise before reset
// 前一帧的位姿、速度、偏置噪声模型
//保存最后的噪声值
gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
// reset graph
// 重置ISAM2优化器
resetOptimization();
// add pose
// 添加位姿先验因子，用前一帧的值初始化
//重置之后还有类似与初始化的过程 区别在于噪声值不同
//prevPose_等三项，也是上一时刻得到的，
//（初始时刻是lidar里程计的pose直接用lidar2IMU变量转到imu坐标系下，而此处则是通过上一时刻，即接下来的后续优化中得到）
gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
graphFactors.add(priorPose);
// add velocity
gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
graphFactors.add(priorVel);
// add bias
gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
graphFactors.add(priorBias);
// add values
graphValues.insert(X(0), prevPose_);
graphValues.insert(V(0), prevVel_);
graphValues.insert(B(0), prevBias_);
// optimize once
optimizer.update(graphFactors, graphValues);
graphFactors.resize(0);
graphValues.clear();
key = 1;
}
// 1. integrate imu data and optimize
// 1. 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，
// 添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
while (!imuQueOpt.empty())
{
// pop and integrate imu data that is between two optimizations
// 提取前一帧与当前帧之间的imu数据，计算预积分
sensor_msgs::Imu *thisImu = &imuQueOpt.front();
double imuTime = ROS_TIME(thisImu);
//currentCorrectionTime是当前回调函数收到的激光里程计数据的时间
if (imuTime < currentCorrectionTime - delta_t)
{
double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
// imu预积分数据输入：加速度、角速度、dt
// 加入的是这个用来因子图优化的预积分器imuIntegratorOpt_,注意加入了上一步算出的dt
//作者要求的9轴imu数据中欧拉角在本程序文件中没有任何用到,全在地图优化里用到的
imuIntegratorOpt_->integrateMeasurement(
gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
//在推出一次数据前保存上一个数据的时间戳
lastImuT_opt = imuTime;
// 从队列中删除已经处理的imu数据
imuQueOpt.pop_front();
}
else
break;
}
// add imu factor to graph
//利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中,
//注意后面容易被遮挡，imuIntegratorOpt_的值经过格式转换被传入preint_imu，
//因此可以推测imuIntegratorOpt_中的integrateMeasurement函数应该就是一个简单的积分轮子，
//传入数据和dt，得到一个积分量,数据会被存放在imuIntegratorOpt_中
const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
// 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预计分量
gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
graphFactors.add(imu_factor);
// add imu bias between factor
// 添加imu偏置因子，前一帧偏置B(key - 1)，当前帧偏置B(key)，观测值，噪声协方差；deltaTij()是积分段的时间
graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
// add pose factor
// 添加位姿因子
gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
graphFactors.add(pose_factor);
// insert predicted values
// 用前一帧的状态、偏置，施加imu预计分量，得到当前帧的状态
gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
// 变量节点赋初值
graphValues.insert(X(key), propState_.pose());
graphValues.insert(V(key), propState_.v());
graphValues.insert(B(key), prevBias_);
// optimize
optimizer.update(graphFactors, graphValues);
optimizer.update();
graphFactors.resize(0);
graphValues.clear();
// Overwrite the beginning of the preintegration for the next step.
// 优化结果
gtsam::Values result = optimizer.calculateEstimate();
// 更新当前帧位姿、速度
prevPose_ = result.at<gtsam::Pose3>(X(key));
prevVel_ = result.at<gtsam::Vector3>(V(key));
// 更新当前帧状态
prevState_ = gtsam::NavState(prevPose_, prevVel_);
// 更新当前帧imu偏置
prevBias_ = result.at<gtsam::imuBias::ConstantBias>(B(key));
// Reset the optimization preintegration object.
//重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量
imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
// check optimization
// imu因子图优化结果，速度或者偏置过大，认为失败
if (failureDetection(prevVel_, prevBias_))
{
resetParams();
return;
}
// 2. after optiization, re-propagate imu odometry preintegration
// 2. 优化之后，执行重传播；优化更新了imu的偏置，
//用最新的偏置重新计算当前激光里程计时刻之后的imu预积分，这个预积分用于计算每时刻位姿
prevStateOdom = prevState_;
prevBiasOdom = prevBias_;
// first pop imu message older than current correction data
// 从imu队列中删除当前激光里程计时刻之前的imu数据
double lastImuQT = -1;
//注意，这里是要“删除”当前帧“之前”的imu数据，是想根据当前帧“之后”的累积递推。
//而前面imuIntegratorOpt_做的事情是，“提取”当前帧“之前”的imu数据，用两帧之间的imu数据进行积分。处理过的就弹出来。
//因此，新到一帧激光帧里程计数据时，imuQueOpt队列变化如下：
//当前帧之前的数据被提出来做积分，用一个删一个（这样下一帧到达后，队列中就不会有现在这帧之前的数据了）
//那么在更新完以后，imuQueOpt队列不再变化，剩下的原始imu数据用作下一次优化时的数据。
//而imuQueImu队列则是把当前帧之前的imu数据都给直接剔除掉，仅保留当前帧之后的imu数据，
//用作两帧lidar里程计到达时刻之间发布的imu增量式里程计的预测。
//imuQueImu和imuQueOpt的区别要明确,imuIntegratorImu_和imuIntegratorOpt_的区别也要明确,见imuhandler中的注释
while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
{
lastImuQT = ROS_TIME(&imuQueImu.front());
imuQueImu.pop_front();
}
// repropogate
// 对剩余的imu数据计算预积分
if (!imuQueImu.empty())
{
// reset bias use the newly optimized bias
// 传入状态,重置预积分器和最新的偏置
imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
// integrate imu message from the beginning of this optimization
// 计算预积分
//利用imuQueImu中的数据进行预积分 主要区别旧在于上一行的更新了bias
for (int i = 0; i < (int)imuQueImu.size(); ++i)
{
sensor_msgs::Imu *thisImu = &imuQueImu[i];
double imuTime = ROS_TIME(thisImu);
double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);
// 注意:加入的是这个用于传播的的预积分器imuIntegratorImu_,(之前用来计算的是imuIntegratorOpt_,）
//注意加入了上一步算出的dt
//结果被存放在imuIntegratorImu_中
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
gtsam::Vector3(thisImu->angular_velocity.x, thisImu->angular_velocity.y, thisImu->angular_velocity.z), dt);
lastImuQT = imuTime;
}
}
++key;
//设置成True，用来通知另一个负责发布imu里程计的回调函数imuHandler“可以发布了”
doneFirstOpt = true;
}
/**
* imu因子图优化结果，速度或者偏置过大，认为失败
*/
bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
{
Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
if (vel.norm() > 30)
{
ROS_WARN("Large velocity, reset IMU-preintegration!");
return true;
}
Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
if (ba.norm() > 1.0 || bg.norm() > 1.0)
{
ROS_WARN("Large bias, reset IMU-preintegration!");
return true;
}
return false;
}
/**
* 订阅imu原始数据
* 1、用上一帧激光里程计时刻对应的状态、偏置，
* 施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态，也就是imu里程计
* 2、imu里程计位姿转到lidar系，发布里程计
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
std::lock_guard<std::mutex> lock(mtx);
// imu原始测量数据转换到lidar系，加速度、角速度、RPY
sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
// 添加当前帧imu数据到队列
// 两个双端队列分别装着优化前后的imu数据
imuQueOpt.push_back(thisImu);
imuQueImu.push_back(thisImu);
// 要求上一次imu因子图优化执行成功，确保更新了上一帧（激光里程计帧）的状态、偏置，预积分已经被重新计算
// 这里需要先在odomhandler中优化一次后再进行该函数后续的工作
if (doneFirstOpt == false)
return;
double imuTime = ROS_TIME(&thisImu);
//lastImuT_imu变量初始被赋值为-1
// 获得时间间隔, 第一次为1/500,之后是两次imuTime间的差
double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
lastImuT_imu = imuTime;
// integrate this single imu message
// imu预积分器添加一帧imu数据，注：这个预积分器的起始时刻是上一帧激光里程计时刻
imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
// predict odometry
// 用上一帧激光里程计时刻对应的状态、偏置，施加从该时刻开始到当前时刻的imu预计分量，得到当前时刻的状态
gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
// publish odometry
// 发布imu里程计（转到lidar系，与激光里程计同一个系）
nav_msgs::Odometry odometry;
odometry.header.stamp = thisImu.header.stamp;
odometry.header.frame_id = odometryFrame; //"odom"
odometry.child_frame_id = "odom_imu";
// transform imu pose to ldiar
//预测值currentState获得imu位姿, 再由imu到雷达变换, 获得雷达位姿
gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
// 这里留疑问，本cpp读完后补充：
// 为什么currentState获得的是imu的位姿？原始imu数据难道不是先转换成雷达坐标系下的数据（this imu）才再送到imu预积分器中吗？
//答： 在之前的优化函数odometryHandler中，thisIMU是直接从imuQueOpt中取值.
//而imuQueOpt中的内容，是已经从imu原始测量数据转换到了lidar"中间系"系下（在本函数第二行）。离真正的雷达系还差了一个平移
//odometryHandler函数中根据prevPose_ = lidarPose.compose(lidar2Imu)得到激光帧先验位姿(lidarPose)转换到imu系下,（相当于从真正的雷达系扭到上面的中间系中）
//作为初值构建了因子进行优化；
//在其imuIntegratorOpt_->integrateMeasurement中得到的应该是dt之间的预积分量，
//由于其处在循环中，因此是在递推累积计算两帧之间的预积分量。
//（相当于是每到一帧，就把二者之间的预积分量计算一遍，并且优化一遍，存储进imuIntegratorOpt_）中。
//因为本函数为imu回调函数，每收到一个imu数据，当之前因子图算完的情况下，
//在imuIntegratorImu_的基础上继续递推新收到的imu数据，并且进行预测。
//最后把imu再转回lidar系下进行发布。
//注意：这里发布的是两帧之间的“增量”imu里程计信息，
//imuIntegratorImu_本身是个积分器，只有两帧之间的预积分，但是发布的时候发布的实际是结合了前述里程计本身有的位姿
//如这个predict里的prevStateOdom:
//currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);
//关于imuIntegratorImu_在两个回调函数中都进行integrateMeasurement操作，之间是否会有冲突呢？
//我觉得关键在于odometryHandler中有一句：imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom)，
//在imuIntegratorOpt_优化完两帧imu数据之后，imuIntegratorImu_直接把积分和bias给reset掉，
//然后开始根据imuIntegratorOpt_优化出的bias来更新imuIntegratorImu_。
//imuIntegratorImu_和imuIntegratorOpt_的区别在于，opt存储的是新到一帧，和上一帧之间的预积分量，作为约束，执行优化。
//优化后，imuIntegratorImu_利用新的bias，在新到的这一帧的基础上，递推“之后”的预积分量。
//（绝对不能把imuIntegratorOpt_和imuIntegratorImu_理解为同一批imu数据在优化前后的不同值）
//在更新的过程中不用担心会被imuHandler中的imuIntegratorImu_->integrateMeasurement给顶掉，
//这是因为imuHandler要根据doneFirstOpt来检查odometryHandler是不是已经更新完bias了。
//因为更新并不是实时的，而是一帧激光数据到了才更新。
//可是下一帧激光并没有到，但是在此期间imu增量式里程计还是得照常发布，
//如果当前帧已经更新完bias了，然后就可以直接利用这个bias计算后面新到的ImuIntegratorImu_，
odometry.pose.pose.position.x = lidarPose.translation().x();
odometry.pose.pose.position.y = lidarPose.translation().y();
odometry.pose.pose.position.z = lidarPose.translation().z();
odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
odometry.twist.twist.linear.x = currentState.velocity().x();
odometry.twist.twist.linear.y = currentState.velocity().y();
odometry.twist.twist.linear.z = currentState.velocity().z();
odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
pubImuOdometry.publish(odometry);
}
};
//还有两个问题：
//1.第一个是，为什么imu原始数据先要根据imuConverter变到lidar系，
//那么之后imuintegrator->integrateMeasurement算到的预积分数据不就是lidar系下的吗？
//在处理的时候又是把lidar里程计的坐标系根据compose函数变到imu系？这难道不是不对应了吗:
//2.变量gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0),
//gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
//这里为什么不用配置文件中的extRot，extQRPY之类的内容呢，而只是用了extTrans数据？
//关于这点，我在github中找到了解释:https://github.com/TixiaoShan/LIO-SAM/issues/30,
//imuConverter() to align the axis of the two coordinates,并没有涉及到平移,
//lidar2Imu or imu2Lidar 却只有平移的内容
//因此收到imu后，先用imuConverter()转换到雷达系下，（但其实和雷达之间仍然差了一个平移），
//因此又把雷达的内容用只含有平移的lidar2Imu 和原本差了一个平移的imu数据真正对齐
//（相当于是imu旋转到雷达系下以后不平移，然后把雷达倒着平移过来,在一个“中间系”对齐）。
//在算完以后，等发布的时候，又用imu2Lidar又倒回到了正儿八经的雷达系。
//那么tixiaoshan为什么在默认里把平移参数设置为0，0，0？
//他在github中的解释为: 我在不同的数据集中改变了几次IMU的安装位置。但是位置总是靠近激光雷达。
//所以每次测试不同的数据集时，我都不必费心去修改这个参数。严格地说，我的方法并不理想。需要提供此参数以获得更好的性能。
int main(int argc, char** argv)
{
ros::init(argc, argv, "roboat_loam");
IMUPreintegration ImuP;
TransformFusion TF;
ROS_INFO("\033[1;32m----> IMU Preintegration Started.\033[0m");
ros::MultiThreadedSpinner spinner(4);
spinner.spin();
return 0;
}
这个cpp文件主要有两个类，一个叫IMUPreintegration类，一个叫TransformFusion类。
现在我们分开讲，先说IMUPreintegration类。
关于IMU原始数据，送入imuhandle中：
1.1 imuhandle：
1.1.1 imu原始数据，会先被坐标系转换，通过调用头文件里的imuConverter函数，转换到一个“雷达中间系”中，其实和真正的雷达系差了一个平移。
1.1.2 转换后，会存入两个队列，一个imuQueOpt队列，一个imuQueImu队列。这两队列有什么区别和联系呢？这个主要在另一个回调函数odometryHandler会被处理，在那个地方我会讲。这里我们可以先理解为，imuQueImu是真正我们要用的数据，imuQueOpt是一个中间缓存的数据结构，用来优化imu的bias之类的东西。
1.1.3 在标志位doneFirstOpt为True的时候（注意这个标志位，这是一个很重要的变量，之后会再提到），每到一个imu数据，就用imuIntegratorImu_这个预积分器，把这一帧imu数据累积进去，然后预测当前时刻的状态：currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。
1.1.4 把currentState，通过imu2Lidar，从“中间系”给平移到真正的雷达系，然后发布出去。发布的话题就叫odometry/imu_incremental，这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。
1.2. odomHandle：这部分订阅的是/mapping/odometry_incremental，这个话题是由mapOptmization.cpp发布的，可以把它理解为激光里程计。同理，也不要被incremental误导，觉得好像是两帧激光之间的变换，可不是这样的啊。它和imu里程计性质类似，就是相对世界坐标系的位姿。
1.2.1 初始化系统：
从imuQueOpt队列里，删掉当前这帧激光里程计时间上之前的数据，然后把雷达的pose变换到“中间系”，保存为prevPose。图优化部分，加入乱七八糟各种东西，priorVel，priorBias，把两个预积分器imuIntegratorImu_，imuIntegratorOpt_给reset一下。（之后简称imu积分器和opt积分器）
这两个预积分器opt积分器和imu积分器有什么区别呢？马上就要讲，在1.2.3部分。
1.2.2 清理缓存：
100帧激光里程计数据了，就把优化器给reset一下（用前面的先验协方差给reset一下）。注意，1.2.1和1.2.2的主要区别在于，1.2.1中的乱七八糟协方差数据，是用构造函数中写入的一大堆（我认为是经验值），而1.2.2这里的协方差，用的是optimizer.marginalCovariance这个轮子算出来的先验协方差。
1.2.3 正式处理：
假设数据如下分布：
之前imu数据 ——————第一帧开始——————第二帧开始————之后imu数据
1.2.3.1 把“第一帧开始”——“第二帧开始”这个之间的imu数据拿出来，送入opt积分器。这样得到二者之间的预积分，构建imu因子。
1.2.3.2 然后把Xkey-1 到Xkey之间，加入这个imu因子以及 激光里程计提供的pose因子，整体做一个优化。优化的结果就是bias，以及“第二帧开始”这个时刻的系统位姿。
1.2.3.3 把优化的结果（主要是bias），重置opt积分器和imu积分器。 然后把当前帧（上图的“第二帧开始”）之前的数据给删掉，用imu积分器，从“第二帧开始”这里开始往后积分。（我们需要明确一点，在这个处理过程中，imu队列也在持续的进数据，（即1.1的imuhandle中）），这里处理完，那么就置donefirst=True，这样1.1.3部分，就可以无缝衔接接着在这里的基础上对imu积分器继续积分。（可以看出，这点处理，Tixiaoshan还是做的比较牛的）
回顾：在1.1.3部分，发布imu里程计，在这里我们可以的出结果，它并非是纯粹的imu里程计，因为时不时是要加入激光里程计的信息做因子来优化得到imu的bias等信息的。
接下来我们开始讲TransformFusion类。
2.1 lidarOdometryHandler：
这部分监听的是/mapping/odometry，（也就是激光雷达里程计）这个回调函数比较特殊，它并没有把雷达里程计的东西再像别的回调函数一样，时刻存到什么队列里去。而是就保存当前的雷达里程计的数据到lidarOdomAffine里面，把时间戳存到lidarOdomTime里面去。
注意，这里/mapping/odometry和/mapping/odometry_incremental有什么区别呢？我认为本质上区别不大，前者是激光里程计部分直接优化得到的激光位姿，后者相当于激光的位姿经过旋转约束和z轴约束的限制以后，又和原始imu信息里面的角度做了一个加权以后的位姿。
2.2 imuOdometryHandler
这部分监听的是/imu/odometry_incremental（也就是上面我一直在说的imu里程计），把imu里程计放到imuodomQueue里面保存，然后把lidarOdomTime之前的数据扔掉，用imu里程计的两时刻差异，再加上lidarOdomAffine的基础进行发布。
实际上，/imu/odometry_incremental本身就是雷达里程计基础上imu数据上的发布，而在现在这里，也是雷达里程计在“imu里程计”上的一个“再次精修”。最后会发布两个内容，一个是odometry/imu，但是这个实际上是无人监听的，我觉得作者主要是发布tf变换，顺手给它发布了。当然我觉得用户可以监听这个数据，我觉得这个应该是频率上最高的一个里程计数据了。
另外还会发布一个path，用来rviz显示，名字叫lio-sam/imu/path。
这个cpp的内容和前三个cpp的内容加起来行数差不多，比较复杂。也是最容易混乱的一个cpp文件，主要是里面有太多的变量，和相近的命名。我用A3纸记录流程，足足写了4页，说实话写到这里的时候我已经不想往上打了，想干脆把A3纸扫描一下传上来。但是我的字迹写的实在是太潦草了，所以这里还是硬着头皮敲吧。因为不方便画箭头，所以还是得自己去编号。
老规矩，先放代码注释：
#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)
using symbol_shorthand::G; // GPS pose
/*
* A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
*/
/**
* 6D位姿点云结构定义
*/
struct PointXYZIRPYT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding
float roll;
float pitch;
float yaw;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment
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::Subscriber subCloud;
ros::Subscriber subGPS;
ros::Subscriber subLoop;
ros::ServiceServer srvSaveMap;
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
// 当前激光帧角点集合，降采样，DS: DownSize
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization
// 当前激光帧平面点集合，降采样
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization
// 当前帧与局部map匹配上了的角点、平面点，加入同一集合；后面是对应点的参数
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
// 当前帧与局部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;
// 局部map的平面点集合
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap;
// 局部map的角点集合，降采样
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS;
// 局部map的平面点集合，降采样
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;
// 局部map平面点数量
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()
{
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);
// 发布激光里程计，rviz中表现为坐标轴
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);
// 发布激光里程计路径，rviz中表现为载体的运行轨迹
pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1);
// 订阅当前激光帧点云信息，来自featureExtraction
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);
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;
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
std::lock_guard<std::mutex> lock(mtx);
// mapping执行频率控制
static double timeLastProcessing = -1;
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
{
timeLastProcessing = timeLaserInfoCur;
// 当前帧位姿初始化
// 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
// 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光初始位姿
updateInitialGuess();
// 提取局部角点、平面点云集合，加入局部map
// 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
// 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
extractSurroundingKeyFrames();
// 当前激光帧角点、平面点集合降采样
downsampleCurrentScan();
// 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坐标
scan2MapOptimization();
// 设置当前帧为关键帧并执行因子图优化
// 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
// 2、添加激光里程计因子、GPS因子、闭环因子
// 3、执行因子图优化
// 4、得到当前帧优化后位姿，位姿协方差
// 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
saveKeyFramesAndFactor();
// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
correctPoses();
// 发布激光里程计
publishOdometry();
// 发布里程计、点云、轨迹
// 1、发布历史关键帧位姿集合
// 2、发布局部map的降采样平面点集合
// 3、发布历史帧（累加的）的角点、平面点降采样集合
// 4、发布里程计轨迹
publishFrames();
}
}
/**
* 当前帧位姿初始化
* 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
* 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
*/
void updateInitialGuess()
{
// save current transformation before any processing
// 前一帧的位姿，注：这里指lidar的位姿，后面都简写成位姿
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
// 前一帧的初始化姿态角（来自原始imu数据），用于估计第一帧的位姿（旋转部分）
static Eigen::Affine3f lastImuTransformation;
// initialization
// 如果关键帧集合为空，继续进行初始化
if (cloudKeyPoses3D->points.empty())
{
// 当前帧位姿的旋转部分，用激光帧信息中的RPY（来自imu原始数据）初始化
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (!useImuHeadingInitialization)
transformTobeMapped[2] = 0;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
// use imu pre-integration estimation for pose guess
// 用当前帧和前一帧对应的imu里程计计算相对位姿变换，再用前一帧的位姿与相对变换，计算当前帧的位姿，存transformTobeMapped
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);
if (lastImuPreTransAvailable == false)
{
// 赋值给前一帧
//lastImuPreTransAvailable是一个静态变量，初始被设置为false,之后就变成了true
//也就是说这段只调用一次，就是初始时，把imu位姿赋值给lastImuPreTransformation
lastImuPreTransformation = transBack;
lastImuPreTransAvailable = true;
} else {
// 当前帧相对于前一帧的位姿变换，imu里程计计算得到
//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;
}
}
/**
* 提取局部角点、平面点云集合，加入局部map
* 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
* 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
*/
void extractSurroundingKeyFrames()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
// if (loopClosureEnableFlag == true)
// {
// extractForLoopClosure();
// } else {
// extractNearby();
// }
extractNearby();
}
void extractNearby()
{
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// extract all the nearby key poses and downsample them
// kdtree的输入，全局关键帧位姿集合（历史所有关键帧集合）
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
//创建Kd树然后搜索 半径在配置文件中
//指定半径范围查找近邻
//球状固定距离半径近邻搜索
//surroundingKeyframeSearchRadius是搜索半径，pointSearchInd应该是返回的index，pointSearchSqDis应该是依次距离中心点的距离
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]);
}
//降采样
//把相邻关键帧位姿集合，进行下采样，滤波后存入surroundingKeyPosesDS
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();
// 把10s内的关键帧也加到surroundingKeyPosesDS中,注意是“也”，原先已经装了下采样的位姿(位置)
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;
if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
{
// transformed cloud available
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
} else {
// transformed cloud not available
// 相邻关键帧对应的角点、平面点云，通过6D位姿变换到世界坐标系下
//transformPointCloud输入的两个形参，分别为点云和变换，返回变换位姿后的点
pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
// 加入局部map
*laserCloudCornerFromMap += laserCloudCornerTemp;
*laserCloudSurfFromMap += laserCloudSurfTemp;
laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
}
}
// Downsample the surrounding corner key frames (or map)
// 降采样局部角点map
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
// Downsample the surrounding surf key frames (or map)
// 降采样局部平面点map
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
// clear map cache if too large
// 太大了，清空一下内存
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}
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 scan2MapOptimization()
{
//根据现有地图与最新点云数据进行配准从而更新机器人精确位姿与融合建图，
//它分为角点优化、平面点优化、配准与更新等部分。
// 优化的过程与里程计的计算类似，是通过计算点到直线或平面的距离，构建优化公式再用LM法求解。
if (cloudKeyPoses3D->points.empty())
return;
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
//构建kdtree
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
//迭代30次
for (int iterCount = 0; iterCount < 30; iterCount++)
{
laserCloudOri->clear();
coeffSel->clear();
//边缘点匹配优化
cornerOptimization();
//平面点匹配优化
surfOptimization();
//组合优化多项式系数
combineOptimizationCoeffs();
if (LMOptimization(iterCount) == true)
break;
}
//使用了9轴imu的orientation与做transformTobeMapped插值，并且roll和pitch收到常量阈值约束（权重）
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}
/**
* 当前激光帧角点寻找局部map匹配点
* 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
*/
void cornerOptimization()
{
//实现transformTobeMapped的矩阵形式转换 下面调用的函数就一行就不展开了 工具类函数
// 把结果存入transPointAssociateToMap中
updatePointAssociateToMap();
#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];
//第i帧的点转换到第一帧坐标系下
//这里就调用了第一步中updatePointAssociateToMap中实现的transPointAssociateToMap，
//然后利用这个变量，把pointOri的点转换到pointSel下，pointSel作为输出
pointAssociateToMap(&pointOri, &pointSel);
//kd树的最近搜索
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0;
// 先求5个样本的平均值
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);
// 边缘：与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大方向）
// 以下这一大块是在计算点到边缘的距离，最后通过系数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对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
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)]
// 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;
// 三角形的高，也就是点到直线距离
// 计算点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];
// 根据当前帧位姿，变换到世界坐标系（map系）下
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);
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;
// 单位法向量
// 对[pa,pb,pc,pd]进行单位化
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;
// 距离越大，s越小，是个距离惩罚因子（权重）
// 后面部分相除求的是[pa,pb,pc,pd]与pointSel的夹角余弦值(两个sqrt，其实并不是余弦值)
// 这个夹角余弦值越小越好，越小证明所求的[pa,pb,pc,pd]与平面越垂直
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
// 点到平面垂线单位法向量（其实等价于平面法向量）
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1) {
// 当前激光帧平面点，加入匹配集合中.
//如果s>0.1,代表fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z))这一项<1,即"伪距离"<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);
}
/**
* scan-to-map优化
* 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* 公式推导：todo
*/
bool LMOptimization(int iterCount)
{
//由于LOAM里雷达的特殊坐标系 所以这里也转了一次
// 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]);
// 当前帧匹配特征点数太少
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;
// 同上，求解的是对pitch的偏导量
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
// lidar -> camera
//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;
}
// 将矩阵由matA转置生成matAt
// 先进行计算，以便于后边调用 cv::solve求解
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// 利用高斯牛顿法进行求解，
// 高斯牛顿法的原型是J^(T)*J * delta(x) = -J*f(x)
// J是雅克比矩阵，这里是A，f(x)是优化目标，这里是-B(符号在给B赋值时候就放进去了)
// 通过QR分解的方式，求解matAtA*matX=matAtB，得到解matX
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));
// 对近似的Hessian矩阵求特征值和特征向量，
//matE特征值,matV是特征向量
//退化方向只与原始的约束方向 A有关，与原始约束的位置 b 无关
//算这个的目的是要判断退化，即约束中较小的偏移会导致解所在的局部区域发生较大的变化
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
//初次优化时，特征值门限设置为100，小于这个值认为是退化了
//系统退化与否和系统是否存在解没有必然联系，即使系统出现退化，系统也是可能存在解的，
//因此需要将系统的解进行调整，一个策略就是将解进行投影，
//对于退化方向，使用优化的状态估计值，对于非退化方向，依然使用方程的解。
//另一个策略就是直接抛弃解在退化方向的分量。
//对于退化方向，我们不考虑，直接丢弃，只考虑非退化方向解的增量。
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);
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
}
/**
* 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
*/
void transformUpdate()
{
if (cloudInfo.imuAvailable == true)
{
// 俯仰角小于1.4
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = imuRPYWeight;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
// roll角求加权均值，用scan-to-map优化得到的位姿与imu原始RPY数据，进行加权平均
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
// pitch角求加权均值，用scan-to-map优化得到的位姿与imu原始RPY数据，进行加权平均
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;
}
}
// 更新当前帧位姿的roll, pitch, z坐标；因为是小车，roll、pitch是相对稳定的，
// 不会有很大变动，一定程度上可以信赖imu的数据，z是进行高度约束
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
// 当前帧位姿
incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
}
/**
* 设置当前帧为关键帧并执行因子图优化
* 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
* 2、添加激光里程计因子、GPS因子、闭环因子
* 3、执行因子图优化
* 4、得到当前帧优化后位姿，位姿协方差
* 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
*/
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();
}
// update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
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: ");
// cloudKeyPoses3D加入当前帧位姿
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);
// cloudKeyPoses6D加入当前帧位姿
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更新当前帧位姿
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);
}
/**
* 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
*/
bool saveFrame()
{
if (cloudKeyPoses3D->points.empty())
return true;
// 前一帧位姿
//注：最开始没有的时候，在函数extractCloud里面有
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);
// 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
// 变量节点设置初始值
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
}
}
/**
* 添加GPS因子
*/
void addGPSFactor()
{
if (gpsQueue.empty())
return;
// wait for system initialized and settles down
// 如果没有关键帧，或者首尾关键帧距离小于5m，不添加gps因子
if (cloudKeyPoses3D->points.empty())
return;
else
{
if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0)
return;
}
// pose covariance small, no need to correct
// 位姿协方差很小，没必要加入GPS数据进行校正
//3和4我猜可能是x和y？（6维，roll，pitch，yaw，x，y，z）
if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold)
return;
// last gps position
static PointType lastGPSPoint;
while (!gpsQueue.empty())
{
// 删除当前帧0.2s之前的里程计
if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2)
{
// message too old
gpsQueue.pop_front();
}
// 超过当前帧0.2s之后，退出
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噪声协方差太大，不能用
// 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;
// GPS里程计位置
float gps_x = thisGPS.pose.pose.position.x;
float gps_y = thisGPS.pose.pose.position.y;
float gps_z = thisGPS.pose.pose.position.z;
if (!useGpsElevation)
{
gps_z = transformTobeMapped[5];
noise_z = 0.01;
}
// GPS not properly initialized (0,0,0)
// (0,0,0)无效数据
if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6)
continue;
// Add GPS every a few meters
// 每隔5m添加一个GPS里程计
PointType curGPSPoint;
curGPSPoint.x = gps_x;
curGPSPoint.y = gps_y;
curGPSPoint.z = gps_z;
if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0)
continue;
else
lastGPSPoint = curGPSPoint;
// 添加GPS因子
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 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 correctPoses()
{
if (cloudKeyPoses3D->points.empty())
return;
if (aLoopIsClosed == true)
{
// clear map cache
// 清空局部map
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 publishOdometry()
{
// Publish odometry for ROS (global)
// 发布激光里程计，odom等价map
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
// 发布TF，odom->lidar
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
// 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
// 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);
}
/**
* 发布里程计、点云、轨迹
* 1、发布历史关键帧位姿集合
* 2、发布局部map的降采样平面点集合
* 3、发布历史帧（累加的）的角点、平面点降采样集合
* 4、发布里程计轨迹
*/
void publishFrames()
{
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
// 发布历史关键帧位姿集合
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// Publish surrounding key frames
// 发布局部map的降采样平面点集合
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);
}
}
void gpsHandler(const nav_msgs::Odometry::ConstPtr& gpsMsg)
{
gpsQueue.push_back(*gpsMsg);
}
// 根据当前帧位姿，变换到世界坐标系（map系）下
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;
}
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);
}
/**
* Eigen格式的位姿变换
*/
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;
}
/**
* 展示线程
* 1、发布局部关键帧map的特征点云
* 2、保存全局关键帧特征点集合
*/
void visualizeGlobalMapThread()
{
ros::Rate rate(0.2);
while (ros::ok()){
rate.sleep();
// 发布局部关键帧map的特征点云
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;
}
}
/**
* 发布局部关键帧map的特征点云
*/
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
// kdtree查找最近一帧关键帧相邻的关键帧集合
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 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();
}
/**
* 闭环线程
* 1、闭环scan-to-map，icp优化位姿
* 1) 在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
* 2) 提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
* 3) 执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿
* 2、rviz展示闭环边
*/
void loopClosureThread()
{
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(loopClosureFrequency);
while (ros::ok())
{
rate.sleep();
performLoopClosure();
visualizeLoopClosure();
}
}
/**
* 闭环scan-to-map，icp优化位姿
* 1、在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
* 2、提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
* 3、执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿
* 注：闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿
*/
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
// find keys
// 当前关键帧索引，候选闭环匹配帧索引
int loopKeyCur;
int loopKeyPre;
// not-used
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形参分别为点云集合，当前帧的索引，搜索半径
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
// 提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
// 如果特征点较少，返回
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
// 发布闭环匹配关键帧局部map
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
// ICP Settings
// ICP参数设置
static pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// Align clouds
// scan-to-map，调用icp匹配
//icp.setInputSource,icp.setInputTarget
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
// 添加闭环因子需要的数据
//这些内容会在函数addLoopFactor中用到
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;
}
/**
* not-used, 来自外部闭环检测程序提供的闭环匹配索引对
*/
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;
}
/**
* 在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
*/
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
// 在历史关键帧中查找与当前关键帧距离最近的关键帧集合
//配置文件中默认historyKeyframeSearchRadius=15m
std::vector<int> pointSearchIndLoop;
std::vector<float> pointSearchSqDisLoop;
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
// 在候选关键帧集合中，找到与当前帧时间相隔较远的帧，设为候选匹配帧
//配置文件中默认30s
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;
}
/**
* 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合，降采样
*/
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
// extract near keyframes
// 提取key索引的关键帧前后相邻若干帧的关键帧特征点集合
nearKeyframes->clear();
int cloudSize = copy_cloudKeyPoses6D->size();
//通过-searchNum 到 +searchNum，搜索key两侧内容
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;
}
/**
* rviz展示闭环边
*/
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);
}
/**
* not-used
*/
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;
}
// 将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
extractCloud(cloudToExtract);
}
void updatePointAssociateToMap()
{
transPointAssociateToMap = trans2Affine3f(transformTobeMapped);
}
//相当于clip函数
float constraintTransformation(float value, float limit)
{
if (value < -limit)
value = -limit;
if (value > limit)
value = limit;
return value;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
mapOptimization MO;
ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m");
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::spin();
loopthread.join();
visualizeMapThread.join();
return 0;
}
1.第一个巨大的回调函数：lasercloudinfoHandle：
这个函数监听的是/feature/cloud_info，关于这个话题里面包含的内容，我已经在featureExtraction.cpp的总结部分说过了。这里回忆一下，cloud_info是作者自定义的一个特殊的msg类型，包含了当前激光帧的角点集合，平面点集合，一维数组，每根线开始的点和结束点在一维数组中的索引……
那么收到数据，先保存时间戳，然后提取已经在featureExtraction.cpp中被提取的角点和平面点信息，保存到*laserCloudCornerLast和*laserCloudSurfLast中。请记住这两个命名。
频率控制：当前时刻-上一时刻>=0.15s时，才开始下列循环：
1.1 UpdateInitGuess：当前帧初始化。
1.1.1 当关键帧集合为空时，用激光帧的imu原始数据角度数据初始化，并且把数据用lastImuTransformation保存（记住这个命名），返回。
此时推荐回顾一下imageProjection.cpp部分的总结3.2
1.1.2 假如cloudInfo.odomAvailable=true时，那么就用一个transBack来记录cloudInfo.initialGuessX等信息，（这个信息其实来自于imupreintegration.cpp中发布的imu里程计数据），然后记录增量，在之前系统状态transformTobeMapped的基础上，加上这个增量，再次存入transformTobeMapped。 注意这个transformTobeMapped，这个数据结构，在这个cpp里，我们可以理解为就是它在存储着激光里程计部分的系统状态，包括位置与姿态。
注意，这里有一个lastImuPreTransformation，这个用来保存上一时刻imu里程计的数据，根据它和当前的imu里程计来算增量。不要和lastImuTransformation变量混起来，虽然这俩变量名字长的很像。
然后覆盖lastImuTransformation（在这个case里没用到），返回；
1.1.3 假如cloudInfo.imuAvailable=true，那么进入这个case：
注意，lastImuTransformation在1.1.1和1.1.2中并未用到，只是不断的在替换成最新数据。当cloudInfo.odomAvailable一直是true的时候，程序压根也不会进入到这个case。
但是，凡事总有例外，万一哪里没有衔接好，imu里程计没有及时算出来，那么就导致此时激光帧的位姿没有一个初始化的数据（我们之后是要在初始化的基础上进行优化的），那么之后的优化就无从进行。因此，就要用到这个case。
这里主要思路是用imuRollInit数据来初始化，如果你回顾过imageProjection.cpp部分的总结3.2 那么你应该就会懂，这里的数据来源是原始imu数据的角度信息。那么如果这里有数据，就用lastImuTransformation当成最新的上一时刻数据，当前数据transBack和它算一个增量，然后累积到系统值transformTobeMapped上面去。最后更新覆盖lastImuTransformation，返回。
1.2 extractSurroundingKeyFrames：这个是比较复杂的一个函数，以下的内容希望读者可以心平气和的，每个字都依次念一遍。
如果没有关键帧，那就算了，返回；
如果有，就调用extractNearby函数。
关键帧是啥？cloudKeyPoses3D，我们要记住这个变量，虽然到现在为止，我们还不知道它是怎么来的，但是这个东西是怎么获取的，我们在后续必须弄明白。
在这里我先剧透一下：它里面装的是历史的“关键帧”的位置，即x，y，z三个值。需要明确：这里装的绝不是历史的关键帧位置处的点云。而是历史关键帧记录时刻机器人所在的位置。
同理还有一个cloudKeyPoses6D，它比这个3D还多了三个角度信息。之所以要用一个6D一个3D分别来装关键帧，我现在直接揭晓答案：用3D装，是因为我们要根据这个来构建KD树，搜索历史上最近的节点。“最近”指的是距离上最近，即xyz空间坐标最近，和角度无关。而cloudKeyPoses6D，是用来投影点云的，把当前帧点云投影到世界坐标系下，那么投影就必须要用角度信息了，所以作者分别用了一个3D和一个6D来装数据。
Kd树的原理我这里不写，随便放一个链接：机器学习——详解KD-Tree原理 - 知乎 ，实际上代码里也只是调库，所以这里我不写。
那么接下来，我介绍extractNearby函数：
1.2.1 使用kd树，搜索当前最近的关键帧，在给定半径内（默认是50m）所有关键帧最近的位置，并把结果返回到pointSearchInd，把距离返回到pointSearchSqDis中。
1.2.2 根据索引pointSearchInd，把相邻关键帧存入到surroundingKeyPoses中。
1.2.3 下采样，装进surroundingKeyPosesDS中，并在原始的surroundingKeyPoses其中找到最近的一个点，替换掉索引。（关于这个，我的理解是，下采样后不太准确了，好几个不同的关键帧可能因为下采样的原因混成了一个，所以要用原始数据对索引进行一个修正，这样以后才方便根据索引投影点云）
1.2.4 顺手把10s内的关键帧cloudKeyPoses3D中的位置也加入到surroundingKeyPosesDS中。
1.2.5 extractCloud：提取边缘和平面点对应的localmap，把surroundingKeyPosesDS传入到函数中：
1.2.5.1 对输入的surroundingKeyPosesDS进行遍历，找到50m之内的位置，然后用transformPointCloud把对应位置的点云，进行变换到世界坐标系下。
如何变换呢？根据上面提到的cloudKeyPoses6D的位姿，然后把cornerCloudKeyFrame和surfCloudKeyFrame中根据索引找到点云，投影到世界坐标系下。
那么在这里，cornerCloudKeyFrame和surfCloudKeyFrame是什么？之前从来没有出现过。我这里同样进行剧透，它里面存放的是下采样的点云。注意总结1中的*laserCloudCornerLast和*laserCloudSurfLast这两个东西，这是瞬时点云，这个东西会在之后被下采样，然后装入cornerCloudKeyFrame中。
1.2.5.2在角点点云和平面点点云被投影到世界坐标系下后，会被加入到laserCloudCornerFromMap和laserCloudSurfFromMap等数据结构中，然后再合出一个pair类型的Container<关键帧号，<角点集合，平面点集合>>。
1.3 downsampleCurrentScan：
这部分比较简单，就是对最外层的回调函数中的laserCloudCornerLast之类的东西，进行一个下采样，保存到laserCloudCornerLastDS这些以DS结尾的数据结构中，并且把数目存到laserCloudCornerLastDSNum这种以DSNUM结尾的数据结构中。其实就是代表了当前帧点云的角点/平面点的下采样集合，和数目值。
1.4 Scan2MapOptimization：
这个函数是本cpp中第二复杂的函数。我们现在把它展开。
1.4.1 首先，没有关键帧保存，那就返回，不处理；
1.4.2 如果DSNUM这种记录角点和平面点的数据结构中，发现数目不够多，也不处理；只有在数目足够多的时候才进行处理，默认最少要10个角点，100个平面点。
迭代30次：
1.4.2.1 边缘点匹配优化：CornerOptimization
1.4.2.2 平面点匹配优化：SurfOptimization
1.4.2.3 组合优化多项式系数：combineOptimizationCoeffs
1.4.2.4 LMOptimization判断迭代误差是否足够小，如果是true则认为迭代完成，返回；
1.4.3 transformUpdate：原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。
接下来，我们依次展开这些函数：
1.4.2.1 边缘点匹配优化：CornerOptimization
- 把系统状态transformTobeMapped做一个数据格式转换，变成transPointAssociateToMap形式
- 从当前角点下采样集合laserCloudCornerLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。
- 求5个样本的均值，协方差矩阵。对协方差矩阵进行特征值分解，如果最大特征值大于次大特征值的3倍，那么就认为构成线。
- 一旦发现构成线，那么就在均值沿着最大特征向量方向（把它看成线的方向）前后各取一个点（+-0.1 x 方向）。
- X0为当前点，X1和X2为“X0附近的5个点一起算出的均值沿方向前后各取的一点”，叉乘计算三点面积a012，x1x2底边长度l12。然后再做一次叉乘，得到X0距离x1，x2连线的垂线段的单位方向向量（la，lb，lc）。并计算点到直线的距离ld2=a012/l12。
- 用一个鲁棒和函数，使得距离ld2越大，s越小。然后用coeff来保存“鲁棒后”的距离，和“鲁棒后”的点到线的垂线的方向向量。
- 如果点到直线的距离小于1m，那么存入数据结构，laserCloudOriCornerVec为原始点云，coeffSelCornerVec为鲁棒距离和鲁棒向量，laserCloudOriCornerFlag代表当前点X0 已经找到对应的线。
思考：为什么要加入方向向量呢？是因为这个在优化的偏导数中会被用到。
1.4.2.2 平面点匹配优化：SurfOptimization
- 和上面的同理，对系统状态量transformTobeMapped进行数据格式转换；
- 从当前角点下采样集合laserCloudSurfLastDS进行遍历，找到世界坐标系下最近的5个点，要求小于1m。
- 直接用matA0存储这个5个点，求解Ax+By+Cz+1=0的ABC系数（用QR分解）
- 然后对ABCD，代码中为pa，pb，pc，pd=1进行单位化。
- 根据点x0到平面的距离d=
(分母为1）判断是否构成平面。如果有一个大于0.2m则不构成。
- pd2为点到平面的距离，也用鲁棒和函数处理，并且比上两次开方（这点我不理解，我猜就是用来鲁棒的，换成1次开方可能也差不多，意义或许不大），然后和角点部分类似，得到s，存入数据结构。
1.4.2.3 组合优化多项式系数：combineOptimizationCoeffs
这个比较简单，就是把CornerOptimization和SurfOptimization中已经确定匹配关系的点提取出来，laserCloudOri统一把角点和平面点装在一起，coeffSel统一装之前计算得到的“鲁棒优化向量”（角点就是点到直线的“鲁棒垂线”，平面点就是点到平面的“鲁棒法线”）。
优化向量会在LMOptimization中进行优化。
1.4.2.4 LMOptimization判断迭代误差是否足够小，如果是true则认为迭代完成，返回。
这一部分大内容，主要麻烦在原理上面。
这里推荐一个阅读：LIO-SAM-scan2MapOptimization()优化原理及代码解析
这个文章中公式写的非常好。我就不照搬了。
另外在推导部分，可以仔细研究一下这篇文章：
LeGO-LOAM中的数学公式推导
虽然是Lego-loam的推导，但是Lego-loam和lio-sam在这部分的原理是一样的，因此可以通用。看完这篇文章，就能理解1.4.2.3中我提到的“优化向量”是干啥用的。
- 总之，照着原理，构建JtJ*delt_x=-JTf，然后构建MatAtA，matAtB，利用cv:solve提供的QR分解，得到matX，即delta_x。
- 当特征点缺乏时，状态估计方法会发生退化。特征值小于阈值，则视为退化方向向量。这块的理论，可以参考LOAM SLAM原理之防止非线性优化解退化
- 更新位姿，判断收敛与否。那么真正的雷达里程计系统状态transformTobeMapped，就是在这里被更新。
1.4.3 transformUpdate：原始的imu的rpy，在这里和优化后的激光里程计位姿进行一个加权融合。
当imuAvailable=True的时候，并且俯仰角<1.4，那么对位姿和原始imu的rpy做一个加权平均，（权重在配置文件中可以被设置为0.01）。主要是对roll，pitch仅加权平均，并且对z进行一个高度约束（也就是clip，不得超过配置文件中的z_tollerance，这个主要是一个小trick，应对不能飞起来的无人小车用的），更新transformTobeMapped。
好了，那么 现在回到回调函数的主流程：
1.5 saveKeyFramesAndFactor：之前函数二话不说就用了一些并没有出现过的数据结构，例如什么cloudKeyPoses3D，cornerCloudKeyFrame之类的东西，看完这个函数将明白这些变量是怎么来的。
1.5.1 saveFrame：计算当前帧和前一帧位姿变换，如果太小不设关键帧。默认是角度<0.2，距离<1m，两者不满足其一就不保存；
1.5.2 addOdomFactor：
这个是要加入激光里程计因子，给图优化的模型gtSAMgraph。在1.5之前别的函数里，如果没有关键帧，直接就跳过了。但是这里不能跳过。
如果暂时还没有关键帧，就把当前激光系统状态transformTobeMapped，打包成一个PriorFactor加入到gtSAMgraph里。如果目前已经有关键帧了，就把最后一个关键帧，和当前状态transformTobeMapped计算一个增量，把这个增量打包成一个BetweenFactor，加入到gtSAMgraph里头去。
initialEstimate代表变量初值，用transformTobeMapped赋值。
1.5.3 addGpsFactor：
GPS的筛选规则为：如果没有GPS信息，没有关键帧信息，首尾关键帧小于5m，或者位姿的协方差很小（x，y的协方差阈值小于25），就不添加GPS。
否则，遍历GPS列表，当前帧0.2s以前的不要，或者GPS的协方差太大了也不要，无效数据也不要…… 找到正常数据，打包成一个gps_factor，加入gtSAMgraph里面。
1.5.4 addLoopFactor：
这个其实和当前的回调函数无关，因为当前回调函数监听的是/feature/cloud_info信息，回环是由其他线程监控和检测的。那么在这里，它查询回环队列，加入回环因子，就是一个顺手的事情，反正现在要更新优化，那么查一下，如果有候选的等在那里，就顺手加入优化。如果用做饭来比喻这件事，那么另外的回环检测的线程就是相当于另一个人在备菜，这里addLoopFactor相当于是在炒菜，备好了就先炒，没有备好就算了。
1.5.5 gtsam正常更新。如果有回环那就多更新几次。
1.5.6 把cloudKeyPoses3D，cloudKeyPoses6D，分别装上信息，cloudKeyPoses3D代表关键帧的位置，cloudKeyPoses6D代表关键帧的位置+姿态，为什么要有一个3D一个6D呢？6D里不已经包含了3D信息吗？这个问题我在1.2处已经解释过了。
1.5.7 用优化结果更新transformTobeMapped。
1.5.8 cornerCloudKeyFrames，surfCloudKeyFrames装入信息，回顾一下，回调函数开头收到的点云数据为laserCloudCornerLast，laserCloudSurfLast，然后在downsampleCurrentScan处这俩信息被下采样，加上了DS后缀。在这里把它装到cornerCloudKeyFrames和surfCloudKeyFrames中。
（回顾：cornerCloudKeyFrames代表关键帧位置处的角点点云，surfCloudKeyFrames代表关键帧位置处的平面点点云。这俩东西就是上面1.2处extractSurroundingKeyFrames用到的内容，cornerCloudKeyFrames通过cloudKeyPoses6D变换到世界系下，被存到laserCloudCornerFromMap里面，这个FromMap又在scan2MapOptimization函数中被设置到kdtreeCornerFromMap这个Kd树里，在cornerOptimization函数里，就是把当前帧的激光点云依据1.1的初值transformTobeMapped，变换到世界坐标系下，再用kdtreeCornerFromMap进行kd搜索，建立匹配关系，优化transformTobeMapped。）
1.5.9 updatePath，更新里程计轨迹。把cloudKeyPoses6D传入，保存在globalPath中。不过暂时还没有进行发布。
1.6 correctPoses：
如果发现回环的话，就把历史关键帧通通更新一遍。我们刚刚在1.5.5里面虽然更新过了，但是结果都是保存在gtsam里面的，cloudKeyPoses3D和cloudKeyPoses6D，这俩保存位置和位姿的变量仍然保留着更新前的关键帧位姿。所以就根据更新结果，把他俩更新一遍。
为什么不更新cornerCloudKeyFrames和surfCloudKeyFrames呢？因为没有必要更新，这俩存的是机器人坐标系下的点云，和机器人在世界系下的位姿是无关的。
1.7 publishOdometry：
到此为止，激光里程计部分的transformTobeMapped就不再更新了。回顾一下transformTobeMapped经历了哪些变换：在1.1部分用imu角度初值或是imu里程计初值赋值，然后在scan2mapOptimization里面根据点到线、点到面的方程进行更新，再在transformUpdate里和原始imu的rpy信息进行一个很小的加权融合（不过这一步我觉得没啥大用），最后在saveKeyFrameAndFactor里面再加入GPS因子和回环因子进行一轮优化。
最后把transformTobeMapped发布出去，其他cpp文件里，接收的“激光里程计”就是这么个东西。也就是lio_sam/mapping/odometry_incremental.
1.8 publishFrames:
这个纯粹就是把乱七八糟东西都发布出去，不管有没有用。如果用户需要就可以监听它。
1.8.1发布关键帧位姿集合，把cloudKeyPoses3D发布成lio_sam/mapping/trajectory
1.8.2发布局部降采样平面点，把laserCloudSurfFromMapDS（历史默认50m内的点，在extractCloud中被设置），发布为lio_sam/mapping/map_local
1.8.3发布当前帧的下采样角点和平面点，用优化后的激光里程计位姿transformTobeMapped投影到世界系下发布，/lio_sam/mapping/cloud_registered
1.8.4发布原始点云经过配准的点云：输入的/feature/cloud_info的cloud_deskewed字段是由featureExtraction.cpp发布的，其cloud_deskewed是源于imageProjection.cpp发布的原始去畸变点云。把它发布到世界坐标系下，然后以/lio_sam/mapping/cloud_registered_raw的形式发布。
1.8.5发布轨迹，把1.5.9里装好在globalPath里面但是还没有发布的轨迹发布出去，名为/lio_sam/mapping/path。
那么到现在，基本上mapOptimization.cpp的内容就结束了，但是还有一些尾巴：
2.gpshandle：监听GPS数据，保存到GPS队列里。
3.loopinfohandle：监听"lio_loop/loop_closure_detection",订阅来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上。
4.loopClosureThread：这个线程在主函数里单独开了一个线程，简要说一下：
4.1 读取配置文件中是否开启回环检测。
4.2开始无限循环：
4.2.1 performLoopClosure：
- 在历史帧中搜索距离关键帧最近，时间间各较远的关键帧（默认是30s以外，15m以内）没找到就返回，如果找到了，结果就放在loopKeyPre当中，loopKeyCur保存最近一个关键帧。
- 把最近一个关键帧的特征点提出来，放入cureKeyframeCloud里；回环候选帧前后各25帧也提取出来，放入prevKeyframeCloud里。
- 把prevKeyframeCloud发布出去，名为lio_sam/mapping/icp_loop_closure_history/cloud
- 调用pcl库的icp轮子，设定阈值，参数，用setInputSource，setInputTarget传入两个点云，用align对齐。成功阈值设定为0.3，成功则存在icp.getFinalTransformation里面。把当前关键帧的点云，用这个结果icp.getFinalTransformation，转换以后，以lio_sam/mapping/icp_loop_closure_corrected_cloud发布出去。
- 把当前帧的的位姿用icp.getFinalTransformation结果校正一下，把pair<当前，回环>，间隔位姿，噪声用队列存起来，等待addLoopFactor来调用，即上面的1.5.4部分。
4.2.2 visualizeLoopClosure:
这部分内容没啥好说的，就是用于rviz展示，把关键帧节点和二者的约束用点和线连起来，以lio_sam/mapping/loop_closure_constraints发布出去。
5. 最后一个线程，visualizeGlobalMapThread:
这个主要是两块内容：
5.1 publishGlobalmap：把当前关键帧附近1000m（默认）的关键帧找出来（其实也就是全局的了），降采样，变换到世界系下，然后发布为lio_sam/mapping/map_global.
5.2 saveMapService:这个用来保存pcd格式的点云地图。在配置文件中可以设置开启与否，和存储位置。注意，当程序结束时，ctrl+c以后，才会启动保存任务。这个部分的代码，和发布globalmap部分的核心内容基本一致，反正就是把cornerCloudKeyFrames，surfCloudKeyFrames用cloudKeyPoses6D变换到世界系下，分别保存角点pcd和平面点pcd，以及全局(合起来）的pcd文件。
我的专栏地址是 微鉴道长SLAM学习笔记（目录），希望感兴趣的朋友多多关注。我目前在杭州从事强化学习导航和slam建图相关的导航工作，我的联系邮箱是zkyy828@163.com，欢迎交流和合作。