视觉SLAM学习：Eigen几何模块使用方法

视觉SLAM学习：Eigen几何模块使用方法

Eigen库的使用

头文件包含：

#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;

基本讲解

数据结构

旋转向量 角1+轴3 『AngleAxisd』

旋转矩阵 3 * 3 『Matrix3d』

欧拉角 3 * 1 『Vector3d』

四元数 4 * 1 『Quaterniond』

欧式变换矩阵、仿射矩阵、射影矩阵 4 * 4 『 Isometry3d、Affine3d、Projective3d』

结构之间的变换

//旋转向量->矩阵形式 
.matrix() 

//旋转向量->旋转矩阵
 .toRotationMatrix()

//旋转矩阵->欧拉角 
.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序

//欧式变换矩阵建立 
T.rotate(rotation_vector);                                     // 按照rotation_vector旋转向量进行旋转
T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)

//四元数建立
Quaterniond q = Quaterniond(rotation_vector);     //按照旋转向量进行建立
q.coeffs().transpose()                           // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

// 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是q*v*q^{-1}
  
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

代码

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

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


int main(int argc, char **argv) {

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();//单位阵
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度，参数：角+轴
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵形式3*1
  // 旋转矩阵
  rotation_matrix = rotation_vector.toRotationMatrix();//变旋转矩阵3*3
  // 用 AngleAxis 可以进行坐标变换
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // 或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

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

  // 用欧式变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}