高翔视觉slam十四讲习题（1）

一、题目

设有小萝卜一号和小萝卜号位于世界坐标系中。
小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 =[0.7, 1.1, 0.2] T (q 的第一项为实部)。这里的 q 和 t 表达的是 T cw ,也就是世界到相机的变换关系。
小萝卜二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。
现在,小萝卜一号看到某个点在自身的坐标系下,坐标为 p 1 = [0.5, −0.1, 0.2] T ,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交你的程序。
提示:

1. 四元数在使用前需要归一化。
2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。
3. 参考答案为 p 2 = [1.08228, 0.663509, 0.686957] T 。你可以用它验证程序是否正确。

二、代码

#include "iostream"
#include "ctime"
using namespace std;

#include "Eigen/Core"
// 包含几何模块，提供了各种旋转和平移的表示
#include "Eigen/Geometry"

int main(int argc, char** argv)
{
    Eigen::Quaterniond q1 = {0.55, 0.3, 0.2, 0.2};//注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    //cout .precision(3);//设置输出的有效数字位数
    cout << "q1 = \n" << q1.coeffs() << endl;
    //四元数使用前须归一化
    q1.normalize();
    cout << "normalize q1 = \n" << q1.coeffs() << endl;
    //Eigen::Vector3d v_vector = {0.7, 1.1, 0.2};
    //T是单位四元数
    //Eigen::Matrix3d R1 = q1.toRotationMatrix();//旋转矩阵R1，将四元数变成旋转矩阵R
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();//欧式变换矩阵使用 Eigen::Isometry
    //T1.rotate(R1);//旋转
    T1.rotate(q1);
    T1.pretranslate(Eigen::Vector3d (0.7, 1.1, 0.2));//平移
    cout << T1.matrix() << endl;

    Eigen::Quaterniond q2 = {-0.1, 0.3, -0.7, 0.2};//注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    cout << "q2 = \n" << q2.coeffs() << endl;
    //四元数使用前须归一化
    q2.normalize();
    cout << "normalize q2 = \n" <<q2.coeffs() << endl;
    //T是单位四元数
    //Eigen::Matrix3d R2 = q2.toRotationMatrix();//旋转矩阵R2
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();//欧式变换矩阵使用 Eigen::Isometry
    //T2.rotate(R2);//旋转
    T2.rotate(q2);
    T2.pretranslate(Eigen::Vector3d (-0.1, 0.4, 0.8));//平移
    cout << T2.matrix() << endl;

    Eigen::Vector4d p1 = {0.5, -0.1, 0.2, 1};
    Eigen::Vector4d pw;
    clock_t clk_time = clock();
    pw = T1.matrix().colPivHouseholderQr().solve(p1);//利用QR分解求出世界坐标
    cout << pw.transpose() << endl;
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - clk_time)/(double)CLOCKS_PER_SEC <<"ms" << endl;
    Eigen::Vector4d p2;
    p2 = T2*pw;
    cout << p2.transpose() << endl;
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - clk_time)/(double)CLOCKS_PER_SEC <<"ms" << endl;
    return 0;
}

三、运行结果