视觉SLAM ch3代码总结 （二）

coordinateTransform.cpp

首先小萝卜一号坐标变换到世界坐标系

Pw =  T w1 *  P 1

其次世界坐标系再变换到小萝卜二号坐标系

P2 = T 2w *  P w

所以 P2 = T 2w  *   Tw1 * P1

T2w 是使用四元数q2初始换欧式变换矩阵

 Isometry3d T2w(q2);

Tw1 =  T1w的逆矩阵   初始化和上面代码一样

 Isometry3d T1w(q1);

 在加上平移向量

 T1w.pretranslate(t1);
 T2w.pretranslate(t2);

计算P2的公式每项都已经知道 Tw1 = T1w{-1}   （{-1}表示逆）

p2 = T2w *  T1w.inverse() * p1;

最后记得在定义完四元数后归一化

q1.normalize();
q2.normalize();

*******************************************************************************************************

plotTrajectory.cpp

（1）关于<unistd.h>

许多在Linux下开发的C程序都需要头文件unistd.h，但VC中没有这个头文件，所以用VC编译总是报错。

unistd.h是unix std的意思，是POSIX标准定义的unix类系统定义符号常量的头文件，包含了许多UNIX系统服务的函数原型，例如read函数、write函数和getpid函数。

unistd.h在unix中类似于window中的windows.h。
1、打开文件，函数open()  2、读取文件，函数read()  3、写文件，函数write()4、关闭文件，函数close()   5、控制文件，函数ioctl()

（2） vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>

vector<xxx,aligned_allocator<xxx>> 
让我们自己定义动态申请变量的方法。

aligned_allocator在做一件事，告诉vector容器Isometry3d的内存大小。

找到一个例子在介绍：

如果STL容器中的元素是Eigen库数据结构，例如这里定义一个vector容器，元素是Matrix4d ，如下所示：

vector<Eigen::Matrix4d>

这个编译不会出错，只有在运行的时候出错。解决的方法很简单，定义改成下面的方式：

vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>;

其实上述的这段代码才是标准的定义容器方法，只是我们一般情况下定义容器的元素都是C++中的类型，所以可以省略，这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。

（3）关于文件操作的一些笔记

• fin对象的作用可以理解成：对文件中的内容进行读操作 【从硬盘–>缓冲区】

• fout对象的作用可以理解成：向文件中进行写操作 【从缓冲区–>硬盘】

• cin对象的作用可以理解成实现：【键盘–>缓冲区】 
  如：string str; cin>>str;

• cout对象的作用可以理解成：【缓冲区–>屏幕上显示】 
  如：cout<<str； 

ifstram fin("文件名")；
ofstream  fout("文件名");

eof()函数返回true时是读到文件结束符0xFF，而文件结束符是最后一个字符的下一个字符。

eof（）为真表示文件为空     !eof为真时表示文件非空

while(!fin.eof())   //文件读操作中使用while语句进行判断

 （4）自定义函数 DrawTrajectory()   

具体代码注释参考下面文章

视觉SLAM十四讲CH3代码解析及课后习题详解_nudt一枚研究生-CSDN博客