这个文件的作用是实现运动补偿和帧间配准
这个文件的核心在于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