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

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

#include 
#include 
#include 
#include 
#include 

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;//求解p3坐标
  cout << "变换矩阵/欧拉矩阵求得的p2坐标" << endl;
  cout << p3.transpose() << endl;
  return 0;
}

四元数解法

先说一下四元数的推导：
假设目标点p在世界坐标系下的坐标为：
$$P_w$$
利用四元数表示旋转，两个小萝卜坐标系下p点坐标有如下公式：
$$\begin{gathered} p_1=q_1\ast P_w+t_1 \\ {p}_2=q_2\ast P_w+t_2 \end{gathered}$$
由以上的坐标变换关系，可以推得：
$$p_2=q_2\ast q_1^{-1}\ast (p_1-t_1)+t_2$$
到这里其实已经能明白程序中的代码含义了
值得一提的是，在Eigen中四元数表示旋转是
$$q\ast P$$
然而数学上应该是
$$q\ast P\ast q^{-1}$$
因为Eigen使用了运算符重载，减少了编写代码的麻烦

变换矩阵解法

若使用欧拉矩阵，我们设两个坐标的变换矩阵为T1和T2，有：
$$\begin{gathered} p_1=T_1\ast P_w \\ {p}_2=T_2\ast P_w \end{gathered}$$
联立可以得到如下计算公式：
$$p_2=T_2\ast T_1^{-1}\ast p_1$$
这样我们就得到了两种方法的计算公式
结合程序中的注释应该不难搞懂啦

结果

程序运行结果如下：
两种计算方法得到了相同的结果啦！
如果对你有帮助，希望你可以点个赞鼓励下啦！