2021SC@SDUSC

ImageProjection_原理分析

1. IMU-物体姿态计算

​ 物体姿态计算是由cloudHandle()中的deskewInfo()中的imuDeskewInfo()完成的。首先，由上一篇blog，我们可以得知，imu和姿态有关的数据是方向（用四元数(Quaternion)表示），角速度来计算。下面开始用这2个数据，来计算当前物体的姿态。

(1) lidar帧初始姿态角计算

​ lidar帧初始姿态角是由imu数据中的方向（用四元数(Quaternion)表示）计算得出。虽然，在代码中，用了以下几行代码就可以轻易计算得出，但其中的数学原理，我们也可以了解下。

template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
    
    double imuRoll, imuPitch, imuYaw;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);

    *rosRoll = imuRoll;
    *rosPitch = imuPitch;
    *rosYaw = imuYaw;
}	

欧拉角（RPY角）

​ 这里要接触2个概念，四元数和欧拉角。四元数在上一篇blog已经介绍了，下面介绍一下欧拉角。

​ 莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于任何参考系，一个刚体的取向，是依照顺序，从这参考系，做三个欧拉角的旋转而设定的。所以，刚体的取向可以用三个基本旋转矩阵来决定。换句话说，任何关于刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。

​ 维基百科定义：

​ 对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系（固定系）又称为实验室参考系，是静止不动的。而坐标系（固连系）则固定于刚体，随着刚体的旋转而旋转。

参阅下图。设定xyz-轴为参考系的参考轴。称xy-平面与XY-平面的相交为交点线，用英文字母（N）代表。zxz顺规的欧拉角可以静态地这样定义：

• $\alpha$（进动角)是x-轴与交点线的夹角，
• $\beta$（章动角)是z-轴与Z-轴的夹角，
• $\gamma$（自旋角)是交点线与X-轴的夹角。

对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴。

实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述，或用不同的名字表示同样的欧拉角。因此，使用欧拉角前，必须先做好明确的定义。

​ 通俗解释一下：描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转$\gamma$，然后绕{A}的Y轴旋转$\beta$，最后绕{A}的Z轴旋转$\alpha$，就能旋转到当前姿态。可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。如下图演示：

Roll:横滚

Pitch: 俯仰

Yaw: 偏航（航向）

旋转矩阵

前面提到，设定刚体取向的旋转矩阵$[\mathbf{R}]$是由三个基本旋转矩阵合成的：
$$[\mathbf{R}]=\left[\begin{array}{ccc}\cos \gamma & \sin \gamma & 0\\ -\sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \beta & \sin \beta \\ 0& -\sin \beta & \cos \beta \end{array}\right]\left[\begin{array}{ccc}\cos \alpha & \sin \alpha & 0\\ -\sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]$$
从左到右依次代表绕着z轴的旋转、绕着交点线的旋转、绕着Z轴的旋转。

经过一番运算,
$$[\mathbf{R}]=\left[\begin{array}{ccc}\cos \alpha \cos \gamma -\cos \beta \sin \alpha \sin \gamma & \sin \alpha \cos \gamma +\cos \beta \cos \alpha \sin \gamma & \sin \beta \sin \gamma \\ -\cos \alpha \sin \gamma -\cos \beta \sin \alpha \cos \gamma & -\sin \alpha \sin \gamma +\cos \beta \cos \alpha \cos \gamma & \sin \beta \cos \gamma \\ \sin \beta \sin \alpha & -\sin \beta \cos \alpha & \cos \beta \end{array}\right]$$
$[\mathbf{R}]$的逆矩阵是：
$$[\mathbf{R}{]}^{-1}=\left[\begin{array}{ccc}\cos \alpha \cos \gamma -\cos \beta \sin \alpha \sin \gamma & -\cos \alpha \sin \gamma -\cos \beta \sin \alpha \cos \gamma & \sin \beta \sin \alpha \\ \sin \alpha \cos \gamma +\cos \beta \cos \alpha \sin \gamma & -\sin \alpha \sin \gamma +\cos \beta \cos \alpha \cos \gamma & -\sin \beta \cos \alpha \\ \sin \beta \sin \gamma & \sin \beta \cos \gamma & \cos \beta \end{array}\right]$$

四元数与RPY角的关系

