题目:假设两个物体位于世界坐标系中,物体1号位姿为q1(四元数表示旋转),t1(表示平移);物体2号位姿为q2(四元数表示旋转),t2(表示平移);现已知物体1号看到某个点在自身坐标系下的坐标为p1,求该点在物体2号坐标系下的坐标p2。
解析:记世界坐标系为W,物体坐标系分别为R1,R2。T表示坐标系之间的变换关系。则公式为 p 2 = T 2 W ∗ T W 1 ∗ p 1 p2=T_{2W} * T_{W1} * p1 p2=T2W∗TW1∗p1,且q1,t1是 T 1 W T_1W T1W,需要求逆。
注意:四元数使用之前需要归一化。
#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);
Isometry3d T1w(q1), T2w(q2);
T1w.pretranslate(t1);
T2w.pretranslate(t2);
Vector3d p2 = T2w * T1w.inverse() * p1;
cout << endl << p2.transpose() << endl;
return 0;
}