基于Eigen的位姿转换

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

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

打印结果：

T1 =         1         0         0         00  0.707107 -0.707106         00  0.707106  0.707107         40         0         0         1

下面的黄色立方体，相对坐标系位置为：(0 0 4)，绕x轴旋转pi /4，得到绿色立方体

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

Pose.h(类Pose声明)

#pragma once
#ifndef POSE_H
#define POSE_H#include<Eigen/Core>
#include<Eigen/Geometry>// namespace Geometryclass Pose
{
public:Pose();Pose& operator= (const Pose& pose);// construct from rotationPose(const Eigen::Matrix3d& rotation);// construct from quaternionPose(const Eigen::Quaterniond& quaternion);// construct from angle axisdPose(const Eigen::AngleAxisd& angle_axis);// construct from euler anglePose(const Eigen::Vector3d& euler_angle);~Pose();// return rotationEigen::Matrix3d rotation() const;// return quaterniondEigen::Quaterniond quaternion() const;// return angle axisdEigen::AngleAxisd angle_axis() const;// return euler angleEigen::Vector3d euler_angle() const;private:Eigen::Matrix3d rotation_;       // 旋转矩阵Eigen::Quaterniond quaternion_;  // 四元数Eigen::AngleAxisd angle_axis_;  // 角轴Eigen::Vector3d euler_angle_;    // 欧拉角 roll(X轴)  pitch(Y轴) yaw(Z轴)};// 姿态组合
Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2);// 求逆
Eigen::Isometry3d inverse(const Eigen::Isometry3d& T);#endif // !POSE_H

Pose.cpp(类Pose的实现)