​ 绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为$\vec{K}=[k_x,k_y,k_z{]}^T$，等效旋转角为$\theta$，则四元数$q=(x,y,z,w)$，其中：
$$\begin{gathered} x=k_x\cdot \sin (\frac{\theta }{2}) \\ y=k_y\cdot \sin (\frac{\theta }{2}) \\ z=k_z\cdot \sin (\frac{\theta }{2}) \\ w=\cos (\frac{\theta }{2}) \end{gathered}$$
且有 $x^2+y^2+z^2+w^2=1$

即四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。

四元数转换为旋转矩阵：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2y^2-2z^2& 2(xy-zw)& 2(xz+yw)\\ 2(xy+zw)& 1-2x^2-2z^2& 2(yz-xw)\\ 2(xz-yw)& 2(yz+xw)& 1-2x^2-2y^2\end{array}\right]$$

已知旋转矩阵为：

则对应的四元数为：

(2) 当前时刻旋转角的计算

​ 旋转角的的计算较为简单，因为imu的信息里面有关角速度的数据。因此，当前时刻的旋转角度可以通过角速度×时间计算得出。角速度的信息提取如下：

template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
    
    *angular_x = thisImuMsg->angular_velocity.x;
    *angular_y = thisImuMsg->angular_velocity.y;
    *angular_z = thisImuMsg->angular_velocity.z;
}

​ 我们设一开始的旋转角度为0。然后，建立一个缓存的地方存放每个帧的旋转角。那么，当前时刻的旋转角就等于，前一时刻旋转角 + 角速度 * 时差。时间差也是有已经缓存的时间信息。然后再把计算得到的旋转角度放入缓存中，等待之后的计算。代码如下：

// imu帧间时差
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
// 当前时刻旋转角 = 前一时刻旋转角 + 角速度 * 时差
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;

2. 里程计计算

​ 物体的里程计计算是由cloudHandle()中的deskewInfo()中的odomDeskewInfo()完成的。这个函数的主要功能为遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿，并用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量。下面开始讲解技术要点。

（1）位姿计算

​ 位姿的计算和上面的四元数与欧拉角的相互转换相同。参考上面的理论介绍即可。

// 提取imu里程计姿态角
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);

double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

// 用当前激光帧起始时刻的odom，初始化lidar位姿，后面用于mapOptmization
cloudInfo.odomX = startOdomMsg.pose.pose.position.x;
cloudInfo.odomY = startOdomMsg.pose.pose.position.y;
cloudInfo.odomZ = startOdomMsg.pose.pose.position.z;
cloudInfo.odomRoll  = roll;
cloudInfo.odomPitch = pitch;
cloudInfo.odomYaw   = yaw;
cloudInfo.odomResetId = (int)round(startOdomMsg.pose.covariance[0]);

（2）坐标系之间的转换 - 仿射变换

​ 在用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量的时候，就涉及到了几何学的知识，需要在2个不同的坐标系之间进行变换。

Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);

tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);

Eigen::Affine3f transBt = transBegin.inverse() * transEnd;

float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);

​ 这里其实是计算起止时刻imu里程计的相对变换，并且提取增量平移，旋转（欧拉角）。这里的相对变换其实是用了仿射变换。因此，我们需要了解仿射变换的相关知识。

仿射变换

​ 仿射变换（Affine transformation），又称仿射映射，是指在几何中，对一个向量空间进行一次线性变换并接上一个平移，变换为另一个向量空间。一个对向量$\vec{x}$平移$\vec{b}$，与旋转放大缩小$\vec{A}$ 的仿射映射为
$$\vec{y}=A\vec{x}+\vec{b}$$
​ 上式在其次坐标上，等价于下面的式子
$$\left[\begin{array}{c}\vec{y}\\ 1\end{array}\right]=\left[\begin{array}{cc}A& \vec{b}\\ 0,...,0& 1\end{array}\right]\left[\begin{array}{c}\vec{x}\\ 1\end{array}\right]$$
​ 在分形的研究里，收缩平移仿射映射可以制作具有自相似性的分形。

​ 简单来讲，“仿射变换”就是：“线性变换”+“平移”。仿射变换从几何直观只有两个要点：

• 变换前是直线，变换后也应该是直线

• 直线比例保持不变

​ 那么什么是线性变换呢？

线性变换

​ 在数学中，线性映射（有的书上将“线性变换”作为其同义词，有的则不然）是在两个向量空间（包括由函数构成的抽象的向量空间）之间的一种保持向量加法和标量乘法的特殊映射。线性映射从抽象代数角度看是向量空间的同态，从范畴论角度看是在给定的域上的向量空间所构成的范畴中的态射。

