视觉slam十四讲课后习题ch3-7

题目回顾:

设有小萝卜一号和小萝卜二号位于世界坐标系中，小萝卜一号的位姿为：q1=[0.35,0.2,0.3,0.1],
t2=[0.3,0.1,0.1]^T (q的第一项为实部。请你把q归一化后在进行计算)。这里的q和t的表达的是Tcw，也就是世界到相机的变换关系。小萝卜二号的位姿为q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^T.现在，小萝卜一号看到某个点在自身的坐标系下，坐标为p=[0.5,0,0.2]^T ,求该向量在小萝卜二号坐标系下的坐标，请编程实现此事。
解:
pw:某个点在世界坐标系下的坐标
T_1w :表示世界坐标系到小萝卜一号坐标系的变换关系
T_2w:表示世界坐标系到小萝卜二号坐标系的变换关系
P2 :表示该点在小萝卜二号坐标系下的坐标（即为所求）
单位四元数到旋转矩阵R的变化关系可参考书上55页。之后变换矩阵T=[R t]
[0 1]
由变换关系可列出下面的式子:
p = T_1w * Pw 可解出来pw
p2=T_2W*pW 带入上式解出来的Pw即可求出来p2

具体代码实现如下:

 1 #include<iostream>
 2 #include<Eigen/Core>
 3
 4 //包含几何模块
 5 #include<Eigen/Geometry>
 6 using namespace std;
 7
 8 int main(int argc,char **argv)
 9 {
10     /*变量定义*/
11     Eigen::Quaterniond Q1(0.2,0.3,0.1,0.35); //四元数的表示(w ,x,y,z)
12     Eigen::Quaterniond Q2(0.4,-0.1,0.2,-0.5);
13     Eigen::Vector3d t1(0.3,0.1,0.1);
14     Eigen::Vector3d t2(-0.1,0.5,0.3);
15     Eigen::Vector3d p(0.5,0,0.2);   //在一号小萝卜下的坐标
16     Eigen::Vector3d pw ;            //世界坐标
17     Eigen::Vector3d p2;             //求在二号小萝卜的坐标 p2
18
19    /*欧氏变换矩阵使用Eigen::Isometry */
20     Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
21     Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();
22
23     /*归一化*/
24     Q1.normalize();
25     Q2.normalize();
26
27     /*输出归一化参数*/
28 //    cout<<"Q1 is "<<Q1.x()<<endl<<Q1.y()<< endl <<Q1.z()<< endl<<Q1.w()<<endl;
29 //    cout<<"Q2 is "<<Q2.x()<<endl<<Q2.y()<< endl <<Q2.z()<< endl<<Q2.w()<<endl;
30
31     cout<<"after normalize； "<< endl << Q2.coeffs()<<endl;
32
33     /*设置变换矩阵的参数*/
34     T_1w.rotate(Q1);
35     T_1w.pretranslate(t1);
36     T_2w.rotate(Q2);
37     T_2w.pretranslate(t2);
38
39     /* p = T1w * pw  求解pw*/
40     pw = T_1w.inverse() * p;
41
42     /* p2 = T_2w * pw  求解p2*/
43     p2 = T_2w * pw;
44
45     /*输出在小萝卜二号下的该点坐标*/
46     cout<<"该点在小萝卜二号下的坐标为: "<<p2.transpose()<<endl;
47
48     return 0;
49 }

转载于:https://www.cnblogs.com/newneul/p/8306456.html

视觉slam十四讲课后习题ch3-7相关推荐

视觉slam十四讲课后习题ch3--5题
题目回顾: 假设有一个大的Eigen矩阵,我想把它的左上角3x3块提取出来,然后赋值为I3x3.编程实现. 解:提取大矩阵左上角3x3矩阵,有两种方式: 1.直接从0-2循环遍历大矩阵的前三行和三列 ...
乔利斯基三角分解_《视觉SLAM十四讲课后作业》第二讲
1.设线性⽅程 Ax = b,在 A 为⽅阵的前提下,请回答以下问题: 1. 在什么条件下,x 有解且唯⼀? 非齐次线性方程在A的秩与[A|B]的秩相同时方程有解,当R(A)=R(A,B)=n时方程有 ...
高翔视觉slam十四讲书籍习题(第一讲)
可百度查询 Ax=b:可解性和解的结构假设A为m x n 的矩阵对增广矩阵(A,b)进行初等行变换,化成最简式, 若A的秩不等于(A,b)的秩,方程组无解若A的秩等于(A,b)的秩且小于n,方程 ...
视觉SLAM十四讲CH3代码解析及课后习题详解
eigenMatrix.cpp #include <iostream>using namespace std;#include <ctime> // Eigen 核心部分 #i ...
视觉SLAM十四讲slambook2/ch3/examples/plotTrajectory.cpp程序报错解决
视觉SLAM十四讲slambook2/ch3/examples/plotTrajectory.cpp程序报错解决原程序编译后出现如下报错: /usr/local/include/pangolin/v ...
视觉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十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记
视觉SLAM十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记一.创建KDevelop项目二.编写程序一.创建KDevelop项目你的电脑上如果还没有安装 ...
视觉SLAM十四讲CH6代码解析及课后习题详解
gaussNewton.cpp #include <iostream> #include <chrono> #include <opencv2/opencv.hpp> ...
《视觉slam十四讲从理论到实践》第一讲习题自测解答
0x00 前言 <视觉slam十四讲从理论到实践>第一讲习题自测解析. 借助自身知识储备和搜索引擎后完成习题,仅供参考. 部分答案会觉得没有说明的必要就会略 0x01 习题部分 1.有 ...

视觉slam十四讲课后习题ch3-7

视觉slam十四讲课后习题ch3-7相关推荐

最新文章

热门文章