这个个系统整体分为: 数据预处理 前端里程计 后端优化 回环检测 显示模块。首先来看一下数据预处理节点做的所有事情：

数据预处理节点

 根据知乎文章以及代码我们知道:

节点	功能	输入	输出
数据预处理	1.接收各传感器信息 2.传感器数据时间同步 3.点云运动畸变补偿 4.传感器信息统一坐标系	1. GNSS组合导航位置、姿态、角速度、线速度等 2.雷达点云信息 3.雷达和IMU相对坐标系	1.GNSS组合导航位置、姿态 2.畸变补偿后的点云
备注：以上信息均是经过时间同步的，时间戳已保持一致。

/** @Description: 数据预处理的node文件* @Author: Ren Qian* @Date: 2020-02-05 02:56:27*/#include <ros/ros.h>
#include "glog/logging.h"
#include "lidar_localization/global_defination/global_defination.h" 
#include "lidar_localization/data_pretreat/data_pretreat_flow.hpp"
using namespace lidar_localization;			//命名空间int main(int argc, char *argv[]) {google::InitGoogleLogging(argv[0]);FLAGS_log_dir = WORK_SPACE_PATH + "/Log";FLAGS_alsologtostderr = 1;ros::init(argc, argv, "data_pretreat_node"); //节点名称ros::NodeHandle nh;//初始化智能指针,调用构造函数std::shared_ptr<DataPretreatFlow> data_pretreat_flow_ptr = std::make_shared<DataPretreatFlow>(nh);//LOG(INFO) << "data_pretreat 准备进入循环" ;ros::Rate rate(100);while (ros::ok()) {ros::spinOnce();data_pretreat_flow_ptr->Run();rate.sleep();}return 0;
}

 注意包含的文件中 global_defination.h 为,这个文件名子为global_defination.h.in.
global_defination.h是由global_defination.h.in自动生成的。

/** @Description: * @Author: Ren Qian* @Date: 2020-02-05 02:27:30*/
#ifndef LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_
#define LIDAR_LOCALIZATION_GLOBAL_DEFINATION_H_IN_#include <string>/** global_defination.h是由global_defination.h.in自动生成的。* 这样WORK_SPACE_PATH变量会随着文件的具体位置不同而改变，可移植性更好。* 除了编写.in文件之外，还需要在项目所在的CMakeLists.txt文件中添加下列代码:* configure_file (${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h.in${PROJECT_BINARY_DIR}/include/${PROJECT_NAME}/global_defination/global_defination.h)*/
namespace lidar_localization {
const std::string WORK_SPACE_PATH = "@WORK_SPACE_PATH@";
}
#endif

 我们看一下include里面的信息可以看到该节点的订阅与发布情况.

/** @Description: 数据预处理模块，包括时间同步、点云去畸变等* @Author: Ren Qian* @Date: 2020-02-10 08:31:22*/
#ifndef LIDAR_LOCALIZATION_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_
#define LIDAR_LOCALIZATION_DATA_PRETREAT_DATA_PRETREAT_FLOW_HPP_#include <ros/ros.h>
// subscriber
#include "lidar_localization/subscriber/cloud_subscriber.hpp"
#include "lidar_localization/subscriber/imu_subscriber.hpp"
#include "lidar_localization/subscriber/velocity_subscriber.hpp"
#include "lidar_localization/subscriber/gnss_subscriber.hpp"
#include "lidar_localization/tf_listener/tf_listener.hpp"
// publisher
#include "lidar_localization/publisher/cloud_publisher.hpp"
#include "lidar_localization/publisher/odometry_publisher.hpp"
// models
#include "lidar_localization/models/scan_adjust/distortion_adjust.hpp"namespace lidar_localization {
class DataPretreatFlow { // 定义相关类public:DataPretreatFlow(ros::NodeHandle& nh, std::string cloud_topic);bool Run();private:bool ReadData();bool InitCalibration();bool InitGNSS();bool HasData();bool ValidData();bool TransformData();bool PublishData();private:// subscriber 这里是该节点需要订阅的所有信息std::shared_ptr<CloudSubscriber> cloud_sub_ptr_; //原始点云数据std::shared_ptr<IMUSubscriber> imu_sub_ptr_; //imu信息std::shared_ptr<VelocitySubscriber> velocity_sub_ptr_; //速度信息std::shared_ptr<GNSSSubscriber> gnss_sub_ptr_; //gnss信息std::shared_ptr<TFListener> lidar_to_imu_ptr_; //坐标系变换关系// publisher 这里是该节点需要发布的所有信息std::shared_ptr<CloudPublisher> cloud_pub_ptr_; //去畸变的点云std::shared_ptr<OdometryPublisher> gnss_pub_ptr_;//gnss信息// modelsstd::shared_ptr<DistortionAdjust> distortion_adjust_ptr_;Eigen::Matrix4f lidar_to_imu_ = Eigen::Matrix4f::Identity();std::deque<CloudData> cloud_data_buff_;std::deque<IMUData> imu_data_buff_;std::deque<VelocityData> velocity_data_buff_;std::deque<GNSSData> gnss_data_buff_;CloudData current_cloud_data_;IMUData current_imu_data_;VelocityData current_velocity_data_;GNSSData current_gnss_data_;Eigen::Matrix4f gnss_pose_ = Eigen::Matrix4f::Identity();
};
}#endif

