SLAM十四讲第三章课后习题

第5题：参考这篇博客，Eigen块操作讲的很细！

Matrix4f m;
m<< 1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16;
cout<<m.block<2,2>(0,0)<<endl;
//或者
for(int i=0;i<4;i++)
{cout<<m.block(0,0,i,i)<<endl;
}

第7题：

设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为 $q_{1}=\left [ \begin{matrix} 0.35, & 0.2, & 0.3, & 0.1 \end{matrix} \right ]$ ， $t_{1}=\left [ \begin{matrix} 0.3 & 0.1 & 0.1 \end{matrix} \right ]^{T}$ ,这里 $q$ 和 $t$ 表达的是 $T_{cw}$ ，也就是世界坐标系到相机坐标系的变换关系。小萝卜二号位姿为 $q_{2}=\left [ \begin{matrix} -0.5, & 0.4, & -0.1, & 0.2 \end{matrix} \right ]$ ， $t_{2}=\left [ \begin{matrix} -0.1, & 0.5, & 0.3 \end{matrix} \right ]^{T}$ 。现在小萝卜一号看到一个点在自身坐标系下坐标为 $p=\left [ \begin{matrix} 0.5, & 0, & 0.2 \end{matrix} \right ]^{T}$ ，求该点在小萝卜二号坐标系下的坐标。请编程实现。

遇到的问题（未解决）

1、归一化 第一次忘了归一化，发现是否归一化会对结果会有影响，不知道为什么？

2、pretranslate()这个函数，加与不加pre，二者有什么不同？

#include<iostream>
#include<Eigen/Dense>
#include<Eigen/Geometry>
using namespace Eigen;
int main()
{
//小萝卜一号位姿Tcw
Quaternion<float> q1TW(0.35,0.2,0.3,0.1);
Vector3f t1(0.3,0.1,0.1);
//小萝卜二号位姿Tcw
Quaternionf q2TW(-0.5,0.4,-0.1,0.2);
Vector3f t2(-0.1,0.5,0.3);//小萝卜一号看到的自身坐标系下的坐标点p
Vector3f p1_camera(0.5,0,0.2);//归一化 第一次忘了归一化，发现是否归一化会对结果会有影响，不知道为什么
q1TW.normalize();
q2TW.normalize();//Tcw1
Isometry3f T1_World2Camera=Isometry3f::Identity();
T1_World2Camera.rotate(q1TW);
T1_World2Camera.pretranslate(t1);Isometry3f T2_World2Camera=Isometry3f::Identity();
T2_World2Camera.rotate(q2TW);
T2_World2Camera.pretranslate(t2);//p点在全局的坐标
Vector3f p2_world=T1_World2Camera.inverse()*p1_camera;
//p点在二号小萝卜二号坐标系下的坐标
Vector3f p2_camera=T2_World2Camera*p2_world;
std::cout<<p2_camera<<std::endl;}

SLAM十四讲第三章课后习题相关推荐

视觉slam十四讲第8章课后习题3+稀疏直接法程序注释
版权声明:本文为博主原创文章,转载请注明出处: http://www.cnblogs.com/newneul/p/8571653.html 3.题目回顾: 在稀疏直接法中,假设单个像素周围小块的光度 ...
《视觉SLAM十四讲第二版》课后习题
本文为<视觉SLAM十四讲>(第二版)的课后习题解答,为本人学习时参考着网上的资源所写的答案,可能有所纰漏,希望大家指出. 文章目录第1讲预备知识第2讲初始SLAM 第3讲三维空 ...
视觉SLAM十四讲-第三章笔记
视觉SLAM14讲-第三章笔记 3.1 旋转矩阵欧式变换欧式变换:改变位资,不改变形状.大小. 旋转矩阵:R(3x3).是两个坐标系基的内积. 正交阵行列式为1 逆表示相反的旋转平移向量:t( ...
【视觉SLAM十四讲】第一章理论详解
文章目录第一讲概述与预备知识 SLAM是什么室内/室外定位稀疏-半稠密重建稠密重建 SLAM可以用在哪些地方? **作业1.** SLAM会在哪些场合中⽤到?⾄少列举三个⽅向. SLAM: ...
深蓝学院视觉slam十四讲第2章作业
首先给出有关四元数讲解一份比较好的pdf 2.1 设线性⽅程 Ax=b\bm Ax = bAx=b,则有解充要条件为:系数矩阵An×n\bm A_{n \times n}An×n与其增广矩阵Bn×( ...
《视觉SLAM十四讲》第二版源码slambook2编译调试
slambook2-master/ch2 编译正常,log如下: slambook2-master/ch2# mkdir build && cd build && cm ...
视觉SLAM十四讲第二章学习与课后题与随笔日记
视觉SLAM十四讲第二章主要内容是linux下c++编程,cpp,lib,smakelist这类文件关联与使用. 前面出现过的问题: P31-P32 make: *** 没有指明目标并且找不到 ma ...
视觉SLAM十四讲从理论到实践第二版源码调试笔记（理论基础1-6章）
2019-2020-2学期机器人工程专业需要开设SLAM技术课程,使用教材为视觉SLAM十四讲从理论到实践第二版. 为方便学生学习课程知识,将Arduino.ROS1.ROS2和SLAM集成到课程定制 ...
《视觉SLAM十四讲第二版》笔记及课后习题（第七讲）
读书笔记:视觉里程计1 之前的内容,介绍了运动方程和观测方程的具体形式,并讲解了以非线性优化为主的求解方法.从本讲开始,我们结束了基础知识的铺垫,开始步入正题:按照第二讲的内容,分别介绍视觉里程计.优 ...

SLAM十四讲第三章课后习题

SLAM十四讲第三章课后习题相关推荐

最新文章

热门文章