Eigen 世界坐标系和相机坐标系之间的转换

博客转载自： https://www.cnblogs.com/-Mr-y/p/7737990.html

机器人一号和二号，分别在世界坐标系中。 
一号的位姿q1=[0.35,0.2,0.3,0.1], t1=[0.3,0.1,0.1]T。 
二号的位姿q2=[−0.5,0.4,−0.1,0.2], t2=[−0.1,0.5,0.3]T。 
q的第一项是实部，且还未归一化。 q和t表达的是T_cw也就是世界坐标系到相机坐标系的变换关系。
已知一号机器人看到某个点，在他的坐标系下是p=[0.5,0,0.2]T, 求在二号机器人坐标系下该点的位置。

#include <iostream>
#include <cmath>

// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>
//Eigen 几何模块
#include <Eigen/Geometry>

using namespace std;


int main(int argc, char **argv) 
{
    Eigen::Quaterniond q1(0.35, 0.2, 0.3, 0.1);
    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 p1(0.5, 0, 0.2);
    
    Eigen::Quaterniond q1_one = q1.normalized();
    Eigen::Quaterniond q2_one = q2.normalized();
  
    //way1
    Eigen::Vector3d v = q1_one.inverse() * (p1 - t1);
    Eigen::Vector3d v2 = q2_one * v + t2;
    cout << "way1 v2 = " << endl << v2.transpose() << endl;
    
    //way2    
    Eigen::Matrix3d R1 = Eigen::Matrix3d(q1_one);
    Eigen::Matrix3d R2 = Eigen::Matrix3d(q2_one);
    Eigen::Vector3d v_2 = R1.inverse() * (p1 - t1);
    Eigen::Vector3d v_2_2 = R2 * v_2 + t2;
    cout << "way2 v2= " << endl << v_2_2.transpose() << endl;
     
    return 0;
}

ubuntu 14.04运行结果如下: 

其实都是先计算出点在世界坐标系下的坐标位置，之后利用另外相机的Tcw转换矩阵进行转换到相应相机坐标系下的坐标位置