(《视觉SLAM十四讲》第三次演习问题7 )萝卜1号和2号设置在世界坐标系中。 1号姿势Q1=[ 0.35，0.2，0.3，0.1 ]，t1=[ 0.3，0.1，0.1 ]。 2号姿势Q2=[-0.5，0.4，- 0.1，0.2 ]，T2=[-0.1，0.5，0.3 ] .某点在1号坐标系中的坐标为p=[ 0.5，0，0.2 ] . 求出p在2号坐标系中的坐标
设世界坐标系中p点的坐标为p。
四元数用于旋转时如下所示。 在Eigen中四元数旋转为qv，在数学中为qvq^-1 )。
q1 P t1=p1
q2 P t2=p2
分别由上2式计算出：
p=Q1^-1(P1-T1 ) ) ) ) ) )。
p=Q2^-1(P2-T2 ) ) )
解除二式联立后如下。
P2=Q2Q1^-1(P1-T1 ) t2
使用欧拉矩阵(设第1个欧拉矩阵为T1，第2个欧拉矩阵为T2 )，如下所示。
p1=T1 P
p2=T2 P
求p :
P=T1^-1 p1
P=T2^-1 p2
解除联合的话如下。
p2=T2 T1^-1 p1
以下是用Eigen实现的代码。
#包含
using namespace std；
#包含
#包含
int main () )
{
//四元数
eigen :3360 quaternion dq1=eigen :3360 quaternion d (0.35，0.2，0.3，0.1 ).normalized )；
eigen :3360 quaternion dq2=eigen : quaternion d (-0.5，0.4，- 0.1，0.2 ).normalized )；
//平移向量
eigen :3360 vector 3d t1=eigen :3360 vector 3d (0.3、0.1、0.1 )；
eigen :3360 vector 3d T2=eigen :3360 vector 3d (-0.1、0.5、0.3 )；
//目标矢量
eigen :3360 vector 3d P1=eigen :3360 vector 3d (0.5，0，0.2 )；
Eigen:Vector3d p2；
//打印输出
//cout q1.coeffs () () ) (n )。
//q2.coeffs () ) (n ) )。
//t1.transpose (() ) (n ) )。
//t2.transpose () endl；
//四元数解决
p2=q2 * q1.inverse () * ) P1-T1 ) t2；
cout p2.transpose () endl；
//欧拉矩阵
eigen :3360 isometry 3d t1=eigen : isometry 3d 33603360 identity (；
eigen :3360 isometry 3d T2=eigen : isometry 3d 33603360 identity (；
T1.rotate(Q1.torotationmatrix ) )；
t1 .预转换(t1；
T2 .旋转(Q2.torotationmatrix ) )；
T2 .预转换(T2；
//cout T1.matrix () endl；
//cout T2.matrix () endl；
//用欧拉矩阵求解
p2=T2 * T1.inverse () * p1；
cout p2.transpose () endl；
}