​ 简单来讲，线性变换就是只含有向量的加法或标量的乘法的一种运算。线性变换从几何直观有三个要点：

• 变换前是直线的，变换后依然是直线
• 直线比例保持不变
• 变换前是原点的，变换后依然是原点

比如说旋转：

比如说推移：

这两个叠加也是线性变换：

在上面通俗地理解了线性变换之后，可以看看它的数学定义：

​ 设$V$和$W$是在相同域*$K$上的向量空间。法则$f:V\to W$被称为是线性映射，如果对于$V$中任何两个向量$x$和$y$与$K$*中任何标量$a$，满足下列两个条件：

可加性： $f(x+y)=f(x)+f(y)$
齐次性： $f(ax)=af(x)$

这等价于要求对于任何向量$x_1,\dots ,x_m$和标量$a_1,\dots ,a_m$，方程

$$f(a_1{x}_1+\cdots +a_m{x}_m)=a_{1}f(x_1)+\cdots +a_{m}f(x_m)$$
成立。

通过线性变换来完成仿射变换

什么意思？继续举例子：

这样我就可以在三维空间下通过$\left[\begin{array}{cc}A& \vec{b}\\ 0& 1\end{array}\right]$ 这个线性变换来操作$z=1$ 平面上的二维正方形，完成仿射变换：

自己动手操作一下：

我们平移到需要的位置的时候：

如果还有没有清楚的地方，可以结合之前的描述，看一下维基百科“仿射变换”词条里的一个gif动图，非常生动的表明了这一过程：

仿射变换在代码中的使用

​ 在代码中，我找到不到pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw)的源代码，也找不到头文件中找不到实际的源码，只有对这个函数的描述：

Create a transformation from the given translation and Euler angles (XYZ-convention)

从给定的平移和欧拉角创建一个变换（XYZ-公约）。

​ 但是按照我个人的理解，这个函数pcl::getTransformation (float x, float y, float z, float roll, float pitch, float yaw),最后得到的是描述这个平面的一个变换矩阵。然后用这个方法得到两个这样的矩阵$A$和$B$，矩阵之间能用这么一个式子来转换。
$$A\cdot T=B$$
​ 然后，只需要求出T就能求得它们的2个平面的点的转换关系。因此，可得下面的式子：
$$T=A^{T}B$$
​ 在上面的代码中，变量transBt就是这么一个T。

3. 降采样

​ 在代码中，检测激光雷达数据的时候有一个检测降采样的判断。而降采样对我来说，也是一个完全陌生的东西，因此，打算了解一下什么是降采样。下面是维基百科对降采样的描述。

在数位信号处理领域中，降采样，又作减采集,是一种多速率数字信号处理的技术或是降低信号采样率的过程，通常用于降低数据传输速率或者数据大小。

​ 不过，这个降采样应该是其他部分处理得，等遇到了我们再深入研究。

4. 激光运动矫正

​ 激光运动矫正也是用上面提到的仿射变换来利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿。

​ 首先计算当前帧相对于起始帧的旋转增量（因为初始帧的旋转设为0，所以增量就是当前的旋转的数值）。这个计算较为简单，要分2种情况讨论。

1. 要计算的激光雷达帧的时间 >= 最新的IMU数据的时间。这时候激光雷达帧的旋转增量就是最近的IMU的旋转的增量。

2. 要计算的激光雷达帧的时间 < 最新的IMU数据的时间。这时候假设2个IMU数据帧之间的旋转增量是线性变化的，激光雷达帧的旋转增量就是利用激光雷达帧时间的最近的前后2个IMU数据帧，通过计算时间加权得出。

   int imuPointerBack = imuPointerFront - 1;
   double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
   double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
   *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
   *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
   *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
   

至于后面利用得到的旋转增量和平移增量进行仿射变换，把当前的激光雷达数据帧变换到初始位置的数据帧则使用到前面的仿射变换的知识。

引用

四元数与欧拉角（RPY角）的相互转换 - XXX已失联 - 博客园

欧拉角 - 维基百科，自由的百科全书

(50 封私信 / 80 条消息) 如何通俗地讲解「仿射变换」这个概念？ - 知乎

仿射变换 - 维基百科，自由的百科全书

降采样 - 维基百科，自由的百科全书