eigen库使用笔记

Matrix

// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

简单声明和输出：

Matrix<float,2,3> matrix23  //生成一个2*3矩阵
matrix23<<1,2,3,1,2,3;    //对其进行赋值
cout<<matrix23;  //输出
//或者使用for循环输出
    for(int i=0;i<2;i++){
        for(int j=0;j<3;j++){
            printf("%f",matrix23(i,j));
        }
    }

其他格式声明

Matrix3d matrix_33 = Matrix3d::Zero(); //3*3矩阵，初始化为零
Matrix<float,Dynamic,1> Vectorn;  //生成n行一列的向量
Vector3d v_3d;                     //Eigen::Matrix<double, 3, 1>

当你的预选之后可以看到对应的定义 

赋值

matrix_33 = Matrix3d::Random();      // 随机数矩阵

运算

// 四则运算就不演示了，直接用+-*/即可。
matrix_33.transpose()  // 转置
matrix_33.sum()                 // 各元素和
matrix_33.trace()             // 迹
10 * matrix_33                  // 数乘
matrix_33.inverse()             // 逆
matrix_33.determinant()       // 行列式

点乘叉乘

Vector3d v(3,4,5);
Vector3d w(0,2,3);
cout<<v.dot(w)<<endl;           //矩阵的点乘VxW
cout<<v.cross(w) <<"\n" <<endl; //叉乘X VXW

解方程Ax=b

MATRIX_SIZE=50;
Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

//--------上面是生成Ax=b中的A（matrix_NN） 和b（v_Nd）------------

  // 直接求逆
  Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  cout << "x = " << x.transpose() << endl;

  // 通常用矩阵分解来求，例如QR分解，速度会快很多
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "x = " << x.transpose() << endl;

几何模块 #include <Eigen/Geometry>

利用旋转向量、变换矩阵、四元数来对向量v进行旋转

#include <iostream>
#include <cmath>

using namespace std;

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

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法
// 主要是利用旋转向量、变换矩阵、四元数来对向量v进行旋转

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()转换成矩阵

  rotation_matrix = rotation_vector.toRotationMatrix();   //将vector转化为旋转矩阵

//--------------------坐标变换---------------------

  // 用 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); // 参数2、1、0表示将旋转矩阵沿z、y、x轴依次旋转
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵  即R（旋转矩阵）加上一个平移t
  T.rotate(rotation_vector);                             // 按照rotation_vector进行旋转，相当于是设置旋转的值
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)，设置平移的值
  cout << "Transform matrix = \n" << T.matrix() << endl;  //将上面两行结合起来，变成4*4变换矩阵进行输出

  // 用变换矩阵进行坐标变换
  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;
}

#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  q1.normalize();
  q2.normalize();
  Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d p1(0.5, 0, 0.2);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);   //T1w：一号萝卜所在位置和世界坐标系之间的关系
  T2w.pretranslate(t2);  //T2w：二号萝卜所在位置和世界坐标系之间的关系
  
//首先利用T1w.inverse() * p1来对观测点从小萝卜一号到世界坐标进行变换；然后再乘以T2w来对观测点进行世界坐标系到萝卜二号的坐标系进行变换
  Vector3d p2 = T2w * T1w.inverse() * p1;   
  cout << endl << p2.transpose() << endl;
  return 0;
}

sophus