word相机坐标转换(手动计算)

知道基坐标到相机的变换参数，求在相机中的点在基座标下的姿态

/*
*	已知手眼标定参数 t1 和四元数q1(base->camrea)的参数
*	求解旋转变换后点的坐标
*   2021.09.15
*/

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

using namespace std;
int main()
{
	// 世界坐标系到相机坐标系的变换参数Q 和T 。
	// 以下为模拟数据
    double Rr,Rp,Ry;
    double Qw,Qx,Qy,Qz;
    double Tx,Ty,Tz;
    Qw = 0.707;
    Qx = 0;
    Qy = 0;
    Qz = 0.707;
    Tx = 1;
    Ty = 1;
    Tz = 1;

    // 平移参数
    Eigen::Vector3d  t1= Eigen::Vector3d(Tx,Ty,Tz);
    // const Scalar& w, const Scalar& x, const Scalar& y, const Scalar& z
    // 旋转参数
    Eigen::Quaterniond q1=Eigen::Quaterniond(Qw,Qx,Qy,Qz);
    cout<<"T: "<<t1[0] <<" " <<t1[1] <<" " <<t1[2]<<endl;
    cout<<"Q: "<<q1.w()<<" "<<q1.x()<<" "<<q1.y()<<" "<<q1.z()<<endl;

    Eigen::Matrix3d rx = q1.toRotationMatrix();
    Eigen::Vector3d ea = rx.eulerAngles(2,1,0); //面按照先旋转ｘ轴(0)，然后y轴(1)，最后z轴得到的角度
    cout<<"eular angle(xyz): "<<(180/M_PI)*ea<<endl;

    // 相机坐标系里的一个点姿态
    double TObjx,TObjy,TObjz;
    double QObjw,QObjx,QObjy,QObjz;
    QObjw = 1;
    QObjx = 0;
    QObjy = 0;
    QObjz = 0;
    TObjx = 1;
    TObjy = 1;
    TObjz = 3;
    Eigen::Vector3d  ObjT= Eigen::Vector3d(TObjx, TObjy, TObjz);
    Eigen::Quaterniond ObjQ=Eigen::Quaterniond(QObjw, QObjx, QObjy, QObjz);
    cout<<"Obj Pose: "<<ObjT<<endl;
    cout<<"Obj Oriention: "<<ObjQ.vec()<<endl;
    Eigen::Matrix3d rx1 = ObjQ.toRotationMatrix();
    Eigen::Vector3d ea1 = rx1.eulerAngles(2,1,0); //面按照先旋转ｘ轴(0)，然后y轴(1)，最后z轴得到的角度
    cout<<"eular angle(xyz): "<<(180/M_PI)*ea1<<endl;
    cout<<"eular angle(zyx)"<<(180/M_PI)*ObjQ.matrix().eulerAngles(0,1,2)<<endl;
     
    // 计算在世界坐标系下的姿态
    Eigen::Quaterniond q2=ObjQ*q1;
   // cout<<" q2 vector " <<q2.vec()<<endl;
    // Eigen::Vector3d t2=t1+q1.toRotationMatrix()*ObjT;
    Eigen::Vector3d t2=t1+(ObjT.transpose()*q1.toRotationMatrix().inverse()).transpose();

    cout<<"t2="<<t2<<endl;
    cout<<"t2 eular angle "<<(180/M_PI)*q2.matrix().eulerAngles(2,1,0)<<endl;

}