基于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 = 1 0 0 0
0 0.707107 -0.707106 0
0 0.707106 0.707107 4
0 0 0 1下面的黄色立方体，相对坐标系位置为：(0 0 4)，绕x轴旋转pi /4，得到绿色立方体
以下是在本地word中笔记的截图：
Pose.h(类Pose声明)
#pragma once
#ifndef POSE_H
#define POSE_H
#include
#include
// namespace Geometry
class Pose
{
public:
Pose();
Pose& operator= (const Pose& pose);
// construct from rotation
Pose(const Eigen::Matrix3d& rotation);
// construct from quaternion
Pose(const Eigen::Quaterniond& quaternion);
// construct from angle axisd
Pose(const Eigen::AngleAxisd& angle_axis);
// construct from euler angle
Pose(const Eigen::Vector3d& euler_angle);
~Pose();
// return rotation
Eigen::Matrix3d rotation() const;
// return quaterniond
Eigen::Quaterniond quaternion() const;
// return angle axisd
Eigen::AngleAxisd angle_axis() const;
// return euler angle
Eigen::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: ZYX
Eigen::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
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() <
写在最后，一个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;
}
运行结果：
其中，R表示旋转，用旋转向量来描述（欧拉角有周期性和方向锁的问题，四元数有单位向量的约束，旋转矩阵冗余度太高且有各个基需要是单位正交的约束）；t表示平移，用平移向量来描述。R和t均采用无约束的向量进行描述，于是也均可以通过网络的学习来得到。因为R和t共有六个自由度，因此姿态估计又称为6D姿态估计。
注：旋转向量的方向代表旋转轴，模长代表旋转角的大小，旋转方向为逆时针。
Eigen对于 translate 与 pretranslate的区别？（先平移、再旋转和 先旋转、再平移，有何区别？）可以看看这个：https://zhuanlan.zhihu.com/p/165020637