• 基于Eigen的位姿转换

    目录

    一、概述

    二、公式讲解

    三、Eigen完整代码

    四、新的测试案例

    4.1 案例1

    4.2 案例2

    一、概述

             位姿中姿态的表示形式有很多种，比如：旋转矩阵、四元数、欧拉角、旋转向量等等。这里基于Eigen实现四种数学形式的相互转换功能。本文利用Eigen实现上述四种形式的相互转换。我这里给出一个SE3（4*4）(先平移、再旋转)的构建方法：

    1. Eigen::Isometry3f T1 = Eigen::Isometry3f::Identity(); // 这一句千万别掉
    2. // <1> 初始化R
    3. Eigen::Matrix3f R;
    4. // 按照 ZYX 顺序旋转
    5. R = Eigen::AngleAxisf(3.14159 / 4, Eigen::Vector3f::UnitX()) *
    6.             Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
    7.             Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());
    8. // <2> 初始化t
    9. Eigen::Vector3f t(0.0, 0.0, 4.0);
    10. // <3> 构建T = (R|t)
    11. T1.rotate(R);
    12. T1.pretranslate(t); // 这一句别搞错
    13. //T1.translate(t);
    14. std::cout << "T1 = " <matrix() <

    打印结果：

    1. T1 =         1         0         0         0
    2.             0  0.707107 -0.707106         0
    3.             0  0.707106  0.707107         4
    4.             0         0         0         1

    二、公式讲解

    以下是在本地word中笔记的截图：

    三、Eigen完整代码

    Pose.h(类Pose声明)

    1. #pragma once
    2. #ifndef POSE_H
    3. #define POSE_H
    4.  
    5. #include
    6. #include
    7.  
    8. // namespace Geometry
    9.  
    10. class Pose
    11. {
    12. public:
    13.     Pose();
    14.  
    15.     Pose& operator= (const Pose& pose);
    16.  
    17.     // construct from rotation
    18.     Pose(const Eigen::Matrix3d& rotation);
    19.  
    20.     // construct from quaternion
    21.     Pose(const Eigen::Quaterniond& quaternion);
    22.  
    23.     // construct from angle axisd
    24.     Pose(const Eigen::AngleAxisd& angle_axis);
    25.  
    26.     // construct from euler angle
    27.     Pose(const Eigen::Vector3d& euler_angle);
    28.  
    29.     ~Pose();
    30.  
    31.     // return rotation
    32.     Eigen::Matrix3d rotation() const;
    33.  
    34.     // return quaterniond
    35.     Eigen::Quaterniond quaternion() const;
    36.  
    37.     // return angle axisd
    38.     Eigen::AngleAxisd angle_axis() const;
    39.  
    40.     // return euler angle
    41.     Eigen::Vector3d euler_angle() const;
    42.  
    43. private:
    44.  
    45.     Eigen::Matrix3d rotation_;       // 旋转矩阵
    46.  
    47.     Eigen::Quaterniond quaternion_;  // 四元数
    48.  
    49.     Eigen::AngleAxisd angle_axis_;  // 角轴
    50.  
    51.     Eigen::Vector3d euler_angle_;    // 欧拉角 roll(X轴)  pitch(Y轴) yaw(Z轴)
    52.  
    53. };
    54.  
    55. // 姿态组合
    56. Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2);
    57.  
    58. // 求逆
    59. Eigen::Isometry3d inverse(const Eigen::Isometry3d& T);
    60.  
    61.  
    62. #endif // !POSE_H

    Pose.cpp(类Pose的实现)

    1. #include "Pose.h"
    2.  
    3.  
    4. Pose::Pose()
    5. {}
    6.  
    7.  
    8. Pose& Pose::operator= (const Pose& pose)
    9. {
    10.     this->rotation_    = pose.rotation();
    11.     this->quaternion_  = pose.quaternion();
    12.     this->angle_axis_ = pose.angle_axis();
    13.     this->euler_angle_ = pose.euler_angle();
    14.     return *this;
    15. }
    16.  
    17. //
    18. Pose::Pose(const Eigen::Matrix3d& rotation) :
    19.     rotation_(rotation),
    20.     quaternion_(Eigen::Quaterniond(rotation_)),
    21.     angle_axis_(Eigen::AngleAxisd(rotation_)),
    22.     euler_angle_(rotation_.eulerAngles(0, 1, 2))
    23. {}
    24.  
    25. Pose::Pose(const Eigen::Quaterniond& quaternion)
    26. {
    27.     quaternion.normalized();
    28.  
    29.     this->rotation_    = quaternion.toRotationMatrix();
    30.     this->quaternion_  = Eigen::Quaterniond(rotation_);
    31.     this->angle_axis_ = Eigen::AngleAxisd(rotation_);
    32.     this->euler_angle_ = rotation_.eulerAngles(0, 1, 2);
    33. }
    34.  
    35.  
    36. Pose::Pose(const Eigen::AngleAxisd& angle_axis) :
    37.     rotation_(angle_axis),
    38.     quaternion_(Eigen::Quaterniond(rotation_)),
    39.     angle_axis_(Eigen::AngleAxisd(rotation_)),
    40.     euler_angle_(rotation_.eulerAngles(0, 1, 2))
    41. {}
    42.  
    43.  
    44. Pose::Pose(const Eigen::Vector3d& euler_angle) :
    45.     rotation_(Eigen::AngleAxisd(euler_angle.x(), Eigen::Vector3d::UnitX()) * // note: ZYX
    46.                 Eigen::AngleAxisd(euler_angle.y(), Eigen::Vector3d::UnitY()) *
    47.               Eigen::AngleAxisd(euler_angle.z(), Eigen::Vector3d::UnitZ())),
    48.     quaternion_(Eigen::Quaterniond(rotation_)),
    49.     angle_axis_(Eigen::AngleAxisd(rotation_)),
    50.     euler_angle_(rotation_.eulerAngles(0, 1, 2))
    51. {}
    52.  
    53.  
    54. Pose::~Pose()
    55. {}
    56.  
    57.  
    58. Eigen::Matrix3d Pose::rotation() const
    59. {
    60.     return this->rotation_;
    61. }
    62.  
    63.  
    64. Eigen::Quaterniond Pose::quaternion() const
    65. {
    66.     return this->quaternion_;
    67. }
    68.  
    69.  
    70. Eigen::AngleAxisd Pose::angle_axis() const
    71. {
    72.     return this->angle_axis_;
    73. }
    74.  
    75.  
    76. Eigen::Vector3d Pose::euler_angle() const
    77. {
    78.     return this->euler_angle_;
    79. }
    80.  
    81.  
    82. Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2)
    83. {
    84.     return T1 * T2;
    85. }
    86.  
    87. Eigen::Isometry3d inverse(const Eigen::Isometry3d& T)
    88. {
    89.     return T.inverse();
    90. }

    test_pose.cpp

    1. #include
    2. using namespace std;
    3.  
    4. #include"Pose.h"
    5. const double M_PI = 3.1415926535;
    6.  
    7. // 对于同一个姿态，从不同的数学形式(旋转矩阵、四元数、欧拉角、角轴)构造类Pose
    8. // 依次得到 pose1 pose2 pose3 pose4
    9. void testClassPose(const Eigen::Matrix3d& R1)
    10. {
    11.  
    12.     Pose pose1(R1);
    13.     cout << "旋转矩阵 = " << endl; cout << pose1.rotation() << endl;
    14.     cout << "欧拉角 = " << endl;   cout << pose1.euler_angle().transpose()*(180 / M_PI) << endl;
    15.     cout << "四元数 = " << endl;   cout << pose1.quaternion().coeffs().transpose() << endl;
    16.     cout << "角轴 = " << endl;
    17.     cout << pose1.angle_axis().angle()* (180 / M_PI) <<" " << pose1.angle_axis().axis().transpose() <
    18.     cout << "-----------------------------" << endl;
    19.  
    20.     Pose pose2(pose1.euler_angle());
    21.     cout << "旋转矩阵 = " << endl; cout << pose2.rotation() << endl;
    22.     cout << "欧拉角 = " << endl;   cout << pose2.euler_angle().transpose()*(180 / M_PI) << endl;
    23.     cout << "四元数 = " << endl;   cout << pose2.quaternion().coeffs().transpose() << endl;
    24.     cout << "角轴 = " << endl;
    25.     cout << pose2.angle_axis().angle()* (180 / M_PI) << " " << pose2.angle_axis().axis().transpose() << endl;
    26.     cout << "-----------------------------" << endl;
    27.  
    28.  
    29.     Pose pose3(pose1.angle_axis());
    30.     cout << "旋转矩阵 = " << endl; cout << pose3.rotation() << endl;
    31.     cout << "欧拉角 = " << endl;   cout << pose3.euler_angle().transpose()*(180 / M_PI) << endl;
    32.     cout << "四元数 = " << endl;   cout << pose3.quaternion().coeffs().transpose() << endl;
    33.     cout << "角轴 = " << endl;
    34.     cout << pose3.angle_axis().angle()* (180 / M_PI) << " " << pose3.angle_axis().axis().transpose() << endl;
    35.     cout << "-----------------------------" << endl;
    36.  
    37.  
    38.     Pose pose4 = pose3;
    39.     cout << "旋转矩阵 = " << endl; cout << pose4.rotation() << endl;
    40.     cout << "欧拉角 = " << endl;   cout << pose4.euler_angle().transpose()*(180 / M_PI) << endl;
    41.     cout << "四元数 = " << endl;   cout << pose4.quaternion().coeffs().transpose() << endl;
    42.     cout << "角轴 = " << endl;
    43.     cout << pose4.angle_axis().angle()* (180 / M_PI) << " " << pose4.angle_axis().axis().transpose() << endl;
    44.     cout << "-----------------------------" << endl;
    45.  
    46.  
    47. }
    48.  
    49. // 测试求逆、compose等
    50. void testTheOthers(Eigen::Matrix3d R1, Eigen::Vector3d t1,
    51.                    Eigen::Matrix3d R2, Eigen::Vector3d t2)
    52. {
    53.     // 初始化T1
    54.     Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    55.     T1.prerotate(R1); T1.pretranslate(t1);
    56.     cout << "T1" << endl; cout << T1.matrix() << endl;
    57.  
    58.     // 初始化T2
    59.     Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    60.     T2.prerotate(R2); T2.pretranslate(t2);
    61.     cout << "T2" << endl; cout << T2.matrix() << endl;
    62.  
    63.     // 求逆
    64.     Eigen::Isometry3d T1_inverse = inverse(T1);
    65.     cout << "T1_inverse = " << endl; cout << T1_inverse.matrix() << endl;
    66.  
    67.     // compose
    68.     Eigen::Isometry3d T12 = compose(T1, T2);
    69.     cout << "T12 = " << endl; cout <<  T12.matrix() << endl;
    70.  
    71.  
    72.     /*cout << "Rotation = " << endl;
    73.     cout << T1.rotation() * T2.rotation() << endl;
    75.     cout << "Translation = " << endl;
    76.     cout << T1.rotation() * T2.translation() + T1.translation() << endl;*/
    77.  
    78. }
    79.  
    80. int main()
    81. {
    82.     Eigen::Matrix3d R1; //R1
    83.     R1 = Eigen::AngleAxisd((30.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*
    84.          Eigen::AngleAxisd((25.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*
    85.          Eigen::AngleAxisd((27.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());
    86.     Eigen::Vector3d t1(1.2, 0.234, 2.3);//t1
    87.  
    88.     Eigen::Matrix3d R2; //R2
    89.     R2 = Eigen::AngleAxisd((23.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*
    90.          Eigen::AngleAxisd((33.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*
    91.          Eigen::AngleAxisd((89.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());
    92.     Eigen::Vector3d t2(0.1, 0.4, 0.1); //t2
    93.  
    94.     // <1> test Class Pose
    95.     testClassPose(R1);
    96.  
    97.     // <2> test halcon's api
    98.     testTheOthers(R1, t1, R2, t2);
    99.  
    100.  
    101.     return 1;
    102. }

    可以看到，同一个姿态，不同的表达形式，他们之间相互转换之后结果数值一致。

    四、新的测试案例

    4.1 案例1

            写在最后，一个T为4×4的变换矩阵，如果旋转分量是欧式正交群，那个这个T为：欧式变换；否者为：仿射变换。如果一个旋转矩阵，不是SO3，那么可以将其转为四元数，接着归一化，再转为旋转矩阵，这样结果就属于SO3。同理，如果一个四元数，最好对齐进行归一化处理，再转为其他形式。例如：构造函数Pose(const Eigen::Quaterniond& quaternion); 其实现中比其他构造函数多了归一化这一步骤；即：quaternion.normalized();

           请看测试案例test3()，我们在第4或第5行进行四元数归一化，那么24行打印出来的矩阵就是单位阵。（不归一化，则不是单位阵）

    1. void test3()
    2. {
    3.     Eigen::Quaterniond q = { 0.1,0.35,0.2,0.3 };
    4.     //q.normalize();
    5.     Eigen::Matrix3d R = q.normalized().toRotationMatrix();
    6.  
    7.     cout << "R = " << endl;
    8.     cout << R << endl;
    9.     cout << endl;
    10.  
    11.     Eigen::Matrix3d R_transpose = R.transpose();
    12.     cout << "R.transpose = " << endl;
    13.     cout << R_transpose << endl;
    14.     cout << endl;
    15.  
    16.     Eigen::Matrix3d R_inverse = R.inverse();
    17.  
    18.     cout << "R.inverse = " << endl;
    19.     cout << R_inverse << endl << endl;
    20.     cout << endl;
    21.  
    22.     Eigen::Matrix3d ret = R * R.transpose();
    23.     cout << "ret = " << endl;
    24.     cout << ret << endl << endl;
    25.     cout << endl;
    26. }

    运行结果：

    4.2 案例2

            现在，如果给定一个旋转矩阵（如果你是随机测试，请你参考博客开始部分公式，每个元素是有取值范围的，给出的数字不要太离谱）；如下测试案例test4。第8行在底层有四元数归一化操作，所以你看下面效果图，R * R.tranpose() 近似为一个单位矩阵。

    1. void test4()
    2. {
    3.     Eigen::Matrix3d R;
    4.     R << 0.74, 0.08, 0.25,
    5.         0.2, 0.575, 0.05,
    6.         0.17, 0.19, 0.675;
    7.     Pose p1(R);
    8.     Pose p2(p1.quaternion()); // Pose中，针对从四元数构造，默认有归一化功能
    9.     R = p2.rotation();
    10.     cout << "R = " << endl;
    11.     cout << R << endl;
    12.     cout << endl;
    13.  
    14.     cout << "R.inverse = " << endl;
    15.     cout << R.inverse() << endl;
    16.     cout << endl;
    17.  
    18.     cout << "R.transpose = " << endl;
    19.     cout << R.transpose() << endl;
    20.     cout << endl;
    21.  
    22.     cout << "R * R.transpose() = " << endl;
    23.     cout << R * R.transpose() << endl;
    24.     cout << endl;
    25. }

    运行结果：

            其中，R表示旋转，用旋转向量来描述（欧拉角有周期性和方向锁的问题，四元数有单位向量的约束，旋转矩阵冗余度太高且有各个基需要是单位正交的约束）；t表示平移，用平移向量来描述。R和t均采用无约束的向量进行描述，于是也均可以通过网络的学习来得到。因为R和t共有六个自由度，因此姿态估计又称为6D姿态估计。

    注：旋转向量的方向代表旋转轴，模长代表旋转角的大小，旋转方向为逆时针。

            Eigen对于 translate 与 pretranslate的区别？（先平移、再旋转和 先旋转、再平移，有何区别？）可以看看这个：https://zhuanlan.zhihu.com/p/165020637

  • 相关阅读:
    什么是RPC？RPC框架dubbo的核心流程
    每个设计师都应该有的五个性格，你具备几个？
    前端 js面试题（二）
    【算法题】翻转对
    可持续建筑分论坛精彩回顾 | 第二届始祖数字化可持续发展峰会
    JDK动态代理与CGLIB动态代理
    Spring原生api操作之如何在spring配置文件添加Bean对象到Spring容器
    面试别问我 微服务架构优缺点？
    青少年python系列 27.turtle库绘制一个四叶花瓣
    JAVA小知识22：迭代器iterator与列表迭代器ListIterator
  • 原文地址：https://blog.csdn.net/m0_72734364/article/details/133808089