视觉SLAM十四讲 3.6.2 实际的坐标变换例子代码解析

在这里介绍一下视觉SLAM十四讲3.6.2实际的坐标变换例子的代码分析
这里作者只给了源代码，其实是有公式推导的
我结合网上其他人的代码，在不影响对源代码的理解的同时，对代码进行了一些改进，大家可以仔细看下注释内容：

#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);//用来表示小萝卜一号坐标系下该点坐标（题干给出）
  Vector3d p2; //用来表示小萝卜二号坐标系下该点坐标
  //!下面分别用四元数以及变换矩阵得到小萝卜二号的该点观测信息
  //以下用四元数求解p2坐标
  p2 = q2 * q1.inverse() * (p1 - t1) + t2;//求解p2坐标
  //这里的inverse是相反的意思，也就是求q1的逆矩阵
  //公式为：p2 = q2  * q1^-1 * (p1 - t1) + t2
  cout << "四元数求得的p2坐标" << endl;
  cout << p2 << endl;//列向量显示
  cout << p2.transpose() << endl;//行向量显示，transpose是转置矩阵的意思

  //以下用欧拉矩阵求解p2坐标
  Isometry3d T1w(q1), T2w(q2);//欧式变换矩阵Isometry（虽然称为3d，实质上是4＊4的矩阵）
  T1w.pretranslate(t1);//设置平移向量，我的理解是加入这个平移向量
  T2w.pretranslate(t2);

  Vector3d p3;//用来表示小萝卜二号坐标系下该点坐标（变换矩阵方法）
  p3 = T2w * T1w.inverse() * p1;