簡單座標變換 ch3 （7）

問題描述

機器人一號和二號，分別在世界座標系中。
一號的位姿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 的第一項是實部，且還未歸一化。
已知一號機器人看到某個點，在他的座標系下是p=[0.5,0,0.2]T , 求在二號機器人座標系下該點的位置。

使用三種方法實現

//#pragma GCC diagnostic error "-std=c++14"
#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 幾何模塊
#include <Eigen/Geometry>

Eigen::Quaterniond q_one(Eigen::Quaterniond q)
{
    auto add = q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w();
    add = sqrt(add);
    q.x() /= add;
    q.y() /= add;
    q.z() /= add;
    q.w() /= add;

    return q;
}


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 );

     //q1.normalize();

     auto qq1 = q1.normalized();
     auto qq2 = q2.normalized();

     //auto qq1 = q_one(q1);
     //auto qq2 = q_one(q2);

     //way1
     Eigen::Vector3d v1 (0.5,0,0.2);
     Eigen::Vector3d v = qq1.inverse()*(v1 - t1);
     Eigen::Vector3d v2 = qq2*v + t2;
     cout << "v2= " << endl << v2 << endl;


     //way2
     Eigen::Matrix3d R1 = Eigen::Matrix3d(qq1);
     Eigen::Matrix3d R2 = Eigen::Matrix3d(qq2);
     Eigen::Vector4d vv1 (0.5,0,0.2,1);

     Eigen::Matrix4d T1 ,T2;
     T1 << R1(0,0), R1(0,1), R1(0,2), t1(0),\
           R1(1,0), R1(1,1), R1(1,2), t1(1),\
           R1(2,0), R1(2,1), R1(2,2), t1(2),\
             0, 0, 0, 1;
     T2 << R2(0,0), R2(0,1), R2(0,2), t2(0),\
           R2(1,0), R2(1,1), R2(1,2), t2(1),\
           R2(2,0), R2(2,1), R2(2,2), t2(2),\
             0, 0, 0, 1;

     Eigen::Vector4d vv2 = T2 * (T1.inverse() * vv1);
     cout << "vv2= "<< endl << vv2 << endl;

     //way3
     Eigen::Vector3d v_3_1 (0.5,0,0.2);
     Eigen::Vector3d v_3 = R1.inverse()*(v1 - t1);
     Eigen::Vector3d v_3_2 = R2*v_3 + t2;
     cout << "vvv2= " << endl << v_3_2 << endl;