eigen库中几何模块变换总结

 1、旋转向量的定义：

// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度

2、旋转矩阵的定义

      

  Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

3、欧式变换矩阵

 // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵

 

 

几个变换关系

   1、旋转向量->旋转矩阵

 rotation_matrix = rotation_vector.toRotationMatrix();
也可以是
rotation_vector.matrix()

2、旋转矩阵->欧拉角

  

 // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序，如果(0,1,2)就是xyz顺序

3、旋转向量和平移->欧式变换

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

4、旋转向量->四元素

// 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

5、旋转矩阵->四元数

  q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}

7.设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为，,这里和表达的是，也就是世界坐标系到相机坐标系的变换关系。小萝卜二号位姿为，。现在小萝卜一号看到一个点在自身坐标系下坐标为，求该点在小萝卜二号坐标系下的坐标。请编程实现。

分析：

目前在编程中，涉及到坐标系变换一般是利用位姿变化矩阵进行实现。所以首先应当将其转化为位姿变化矩阵。由于在世界坐标系下，被观测点属于静态不动点，所以被观测点的世界坐标系下坐标为常量，由此构建被观测点在两个小萝卜坐标系的投影坐标的恒等式。假设该点在小萝卜二号坐标系下的坐标为，小萝卜一号位姿矩阵为，小萝卜二号的位姿矩阵为，则可列等式：

 

由此可进行编程求解坐标：

CMakeLists.txt如下所示：

cmake_minimum_required(VERSION 2.6)
project(homeworks3_7)
 
include_directories("/usr/include/eigen3")
 
add_executable(homeworks3_7 main.cpp)
 

main.cpp

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
 
using namespace std;
 
 
int main(int argc, char **argv) {
  
  //导入小萝卜一号数据  
  Eigen::Quaterniond q1 (0.35, 0.2, 0.3, 0.1);
  Eigen::Vector3d t1 (0.3, 0.1, 0.1);
  //导入小萝卜二号数据
  Eigen::Quaterniond q2 (-0.5, 0.4, -0.1, 0.2);
  Eigen::Vector3d t2 (-0.1, 0.5, 0.3);
  //导入小萝卜一号观测观测点数据
  Eigen::Vector3d p (0.5, 0, 0.2);
  //定义小萝卜二号观测观测点坐标
  Eigen::Vector3d o;
  
  //初始化欧式变换矩阵
  Eigen::Isometry3d T_1cw = Eigen::Isometry3d::Identity();
  Eigen::Isometry3d T_2cw = Eigen::Isometry3d::Identity();
  
  //开始求解
  //归一化四元数
  q1.normalize();
  q2.normalize();
  //输出归一化后信息
  cout<<"q1 = "<< endl << q1.x()<<endl<<q1.y()<< endl <<q1.z()<< endl<<q1.w()<<endl;
  cout<<"q2 = "<< endl << q2.x()<<endl<<q2.y()<< endl <<q2.z()<< endl<<q2.w()<<endl;
  
  //将四元数和位移融合
  T_1cw.rotate(q1);
  T_1cw.pretranslate(t1);
  
  T_2cw.rotate(q2);
  T_2cw.pretranslate(t2);
  
  //输出位姿矩阵信息
  cout << "T_1cw: " << endl << T_1cw.matrix() << endl;
  cout << "T_2cw: " << endl << T_2cw.matrix() << endl;
  
  //求解o坐标
  o = T_2cw * T_1cw.inverse() * p;
  
  //输出o的坐标
  cout << "o = " << o.transpose() << endl;
  
    return 0;
}