接下来按照执行顺序,我们要看DataPretreatFlow类的构造函数,在data_pretreat_flow.cpp中:

namespace lidar_localization {
DataPretreatFlow::DataPretreatFlow(ros::NodeHandle& nh) {// subscribercloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "/kitti/velo/pointcloud", 100000);imu_sub_ptr_ = std::make_shared<IMUSubscriber>(nh, "/kitti/oxts/imu", 1000000);velocity_sub_ptr_ = std::make_shared<VelocitySubscriber>(nh, "/kitti/oxts/gps/vel", 1000000);gnss_sub_ptr_ = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);lidar_to_imu_ptr_ = std::make_shared<TFListener>(nh, "/imu_link", "velo_link");// publishercloud_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/synced_cloud", "/velo_link", 100);gnss_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "/synced_gnss", "/map", "/velo_link", 100);distortion_adjust_ptr_ = std::make_shared<DistortionAdjust>(); //点云去畸变
}

根据定义我们可以看到,该节点订阅了4个kitti的话题以及一个TF监听模块

CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);//构造函数\
topic_name: "/kitti/velo/pointcloud""/kitti/oxts/imu""/kitti/oxts/gps/vel""/kitti/oxts/gps/fix"

  TFListener(ros::NodeHandle& nh, std::string base_frame_id, std::string child_frame_id);// 从imu_link到velo_link

实例化了连个发布类型的对象，分别为:

 CloudPublisher(ros::NodeHandle& nh,std::string topic_name,std::string frame_id,size_t buff_size)OdometryPublisher(ros::NodeHandle& nh, std::string topic_name, std::string base_frame_id,std::string child_frame_id,int buff_size);

  在最后,是实例化了一个点云去畸变的对象,因为在数据预处理节点中,我们是要对点云进行去畸变处理的,这个点云去畸变具体是怎么实现的,我们在后面会结合代码来详细描述一下:

  distortion_adjust_ptr_ = std::make_shared<DistortionAdjust>(); 

之后我们看到，在主函数的循环里面我们一直调用函数：

 data_pretreat_flow_ptr->Run();

这个函数的具体实现在 data_pretreat_flow.cpp 中，我们在其他代码节点部分可以看到，每个节点的调用流程都是这样的，所以以后这个函数我们就不会仔细讲解了。：

bool DataPretreatFlow::Run() {if (!ReadData())  //读取数据return false;if (!InitCalibration())  //初始化校准.TF监听里面,进行坐标系转换??return false;if (!InitGNSS())  //初始化GNSS信息return false;while(HasData()) {if (!ValidData())continue;TransformData(); PublishData();//发布数据}return true;
}

PS: GNSS:Global Navigation Satellite System(全球卫星导航系统)
  GPS:Global Positioning System(全球定位系统)

在 Run() 中调用的第一个函数 ReadData()，这个函数：

