高翔视觉slam——ch3

高翔视觉slam——ch3

第四节分为四个部分：

1. usingEigen（Eigen库基本操作）
2. usingGeometry（Eigen库Geometry基本操作）
3. examples（求给定俩点的的转换关系、画出一个预先存储的轨迹）
4. 设计一个立方体，拖动、输出实时欧拉角、四元数、旋转矩阵

安装库

Eigen安装

sudo apt-get install libeigen3-dev

eigen只有头文件没有库文件，也不需要cmake编译安装。
一般路径为：/usr/include/eigen3

#添加Eigen头文件
include_directories("/usr/include/eigen3")

pangolin安装

pangolin是基于OPenGl的3D绘图库
安装过程：（pangolin GitHub网址）
PS：我遇到的情况：安装完pangolin再安装OpenCV会使pangolin库不好使、报各种意想不到的错误，重新安装pangolin后就好了。

下载到本地
git clone https://github.com/stevenlovegrove/Pangolin.git
安装依赖
/*****必需的依赖项*****/
sudo apt-get install libglew-dev
sudo apt-get install cmake
sudo apt-get install libpython2.7-dev//python2.7是许多系统默认安装的，有Python2的各个版本都可，不用重复安装
/*****视频输入的可选依赖项********/（这些之后安装OpenCV都必须安装的）
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libavdevice-dev
sudo apt-get install libdc1394-22-dev libraw1394-dev
sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev
/******Building**********/
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
cmake --build .

//如果要生成文档并且已安装Doxygen，可以执行：
cmake --build . --target doc

知识点

usingEigen

1. Matrix<float, 2, 3> A ——三行俩列的float矩阵A
2. Matrix3d等价于Matrix<double,3, 3>
3. .cast() ：转换格式为double
4. .transpose() ： 转置
5. Matrix3d::Random(); // 随机数矩阵3*3double型矩阵
6. SelfAdjointEigenSolver A —— 使实对称矩阵自共轭
7. .inverse() ——求逆

#include <iostream>

using namespace std;

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

using namespace Eigen;

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main(int argc, char **argv) {
  // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
  // 声明一个2*3的float矩阵
  Matrix<float, 2, 3> matrix_23;

  // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
  // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
  Vector3d v_3d;
  // 这是一样的
  Matrix<float, 3, 1> vd_3d;

  // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
  Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零
  // 如果不确定矩阵大小，可以使用动态大小的矩阵
  Matrix<double, Dynamic, Dynamic> matrix_dynamic;
  // 更简单的
  MatrixXd matrix_x;
  // 这种类型还有很多，我们不一一列举

  // 下面是对Eigen阵的操作
  // 输入数据（初始化）
  matrix_23 << 1, 2, 3, 4, 5, 6;
  // 输出
  cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

  // 用()访问矩阵中的元素
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

  // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
  // Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

  //              .cast<double>() ：转换格式为double
  //              .transpose()    ： 转置
  Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

  Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

  // 同样你不能搞错矩阵的维度
  // 试着取消下面的注释，看看Eigen会报什么错
  // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

  // 一些矩阵运算
  // 四则运算就不演示了，直接用+-*/即可。
  matrix_33 = Matrix3d::Random();      // 随机数矩阵
  cout << "random matrix: \n" << matrix_33 << endl;
  cout << "transpose: \n" << matrix_33.transpose() << endl;     // 转置
  cout << "sum: " << matrix_33.sum() << endl;                   // 各元素和
  cout << "trace: " << matrix_33.trace() << endl;               // 迹
  cout << "times 10: \n" << 10 * matrix_33 << endl;             // 数乘
  cout << "inverse: \n" << matrix_33.inverse() << endl;         // 逆
  cout << "det: " << matrix_33.determinant() << endl;           // 行列式

  // 特征值
  // 实对称矩阵可以保证对角化成功
  SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

  // 解方程
  // 我们求解 matrix_NN * x = v_Nd 这个方程
  // N的大小在前边的宏里定义，它由随机数生成
  // 直接求逆自然是最直接的，但是求逆运算量大

  Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

  clock_t time_stt = clock(); // 计时
  // 直接求逆
  Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 通常用矩阵分解来求，例如QR分解，速度会快很多
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // 对于正定矩阵，还可以用cholesky分解来解方程
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  return 0;
}

