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

一、三维空间的位姿描述和齐次变换首先规定一个坐标系，相对于该坐标系，点的位置可以用3维列向量表示;刚体的方位可用3×3的旋转矩阵来表示。而4×4的齐次变换矩阵则可将刚体位置和姿态（位姿）的描述统一起来，它具有以下优点：(1) 它可描述刚体的位姿，描述坐标系的相对位姿（描述）。(2) 它可表示点从一个坐标系的描述转换到另一坐标系的描述（映射）。(3)它可表示刚体运动前、...

cjn_

10625人浏览 · 2018-09-05 19:27:42

cjn_ · 2018-09-05 19:27:42 发布

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

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

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

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

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

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

$_{}^{A}\textrm{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轴旋转β角。

绕定轴分解示意图：

将Odo坐标系先绕X轴（即Xo轴）转顺时针转90°（顺时针为负，顺时针还是逆时针需要把轴朝向自己后再判断），然后绕原Y轴 （即Yo轴）转0°，最后绕原Z轴（即Zo轴）顺时针转90°，可以转到Cam坐标系下。

所以旋转矩阵按照定轴旋转方式分解为欧拉角可以表示为：

用四元数表示为：

 Eigen::Vector3d EulerAngles_OC(-1.57, 0, -1.57);
    Eigen::Vector3d t_OC(0.15, 0, 1);
    Eigen::Matrix3d R_OC;

    R_OC = Eigen::AngleAxisd(EulerAngles_OC[2], Eigen::Vector3d::UnitZ()) *
           Eigen::AngleAxisd(EulerAngles_OC[1], Eigen::Vector3d::UnitY()) *
           Eigen::AngleAxisd(EulerAngles_OC[0], Eigen::Vector3d::UnitX());

    Eigen::Vector3d EulerAngles_OC1(0.72, 0, 0);
    Eigen::Matrix3d R_OC1;

    R_OC1 = Eigen::AngleAxisd(EulerAngles_OC1[2], Eigen::Vector3d::UnitZ()) *
            Eigen::AngleAxisd(EulerAngles_OC1[1], Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(EulerAngles_OC1[0], Eigen::Vector3d::UnitX());


    Eigen::Matrix4d T_OC = Eigen::Matrix4d::Identity();
    T_OC.block<3, 3>(0, 0) = R_OC * R_OC1;
    T_OC.block<3, 1>(0, 3) = t_OC;

    Eigen::Vector3d t = T_OC.block<3, 1>(0, 3);
    Eigen::Vector3d r = T_OC.block<3, 3>(0, 0).eulerAngles(2,1,0);
    Eigen::Quaterniond quaternion(T_OC.block<3, 3>(0, 0));
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(t[0], t[1], t[2]));
    tf::Quaternion q(quaternion.x(),quaternion.y(),quaternion.z(),quaternion.w());
//    q.setRPY(r[0], r[1], r[2]);
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "camera23"));

二、旋转角与四元数之间的转换

四元数（Quaternions）是由爱尔兰数学家哈密顿（William Rowan Hamilton）在1843年提出。

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量(x,y,z)来表示，θ可以用一个角度值来表示，直观来讲，一个四维向量(θ,x,y,z)(θ,x,y,z)就可以表示出三维空间任意的旋转。注意，这里的三维向量(x,y,z)只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值θ。这样以来，可以用一个三维向量(θ∗x,θ∗y,θ∗z)就可以表示出三维空间任意的旋转，前提是其中(x,y,z)是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

单位向量(x,y,z)旋转θ角度后的四元数：(cosθ/2, x*sinθ/2, y*sinθ/2, z*sinθ/2)
对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

首先是轴角到四元数：

给定一个单位长度的旋转轴(x, y, z)和一个角度θ。对应的四元数为：q=((x,y,z)sinθ/2, cosθ/2)

欧拉角到四元数：

给定一个欧拉旋转(X, Y, Z)（即分别绕x轴、y轴和z轴旋转X、Y、Z度），则对应的四元数为：

x = sin(Y/2)sin(Z/2)cos(X/2)+cos(Y/2)cos(Z/2)sin(X/2)
y = sin(Y/2)cos(Z/2)cos(X/2)+cos(Y/2)sin(Z/2)sin(X/2)
z = cos(Y/2)sin(Z/2)cos(X/2)-sin(Y/2)cos(Z/2)sin(X/2)
w = cos(Y/2)cos(Z/2)cos(X/2)-sin(Y/2)sin(Z/2)sin(X/2)

q = ((x, y, z), w)

注：q和-q表示同一个旋转

欧拉角奇异性：https://blog.csdn.net/lipi37/article/details/83871031

万向节死锁：https://blog.csdn.net/AndrewFan/article/details/60981437#

三、eigen 代码

1、表示：

旋转矩阵直接使用 Matrix3d 或 Matrix3f：

Eigen::Matrix3d rotationMatrix=Eigen::Matrix3d::Identity();//初始化为一个单位阵。

旋转向量使用 AngleAxis：

Eigen::AngleAxisd rotationVector(M_PI/4,Eigen::Vector3d(0,0,1)); //角+轴：沿 Z 轴旋转 45 度

欧拉角：

Eigen::Vector3d ea0(yaw,pitching,droll);

2、转化：

旋转向量->旋转矩阵：rotationMatrix=rotation_vector.toRotationMatrix();

旋转向量->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );

旋转矩阵->四元数：Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_matrix );

四元素->旋转矩阵：Eigen::Matrix3d Rx = q.toRotationMatrix();

旋转向量->欧拉角：Eigen::Vector3d eulerAngle=rotationVector.matrix().eulerAngles(0,1,2);

旋转矩阵->欧拉角：Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}