bool DataPretreatFlow::ReadData() {cloud_sub_ptr_->ParseData(cloud_data_buff_);//将点云数据放到cloud_data_buff_中//定义未校准数据存放的队列,这三个是需要校准的数据:IMU,GNSS,Velocitystatic std::deque<IMUData> unsynced_imu_;static std::deque<VelocityData> unsynced_velocity_;static std::deque<GNSSData> unsynced_gnss_;
//将未校准的数据放到各自对应的队列中， （多态）imu_sub_ptr_->ParseData(unsynced_imu_);velocity_sub_ptr_->ParseData(unsynced_velocity_);gnss_sub_ptr_->ParseData(unsynced_gnss_);if (cloud_data_buff_.size() == 0)//判断点云是否为空return false;double cloud_time = cloud_data_buff_.front().time;//第一个点云数据的时间//传入:unsynced_imu_；传出：（校准好的数据）imu_data_buff_；传入校准时间:cloud_time； //相关数据调用的校准函数SyncData()，是自己类里面定义的那个，即多态。bool valid_imu = IMUData::SyncData(unsynced_imu_, imu_data_buff_, cloud_time);bool valid_velocity = VelocityData::SyncData(unsynced_velocity_, velocity_data_buff_, cloud_time);bool valid_gnss = GNSSData::SyncData(unsynced_gnss_, gnss_data_buff_, cloud_time);static bool sensor_inited = false;if (!sensor_inited) {if (!valid_imu || !valid_velocity || !valid_gnss) {cloud_data_buff_.pop_front();//如果三者中有一个数据没有校准成功,就将这一帧数据舍弃return false;}sensor_inited = true;}return true;
}

第二个函数 InitCalibration() ，主要就是得到变换矩阵：

