A-LOAM阅读笔记（6）：src文件：laserOdometry.cpp

 这个文件的作用是实现运动补偿和帧间配准

这个文件的核心在于main函数，在里面分别完成了两个非线性优化问题，点到线、点到面。

main函数之前还有几个简单的函数

#include <cmath>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <queue>

#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"

#define DISTORTION 0


int corner_correspondence = 0, plane_correspondence = 0;

constexpr double SCAN_PERIOD = 0.1;//扫描周期
constexpr double DISTANCE_SQ_THRESHOLD = 25;// KDTree搜索时相关的阈值
constexpr double NEARBY_SCAN = 2.5;// 对应论文中Fig.7找临近点时的nearby_scan的范围

int skipFrameNum = 5; // 控制建图的频率，通过.launch文件设定
bool systemInited = false; // 系统是否初始化，主要用来跳过第一帧

double timeCornerPointsSharp = 0;// 读取queue中数据时，存储时间戳的，没必要弄成全局变量
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;

pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());

pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());

pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());

int laserCloudCornerLastNum = 0;
int laserCloudSurfLastNum = 0;

//当前帧的在world世界坐标系下的位姿,Lidar Odometry线程估计的frame在world坐标系的位姿P
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr(0, 0, 0);

// 点云特征匹配时的优化变量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};

// 下面的2个分别是优化变量para_q和para_t的映射：表示的是两个world坐标系下的位姿P之间的增量，例如△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

//接收上游的5个topic函数，将消息放在queue中，方便后续处理
//存放数据的queue
 
//只能访问 queue<T> 容器适配器的第一个和最后一个元素。只能在容器的末尾添加新元素，只能从头部移除元素。
//queue 和 stack 有一些成员函数相似，但在一些情况下，工作方式有些不同：
//front()：返回 queue 中第一个元素的引用。如果 queue 是常量，就返回一个常引用；如果 queue 为空，返回值是未定义的。
//back()：返回 queue 中最后一个元素的引用。如果 queue 是常量，就返回一个常引用；如果 queue 为空，返回值是未定义的。
//push(const T& obj)：在 queue 的尾部添加一个元素的副本。这是通过调用底层容器的成员函数 push_back() 来完成的。
//push(T&& obj)：以移动的方式在 queue 的尾部添加元素。这是通过调用底层容器的具有右值引用参数的成员函数 push_back() 来完成的。
//pop()：删除 queue 中的第一个元素。
//size()：返回 queue 中元素的个数。
//empty()：如果 queue 中没有元素的话，返回 true。
//emplace()：用传给 emplace() 的参数调用 T 的构造函数，在 queue 的尾部生成对象。
//swap(queue<T> &other_q)：将当前 queue 中的元素和参数 queue 中的元素交换。它们需要包含相同类型的元素。
//也可以调用全局函数模板 swap() 来完成同样的操作。

std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;
std::mutex mBuf;


//将当前帧点云转换到上一帧，根据运动模型对点云去畸变
//（TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下（也就是当前帧的初始位姿，起始位姿，所以函数名是TransformToStart），
//因为kitti点云已经去除了畸变，所以不再考虑运动补偿。（如果点云没有去除畸变，用slerp差值的方式计算出每个点在fire时刻的位姿，
//然后进行TransformToStart的坐标变换，一方面实现了变换到上一帧Lidar坐标系下；
//另一方面也可以理解成点都将fire时刻统一到了开始时刻，即去除了畸变，完成了运动补偿）
//功能函数的名字 : TransformToStart
//形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址
void TransformToStart(PointType const *const pi, PointType *const po)
{
    double s;//s代表要转换的点，根据时间，在这一帧里占的比率
    
    //有的lidar在内部做了去畸变,那么可以设置DISTORTION为1
    // 由于kitti数据集上的lidar已经做过了运动补偿，因此这里就不做具体补偿了
    if (DISTORTION)//判断是否失真
     // intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
    // intensity的整体减去实数部分,就是时间差,那么除以周期,也就是时间占比了
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//求出了点占的时间比率，SCAN_PERIOD是一帧的时间,10hz的lidar,那么周期就是0.1s
    else
        s = 1.0;// s = 1s说明全部补偿到点云结束的时刻
    //s = 1;
    // 所有点的操作方式都是一致的，相当于从结束时刻补偿到起始时刻，
    //相当于是一个匀速模型的假设，即上一帧的位姿变换,就是这帧的位姿变换

    // 把位姿变换分解成平移和旋转
    // 四元数插值有线性插值和球面插值，球面插值更准确，但是两个四元数差别不大时，二者精度相当
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//slerp函数(球面线性插值)
    Eigen::Vector3d t_point_last = s * t_last_curr;
    Eigen::Vector3d point(pi->x, pi->y, pi->z);//把当前点的坐标取出
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;//通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标
    //把当前点的坐标取出
    // 通过旋转和平移将 当前点转到帧起始时刻坐标系下的坐标
    po->x = un_point.x();//将求得的转换后的坐标赋值给输出点
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;//赋值原的intensity
}

//转换所有的激光雷达点到下一帧的开始
//这个是通过反变换的思想,首先把点统一到起始时刻坐标系下,再通过反变换,得到结束时刻坐标系下的点
//功能函数的名字 : TransformToEnd
//形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址
void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // 首先先做畸变校正,都转到起始时刻
    pcl::PointXYZI un_point_tmp;//转到帧起始时刻坐标系下的点
    TransformToStart(pi, &un_point_tmp);//调用 函数TransformToStart
    //再 通过反变换的方式 将起始时刻坐标系下的点 转到 结束时刻坐标系下 
    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//取出起始时刻坐标系下的点
    //q_last_curr  \ t_last_curr 是结束时刻坐标系转到起始时刻坐标系 的 旋转 和 平移 
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//通过反变换,求得转到 结束时刻坐标系下 的点坐标
    //将求得的转换后的坐标赋值给输出点
    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //Remove distortion time info 移除掉 intensity 相对时间的信息
    po->intensity = int(pi->intensity);
}

// 操作都是送去各自的队列中，加了线程锁以避免线程数据冲突
// 之后的5个Handler函数为接受上游5个topic的回调函数，作用是将消息缓存到对应的queue中，以便后续处理。）
// 只能访问 queue<T> 容器适配器的第一个和最后一个元素。只能在容器的末尾添加新元素，只能从头部移除元素。
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
    mBuf.lock();//std::mutex mBuf 线程互斥锁   lock()和unlock成对存在，加锁时
    //push(T&& obj)：以移动的方式在 queue 的尾部添加元素。这是通过调用底层容器的具有右值引用参数的成员函数 push_back() 来完成的
    cornerSharpBuf.push(cornerPointsSharp2);
    mBuf.unlock();
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
    mBuf.lock();
    cornerLessSharpBuf.push(cornerPointsLessSharp2);
    mBuf.unlock();
}

void laserCloudFlatHandler