网站空间域名每年都得交吗,网页版梦幻西游伙伴,建设基金会网站,微信小程序怎么注销账号位姿中姿态的表示形式有很多种#xff0c;比如#xff1a;旋转矩阵、四元数、欧拉角、旋转向量等等。这里基于Eigen实现四种数学形式的相互转换功能。本文利用Eigen实现上述四种形式的相互转换。我这里给出一个SE3#xff08;4*4#xff09;(先平移、再旋转)的构建方法比如旋转矩阵、四元数、欧拉角、旋转向量等等。这里基于Eigen实现四种数学形式的相互转换功能。本文利用Eigen实现上述四种形式的相互转换。我这里给出一个SE34*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#includeEigen/Core
#includeEigen/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.hPose::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
#includeiostream
using namespace std;#includePose.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 halcons 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;
}
运行结果 其中R表示旋转用旋转向量来描述欧拉角有周期性和方向锁的问题四元数有单位向量的约束旋转矩阵冗余度太高且有各个基需要是单位正交的约束t表示平移用平移向量来描述。R和t均采用无约束的向量进行描述于是也均可以通过网络的学习来得到。因为R和t共有六个自由度因此姿态估计又称为6D姿态估计。
注旋转向量的方向代表旋转轴模长代表旋转角的大小旋转方向为逆时针。 Eigen对于 translate 与 pretranslate的区别先平移、再旋转和 先旋转、再平移有何区别可以看看这个https://zhuanlan.zhihu.com/p/165020637
