slambook2+ch3+Eigen使用

代码

#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

int main() {
    Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
    q1.normalize();     //归一化四元数，不归一化可能导致旋转矩阵不正交
    q2.normalize();
    Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
    Vector3d p1(0.5, 0, 0.2);

    //将四元数表示的旋转q平移t转换成R，t
    Isometry3d T1w(q1), T2w(q2);
    T1w.pretranslate(t1);
    T2w.pretranslate(t2);

    Vector3d p2 = T2w * T1w.inverse() *p1;
    cout << endl << p2.transpose() << endl;
    return 0;
}

结果

-0.0309731    0.73499   0.296108