lidarSLAM-第三次作业

本文是我在学习深蓝学院的激光slam课程第三次作业的笔记。本次作业第一题是通过代码实践课上学习到的用里程计辅助去除激光扫描数据畸变的方法；第二题是补充一下课上提到的 ICP 算法的原理推导；第三题是复习激光雷达的测距原理；第四题是设计用 IMU 去除运动畸变的思路。

结合里程计数据去除激光雷达运动畸变

作业里已经包括了代码大致框架，只需要补充其中一个函数即可。首先还是来学习一下代码结构，具体推导过程见前文 _激光雷达运动畸变去除_。

代码结构

思路和上一次作业差不多，main() 初始化节点和一个用于矫正激光雷达运动畸变的实例 tmpLidarMotionCalib，由该实例完成全部工作。

LidarMotionCalibrator 通过一个 Subscriber 订阅一个自定义的激光扫描类型 ChampionNavLaserScan 话题 champion_scan 来启动 callback 函数，完成后续流程，看了一下 ChampionNavLaserScan 内部，本次作业应该要用到的是扫描时间 scan_time， 距离 ranges 和角度angles 。

每次收到一帧激光数据时节点会进入 ScanCallBack() 函数，首先通过开始时间和激光束数量获得当前激光帧起止时间，然后将距离和角度数据复制一份用于校正，并将校正前的扫描数据计算成 xyz 坐标（因为是单线激光雷达，这里 z 设为 0）通过 pcl 可视化， 接着进入 Lidar_Calibration() 进行校正。

Lidar_Calibration() 函数主要是将当前激光帧按照一定时间间隔进行分段（这里是取 5ms），每一段分别通过 getLaserPose() 方法取得当前段的起止激光束对应的位姿，通过 Lidar_MotionCalibration() 进行分段线性插值校正。

getLaserPose() 通过 tf_->waitForTransform("/odom", "/base_laser", dt, ros::Duration(0.5)) 找到激光数据到里程计的位姿转换， 并通过 tf_->transformPose("/odom", robot_pose, odom_pose)求出转换后的位姿返回。

Lidar_MotionCalibration() 为要补充的函数，在下一部分记录。

补充线性插值函数

通过上一部分的分析，Lidar_MotionCalibration()的思路很明确只需要将给定的一段激光束数据通过起止时间以及对应的位姿进行插值求出该段内每个激光束的位姿，并将其转换到 frame_base_pose 坐标系上，具体代码如下：

/**
 * @brief Lidar_MotionCalibration
 *        激光雷达运动畸变去除分段函数;
 *        在此分段函数中，认为机器人是匀速运动；
 * @param frame_base_pose       标定完毕之后的基准坐标系
 * @param frame_start_pose      本分段第一个激光点对应的位姿
 * @param frame_end_pose        本分段最后一个激光点对应的位姿
 * @param ranges                激光数据－－距离
 * @param angles                激光数据－－角度
 * @param startIndex            本分段第一个激光点在激光帧中的下标
 * @param beam_number           本分段的激光点数量
 */
void Lidar_MotionCalibration(
        tf::Stamped<tf::Pose> frame_base_pose,
        tf::Stamped<tf::Pose> frame_start_pose,
        tf::Stamped<tf::Pose> frame_end_pose,
        std::vector<double>& ranges,
        std::vector<double>& angles,
        int startIndex,
        int& beam_number)
{
   // get the transfomation from start/end to base
   tf::Pose baseToworld = frame_base_pose.inverse();
   tf::Pose startTobase = baseToworld * frame_start_pose;
   tf::Pose endTobase = baseToworld * frame_end_pose;

   // get translation and rotation for start/end pose
   tf::Vector3 startTranslation = startTobase.getOrigin();
   tf::Vector3 endTranslation = endTobase.getOrigin();

   tf::Quaternion startRotation = startTobase.getRotation();
   tf::Quaternion endRotation = endTobase.getRotation();

   int index = startIndex;
   for (int i = 0; i < beam_number; i++) {
        // linear interpolate translation and rotation
        tf::Vector3 currTrans = startTranslation.lerp(endTranslation, i / (1.0 * beam_number - 1));
        tf::Quaternion currRot = startRotation.slerp(endRotation, i / (1.0 * beam_number - 1));

        // get the scan point coordinate before calibration, set Z to 0.00
        float currX = ranges[index] * cos(angles[index]);
        float currY = ranges[index] * sin(angles[index]);
        tf::Vector3 currXYZ(currX, currY, 0.00);

        // transform coordinates to base coordinates
        tf::Transform currTobase;
        currTobase.setOrigin(currTrans);
        currTobase.setRotation(currRot);
        tf::Vector3 baseCoordinates = currTobase * currXYZ;

        // update scan data point
        ranges[index] = sqrt(baseCoordinates[0] * baseCoordinates[0] + baseCoordinates[1] * baseCoordinates[1]);
        angles[index] = atan2(baseCoordinates[1], baseCoordinates[0]);
        index++;
   }
}

运行结果如下，可以看出校正效果比较好，校正后的激光显示出来的走廊轮廓比较稳定。

推导已知对应点的 ICP 求解方法

题目要求：阅读论文 Least-Squares Fitting of Two 3-D Points Sets,推导并证明已知对应点的 ICP 求解方法。

这里主要是归纳一下文章的思路和推导过程。