bool DataPretreatFlow::InitCalibration() {static bool calibration_received = false;if (!calibration_received) {// std::shared_ptr<TFListener> lidar_to_imu_ptr_;if (lidar_to_imu_ptr_->LookupData(lidar_to_imu_)) {//得到child_frame_id 到 base_frame_id 的转换矩阵,存储在lidar_to_imu_中calibration_received = true;}}return calibration_received;
}

第三个函数就是初始化GNSS :

bool DataPretreatFlow::InitGNSS() {static bool gnss_inited = false;if (!gnss_inited) {GNSSData gnss_data = gnss_data_buff_.front();gnss_data.InitOriginPosition();gnss_inited = true;}return gnss_inited;
}

第四个函数，检验所有需要的信息是否为空：

bool DataPretreatFlow::HasData() {if (cloud_data_buff_.size() == 0)return false;if (imu_data_buff_.size() == 0)return false;if (velocity_data_buff_.size() == 0)return false;if (gnss_data_buff_.size() == 0)return false;return true;
}

第五个函数

bool DataPretreatFlow::ValidData() {current_cloud_data_ = cloud_data_buff_.front();current_imu_data_ = imu_data_buff_.front();current_velocity_data_ = velocity_data_buff_.front();current_gnss_data_ = gnss_data_buff_.front();double diff_imu_time = current_cloud_data_.time - current_imu_data_.time;double diff_velocity_time = current_cloud_data_.time - current_velocity_data_.time;double diff_gnss_time = current_cloud_data_.time - current_gnss_data_.time;if (diff_imu_time < -0.05 || diff_velocity_time < -0.05 || diff_gnss_time < -0.05) {cloud_data_buff_.pop_front();return false;}if (diff_imu_time > 0.05) {imu_data_buff_.pop_front();return false;}if (diff_velocity_time > 0.05) {velocity_data_buff_.pop_front();return false;}if (diff_gnss_time > 0.05) {gnss_data_buff_.pop_front();return false;}cloud_data_buff_.pop_front();imu_data_buff_.pop_front();velocity_data_buff_.pop_front();gnss_data_buff_.pop_front();return true;
}

第六个函数：

bool DataPretreatFlow::TransformData() {gnss_pose_ = Eigen::Matrix4f::Identity();current_gnss_data_.UpdateXYZ();gnss_pose_(0,3) = current_gnss_data_.local_E;gnss_pose_(1,3) = current_gnss_data_.local_N;gnss_pose_(2,3) = current_gnss_data_.local_U;gnss_pose_.block<3,3>(0,0) = current_imu_data_.GetOrientationMatrix();gnss_pose_ *= lidar_to_imu_;
//下面这三个是点云畸变补偿需要调用的函数/*数据中提供的速度是IMU所处位置的速度，而我们要的是激光雷达所处位置的速度，由于这两者并不重合，即存在杆臂，所以在车旋转时他们的速度并不一致，需要按照这两者之间的相对坐标，把速度转到雷达对应的位置上去，*/current_velocity_data_.TransformCoordinate(lidar_to_imu_);//0.1s为扫描周期,即雷达转一圈的时间,是100ms,但是在实际中是会有几ms的偏差的,这是程序可以优化的地方distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);return true;
}

---

去畸变原理概述：
首先，根据文章：
  在基于机械雷达的定位系统中，点云畸变补偿是必须要做的事情，因为按照机械雷达的原理，有运动就有畸变。

  畸变产生的原因是一帧点云中的点不是在同一时刻采集的，在采集过程中，雷达随着载体在运动，但是雷达点测量的是物体和雷达之间的距离，所以不同激光点的坐标系就不一样了。

  解决这一问题，就需要把采集过程中雷达的运动计算出来，并在对应的激光点上补偿这个运动量。

  大家接触比较多的运动畸变补偿代码应该是loam里面的那段，这里稍有些不同，首先，雷达的点云输出一般是按照列排列，而此处经过第三方程序对kitti数据进行二次加工后，它变成了行排列，这种排列方式的改变会导致补偿方法有所区别。另一点不同是，loam使用了高频率的imu数据进行补偿，从原理上讲，这样确实更精确，尤其是高动态环境下，但是稍显复杂，对于车来讲，运动并不剧烈，没有高频运动，所以我们在点云的一个采集周期（100ms）内可以认为它是匀速运动，这样实现起来就简单多了。

---

点云去畸变原理：
  首先要说一帧点云的产生过程。对于kitti数据集来讲，使用的是velodyne HDL 64E。
  这个雷达在纵向上排列着64个激光发射器和接收器，也就是说，它一次发射和接收会得到一列64个点。这64个发射器沿着不同的水平角发射，在HDL 64E中，最大的水平角是2°，最小的水平角是-24.8°，中间的水平角均匀变化，相邻的两个发射器的水平角相差大概0.4°。在采集过程中，这一列设备绕着竖直方向旋转，在100ms内旋转一周，在旋转过程中，不断地发射和接收，就会得到n列激光点，这些激光点共同构成了一帧点云。HDL 64E中，这个n是4500，也就是水平方向上分辨率是0.08°。
  由于竖直方向分辨率是0.4°，水平方向上分辨率是0.08°，即竖直方向间隔要比水平方向大很多，所以我们看到的点云都是一圈一圈的。像这样

  假设我们把这个雷达放在一个圆柱形的建筑正中心，那么在静止状况下，如果我们只取64条线中的一条线，那么我们得到的就应该是一个圆环。就像下面这张丑图画的这样:

 那如果不是在静止情况下呢？比如雷达装在车上，车在原地逆时针旋转。再上一张丑图:   我们看到出现了两个正前方，即发射第一束激光时的正前方和发射最后一束激光时的正前方。在发射第一束时，就是它原来静止的位置，当发射最后一束时，**相对于第一束时间已经过去了100ms**，由于雷达自身在运动，此时的正前方往逆时针旋转了一定角度，所以出现了一个缺口。 如果是发生了平移，也可以画张图对应，为了便于理解，在平移时先不旋转。   原理类似，只不过这里是原点的变化，原点越往前，前方建筑物离自己就越近，得到的激光点距离就越小。

  由于雷达计算激光点坐标时，都是以接收到激光束时刻的雷达自身坐标系为基础的，所以载体运动过程中，每一列激光点的基准坐标系都是不一样的，但是他们在同一帧点云里，我们希望能统一在同一个坐标系下，所以我们需要知道每次采集时的坐标系相对于初始时刻坐标系的转换关系。

  到这里，我们就找到了问题的核心，只要计算出来每个激光束接收时刻，雷达相对于初始时刻的相对运动就可以了，它就是我们需要的坐标系转换关系，然后这个激光点坐标乘以这个转换关系，就转换成了在初始坐标系下的激光点了。

---

畸变补偿方法

补偿可分为计算和转换两步
1. 计算相对坐标
  在匀速模型假设前提下，坐标 = 运动×时间，所以又可以分解为两步：
1）获取载体运动信息
  运动信息在原始数据里已经存在，包括角速度、速度，分别用来计算相对角度和相对位移。而且数据是我们已经做好时间同步的。
2）获取该激光点相对于起始时刻的时间差
  由于是顺序扫描，我们可以很容易通过$a\tan 2(y,x)$来计算出该激光点相对于第一个激光点旋转过的角度 $\beta$，我们知道雷达内部旋转360°用了100ms，那么旋转$\beta$角度用了多长时间，就了然了。
2. 转换激光点
  其实就是坐标系×向量，坐标系是转换矩阵，向量是转换之前的激光点坐标，这个转换可以先旋转再平移，也可以用4X4矩阵一次性计算，都行，不是核心问题。

---

点云去畸变程序实现

  由于点云补畸变是一个独立的功能，所以作者在models文件夹下新建一个文件夹scan_adjust，用于存放这个功能的类文件，专门用来补偿畸变。
  这个类需要运动信息，通过函数 SetMotionInfo 在每次补畸变之前传入。激光点坐标转换在 AdjustCloud 函数里.
先看 distortion_adjust.hpp 文件

#include <pcl/common/transforms.h>
#include <Eigen/Dense>
#include "glog/logging.h"#include "lidar_localization/models/scan_adjust/distortion_adjust.hpp"
#include "lidar_localization/sensor_data/velocity_data.hpp"//需要用到运动信息,所以包含头文件
#include "lidar_localization/sensor_data/cloud_data.hpp"//需要用到点云,包含点云类型的头文件namespace lidar_localization {
class DistortionAdjust {public:void SetMotionInfo(float scan_period, VelocityData velocity_data); //用于传入 去畸变需要 的运动信息bool AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr);//坐标转换private:inline Eigen::Matrix3f UpdateMatrix(float real_time);private:float scan_period_;Eigen::Vector3f velocity_;Eigen::Vector3f angular_rate_;
};
} // namespace lidar_slam

  我们在 front_end_flow.cpp 中调用这个类，其实就两行程序就搞定了，具体是在数据预处理的 DataPretreatFlow::TransformData() 函数中调用的：

