精选使用Eigen计算三维坐标变换

wangpenghnu

分类：学习笔记

发布时间 2023.05.23阅读数 3176 评论数 0

前言

本文讲述物体在三维空间的位姿表示与变换，欧拉角、四元数、旋转矩阵都是表示两个坐标系相对旋转关系，欧拉角有歧义，旋转矩阵有冗余，四元数不易理解，介绍了这三者相互转换公式以及如何使用Eigen库计算。具体原理可以参见《视觉SLAM十四讲》。

一、三维空间的位姿描述和齐次变换

首先规定一个坐标系，相对于该坐标系，点的位置可以用3维列向量表示;刚体的方位可用3×3的旋转矩阵来表示。而4×4的齐次变换矩阵则可将刚体位置和姿态（位姿）的描述统一起来，它具有以下优点：

(1) 它可描述刚体的位姿，描述坐标系的相对位姿（描述）。

(2) 它可表示点从一个坐标系的描述转换到另一坐标系的描述（映射）。

(3)它可表示刚体运动前、后位姿描述的变换（算子）。

1.位置描述——位置矢量

$A_{p} = \begin{bmatrix} P_x\\ P_y\\ P_z \end{bmatrix}$

2.方位的描述——旋转矩阵

经常用到的旋转变换矩阵是绕X轴、绕Y轴或绕Z轴转一角度 $\theta$ 。它们是

$R(X, \theta) = \begin{bmatrix} 1 & 0 &0 \\ 0 & \cos\theta& -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{bmatrix}$

$R(Y, \theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$

$R(Z, \theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}$

向量a经过一次旋转R和一次平移t后，得到a':

非齐次表述

$a^{'} = R * a + t$

齐次表述

$\begin{bmatrix} a^{'}\\1 \end{bmatrix} = \begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix}\begin{bmatrix} a\\1 \end{bmatrix} = T\begin{bmatrix} a\\ 1 \end{bmatrix}$

通用旋转矩阵R为：

$R = R_z(\beta)R_y(\alpha)R_x(\theta)$

这里的旋转矩阵旋转过程，先绕X轴旋转θ角，然后绕Y轴旋转α角，最后绕Z轴旋转β角。

二、欧拉角、四元数、旋转矩阵之间的转换

内旋(intrinsic rotation)和外旋(extrinsic rotation)：内旋的每个elemental绕的是object space basis的轴，外旋的每个elemental rotation绕的是world space（惯性系）basis的轴。

一般欧拉角都使用的是 Z − Y − X的内旋方式表示两个坐标系之间的转换关系。

1.欧拉角转旋转矩阵

假设参考坐标系O−XrYrZr，和本体坐标系 O−XbYbZb,它们之间成一定角度，如果用欧拉角描述这个旋转关系的时候一定要说明旋转的顺序和旋转方式，旋转方式又分为外旋和内旋两种情况。
1.内旋：参考坐标系O−XrYrZr绕 Xr轴旋转α角度，然后再绕着新的Yr轴旋转β角，最后再绕着新的Zr轴旋转γ角度后与本体坐标系重合。
这三种旋转每次的旋转矩阵为分别为：
$C_x = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\alpha & \sin\alpha\\ 0 & -\sin\alpha & \cos\alpha \end{bmatrix}$

$C_y = \begin{bmatrix} \cos\beta & 0 & -\sin\beta\\ 0 & 1 & 0\\ \sin\beta & 0 & \cos\beta \end{bmatrix}$

$C_z = \begin{bmatrix} \cos\gamma & \sin\gamma & 0\\ -\sin\gamma & \cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix}$

那么参考坐标系到本体坐标系之间的转换矩阵为

$C_{r}^{b} = C_z C_y C_x$

本体坐标系到参考坐标系之间的转换矩阵是它的转置。

$C_b^r = (C_r^b)^T$

当然，如果你不按照这个顺序转动，比如，参考坐标系转动顺序为Zr−Yr−Xr时的参考坐标系到本体坐标系之间的转换矩阵就是：

$C_r^b = C_x C_y C_z$

2外旋：参考坐标系O−XrYrZr绕Xr轴转动α角度，然后再绕着原来的Yr轴旋转β角，最后绕着最初的 Zr轴旋转γ角度与本体坐标系重合。在这种情况下三次旋转，每次的旋转矩阵分别为：

$C_x = \begin{bmatrix} 1 & 0 & 0\\ 0 & \cos\alpha & -\sin\alpha\\ 0 & \sin\alpha & \cos\alpha \end{bmatrix}$

$C_y = \begin{bmatrix} \cos\beta & 0 & \sin\beta\\ 0 & 1 & 0\\ -\sin\beta & 0 & \cos\beta \end{bmatrix}$

$C_z = \begin{bmatrix} \cos\gamma & -\sin\gamma & 0\\ \sin\gamma & \cos\gamma & 0\\ 0 & 0 & 1 \end{bmatrix}$

由于旋转方式不同，每次旋转的旋转矩阵也不同，这种旋转方式下参考坐标系到本体坐标系之间的转换矩阵为：

$C_{r}^{b} = C_z C_y C_x$

本体坐标系到参考坐标系之间的转换矩阵依然是它的转置。内旋方式下按照某一顺序的旋转矩阵等于外旋方式下按照反着顺序来的旋转矩阵，比如说参考坐标系内旋顺序为Xr−Yr−Zr时的旋转矩阵 $C_b^r$ 等于参考坐标系外旋顺序为Zr−Yr−Xr时的旋转矩阵 $C_b^r$ 。

2.旋转矩阵转欧拉角

对于一个旋转矩阵而言，不同的旋转顺序和旋转方式对应不同的欧拉角，旋转矩阵唯一，而欧拉角不唯一，这里给出最常用的一种转换关系。
参考坐标系为O−XrYrZr，本体坐标系为O−XbYbZb。
如果已知参考坐标系到本体坐标系的旋转矩阵为：

$C_r^b = \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix}$

旋转矩阵转欧拉角的转换公式为：

$\alpha = \arctan(\frac{r_{23}}{r_{33}})$

$\beta = \arctan(-\frac{r_{13}}{\sqrt{r_{23}^2+r_{33}^2}})$

$\gamma = \arctan(\frac{r_{12}}{r_{11}})$

此时计算出来的欧拉角γ,β,α的含义就是参考坐标系以内旋的方式按照Zr−Yr−Xr的顺序分别旋转 γ,β,α角，这是旋转矩阵和欧拉角的对应关系，这个对应关系很重要，比如，如果说计算的时候使用的旋转矩阵是而不是，那再使用这个转换公式计算出来的欧拉角就不是这个含义了，这一点值得注意。

3.旋转矩阵转四元数

  参考坐标系为O−XrYrZr，本体坐标系为O−XbYbZb。
        四元数 $q_r^b$ 为参考坐标系到本体坐标系的四元数。
        四元数 $q_b^r$ 为本体坐标系到参考坐标系的四元数。
         $q_r^b$ 与 $q_b^r$ 之间是共轭关系。
        如果我们已知旋转矩阵 $C_r^b$ 。

$C_r^b = \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix}$

