【标定矩阵初始化】将一个坐标从LiDAR系统变换到IMU系统中

发布于：2024-06-24 ⋅ 阅读:(48) ⋅ 点赞:(0)

标定矩阵初始化

计算示例

假设我们已经计算得到了以下信息：

从LiDAR到IMU的旋转矩阵（以四元数转换后的矩阵形式表示）为：
$R_{lidar2imu} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}$
从IMU坐标系观察到的LiDAR坐标系原点的平移向量为：
$t_{in_imu} = \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$

构建转换矩阵

转换矩阵 lidar_to_imu_ 是一个 4x4 的矩阵，它包含了从 LiDAR 坐标系到 IMU 坐标系的完整变换信息。矩阵的结构如下：

第一行：[r_{11}, r_{12}, r_{13}, t_x]，这一行描述了LiDAR坐标系的X轴在IMU坐标系的新方向（由前三项决定），以及从LiDAR坐标系原点沿X轴方向平移到IMU坐标系的分量（第四项）。
第二行：[r_{21}, r_{22}, r_{23}, t_y]，类似地，描述了LiDAR的Y轴在IMU坐标系中的新方向及沿Y轴的平移分量。
第三行：[r_{31}, r_{32}, r_{33}, t_z]，描述了LiDAR的Z轴在IMU坐标系中的新方向及沿Z轴的平移分量。
第四行：[0, 0, 0, 1]，这一行是固定的，其中的1保证了齐次坐标的正确转换，而前三项的0表明没有额外的旋转或平移发生在时间或任何其他非欧几里得维度上。

简而言之，这个矩阵的前三行前三列定义了旋转操作，使得LiDAR坐标系中的一个向量通过右乘这个矩阵后，会按照正确的方向旋转到IMU坐标系中。而前三行的最后一列加上第四行，共同决定了从LiDAR坐标系原点到IMU坐标系中对应点的平移。这样的设计允许我们在一个步骤中同时处理三维空间中的旋转和平移变换。

应用实例

假设你有一个点在LiDAR坐标系里的位置是 (x_L, y_L, z_L)。我们的目标是通过已知的转换矩阵 lidar_to_imu_ 来找出这个点在IMU坐标系中的对应位置 (x_I, y_I, z_I)。

转换矩阵看起来像这样：

$\begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix}$

这个过程就像是一个“魔法公式”，让我们能够同时完成两件事：

旋转：首先，(r_{11}, r_{12}, r_{13})、(r_{21}, r_{22}, r_{23}) 和 (r_{31}, r_{32}, r_{33}) 分别作用于点的(x_L, y_L, z_L)分量上，将点沿着各个轴旋转到IMU坐标系的正确方向上。
平移：然后，t_x, t_y, t_z 直接添加到旋转后的点上，表示从LiDAR坐标系原点到IMU坐标系中相应点的总平移距离。

当我们将LiDAR坐标系下的点表示为一列向量 [x_L, y_L, z_L, 1] 并乘以转换矩阵时，实际上执行了以下计算：

新的X坐标 (x_I) 是：x_L*r_{11} + y_L*r_{21} + z_L*r_{31} + t_x
新的Y坐标 (y_I) 是：x_L*r_{12} + y_L*r_{22} + z_L*r_{32} + t_y
新的Z坐标 (z_I) 是：x_L*r_{13} + y_L*r_{23} + z_L*r_{33} + t_z
最后的1乘以第四行的1确保了齐次坐标的正确传递，我们通常对此不关心，因为它帮助保持了向量的性质但在最终结果中并不影响实际的(x_I, y_I, z_I)坐标值。

所以，通过这一步简单的矩阵乘法操作，我们就轻松地将一个坐标从LiDAR系统变换到了IMU系统中。