distortion_adjust_ptr_->SetMotionInfo(0.1, current_velocity_data_);
distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr);

   0.1s为扫描周期,即雷达转一圈的时间,是100ms,但是在实际中是会有几ms的偏差的,这是程序可以优化的地方。
看 distortion_adjust.cpp 文件，里面一共就三个函数，SetMotionInfo 函数是读取 线速度xyz和 角速度xyz：

 Eigen::Vector3f velocity_;Eigen::Vector3f angular_rate_;

void DistortionAdjust::SetMotionInfo(float scan_period, VelocityData velocity_data) {scan_period_ = scan_period;velocity_ << velocity_data.linear_velocity.x, velocity_data.linear_velocity.y, velocity_data.linear_velocity.z;angular_rate_ << velocity_data.angular_velocity.x, velocity_data.angular_velocity.y, velocity_data.angular_velocity.z;
}

AdjustCloud 函数：

bool DistortionAdjust::AdjustCloud(CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& output_cloud_ptr) {CloudData::CLOUD_PTR origin_cloud_ptr(new CloudData::CLOUD(*input_cloud_ptr)); //原始点云指针（读入）output_cloud_ptr.reset(new CloudData::CLOUD());//初始化校正后的点云指针float orientation_space = 2.0 * M_PI;float delete_space = 5.0 * M_PI / 180.0; //5度角度float start_orientation = atan2(origin_cloud_ptr->points[0].y, origin_cloud_ptr->points[0].x); //开始的角度，point{0]Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());//旋转向量 3*1Eigen::Matrix3f rotate_matrix = t_V.matrix();//旋转矩阵 3*3Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();//变换矩阵 4*4//block子矩阵操作:  matrix.block<p,q>(i,j);  提取块大小为(p,q),起始于(i,j)。transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();//逆pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);//让第一个点的角度为0velocity_ = rotate_matrix * velocity_;//线速度向量转换angular_rate_ = rotate_matrix * angular_rate_;//角速度向量转换for (size_t point_index = 1; point_index < origin_cloud_ptr->points.size(); ++point_index) {//当前索引点云的角度float orientation = atan2(origin_cloud_ptr->points[point_index].y, origin_cloud_ptr->points[point_index].x);if (orientation < 0.0)orientation += 2.0 * M_PI;//delete_space=5度, 这是滤波滤掉正负5度的点?if (orientation < delete_space || 2.0 * M_PI - orientation < delete_space)continue;
//角度/360 *扫描周期
//bag里点云的时刻是该帧点云起始时刻和终止时刻的平均值
// - scan_period_ / 2.0是因为:我们要的是把点云转换到该帧采集的中间时刻上去，不是起始时刻
// 前面定义了 orientation_space = 2.0 * M_PI;float real_time = fabs(orientation) / orientation_space * scan_period_ - scan_period_ / 2.0;Eigen::Vector3f origin_point(origin_cloud_ptr->points[point_index].x,origin_cloud_ptr->points[point_index].y,origin_cloud_ptr->points[point_index].z);Eigen::Matrix3f current_matrix = UpdateMatrix(real_time);//根据畸变的角度更新旋转矩阵Eigen::Vector3f rotated_point = current_matrix * origin_point;//这里是消除旋转Eigen::Vector3f adjusted_point = rotated_point + velocity_ * real_time;//velocity_是线速度,这里是消除平移CloudData::POINT point;point.x = adjusted_point(0);point.y = adjusted_point(1);point.z = adjusted_point(2);output_cloud_ptr->points.push_back(point);}pcl::transformPointCloud(*output_cloud_ptr, *output_cloud_ptr, transform_matrix.inverse());return true;
}

  旋转向量单独说一下：对于坐标系的旋转，我们知道，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量， 其方向与旋转轴一致， 而长度等于旋转角。这种向量， 称为旋转向量（或轴角， Axis-Angle） 。这种表示法只需一个三维向量即可描述旋转{取自 高翔 视觉SLAM 十四讲}。

  Eigen::AngleAxisf t_V(start_orientation, Eigen::Vector3f::UnitZ());//旋转向量 3*1

