视觉SLAM——第三章 Eigen几何模块Geometry使用 四元素 欧式变换矩阵

视觉SLAM——第三章

Eigen几何模块Geometry使用 四元素 欧式变换矩阵

github链接 点击打开链接

博文末尾支持二维码赞赏哦 ^_^

* 本程序演示了 Eigen 几何模块的使用方法

* 旋转向量 Eigen::AngleAxisd 角度 轴 Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度

* 旋转矩阵 Eigen::Matrix3d rotation_vector.toRotationMatrix(); //旋转向量转换到旋转矩阵

* Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();// 直接转

 

* 欧拉角 Eigen::Vector3d rotation_matrix.eulerAngles ( 2,1,0 );// ( 2,1,0 )表示ZYX顺序，即roll pitch yaw顺序 旋转矩阵到 欧拉角转换到欧拉角

 

* 四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 旋转向量 定义四元素

* q = Eigen::Quaterniond ( rotation_matrix );//旋转矩阵定义四元素

 

* 欧式变换矩阵 Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵 旋转 R+ 平移T

 

* T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转

* 也可 Eigen::Isometry3d T(q)　 // 一步 按四元素表示的旋转 旋转 转换矩阵

* T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)

*  输出  cout<< T.matrix() <

#include 
#include 
using namespace std;

#include 
// Eigen 几何模块
#include 

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{  
  //注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型，两种类型不能混用，必须显示转换
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
 /****旋转向量****/
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    // 乘以该向量，表示进行一个坐标变换
    //任意旋转可用一个旋转轴和一个旋转角度来表示。
    //旋转向量，旋转向量的方向与旋转轴一致，长度为旋转角度。
     /*********************************/
    /*旋转向量　沿 Z 轴旋转 45 度         角度　轴 */
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"< pw=Tc1w.inverse()*p1;    //将c1坐标系下的点p1变换到world坐标系下

Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//单位阵
q2rotation_matrix=q2.toRotationMatrix();
Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵　　齐次坐标
  
Tc2w.rotate (q2rotation_matrix );                                    // 按照q1rotation_matrix进行旋转
Tc2w.pretranslate ( t2);                                                     // 把平移向量设成t1

Eigen::Matrix p2=Tc2w*pw;    //将world坐标系下的点pw变换到c2坐标系下
cout<<"the loc of p1 in c1  = \n"<< p1<