vlsam旋转向量与旋转矩阵—Apple的学习笔记

旋转可以用旋转矩阵（9个量）或者旋转向量(3个量)，或者四元素（4个量）表示。旋转矩阵中的元素不是相互独立的，这在非线性优化中会带来问题。用欧拉角会有万向节死锁的问题，而四元数则弥补了这个缺陷，是最好的实现旋转的方案。

欧拉角：偏航(z)-俯仰(y)-滚转(x):yaw-pitch-roll

Eigen矩阵转换的code

#include

#include

#include

#include

#include

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;

}