#include "Pose.h"Pose::Pose()
{}Pose& Pose::operator= (const Pose& pose)
{this->rotation_    = pose.rotation();this->quaternion_  = pose.quaternion();this->angle_axis_ = pose.angle_axis();this->euler_angle_ = pose.euler_angle();return *this;
}//
Pose::Pose(const Eigen::Matrix3d& rotation) :rotation_(rotation),quaternion_(Eigen::Quaterniond(rotation_)),angle_axis_(Eigen::AngleAxisd(rotation_)),euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}Pose::Pose(const Eigen::Quaterniond& quaternion)
{quaternion.normalized();this->rotation_    = quaternion.toRotationMatrix();this->quaternion_  = Eigen::Quaterniond(rotation_);this->angle_axis_ = Eigen::AngleAxisd(rotation_);this->euler_angle_ = rotation_.eulerAngles(0, 1, 2);
}Pose::Pose(const Eigen::AngleAxisd& angle_axis) :rotation_(angle_axis),quaternion_(Eigen::Quaterniond(rotation_)),angle_axis_(Eigen::AngleAxisd(rotation_)),euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}Pose::Pose(const Eigen::Vector3d& euler_angle) :rotation_(Eigen::AngleAxisd(euler_angle.x(), Eigen::Vector3d::UnitX()) * // note: ZYXEigen::AngleAxisd(euler_angle.y(), Eigen::Vector3d::UnitY()) *Eigen::AngleAxisd(euler_angle.z(), Eigen::Vector3d::UnitZ())),quaternion_(Eigen::Quaterniond(rotation_)),angle_axis_(Eigen::AngleAxisd(rotation_)),euler_angle_(rotation_.eulerAngles(0, 1, 2))
{}Pose::~Pose()
{}Eigen::Matrix3d Pose::rotation() const
{return this->rotation_;
}Eigen::Quaterniond Pose::quaternion() const
{return this->quaternion_;
}Eigen::AngleAxisd Pose::angle_axis() const
{return this->angle_axis_;
}Eigen::Vector3d Pose::euler_angle() const
{return this->euler_angle_;
}Eigen::Isometry3d compose(const Eigen::Isometry3d& T1, const Eigen::Isometry3d& T2)
{return T1 * T2;
}Eigen::Isometry3d inverse(const Eigen::Isometry3d& T)
{return T.inverse();
}

test_pose.cpp

#include<iostream>
using namespace std;#include"Pose.h"
const double M_PI = 3.1415926535;// 对于同一个姿态，从不同的数学形式(旋转矩阵、四元数、欧拉角、角轴)构造类Pose
// 依次得到 pose1 pose2 pose3 pose4
void testClassPose(const Eigen::Matrix3d& R1)
{Pose pose1(R1);cout << "旋转矩阵 = " << endl; cout << pose1.rotation() << endl;cout << "欧拉角 = " << endl;   cout << pose1.euler_angle().transpose()*(180 / M_PI) << endl;cout << "四元数 = " << endl;   cout << pose1.quaternion().coeffs().transpose() << endl;cout << "角轴 = " << endl;cout << pose1.angle_axis().angle()* (180 / M_PI) <<" " << pose1.angle_axis().axis().transpose() <<endl;cout << "-----------------------------" << endl;Pose pose2(pose1.euler_angle());cout << "旋转矩阵 = " << endl; cout << pose2.rotation() << endl;cout << "欧拉角 = " << endl;   cout << pose2.euler_angle().transpose()*(180 / M_PI) << endl;cout << "四元数 = " << endl;   cout << pose2.quaternion().coeffs().transpose() << endl;cout << "角轴 = " << endl;cout << pose2.angle_axis().angle()* (180 / M_PI) << " " << pose2.angle_axis().axis().transpose() << endl;cout << "-----------------------------" << endl;Pose pose3(pose1.angle_axis());cout << "旋转矩阵 = " << endl; cout << pose3.rotation() << endl;cout << "欧拉角 = " << endl;   cout << pose3.euler_angle().transpose()*(180 / M_PI) << endl;cout << "四元数 = " << endl;   cout << pose3.quaternion().coeffs().transpose() << endl;cout << "角轴 = " << endl;cout << pose3.angle_axis().angle()* (180 / M_PI) << " " << pose3.angle_axis().axis().transpose() << endl;cout << "-----------------------------" << endl;Pose pose4 = pose3;cout << "旋转矩阵 = " << endl; cout << pose4.rotation() << endl;cout << "欧拉角 = " << endl;   cout << pose4.euler_angle().transpose()*(180 / M_PI) << endl;cout << "四元数 = " << endl;   cout << pose4.quaternion().coeffs().transpose() << endl;cout << "角轴 = " << endl;cout << pose4.angle_axis().angle()* (180 / M_PI) << " " << pose4.angle_axis().axis().transpose() << endl;cout << "-----------------------------" << endl;}// 测试求逆、compose等
void testTheOthers(Eigen::Matrix3d R1, Eigen::Vector3d t1,Eigen::Matrix3d R2, Eigen::Vector3d t2)
{// 初始化T1Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();T1.prerotate(R1); T1.pretranslate(t1);cout << "T1" << endl; cout << T1.matrix() << endl;// 初始化T2Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();T2.prerotate(R2); T2.pretranslate(t2);cout << "T2" << endl; cout << T2.matrix() << endl;// 求逆Eigen::Isometry3d T1_inverse = inverse(T1);cout << "T1_inverse = " << endl; cout << T1_inverse.matrix() << endl;// composeEigen::Isometry3d T12 = compose(T1, T2);cout << "T12 = " << endl; cout <<  T12.matrix() << endl;/*cout << "Rotation = " << endl;cout << T1.rotation() * T2.rotation() << endl;cout << "Translation = " << endl;cout << T1.rotation() * T2.translation() + T1.translation() << endl;*/}int main()
{Eigen::Matrix3d R1; //R1R1 = Eigen::AngleAxisd((30.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*Eigen::AngleAxisd((25.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*Eigen::AngleAxisd((27.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());Eigen::Vector3d t1(1.2, 0.234, 2.3);//t1Eigen::Matrix3d R2; //R2R2 = Eigen::AngleAxisd((23.0 / 180) * M_PI, Eigen::Vector3d::UnitX())*Eigen::AngleAxisd((33.0 / 180) * M_PI, Eigen::Vector3d::UnitY())*Eigen::AngleAxisd((89.0 / 180) * M_PI, Eigen::Vector3d::UnitZ());Eigen::Vector3d t2(0.1, 0.4, 0.1); //t2// <1> test Class PosetestClassPose(R1);// <2> test halcon's apitestTheOthers(R1, t1, R2, t2);return 1;
}

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

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

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

void test3()
{Eigen::Quaterniond q = { 0.1,0.35,0.2,0.3 };//q.normalize();Eigen::Matrix3d R = q.normalized().toRotationMatrix();cout << "R = " << endl;cout << R << endl;cout << endl;Eigen::Matrix3d R_transpose = R.transpose();cout << "R.transpose = " << endl;cout << R_transpose << endl;cout << endl;Eigen::Matrix3d R_inverse = R.inverse();cout << "R.inverse = " << endl;cout << R_inverse << endl << endl;cout << endl;Eigen::Matrix3d ret = R * R.transpose();cout << "ret = " << endl;cout << ret << endl << endl;cout << endl;
}

运行结果：

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

void test4()
{Eigen::Matrix3d R;R << 0.74, 0.08, 0.25,0.2, 0.575, 0.05,0.17, 0.19, 0.675;Pose p1(R);Pose p2(p1.quaternion()); // Pose中，针对从四元数构造，默认有归一化功能R = p2.rotation();cout << "R = " << endl;cout << R << endl;cout << endl;cout << "R.inverse = " << endl;cout << R.inverse() << endl;cout << endl;cout << "R.transpose = " << endl;cout << R.transpose() << endl;cout << endl;cout << "R * R.transpose() = " << endl;cout << R * R.transpose() << endl;cout << endl;
}

运行结果：