旋转可以用旋转矩阵(9个量)或者旋转向量(3个量),或者四元素(4个量)表示。旋转矩阵中的元素不是相互独立的,这在非线性优化中会带来问题。用欧拉角会有万向节死锁的问题,而四元数则弥补了这个缺陷,是最好的实现旋转的方案。
欧拉角:偏航(z)-俯仰(y)-滚转(x):yaw-pitch-roll
Eigen矩阵转换的code
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
using namespace std;
int main ( int argc, char** argv )
{
// Eigen/Geometry模块提供了各种旋转和平移的表示
// 3D旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
cout<<"rotation matrix =\n"<
//旋转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
//将单位矩阵通过旋转变成旋转矩阵
Eigen::AngleAxisd rotation_vector ( M_PI/6, Eigen::Vector3d ( 1,0,0 )); //沿 Z 轴旋转 45 度
cout .precision(3);
cout<<"rotation matrix=\n"<
//也可以直接赋值,通过向量转为矩阵
rotation_matrix = rotation_vector.toRotationMatrix();
cout<<"rotation matrix =\n"<
//验证旋转矩阵的特性,它的逆等于转置,它的行列式为1.
cout<<"transpose=\n"<
cout<<"inverse=\n"<
cout<<"determinant=\n"<
//求特征值和特征向量
Eigen::SelfAdjointEigenSolvereigenSolver(rotation_matrix);
if (eigenSolver.info() == Eigen::Success)
{
std::cout <<"eigenvalues=\n"<
std::cout <<"eigenvectors=\n"<
}
//四元数
//可以直接把AngleAxis赋值给四元数,反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<
//也可以把旋转矩阵赋给它
q= Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<
return 0;
}
网友评论