那么可以根据计算以下公式计算四元数 $q_r^b$ ， $q_r^b = q_0 + q_1 i + q_2 j + q_3 k$

$q_0 = \frac{1}{2}\sqrt{1+r_{11} + r_{22} +r_{33}}$

$q_1 = \frac{r_{32}-r_{23}}{4q_0}$

$q_2 = \frac{r_{13}-r_{31}}{4q_0}$

$q_3 = \frac{r_{21}-r_{12}}{4q_0}$

4.四元数转旋转矩阵

如果我们已知四元数 $q_r^b$ ,旋转矩阵 $C_r^b$ 的计算公式如下：

$C_r^b = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2 - q_0 q_3) & 2(q_0 q_2 + q_1 q_3)\\ 2(q_0 q_3 + q_1 q_2) & q_0^2+q_2^2-q_1^2-q_3^2 & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_0 q_1 + q_2 q_3) & q_0^2+q_3^2-q_1^2-q_2^2 \end{bmatrix}$

5.四元数转欧拉角

   参考坐标系为O−XrYrZr，本体坐标系为O−XbYbZb。
        四元数 $q_r^b$ 为参考坐标系到本体坐标系的四元数，且 $q_r^b = q_0 + q_1 i + q_2 j + q_3 k$
        使用以下公式得到的欧拉角的含义为：参考坐标系以内旋的方式按照Zr−Yr−Xr的顺序分别旋转γ,β,α角。

$\alpha = \arctan(\frac{2(q_2 q_3-q_0 q_1)}{1-2(q_1^2+q_2^2)})$

$\beta = \arcsin(2(q_0 q_2 + q_1 q_3))$

$\gamma = \arctan(\frac{2(q_1 q_2 - q_0 q_3)}{1-2(q_2^2+q_3^2)})$

6.欧拉角转四元数

