视觉SLAM十四讲中P61作业7。

设有小萝卜 1 一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 = [0.7, 1.1, 0.2] T (q 的第一项为实部)。这里的 q 和 t 表达的是 T cw ,也就是世界到相机的变换关系。小萝卜
二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。现在,小萝卜一号看到某个点在自身的坐
标系下,坐标为 p 1 = [0.5, −0.1, 0.2] T ,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交
你的程序。
提示:

1. 四元数在使用前需要归一化。
2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。
3. 参考答案为 p 2 = [1.08228, 0.663509, 0.686957] T 。你可以用它验证程序是否正确。

int main ( int argc, char** argv )
{
  Eigen::Quaternion<double> q1(0.55,0.3, 0.2, 0.2 );//小萝卜1号坐标相对于世界坐标的四元数
  Eigen::Quaternion<double> q2(-0.1,0.3, -0.7, 0.2);//小萝卜2号坐标相对于世界坐标的四元数
  q1.normalize();//四元数归一化
  q2.normalize();//四元数归一化
  
  Eigen::Vector3d t1(0.7, 1.1, 0.2);//小萝卜1号坐标相对于世界坐标的平移向量
  Eigen::Vector3d t2(-0.1, 0.4, 0.8);//小萝卜2号坐标相对于世界坐标的平移向量
  
  Eigen::Matrix3d R1 = q1.toRotationMatrix();//小萝卜1号的四元数 转为 旋转矩阵
  Eigen::Matrix3d R2 = q2.toRotationMatrix();//小萝卜2号的四元数 转为 旋转矩阵
  
  Eigen::Isometry3d Tcw1(R1);//创建小萝卜1号相对于世界坐标的欧氏变换矩阵，并用旋转矩阵 初始化  
  Eigen::Isometry3d Tcw2(R2);//创建小萝卜2号相对于世界坐标的欧氏变换矩阵，并用旋转矩阵 初始化   
  Tcw1.pretranslate(t1);//接着，用平移向量初始化小萝卜1号的欧氏变换矩阵
  Tcw2.pretranslate(t2);//接着，用平移向量初始化小萝卜2号的欧氏变换矩阵
  
  Eigen::Vector3d p1(0.5, -0.1, 0.2);//点d在小萝卜1号坐标下的向量表示
  Eigen::Vector3d pd_w = Tcw1.inverse() * p1;//算出点d在世界坐标下的向量表示，其中，Tcw表示world to camera 的坐标变换，Tcw.inverse表示camera to world的变换
  Eigen::Vector3d p2 = Tcw2 * pd_w;//算出点d在小萝卜2号的向量表示
  
  cout << p2.transpose() << endl;
  
  return 0;
}