高翔视觉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 ,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交你的程序。
提示:

四元数在使用前需要归一化。
请注意 Eigen 在使用四元数时的虚部和实部顺序。
参考答案为 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，将四元数变成旋转矩阵REigen::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();//旋转矩阵R2Eigen::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;
}

三、运行结果

高翔视觉slam十四讲习题（1）相关推荐

高翔视觉SLAM十四讲（第二版）各种软件、库安装的以及报错解决方法
目录前言系统版本下载高翔视觉SLAM十四讲第二版的源代码一.安装 Vim 二.安装 g++ 三.安装 KDevelop 以及汉化 1.安装 2.汉化四.安装 Eigen 库五.安装 Pan ...
高翔视觉slam十四讲书籍习题(第一讲)
可百度查询 Ax=b:可解性和解的结构假设A为m x n 的矩阵对增广矩阵(A,b)进行初等行变换,化成最简式, 若A的秩不等于(A,b)的秩,方程组无解若A的秩等于(A,b)的秩且小于n,方程 ...
高翔视觉SLAM十四讲课本代码运行
刚开始学这本书,想先把书上的大小案例都跑一下. 这篇文章记录了中间踩过的各种大小坑,有的是因为版本更新,有的是因为自己真的蠢哈哈. 感谢文中提到的链接博文作者! 同学们有问题可以交流-大家加油鸭首先 ...
【视觉SLAM入门】一些关于视觉SLAM十四讲的重要基础概念
文章目录高翔. *视觉SLAM十四讲从理论到实践*. 电子工业出版社, 2017. Print. 第二章:初识SLAM 第三章:三维空间刚体运动第四章:李群与李代数第五章:相机与图像第六章: ...
《视觉SLAM十四讲》第二版源码slambook2编译调试
slambook2-master/ch2 编译正常,log如下: slambook2-master/ch2# mkdir build && cd build && cm ...
视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...
视觉SLAM十四讲CH8代码解析及课后习题详解
第一版的代码: direct_semidense.cpp #include <iostream> #include <fstream> #include <list> ...
视觉SLAM十四讲（高翔第二版)
视觉SLAM十四讲高翔第二版调试记录前言一.3.7可视化演示程序运行全过程Pangolin plotTrajectory.cpp 1. **在3rdparty中下载Pangolin** 2. ** ...
《视觉SLAM十四讲第二版》课后习题
本文为<视觉SLAM十四讲>(第二版)的课后习题解答,为本人学习时参考着网上的资源所写的答案,可能有所纰漏,希望大家指出. 文章目录第1讲预备知识第2讲初始SLAM 第3讲三维空 ...
视觉SLAM十四讲CH6代码解析及课后习题详解
gaussNewton.cpp #include <iostream> #include <chrono> #include <opencv2/opencv.hpp> ...

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

一、题目

二、代码

三、运行结果

高翔视觉slam十四讲习题（1）相关推荐

最新文章

热门文章