loam源码解析3 ： laserOdometry（一）

源码解析3 ： laserOdometry（一）"/>

loam源码解析3 ： laserOdometry（一）

transformMaintenance.cpp解析

• 一、概述
• 二、变量说明
• 三、数据预处理
• • 1. 接受来自scanRegistration的ROS消息
  • • (1) laserCloudSharpHandler
    • (2) laserCloudLessSharpHandler
    • (3) laserCloudFlatHandler
    • (4) laserCloudFlatHandler
    • (5) laserCloudFullResHandler
    • (6) imuTransHandler
  • 2. 定义ROS发布
  • 3. 创建相关的对象
• 四、里程计初始化
• 五、畸变去除函数
• • 1.去除相对开始点匀速运动畸变TransformToStart
  • 2.去除相对结束点匀速运动畸变TransformToEnd
• 六、姿态变换函数
• • 1. 积累旋转量AccumulateRotation
  • 2.修正当前点欧拉角PluginIMURotation

loam源码地址： .
论文学习： LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.
loam源码解析汇总：
loam源码解析1 ： scanRegistration（一）.
loam源码解析2 ： scanRegistration（二）.
loam源码解析3 ： laserOdometry（一）.
loam源码解析4 ： laserOdometry（二）.
loam源码解析5 ： laserOdometry（三）.
loam源码解析6 ： laserMapping（一）.
loam源码解析7 ： laserMapping（二）.
loam源码解析8 ： transformMaintenance.

一、概述

到这里，我们已经在scanRegistration中完成了点云预处理的工作，包括运动畸变的消除、特征提取等等。流程来到了laserOdometry，其主要工作就是特征点匹配和姿态解算。将收到的两种特征点（边缘点和平面点），使用scan-to-scan的方式，对k时刻和k+1时刻相邻两帧点云数据进行匹配，计算点线、点面距离。而姿态解算则是根据匹配的特征点云估计相对位姿。我们将分别进行分析（吐槽一下：为啥作者把所有内容都要放到mian函数里阿～）。

二、变量说明

里程计算法的频率和lidar的频率均是10hz，所以一个点云周期0.1s:

const float scanPeriod = 0.1;

发送给建图算法的频率1hz，周期1s：

//跳帧数，控制发给laserMapping的频率
const int skipFrameNum = 1;
bool systemInited = false;

时间戳信息及消息接收标志，在我们上一节分析的ROS发布消息是有说明，包括四种点云特征、全部点云、imu6组消息：

double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
double timeImuTrans = 0;
//消息接收标志
bool newCornerPointsSharp = false;
bool newCornerPointsLessSharp = false;
bool newSurfPointsFlat = false;
bool newSurfPointsLessFlat = false;
bool newLaserCloudFullRes = false;
bool newImuTrans = false;

接受来自scanRegistration的点云信息：

//曲率大的点
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 laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//所有点云			
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//imu信息
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//上一帧边缘点（曲率较大的点）构成kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
//上一帧平面点（曲率较小的点）构成kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());

特征点数目计数：

int laserCloudCornerLastNum;
int laserCloudSurfLastNum;//unused
int pointSelCornerInd[40000];
//save 2 corner points index searched
float pointSearchCornerInd1[40000];
float pointSearchCornerInd2[40000];//unused
int pointSelSurfInd[40000];
//save 3 surf points index searched
float pointSearchSurfInd1[40000];
float pointSearchSurfInd2[40000];
float pointSearchSurfInd3[40000];

相对与上一帧和第一帧的相对转移

//当前帧相对上一帧的状态转移量，in the local frame
float transform[6] = {0};
//当前帧相对于第一帧的状态转移量，in the global frame
float transformSum[6] = {0};

畸变记录：

//点云第一个点的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
//点云最后一个点的RPY
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变位移
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变速度
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

三、数据预处理

1. 接受来自scanRegistration的ROS消息

在mian函数的一开始，就接受来自scanRegistration的消息并作相应的解析：

int main(int argc, char** argv)
{ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_2", 2, laserCloudFullResHandler);ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> ("/imu_trans", 5, imuTransHandler);

接下来我们逐一查看对应的回调函数。

(1) laserCloudSharpHandler

接受大曲率点，将ros的点云格式转为pcl格式并去除空点：

void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharp2)
{timeCornerPointsSharp = cornerPointsSharp2->header.stamp.toSec();cornerPointsSharp->clear();pcl::fromROSMsg(*cornerPointsSharp2, *cornerPointsSharp);std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);newCornerPointsSharp = true;
}

(2) laserCloudLessSharpHandler

