ch3 3.6.2实际的坐标变换例子 是否有错误

[LadissonLai]

为了验证程序的正确性，我把萝卜一号的位姿设置为(0.5, 0.5, 0)且没有旋转，萝卜二号的位置设置为坐标原点且没有旋转（即世界坐标原点），设萝卜一号坐标系下看到的p1=(0.5, 0.5, 0)，则萝卜二号下的看到p1的坐标为(1, 1, 0)。但是得到的结果却是(0, 0, 0)，因此，这个例子是否有误。下面是在原来代码coordinateTransform.cpp基础上修改的。

#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);
  Quaterniond q1(1, 0, 0, 0), q2(1, 0, 0, 0);
  q1.normalize();
  q2.normalize();
  // Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d t1(0.5, 0.5, 0), t2(0, 0, 0);
  // Vector3d p1(0.5, 0, 0.2);
  Vector3d p1(0.5, 0.5, 0);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  Vector3d p2 = T2w * T1w.inverse() * p1;  // 原程序有误
  cout << endl << p2.transpose() << endl;

  return 0;
}

上面程序结果是： 0 0 0

我觉得这里T1w应该表示的从萝卜一号到世界坐标的坐标变换矩阵，应该叫做Tw1才对。 因此，我修改计算代码为：

Vector3d p2 = T2w.inverse() * T1w * p1;

最终得到正确答案: （1， 1， 0）。 修改后完整代码如下：

#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);
  Quaterniond q1(1, 0, 0, 0), q2(1, 0, 0, 0);
  q1.normalize();
  q2.normalize();
  // Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
  Vector3d t1(0.5, 0.5, 0), t2(0, 0, 0);
  // Vector3d p1(0.5, 0, 0.2);
  Vector3d p1(0.5, 0.5, 0);

  Isometry3d T1w(q1), T2w(q2);
  T1w.pretranslate(t1);
  T2w.pretranslate(t2);

  // Vector3d p2 = T2w * T1w.inverse() * p1; // 原程序有误
  Vector3d p2 = T2w.inverse() * T1w * p1; // 修改部分
  cout << endl << p2.transpose() << endl; 

  return 0;
}

上面程序结果是： 1 1 0 正确。 请问大家是怎么理解的？

[byheng]

我也认为这里的代码有错误，我是这样理解的： Tw1和Tw2分别为小萝卜一号和小萝卜二号从世界坐标系原点变换到R1和R2点的变换过程，小萝卜一号在R1位置观测到p点在自身坐标系下的坐标为p1。 记p点的世界坐标为pw，那么pw的求解可以分为两个过程：第一个过程是小萝卜一号从世界坐标系原点变换到R1点的过程，对应为Tw1，第二个过程是小萝卜一号在R1点观测记录p点在自身坐标系下位置的过程，对应为p1，所以对于p点世界坐标pw的求解就是这两个过程的叠加，即pw = Tw1 p1. 对于p点在小萝卜二号坐标系下观测的坐标可以这样求解：根据上述过程同样的方式，小萝卜二号变换到R2点位置，变换过程为Tw2，然后记录下在该位置观测p点坐标p2，可以得到pw = Tw2 p2，于是p2 = Tw2.reverse() pw = Tw2.reverse() Tw1 * p1.