几何运算练习（使用 Eigen/Geometry 计算⼀个具体的例子，欧拉角，旋转向量）

设有⼩萝⼘ 1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： q1 = [0:55; 0:3; 0:2; 0:2]; t1 =[0:7; 1:1; 0:2]T（q 的第⼀项为实部）。这⾥的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。⼩萝⼘⼆号的位姿为 q2 = [-0:1; 0:3; -0:7; 0:2]; t2 = [-0:1; 0:4; 0:8]T。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下，坐标为 p1 = [0:5; -0:1; 0:2]T，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交你的程序。

1.题目基础知识

题中的q1和q2是四元数，一共四个项，第一项为实部，后三项为虚部，具体结构如下（懒得敲公式了，直接粘图）：

其中角θ为旋转的角度大小，nx，ny，nz为旋转轴的xyz坐标。

q表示了旋转，t表示平移，组合为小萝卜的位姿

四元数使用方法：

其中q为四元数，

p为原始坐标为

p‘为旋转后坐标。

2. 解题思路：

思路上挺简单的，就是两个坐标转换，把点坐标从小萝卜一号自身坐标系转到时间坐标系，再从世界坐标系转到小萝卜二号的自身坐标系。

3. 注意事项

• 四元数在使⽤前需要归⼀化。

主要有两个原因：第一个是四元数的逆计算如下，当四元数归一化之后，其逆即为其共轭

第二个是如果将四元数转为旋转矩阵，四元数不归一化的话求出来的旋转矩阵不正交，也就是错误的

• 请注意 Eigen 在使⽤四元数时的虚部和实部顺序

Eigen使用四元数时顺序是先虚数后实数，与平常相反。

3. 具体编程

参考：

Eigen库之Quaterniond_eigen::quaterniond-CSDN博客

Eigen四元数文档

#include <iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>//用于四元数、旋转矩阵、rpy角和旋转向量之间的转换
#include<cmath>
using namespace std;
using namespace Eigen;

int main(int, char**){
    //四元数定义（虚部在前）
    Quaterniond q1(0.55,0.3,0.2,0.2);
    Quaterniond q2(-0.1,-0.3,-0.7,0.2);
    cout<<q1.coeffs()<<endl;//打印输出，前三个为虚部，最后为实部
    //归一化
    q1.normalize();
    q2.normalize();
    //坐标转换
    Vector3d p1(0.5,-0.1,0.2);
    Vector3d t1(0.7,1.1,0.2);
    Vector3d t2(-0.1,0.4,0.8);
    //定义刚体变换
    Isometry3d TW1(q1);
    Isometry3d TW2(q2);
    //先进行预平移
    TW1.pretranslate(t1);
    TW2.pretranslate(t2);
    Vector3d p2=TW2*TW1.inverse()*p1;
    cout<<p2.transpose()<<endl;
    return 0;

    
}

抛出来结果如下： 

 0.3
 0.2
 0.2
0.55
 1.08228 0.663509 0.686957

说明四元数输出，前三个为虚部，最后为实部，结果与实际一致，计算正确