接受较大曲率点，将ros的点云格式转为pcl格式并去除空点：

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharp2)
{timeCornerPointsLessSharp = cornerPointsLessSharp2->header.stamp.toSec();cornerPointsLessSharp->clear();pcl::fromROSMsg(*cornerPointsLessSharp2, *cornerPointsLessSharp);std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsLessSharp,*cornerPointsLessSharp, indices);newCornerPointsLessSharp = true;
}

(3) laserCloudFlatHandler

接受小曲率点，将ros的点云格式转为pcl格式并去除空点：

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlat2)
{timeSurfPointsFlat = surfPointsFlat2->header.stamp.toSec();surfPointsFlat->clear();pcl::fromROSMsg(*surfPointsFlat2, *surfPointsFlat);std::vector<int> indices;pcl::removeNaNFromPointCloud(*surfPointsFlat,*surfPointsFlat, indices);newSurfPointsFlat = true;
}

(4) laserCloudFlatHandler

接受较小曲率点，将ros的点云格式转为pcl格式并去除空点：

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlat2)
{timeSurfPointsLessFlat = surfPointsLessFlat2->header.stamp.toSec();surfPointsLessFlat->clear();pcl::fromROSMsg(*surfPointsLessFlat2, *surfPointsLessFlat);std::vector<int> indices;pcl::removeNaNFromPointCloud(*surfPointsLessFlat,*surfPointsLessFlat, indices);newSurfPointsLessFlat = true;
}

(5) laserCloudFullResHandler

接受全部点云，将ros的点云格式转为pcl格式并去除空点：

//接收全部点
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();laserCloudFullRes->clear();pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);std::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudFullRes,*laserCloudFullRes, indices);newLaserCloudFullRes = true;
}

(6) imuTransHandler

接受imu信息，自己挖的坑自己填～，将以点云信息存储的imu信息解析：

void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2)
{timeImuTrans = imuTrans2->header.stamp.toSec();imuTrans->clear();pcl::fromROSMsg(*imuTrans2, *imuTrans);//根据发来的消息提取imu信息imuPitchStart = imuTrans->points[0].x;imuYawStart = imuTrans->points[0].y;imuRollStart = imuTrans->points[0].z;imuPitchLast = imuTrans->points[1].x;imuYawLast = imuTrans->points[1].y;imuRollLast = imuTrans->points[1].z;imuShiftFromStartX = imuTrans->points[2].x;imuShiftFromStartY = imuTrans->points[2].y;imuShiftFromStartZ = imuTrans->points[2].z;imuVeloFromStartX = imuTrans->points[3].x;imuVeloFromStartY = imuTrans->points[3].y;imuVeloFromStartZ = imuTrans->points[3].z;newImuTrans = true;
}

2. 定义ROS发布

设置4个pub发布器发布当前的边缘点、平面点、全部点云和里程计初始化信息：

ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2);ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);

3. 创建相关的对象

创建里程计对象：

  nav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";

创建坐标变换对象 ，tf包提供了TransformBroadcaster类的实现，以帮助简化tf发布转换的任务。
要使用TransformBroadcaster，我们需要包含<tf/transform_Broadcaster.h>头文件。而StampedTransform（姿态变换）由tf::Transform作为父类继承而来的， 使用参数初始化得到，如stamped_transform = tf::StampedTransform( transform, ros::Time::now(), “frame”, “child_frame”); ：

  tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform laserOdometryTrans;laserOdometryTrans.frame_id_ = "/camera_init";laserOdometryTrans.child_frame_id_ = "/laser_odom";

创建搜索到的对应点的相关信息：

  std::vector<int> pointSearchInd;//搜索到的点序std::vector<float> pointSearchSqDis;//搜索到的点平方距离

创建特征点

  PointType pointOri, pointSel/*选中的特征点*/, tripod1, tripod2, tripod3/*特征点的对应点*/, pointProj/*unused*/, coeff;

创建预测矩阵和退化标志

  //退化标志bool isDegenerate = false;//P矩阵，预测矩阵cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));