首先，要解决的问题是：给定两组点 $\{p_i\}$ 和 $\{p’_i\}$，假设他们符合以下关系，其中 $R$ 是 3x3 的旋转矩阵， T 是 3x1 的平移向量， $N_i$ 是噪声：

$$p_i' = Rp_i + T + N_i$$

我们需要找到一个找到一个 R 和 T 使得下式结果最小化：

$$\Sigma^2 = \sum_{i=1}^N||p_i' - (Rp_i + T)||^2$$

作者的思路是首先想办法将问题解耦成分别求 R 和 T，使问题简单化，解耦过程如下：

根据已有结论（在另一篇文章里），假设 $\hat{R}$ 和 $\hat{T}$ 使得要求的的最优解，那么 $\{p_i’\}$ 和 $\{p_i’’\triangleq \hat{R}p_i + T_i \}$ 两组点会有一样的重心（这里表现是几何中心），即满足下列等式：

$$\begin{aligned} p'&\triangleq \frac{1}{N}\sum_{i = 1}^{N}p_i'\\ p &\triangleq \frac{1}{N}\sum_{i = 1}^{N}p_i\\ p'' &\triangleq \frac{1}{N}\sum_{i = 1}^N p_i'' = \hat{R}p+\hat{T}\\ \rightarrow p' &= p'' \end{aligned}$$

接下来我们令：

$$\begin{aligned} q_i &\triangleq p_i - p\\ q_i' &\triangleq p_i' - p' \end{aligned}$$

则有：

$$\begin{aligned} p_i' - (Rp_i + T) &= q_i' - (\hat{R}q_i + \hat{T} + \hat{R}p) + p' \\ &= q_i' - \hat{R}q_i + (p' - (\hat{R}p + \hat{T}))\\ &= q_i' - \hat{R}q_i - (p' - p'')\\ &= q_i' - \hat{R}q_i \end{aligned}$$

所以要求解的问题就变成令下式最小

$$\Sigma^2 = \sum_{i=1}^N||q_i' - Rq_i||^2$$

这样问题就变成两个子问题：

• 求解一个旋转矩阵 $\hat{R}$ 令上式最小；
• 通过 $\hat{T} = p’ - \hat{R}p$ 求解平移向量。

作者在第三部分分别给出了求解算法和推导过程，这边只讲推导过程具体算法可以参考我的上一篇博文：

首先将上式展开：

$$\begin{aligned} \Sigma^2 &= \sum_{i = 1}^N(q'_i - Rq_i)^T(q_i' - Rq_i)\\ &= \sum_{i = 1}^N(q_i'^Tq_i' + q_i^TR^TRq_i - q_i'^TRq_i - q_i^TR^Tq_i')\\ &\overset{R^TR = I}{=} \sum_{i = 1}^N(q_i'^Tq_i' + q_i^Tq_i - 2q_i'^TRq_i) \end{aligned}$$

因为前两项与R无关，所以令上式最小等效于令下式最大，$H = \sum_{i = 1}^N(q_i’^Tq_i)$：

$$\begin{aligned} F &= \sum_{i = 1}^Nq_i'^TRq_i\\ &= Trace(\sum_{i = 1}^NRq_i'q_i^T)\\ &= Trace(RH) \end{aligned}$$

这里引入一条引理(这里不证了)：对于任何正定矩阵 $AA^T$ 和 任何正交矩阵 $B$，满足以下不等式：

$$Trace(AA^T) \geq Trace(BAA^T)$$

令 $H$ 的 SVD 分解如下：

$$H = U\Lambda V^T$$

令 $X = VU^T$, 容易知道 $X$ 也是正交的

则有：

$$\begin{aligned} XH &= VU^TU\Lambda V\\ &= V\Lambda V^T \end{aligned}$$

这个矩阵显然是正定的，从上述引理可得，对于任何一个正交矩阵 $B$， 都有：

$$Trace(XH) \geq Trace(BXH)$$

所以可以知道对于我们要求的式子， $X$ 使 $F$ 最大化， 假如 $det(X) = 1$就是我们想取得的 $\hat{R}$， 否则它就是仿射变换矩阵，但是这种情况通常不会发生，作者在后半部分论证了这部分，这里就不赘述了。

简述激光测距原理

1. 阅读论文 Precise indoor localization for mobile laser scanner 前两章，简述激光雷达测距原理；
2. 简要介绍一下下图的含义。

这一部分的答案大部分都在我的上一篇笔记记录过，这里不再重复。

使用 IMU 去除激光雷达运动畸变的方法

1. 仅用 IMU 去除运动畸变可能会有哪些不足之处?
2. 在仅有 IMU 和激光雷达传感器的情况下,你会如何设计运动畸变去除方案(平移+旋转),达到较好的畸变去除效果?

使用 IMU 去除运动畸变的思路同样可以参考运用里程计的方法，通过 IMU 来计算出一个位姿然后，然后同样将一帧数据分段每段取起止时间的对应位姿，然后对应激光束的位姿插值并转换到统一的坐标系下（该帧起始位姿坐标系），但是不足之处也很明显：IMU 的线加速度精度太低，过一段时间很容易偏移。
改进方法可以是只用 IMU 进行旋转部分校正，平移部分根据匀速假设进行校正，将校正后的结果于原结果相比得到一个误差然后均分到每一个时刻的 IMU 结果上作为 IMU 的误差，重新修正，直至误差小至合理范围内（收敛为止）。