SLAM中坐标系的转换问题

题目回顾:

设有小萝卜一号和小萝卜二号位于世界坐标系中，小萝卜一号的位姿为：q1=[0.35,0.2,0.3,0.1],
t2=[0.3,0.1,0.1]^T (q的第一项为实部。请你把q归一化后在进行计算)。这里的q和t的表达的是Tcw，也就是世界到相机的变换关系。小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^{T.现在，小萝卜一号看到某个点在自身的坐标系下，坐标为p=[0.5,0,0.2]}T ,求该向量在小萝卜二号坐标系下的坐标，请编程实现此事。
解:
pw:某个点在世界坐标系下的坐标
T_1w :表示世界坐标系到小萝卜一号坐标系的变换关系
T_2w:表示世界坐标系到小萝卜二号坐标系的变换关系
P2 :表示该点在小萝卜二号坐标系下的坐标（即为所求）
单位四元数到旋转矩阵R的变化关系可参考书上55页。之后变换矩阵T=[R t]
[0 1]
由变换关系可列出下面的式子:
p = T_1w * Pw 可解出来pw
p2=T_2W*pW 带入上式解出来的Pw即可求出来p2

具体代码实现如下:
放出两个版本

这里q1的顺序是 qw qx qy qz ，需要注意的是C++中调用的Eigen模块不需要调整顺序，而python中调用的Rotation.from_quat要求顺序是qx,qy,qz,qw，是否归一化不影响结果

C++版本

#include<iostream>
#include<Eigen/Core>

//包含几何模块
#include<Eigen/Geometry>
using namespace std;

int main(int argc, char** argv)
{
    /*变量定义*/
    Eigen::Quaterniond Q1(0.35, 0.2, 0.3, 0.1); //四元数的表示(w,x,y,z)
    Eigen::Quaterniond Q2(-0.5, 0.4, -0.1, 0.2);
    Eigen::Vector3d t1(0.3, 0.1, 0.1);
    Eigen::Vector3d t2(-0.1, 0.5, 0.3);
    Eigen::Vector3d p(0.5, 0, 0.2);   //在一号小萝卜下的坐标
    Eigen::Vector3d pw;            //世界坐标
    Eigen::Vector3d p2;             //求在二号小萝卜的坐标 p2

    /*欧氏变换矩阵使用Eigen::Isometry */
    Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();

    /*归一化*/
    Q1.normalize();
    Q2.normalize();

    /*输出归一化参数*/
//    cout<<"Q1 is "<<Q1.x()<<endl<<Q1.y()<< endl <<Q1.z()<< endl<<Q1.w()<<endl;
//    cout<<"Q2 is "<<Q2.x()<<endl<<Q2.y()<< endl <<Q2.z()<< endl<<Q2.w()<<endl;

    cout << "after normalize； " << endl << Q1.coeffs() << endl;

    /*设置变换矩阵的参数*/
    T_1w.rotate(Q1);
    T_1w.pretranslate(t1);
    T_2w.rotate(Q2);
    T_2w.pretranslate(t2);

    /* p = T1w * pw  求解pw*/
    pw = T_1w.inverse() * p;

    /* p2 = T_2w * pw  求解p2*/
    p2 = T_2w * pw;

    
    /*输出在小萝卜二号下的该点坐标*/
    cout << "该点在小萝卜二号下的坐标为: " << p2.transpose() << endl;

    return 0;
}

python版本

import numpy as np

t1 = np.array([0.3,0.1,0.1,0.2,0.3,0.1,0.35])       # tx,ty,tz,qx,qy,qz,qw
t2 = np.array([-0.1,0.5,0.3,0.4,-0.1,0.2,-0.5])

p1 = np.array([[0.5],[0],[0.2],[1]])

# tx ty tz qx qy qz qw

def normlized(pose):
    pose[3:] = pose[3:]/np.linalg.norm(pose[3:])
    return pose

def pose_matrix_from_quaternion(pvec):
    """ convert 4x4 pose matrix to (t, q) """
    from scipy.spatial.transform import Rotation

    pose = np.eye(4)
    pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix()
    pose[:3, 3] = pvec[:3]
    return pose

# t1 = normlized(t1)
# t2 = normlized(t2)

w2c_1 = pose_matrix_from_quaternion(t1)
w2c_2 = pose_matrix_from_quaternion(t2)

print(w2c_1)
print(w2c_2)

p = w2c_2@np.linalg.inv(w2c_1)@p1

print(p.shape)
print(p)

输出结果：