参考坐标系为O−XrYrZr，本体坐标系为O−XbYbZb。
已知欧拉角的含义为参考坐标系以内旋的方式按照Zr−Yr−Xr的顺序分别旋转γ,β,α角，那么可以计算得到参考坐标系到本体坐标系的四元数 $q_r^b$ ，其中 $q_r^b = q_0 + q_1 i + q_2 j + q_3 k$

$\begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix} = \begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}-\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix}$

三、Eigen库计算位姿变换

1.旋转向量

旋转角为alpha(顺时针)，旋转轴为(x,y,z)

初始化

Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
 
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());

旋转向量转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();

旋转向量转欧拉角

Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);

旋转向量转四元数

Eigen::Quaterniond quaternion(rotation_vector);
 
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;

2.旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

初始化

Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;

旋转矩阵转旋转向量

Eigen::AngleAxisd rotation_vector(rotation_matrix);
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
 
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);

旋转矩阵转欧拉角

Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);

旋转矩阵转四元数

Eigen::Quaterniond quaternion(rotation_matrix);
 
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;

3.欧拉角

初始化

Eigen::Vector3d eulerAngle(roll,pitch,yaw);

欧拉角转旋转向量

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;

欧拉角转旋转矩阵

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;

欧拉角转四元数

Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
 
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;

4.四元数

初始化

Eigen::Quaterniond quaternion(w,x,y,z);

四元数转旋转向量

Eigen::AngleAxisd rotation_vector(quaternion);
 
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;

四元数转旋转矩阵

Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
 
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();

四元数转欧拉角

Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);
//也可能
//Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
//eulerAngle(0)为yaw
//eulerAngle(1)为pitch
//eulerAngle(2)为roll

4.1Eigen::eulerAngles(2,1,0)有大坑

普通的方法是，用Eigen，把四元数转成旋转矩阵，再从旋转矩阵转到欧拉角：
::Eigen::Quaterniond q(w, x, y, z);
::Eigen::Matrix3d rx = q.toRotationMatrix();
::Eigen::Vector3d ea = rx.eulerAngles(2,1,0);
但这个方法存在问题，即可能转出来的欧拉角，不是想要的，因为因为同一个四元数，可以用2个欧拉角来表示，而这个方法得到的结果有可能是用转角大于2PI的方式表达的。例如，四元数(0.00392036, -0.00511095, -0.613622, 0.789573)应当转为欧拉角(-1.32133, -0.00325971, 0.0124636)，但用Eigen却被转成了(1.82026, -3.13833, -3.12913)。为了避免这个问题，可以使用 Conversion between quaternions and Euler angles 中给出的一个算法（如下），这个算法可以保证出来的欧拉角不会超过2PI。

//四元数转欧拉角
static void toEulerAngle(const Quaterniond& q, double& roll, double& pitch, double& yaw)
{
// roll (x-axis rotation)
double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
roll = atan2(sinr_cosp, cosr_cosp);
 
// pitch (y-axis rotation)
double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
if (fabs(sinp) >= 1)
pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
pitch = asin(sinp);
 
// yaw (z-axis rotation)
double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
yaw = atan2(siny_cosp, cosy_cosp);
}
 
//或者使用pcl库，但精度略微损失
#include <pcl/common/transforms.h>
#include <Eigen/Core>
     
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f tmp(T_utm_lidar.cast<float>());
pcl::getTranslationAndEulerAngles(tmp, x, y, z, roll, pitch, yaw);
 
 
//欧拉角转四元数，这就非常容易：
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
 
Eigen的方式是：
Eigen::Vector3d ea0(yaw,pitch,roll);
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
Eigen::Quaterniond q;
q = R;

5.齐次欧式变换

Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
T.rotate(rotation_vector1);
T.pretranslate(t);
cout<<"齐次欧式变换：\n"<<T.matrix()<<endl;
 
//附两个函数
//取反对称矩阵
Eigen::Matrix3f SkewSymmetric(const Eigen::Vector3f &vec)
{
    Eigen::Matrix3f mat;
    mat << 0.0, -vec(2), vec(1),
    vec(2), 0.0, -vec(0),
    -vec(1), vec(0), 0.0;
    return mat;
}
 
//将微小增量转换为四元数
Eigen::Quaternionf DeltaQ(const Eigen::Vector3f &_vec)
{
    Eigen::Quaternionf quater;
    quater.w() = 1.0;
    quater.x() = _vec(0) / 2;
    quater.y() = _vec(1) / 2;
    quater.z() = _vec(2) / 2;
    return quater;
}