创建发送给建图任务的计数和循环频率（100hz），对于ros::spinOnce()和ros::spin() 都是ROS消息回调处理函数，区别在于后者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而前者在调用后还可以继续执行之后的程序：

  int frameCount = skipFrameNum;ros::Rate rate(100);bool status = ros::ok();while (status) {ros::spinOnce();

检测是否全部信息接受到并且要求接受时间同步，接受后全部标志置false：

    if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {  //同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入newCornerPointsSharp = false;newCornerPointsLessSharp = false;newSurfPointsFlat = false;newSurfPointsLessFlat = false;newLaserCloudFullRes = false;newImuTrans = false;

四、里程计初始化

将当前帧的边缘点和平面点，保存为上一时刻数据

      //将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理if (!systemInited) {//将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;//将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;

构建kd树

//使用上一帧的特征点构建kd-treekdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合

发布第一帧特征点云信息，ros::Time().fromSec（）实现将具体时间转换为时间戳：

        //将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMappingsensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

记录初始角度

        //记住原点的翻滚角和俯仰角transformSum[0] += imuPitchStart;transformSum[2] += imuRollStart;

初始化完成退出本次循环：

        systemInited = true;continue;}

T平移量的初值赋值：

      //T平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）transform[3] -= imuVeloFromStartX * scanPeriod;transform[4] -= imuVeloFromStartY * scanPeriod;transform[5] -= imuVeloFromStartZ * scanPeriod;

五、畸变去除函数

首先我们要弄清楚上一章关于imu的畸变去除的不同，或者说弄清楚上一章基于imu的畸变去除是怎么为这里的畸变去除服务的。首先需要明确的一点是，物体运过程中，肯定不可能是一直匀速运动的，但是呢，此处的运动畸变的去除，就是基于匀速运动模型假设的，这就存在了模型导致的误差，如果我们加入了imu的数据，那么可以在建立模型之前，把模型带来的误差给补偿掉。回顾上一章的去除加减速带来的运动畸变消除，我们会发现就是将当前点云点减去第一个点云点，再减去匀速运动的位移得到的差补偿给当前点云点。这时候，如果我们在这里假设匀速运动模型，那由于模型带来的误差就可以消除了。

1.去除相对开始点匀速运动畸变TransformToStart

回顾上一章点云点强度信息的计算: s c a n I D + s c a n P e r i o d ∗ r e l T i m e scanID+scanPeriod*relTime scanID+scanPeriod∗relTime ，又因为雷达的周期为0.1s，所以点云的相对时间为： 10 ∗ r e l T i m e 10*relTime 10∗relTime ，利用该值进行线性插值即可得到相当于在起始点静止的点云。

//当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云
void TransformToStart(PointType const * const pi, PointType * const po)
{//插值系数计算，云中每个点的相对时间/点云周期10float s = 10 * (pi->intensity - int(pi->intensity));//线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数float rx = s * transform[0];float ry = s * transform[1];float rz = s * transform[2];float tx = s * transform[3];float ty = s * transform[4];float tz = s * transform[5];

利用插值后的相对姿态，将当前点转换到第一个点，需要注意的是，相对应的欧拉角都要取负值：

  //平移后绕z轴旋转（-rz）float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);float z1 = (pi->z - tz);//绕x轴旋转（-rx）float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;//绕y轴旋转（-ry）po->x = cos(ry) * x2 - sin(ry) * z2;po->y = y2;po->z = sin(ry) * x2 + cos(ry) * z2;po->intensity = pi->intensity;
}

2.去除相对结束点匀速运动畸变TransformToEnd

该函数主要功能为将当前帧激光点转换至当前帧末尾时刻并去除imu偏差。函数首先根据当前激光点的相对时刻插值位姿矩阵函数首先根据当前激光点的相对时刻，将激光点转换至当前帧初始点坐标系，然后再转换至末尾点坐标系。接着再补偿imu位置误差，由于imu位置误差是当前帧终止点在初始时刻坐标系，因此先旋转到世界坐标系，再旋转至终止时刻imu坐标系。
首先求出当前点与第一个点的相对变换，并转换到第一个点的坐标下（与TransformToStart类似）：

void TransformToEnd(PointType const * const pi, PointType * const po)
{//插值系数计算float s = 10 * (pi->intensity - int(pi->intensity));float rx = s * transform[0];float ry = s * transform[1];float rz = s * transform[2];float tx = s * transform[3];float ty = s * transform[4];float tz = s * transform[5];

利用插值后的相对姿态，将当前点转换到第一个点所在的坐标系，需要注意的是，相对应的欧拉角都要取负值：

  //平移后绕z轴旋转（-rz）float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);float z1 = (pi->z - tz);//绕x轴旋转（-rx）float x2 = x1;float y2 = cos(rx) * y1 + sin(rx) * z1;float z2 = -sin(rx) * y1 + cos(rx) * z1;//绕y轴旋转（-ry）float x3 = cos(ry) * x2 - sin(ry) * z2;float y3 = y2;float z3 = sin(ry) * x2 + cos(ry) * z2;//求出了相对于起始点校正的坐标

将点从第一个点所在坐标系转换到最后一个点所在坐标系：

  rx = transform[0];ry = transform[1];rz = transform[2];tx = transform[3];ty = transform[4];tz = transform[5];//绕y轴旋转（ry）float x4 = cos(ry) * x3 + sin(ry) * z3;float y4 = y3;float z4 = -sin(ry) * x3 + cos(ry) * z3;//绕x轴旋转（rx）float x5 = x4;float y5 = cos(rx) * y4 - sin(rx) * z4;float z5 = sin(rx) * y4 + cos(rx) * z4;//绕z轴旋转（rz），再平移float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;float z6 = z5 + tz;

对不起～～～，我已经被绕晕在这里的坐标转换了～～～～

  //平移后绕z轴旋转（imuRollStart）float x7 = cos(imuRollStart) * (x6 - imuShiftFromStartX) - sin(imuRollStart) * (y6 - imuShiftFromStartY);float y7 = sin(imuRollStart) * (x6 - imuShiftFromStartX) + cos(imuRollStart) * (y6 - imuShiftFromStartY);float z7 = z6 - imuShiftFromStartZ;//绕x轴旋转（imuPitchStart）float x8 = x7;float y8 = cos(imuPitchStart) * y7 - sin(imuPitchStart) * z7;float z8 = sin(imuPitchStart) * y7 + cos(imuPitchStart) * z7;//绕y轴旋转（imuYawStart）float x9 = cos(imuYawStart) * x8 + sin(imuYawStart) * z8;float y9 = y8;float z9 = -sin(imuYawStart) * x8 + cos(imuYawStart) * z8;//绕y轴旋转（-imuYawLast）float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;float y10 = y9;float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;//绕x轴旋转（-imuPitchLast）float x11 = x10;float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;//绕z轴旋转（-imuRollLast）po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;po->z = z11;//只保留线号po->intensity = int(pi->intensity);
}

六、姿态变换函数

本部分涉及到的两个函数均在第八部分的坐标变换中使用。

1. 积累旋转量AccumulateRotation

在得到关于向后两帧的相对旋转之后，需要计算当前帧相对于第一帧的旋转变换，即旋转量的积累。
计算公式： R c u r s t a r t = R l a s t s t a r t ∗ ( R l a s t c u r ) − 1 R_{cur}^{start}=R_{last}^{start}*(R_{last}^{cur })^{-1} Rcurstart​=Rlaststart​∗(Rlastcur​)−1
需要注意的是： R c u r l a s t = R y R x R z R_{cur}^{last}=R_yR_xR_z Rcurlast​=Ry​Rx​Rz​,而 R l a s t s t a r t = R z R x R y R_{last}^{start}=R_zR_xR_y Rlaststart​=Rz​Rx​Ry​
最后求出来(-sin(rx))=cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx) - cos(cx)*cos(lx)*sin(cz)*sin(ly)
而程序中是(-sin(rx))= cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);（程序里的srx=(-sin(rx))）
可以发现两个公式之间差了lx,ly,lz的负号，所以accumulateRotation()函数传入的是transform[0]~[2]的负值.