usingGeometry

1. Matrix3d::Identity(); 3*3double型单位矩阵
2. AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));旋转向量
3. cout.precision(val)其实就是在输出的时候设定输出值以新的浮点数精度值显示，即小数点后保留val位。
4. Isometry3d T —— 欧氏变换矩阵使用 Eigen::Isometry
5. Quaterniond q = Quaterniond(rotation_vector);可以直接把旋转向量赋值给四元数，反之亦然
6. q = Quaterniond(rotation_matrix); 也可以把旋转矩阵赋给它

#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();      //Identity() 即用"单位矩阵"对x变量进行了初始化
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
/*********************初始化旋转向量******************************/
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
/*********************（旋转向量->旋转矩阵）方法一*******************/
  //cout.precision(val)其实就是在输出的时候设定输出值以新的浮点数精度值显示，即小数点后保留val位。
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用.matrix()：转换成旋转矩阵
  // 也可以直接赋值
/*********************（旋转向量->旋转矩阵）方法二*******************/
  rotation_matrix = rotation_vector.toRotationMatrix();                //.toRotationMatrix();转为旋转矩阵
  cout<<"after '.toRotationMatrix()':\n"<<rotation_matrix<<endl;

/********************向量（1,0,0）经过旋转变换后的坐标***************/
  // 用 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为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  
  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;
}

examples_1

1. .normalize(); —— 四元数归一化：对四元数的单位化，单位化的四元数可以表示一个旋转.
   规范化四元数作用：
   1.表征旋转的四元数应该是规范化的四元数，但是由于计算误差等因素, 计算过程中四元数会逐渐失去规范化特性，因此必须对四元数做规范化处理
   2.意义在于单位化四元数在空间旋转时是不会拉伸的,仅有旋转角度.这类似与线性代数里面的正交变换.
   3.由于误差的引入，使得计算的变换四元数的模不再等于1，变换四元数失去规范性，因此再次更新四元数.

2. pretranslate（）是用旋转前的坐标系平移；translate（）用旋转后的坐标系平移

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

using namespace std;
using namespace Eigen;

int main() {
  Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
  cout<<"q1 before .normalize():\n"<<q1.matrix()<<endl;
  q1.normalize();  //四元数运算归一化，实现规范化，让一个向量保持之前的方向，但它的长度为1.0，如果这个向量太小而不能被规范化，一个零向量将会被返回。
  q2.normalize();  //所给q1、q2并非单位四元数，所以单位（归一）化
  cout<<"q1 after .normalize():\n"<<q1.matrix()<<endl;

  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);         //平移t1  pretranslate（）是用旋转前的坐标系平移；translate（）用旋转后的坐标系平移
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;      //inverse()求逆
  cout << endl <<"p2:\n"<< p2.transpose() << endl;
  return 0;
}

总结

1. Eigen、pangolin的使用还需要付诸实践去学习。
   参考：https://blog.csdn.net/u010949998/article/details/110948946

2. 四元数表示旋转：
   空间点p经过四元数q旋转变换得到p~
   则：p~=qpq^(-1) （若有平移+t）

3. 旋转矩阵表示旋转：
   空间一点a经过旋转矩阵R旋转到a~
   则a~=R*a
   若有平移在加‘t’（注意区分‘t’为旋转前位移还是旋转后位移）

4. 旋转向量一般转换为旋转矩阵或者四元数进行矩阵运算。

5. 所以应该可以通过易得的旋转向量获得四元数，所以归一（单位）化尤为重要。