在这一讲中，作者没有给出详细的过程，作为初学者还是有点懵。
这道题是有两种解法的
- 四元数解法
- 欧拉矩阵解法
四元数解法
假设一个目标点 \(p\)在世界坐标系下的坐标是
\[P_W
\]
利用四元数表示旋转，两个小萝卜坐标系下 \(p\) 点坐标有如下公式：
\[p_1 = q_1 * P_w + t_1 \\
p_2 = q_2 * P_w + t_2
\]
以上公式实际上是Eigen的计算公式，经过了运算符重载，实际上，数学表达上与Eigen运算有所不同。
在Eigen中四元数表示旋转是
\[q*P
\]
数学表达上应该是
\[q*P*q^{-1}
\]
因为Eigen使用了运算符重载，减少了编写代码的麻烦。
由以上的坐标变换关系，可以推得：
\[p_2=q_2 ∗q_{1}^{−1} ∗(p_1 −t_1 )+t_2
\]
变换矩阵解法
若使用欧拉矩阵，我们设两个坐标的变换矩阵为T1和T2，有：
\[p_1 = T_1*P_w\\
p_2 = T_2*P_w
\]
联立可以得到如下计算公式：
\[p_2 = T_2*T_1^{-1} *p_1
\]
code
void main()
{
//定义两个小萝卜自身姿态的两个四元数
Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
//对两个四元数进行归一化
q1.normalize();
q2.normalize();
//定义两个小萝卜的位置的两个三维坐标
Eigen::Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
Eigen::Vector3d p1(0.5, 0, 0.2);//用来表示小萝卜一号坐标系下该点坐标（题干给出）
Eigen::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.transpose() << endl;//行向量显示，transpose是转置矩阵的意思
//以下用欧拉矩阵求解p2坐标
Eigen::Isometry3d T1w(q1), T2w(q2);//欧式变换矩阵Isometry（虽然称为3d，实质上是4＊4的矩阵）
T1w.pretranslate(t1);//设置平移向量，我的理解是加入这个平移向量
T2w.pretranslate(t2);
Eigen::Vector3d p3;//用来表示小萝卜二号坐标系下该点坐标（变换矩阵方法）
p3 = T2w * T1w.inverse() * p1;//求解p3坐标
cout << "变换矩阵/欧拉矩阵求得的p2坐标" << endl;
cout << p3.transpose() << endl;
}
以上两个输出结果是一致的。