void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz)
{float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);ox = -asin(srx);float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));oy = atan2(srycrx / cos(ox), crycrx / cos(ox));float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
}

2.修正当前点欧拉角PluginIMURotation

其实修正的原理并不复杂,利用公式 ( R c u r s t a r t ) ′ = R e n d R s t a r t − 1 R c u r s t a r t (R_{cur}^{start})'=R_{end}R_{start}^{-1}R_{cur}^{start} (Rcurstart​)′=Rend​Rstart−1​Rcurstart​即可，注意 R e n d R_{end} Rend​与 R s t a r t − 1 R_{start}^{-1} Rstart−1​均是zxy旋转。主要是推导烦人，我这里就不展示了。

//利用IMU修正旋转量，根据起始欧拉角，当前点云的欧拉角修正
void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, float alx, float aly, float alz, float &acx, float &acy, float &acz)
{float sbcx = sin(bcx);float cbcx = cos(bcx);float sbcy = sin(bcy);float cbcy = cos(bcy);float sbcz = sin(bcz);float cbcz = cos(bcz);float sblx = sin(blx);float cblx = cos(blx);float sbly = sin(bly);float cbly = cos(bly);float sblz = sin(blz);float cblz = cos(blz);float salx = sin(alx);float calx = cos(alx);float saly = sin(aly);float caly = cos(aly);float salz = sin(alz);float calz = cos(alz);float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);acx = -asin(srx);float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);acy = atan2(srycrx / cos(acx), crycrx / cos(acx));float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*cblx*salz*sblz);float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - calx*calz*cblx*sblz);acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));
}