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

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

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

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

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

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

 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;
}

原文链接：https://blog.csdn.net/HERO_CJN/article/details/82426372