初始化时，各参数含义如下：

//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度

所以 t_V 时初始化为绕Z轴,旋转角度：start_orientation

  旋转向量使用 AngleAxis, 它底层不直接是 Matrix ，但运算可以当作矩阵（因为重载了运算符）。

 Eigen::Matrix3f rotate_matrix = t_V.matrix();//旋转矩阵 3*3

t_V.matrix() 旋转向量转换成旋转矩阵
当然也可以直接转换：

rotation_matrix = rotation_vector.toRotationMatrix();

旋转矩阵（3 × 3） ：Eigen::Matrix3d。
旋转向量（3 × 1） ：Eigen::AngleAxisd。
欧拉角（3 × 1） ：Eigen::Vector3d。
四元数（4 × 1） ：Eigen::Quaterniond。
欧氏变换矩阵（4 × 4） ：Eigen::Isometry3d。
仿射变换（4 × 4） ：Eigen::Affine3d。
射影变换（4 × 4） ：Eigen::Projective3d。

正常旋转矩阵到变换矩阵的复制为：

T2.block<3,3>(0,0) = rotation_matrix1;

这里我们取了旋转矩阵的逆矩阵：

 transform_matrix.block<3,3>(0,0) = rotate_matrix.inverse();

transformPointCloud 函数主要用于点云变换，的定义为：

void pcl::transformPointCloud(const pcl::PointCloud< PointT > &  cloud_in,pcl::PointCloud< PointT > &  cloud_out,const Eigen::Matrix4f  & transform )

在程序中，可以看到我们是对点云进行了 transform_matrix 的变换，根据上面的定义，transform_matrix 是旋转矩阵的逆矩阵，所以这里应该是让第一个点的旋转角度为０，也就是 把所有的雷达点旋转到保证数据中第一个雷达点是正向前的状态下。这样子在遍历点云的时候直接求变换后点云的反正切角度就行。在函数的最后，最后会转换回去。或者不做这个变换，在遍历点云的时候，每个点的反正切值都减去第一个点的反正切，也应该会有一样的效果。

  pcl::transformPointCloud(*origin_cloud_ptr, *origin_cloud_ptr, transform_matrix);//让第一个点的角度为0,旋转所有点云

UpdateMatrix 函数：

Eigen::Matrix3f DistortionAdjust::UpdateMatrix(float real_time) {Eigen::Vector3f angle = angular_rate_ * real_time;Eigen::AngleAxisf t_Vz(angle(2), Eigen::Vector3f::UnitZ());Eigen::AngleAxisf t_Vy(angle(1), Eigen::Vector3f::UnitY());Eigen::AngleAxisf t_Vx(angle(0), Eigen::Vector3f::UnitX());Eigen::AngleAxisf t_V;t_V = t_Vz * t_Vy * t_Vx;return t_V.matrix();
}
} // namespace lidar_localization

---

这里之前看代码的时候有一个疑问，正好作者原文下面有网友问了。作者给了解答。：

---

第七个函数：

bool DataPretreatFlow::PublishData() {cloud_pub_ptr_->Publish(current_cloud_data_.cloud_ptr, current_cloud_data_.time);gnss_pub_ptr_->Publish(gnss_pose_, current_gnss_data_.time);return true;
}

---

参考链接：https://www.zhihu.com/column/c_1114864226103037952