自动化专业小白学习小记

学习内容

• C++ 基本语法复习

• google style 编程规范学习

• git 学习

• draw.io 与 xmind 学习

• ros 学习

• 视觉 SLAM 十四讲学习与实践

---

一、C++ 基本语法

本周简单复习了一下 C++类和对象，类的使用，类和动态内存分配内容。

二、Google style 编程规范学习

2.1 头文件

正确使用头文件可以改善代码的可读性、文件大小和性能。

2.1.1 Self-contained 头文件

所有头文件要能够自给自足。一个头文件要有#define保护，统统包含它所需要的其它头文件，也不要求定义任何特别 symbols。

2.1.2 #define 保护

关键是头文件保护格式，编写方式如下：

#ifndef FOO_BAR_BAZ_H_
#define FOO_BAR_BAZ_H_
...
#endif // FOO_BAR_BAZ_H_

2.1.3 前置声明

尽可能地避免使用前置声明。使用 #include 包含需要的头文件即可。

结论：

• 尽量避免前置声明那些定义在其他项目中的实体。

• 函数：总是使用 #include.

• 类模板：优先使用 #include.

2.1.4 内联函数

只有当函数只有 10 行甚至更少时才将其定义为内联函数。

定义：当函数被声明为内联函数之后，编译器会将其内联展开，而不是按通常的函数调用机制进行调用。

滥用内联将导致程序变得更慢。内联可能使目标代码量或增或减，这取决于内联函数的大小。内联非常短小的存取函数通常会减少代码大小，但内联一个相当大的函数将戏剧性的增加代码大小。现代处理器由于更好的利用了指令缓存，小巧的代码往往执行更快。

这里有点点不太懂，可能是 C++有部分没学明白。后续回过头来复习

2.1.5 #include 的路径及顺序

使用标准的头文件包含顺序可增强可读性，避免隐藏依赖：相关头文件，C 库，C++ 库，其他库的 .h, 本项目内的 .h.

包含头文件的次序如下：

1. dir2/foo2.h （优先位置）

2. C 系统文件

3. C++ 系统文件

4. 其他库的 .h 文件

5. 本项目内 .h 文件

2.2 作用域

2.2.1 命名空间

鼓励在 .cc 文件内使用匿名命名空间或 static 声明。使用具名的命名空间时，其名称可基于项目名或相对路径。禁止使用 using 指示（using-directive）。禁止使用内联命名空间（inline namespace）。

定义：命名空间将全局作用域细分为独立的，具名的作用域，可有效防止全局作用域的命名冲突。

结论：

• 遵守命名空间命名中的规则。

• 像之前的几个例子中一样，在命名空间的最后注释出命名空间的名字。

• 用命名空间把文件包含，gflags 的声明/定义，以及类的前置声明以外的整个源文件封装起来，以区别于其它命名空间：

  // .h 文件
  namespace mynamespace {
  ​
  // 所有声明都置于命名空间中
  // 注意不要使用缩进
  class MyClass {
      public:
      ...
      void Foo();
  };
  ​
  } // namespace mynamespace
  ​
  // .cc 文件
  namespace mynamespace {
  ​
  // 函数定义都置于命名空间中
  void MyClass::Foo() {
      ...
  }
  ​
  } // namespace mynamespace

  更复杂的 .cc 文件包含更多，更复杂的细节，比如 gflags 或 using 声明。

  #include "a.h"
  ​
  DEFINE_FLAG(bool, someflag, false, "dummy flag");
  ​
  namespace a {
  ​
  ...code for a...                // 左对齐
  ​
  } // namespace a

• 不要在命名空间 std 内声明任何东西，包括标准库的类前置声明。在 std 命名空间声明实体是未定义的行为，会导致不可移植。声明标准库下的实体，需要包含对应的头文件。

• 不应该使用 using 指示 引入整个命名空间的标识符号。

  // 禁止 —— 污染命名空间
  using namespace foo;

• 不要在头文件中使用 命名空间别名 除非显式标记内部命名空间使用。因为任何在头文件中引入的命名空间都会成为公开 API 的一部分。

  // 在 .cc 中使用别名缩短常用的命名空间
  namespace baz = ::foo::bar::baz;
  ​
  // 在 .h 中使用别名缩短常用的命名空间
  namespace librarian {
  namespace impl {  // 仅限内部使用
  namespace sidetable = ::pipeline_diagnostics::sidetable;
  }  // namespace impl
  ​
  inline void my_inline_function() {
    // 限制在一个函数中的命名空间别名
    namespace baz = ::foo::bar::baz;
    ...
  }
  }  // namespace librarian

• 禁止用内联命名空间

2.2.2 匿名命名空间和静态变量

在 .cc 文件中定义一个不需要被外部引用的变量时，可以将它们放在匿名命名空间或声明为 static 。但是不要在 .h 文件中这么做。

定义：

所有置于匿名命名空间的声明都具有内部链接性，函数和变量可以经由声明为 static 拥有内部链接性，这意味着你在这个文件中声明的这些标识符都不能在另一个文件中被访问。即使两个文件声明了完全一样名字的标识符，它们所指向的实体实际上是完全不同的。

结论：

推荐、鼓励在 .cc 中对于不需要在其他地方引用的标识符使用内部链接性声明，但是不要在 .h 中使用。

匿名命名空间的声明和具名的格式相同，在最后注释上 namespace :

2.2.3 非成员函数、静态成员函数和全局函数

使用静态成员函数或命名空间内的非成员函数，尽量不要用裸的全局函数。将一系列函数直接置于命名空间中，不要用类的静态方法模拟出命名空间的效果，类的静态方法应当和类的实例或静态数据紧密相关。

结论：有时，把函数的定义同类的实例脱钩是有益的，甚至是必要的。这样的函数可以被定义成静态成员，或是非成员函数。非成员函数不应依赖于外部变量，应尽量置于某个命名空间内。相比单纯为了封装若干不共享任何静态数据的静态成员函数而创建类，不如使用命名空间。举例而言，对于头文件 myproject/foo_bar.h , 应当使用

namespace myproject {
namespace foo_bar {
void Function1();
void Function2();
}  // namespace foo_bar
}  // namespace myproject

而非

namespace myproject {
class FooBar {
 public:
  static void Function1();
  static void Function2();
};
}  // namespace myproject

定义在同一编译单元的函数，被其他编译单元直接调用可能会引入不必要的耦合和链接时依赖；静态成员函数对此尤其敏感。可以考虑提取到新类中，或者将函数置于独立库的命名空间内。 ​ 如果你必须定义非成员函数，又只是在 .cc 文件中使用它，可使用匿名 2.1. 命名空间 或 static 链接关键字（如 static int Foo() {...}) 限定其作用域。

2.2.4 局部变量

将函数变量尽可能置于最小作用域内，并在变量声明时进行初始化。

2.2.5 静态和全局变量

禁止定义静态储存周期非 POD 变量，禁止使用含有副作用的函数初始化 POD 全局变量，因为多编译单元中的静态变量执行时的构造和析构顺序是未明确的，这将导致代码的不可移植。

三、git 学习

3.1 git 工作流程

• 克隆 Git 资源作为工作目录。

• 在克隆的资源上添加或修改文件。

• 如果其他人修改了，你可以更新资源。

• 在提交前查看修改。

• 提交修改。

• 在修改完成后，如果发现错误，可以撤回提交并再次修改并提交。

3.2 Git 工作区、暂存区和版本库

先理解基本概念：

• 工作区：就是电脑里能看到的目录。

• 暂存区：英文叫 stage 或 index。一般存放在 .git 目录下的 index 文件（.git/index）中，所以我们把暂存区有时也叫作索引（index）。

• 版本库：工作区有一个隐藏目录 .git，这个不算工作区，而是 Git 的版本库。

常用命令：

• 执行git add命令时，暂存区的目录树被更新，同时工作区修改（或新增）的文件内容被写入到对象库中的一个新的对象中，而该对象的 ID 被记录在暂存区的文件索引中。

• 执行提交操作git commit命令时，暂存区的目录树写到版本库（对象库）中，master 分支会做相应的更新。即 master 指向的目录树就是提交时暂存区的目录树。

• 执行git reset HEAD命令时，暂存区的目录树会被重写，被 master 分支指向的目录树所替换，但是工作区不受影响。

• 执行git rm --cached <file>命令时，会直接从暂存区删除文件，工作区则不做出改变。

• 执行git checkout .或者git checkout -- <file>命令时，会用暂存区全部或指定的文件替换工作区的文件。这个操作很危险，会清除工作区中未添加到暂存区中的改动。

• 当执行 git checkout HEAD . 或者 git checkout HEAD <file> 命令时，会用 HEAD 指向的 master 分支中的全部或者部分文件替换暂存区和以及工作区中的文件。这个命令也是极具危险性的，因为不但会清除工作区中未提交的改动，也会清除暂存区中未提交的改动。

四、draw.io 与 xmind 学习

4.1 draw.io 学习

自己曾经在参与美赛的时候使用过 visio 和 ppt 来绘制流程图，visio 的直观感觉是绘制较方便，但总感觉绘制出来的一般般好看，自带的库的图形也没有想象中的丰富，用 ppt 绘制的流程图反倒还好点。

draw.io 的重要特点就是轻量化，绘制流程图门槛低，而且还免费，还可以把流程图变成手绘风格（虽然不常用，哈哈） 。而且当我把文件误转为 png 格式时，用 draw.io 打开之后就可以还原成可编辑状态，很棒。库的图形也很丰富，比较适合科研绘图。

总而言之用起来很方便，学习门槛也较低，很容易上手。

4.2 xmind 学习

Tab 键可以添加子主题；Enter 键可以添加同级主题；Space 键可以添加自己的内容。三个键结合鼠标可以轻松绘制属于自己的思维导图，上手容易学习成本低，较快便可以熟练掌握。

五、ros 学习

5.1 ros 文件系统

5.1.1 Catkin 编译系统

工作原理：简单来说，Catkin 就是将cmake与make指令做了一个封装从而完成整个编译过程的工具。

使用方法：

cd ~/catkin_ws #回到工作空间，catkin_make 必须在工作空间下执行
catkin_make    #开始编译
source ~/catkin_ws/devel/setup.bash #刷新坏境

最后一句可以放到终端窗口启动的默认执行程序中执行。

5.1.2 Catkin 工作空间

catkin 的工作空间，直观的形容就是一个仓库，里面装载着 ROS 的各种项目工程，便于系统组织管理调用。

指令：

mkdir -p ~/catkin_ws/src　　
cd ~/catkin_ws/
catkin_make #初始化工作空间

5.1.3 Package 软件包

ROS 中的 package 的定义更加具体，它不仅是 Linux 上的软件包，更是 catkin 编译的基本单元，我们调用catkin_make编译的对象就是一个个 ROS 的 package，也就是说任何 ROS 程序只有组织成 package 才能编译。

一个 package 可以编译出来多个目标文件（ROS 可执行程序、动态静态库、头文件等等）。

结构：

 ├── CMakeLists.txt    #package 的编译规则（必须）
 ├── package.xml       #package 的描述信息（必须）
 ├── src/              #源代码文件
 ├── include/          #C++头文件
 ├── scripts/          #可执行脚本
 ├── msg/              #自定义消息
 ├── srv/              #自定义服务
 ├── models/           #3D 模型文件
 ├── urdf/             #urdf 文件
 ├── launch/           #launch 文件   

创建：创建一个 package 需要在catkin_ws/src下，用到catkin_create_pkg命令，用法是： catkin_create_pkg package depends 其中 package 是包名，depends 是依赖的包名，可以依赖多个软件包。

例如，新建一个 package 叫做test_pkg, 依赖 roscpp、rospy、std_msgs\（常用依赖、)。

catkin_create_pkg test_pkg roscpp rospy std_msgs

相关命令：

rospack

rospack 是对 package 管理的工具，命令的用法如下：

rostopic 命令	作用
rospack help	显示 rospack 的用法
rospack list	列出本机所有 package
rospack depends [package]	显示 package 的依赖包
rospack find [package]	定位某个 package
rospack profile	刷新所有 package 的位置记录

以上命令如果 package 缺省，则默认为当前目录、（如果当前目录包含 package.xml)

roscd

roscd命令类似与 Linux 系统的cd，改进之处在于roscd可以直接cd到 ROS 的软件包。

rostopic 命令	作用
roscd [pacakge]	cd 到 ROS package 所在路径

rosls

rosls也可以视为 Linux 指令ls的改进版，可以直接lsROS 软件包的内容。

rosls 命令	作用
rosls [pacakge]	列出 pacakge 下的文件

rosdep

rosdep是用于管理 ROS package 依赖项的命令行工具，用法如下：

rosdep 命令	作用
rosdep check [pacakge]	检查 package 的依赖是否满足
rosdep install [pacakge]	安装 pacakge 的依赖
rosdep db	生成和显示依赖数据库
rosdep init	初始化/etc/ros/rosdep 中的源
rosdep keys	检查 package 的依赖是否满足
rosdep update	更新本地的 rosdep 数据库

一个较常使用的命令是rosdep install --from-paths src --ignore-src --rosdistro=melodic -y, 用于安装工作空间中src路径下所有 package 的依赖项（由 pacakge.xml 文件指定）。

5.1.4 CMakeLists.txt

这个文件的编写十分关键，关系到包编译的顺序、依赖等，之前使用的时候经常因为该文件写的不对而疯狂报错。这个得好好记记，感觉主要是多练。

5.1.5 package.xml

package.xml也是一个 catkin 的 package 必备文件，它是这个软件包的描述文件，在较早的 ROS 版本、(rosbuild 编译系统、) 中，这个文件叫做manifest.xml，用于描述 pacakge 的基本信息。

这个好像可以自己生成来着。感觉问题不大

5.1.6 Metapackage

就是功能包集，把一些相近的功能模块、软件包放到一起。

常见的 Metapackage 有：

Metapacakge 名称	描述	链接
navigation	导航相关的功能包集	https://github.com/ros-planning/navigation
moveit	运动规划相关的（主要是机械臂）功能包集	https://github.com/ros-planning/moveit
image_pipeline	图像获取、处理相关的功能包集	https://github.com/ros-perception/image_common
vision_opencv	ROS 与 OpenCV 交互的功能包集	https://github.com/ros-perception/vision_opencv
turtlebot	Turtlebot 机器人相关的功能包集	https://github.com/turtlebot/turtlebot
pr2_robot	pr2 机器人驱动功能包集	https://github.com/PR2/pr2_robot
...	...	...

metapacakge 中的以上两个文件和普通 pacakge 不同点是：

• CMakeLists.txt: 加入了 catkin_metapackage() 宏，指定本软件包为一个 metapacakge。

• package.xml:<run_depend>标签将所有软件包列为依赖项，<export>标签中添加<metapackage>标签声明。

metapacakge 在我们实际开发一个大工程时可能有用，感觉就是一个大包，方便调用。

5.1.7 其他常见文件类型

1. launch 文件

launch 文件一般以。launch 或。xml 结尾，它对 ROS 需要运行程序进行了打包，通过一句命令来启动。一般 launch 文件中会指定要启动哪些 package 下的哪些可执行程序，指定以什么参数启动，以及一些管理控制的命令。 launch 文件通常放在软件包的launch/路径中。

1. msg/srv/action 文件

ROS 程序中有可能有一些自定义的消息/服务/动作文件，为程序的发者所设计的数据结构，这类的文件以.msg,.srv,.action结尾，通常放在 package 的msg/,srv/,action/路径下。

1. urdf/xacro 文件

urdf/xacro 文件是机器人模型的描述文件，以。urdf 或。xacro 结尾。它定义了机器人的连杆和关节的信息，以及它们之间的位置、角度等信息，通过 urdf 文件可以将机器人的物理连接信息表示出来。并在可视化调试和仿真中显示。

1. yaml 文件

yaml 文件一般存储了 ROS 需要加载的参数信息，一些属性的配置。通常在 launch 文件或程序中读取。yaml 文件，把参数加载到参数服务器上。通常我们会把 yaml 文件存放在param/路径下

1. dae/stl 文件

dae 或 stl 文件是 3D 模型文件，机器人的 urdf 或仿真环境通常会引用这类文件，它们描述了机器人的三维模型。相比 urdf 文件简单定义的性状，dae/stl 文件可以定义复杂的模型，可以直接从 solidworks 或其他建模软件导出机器人装配模型，从而显示出更加精确的外形。

1. rviz 文件

rviz 文件本质上是固定格式的文本文件，其中存储了 RViz 窗口的配置（显示哪些控件、视角、参数）。通常 rviz 文件不需要我们去手动修改，而是直接在 RViz 工具里保存，下次运行时直接读取。

六、视觉 SLAM 十四讲学习

6.1 三维空间刚体运动实践部分

代码部分

    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是 Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用 matrix() 转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
​
    // 欧拉角：可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX 顺序，即 roll pitch yaw 顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
​
    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();            
    T.rotate ( rotation_vector );                                     // 按照 rotation_vector 进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成 (1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;
​
    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于 R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
​
    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略
​
    // 四元数
    // 可以直接把 AngleAxis 赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意 coeffs 的顺序是 (x,y,z,w),w 为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; 

此程序主要演示了四元数、欧拉角和旋转矩阵之间的变换方式，这个很常用，需要牢记。

6.2 李群与李代数

6.2.1 李群李代数基础

三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3):

$$
SO(3)=\lbrace {R} \in {\mathbb{R}^{3 \times 3} {\vert} {R}{R}^T=I,det(R)=1} \rbrace \\ SE(3)=\lbrace T=\left[ \matrix{ R & t \\ {0}^T & 1 } \right] \in {\mathbb{R}^{4 \times 4} {\vert} {R} \in SO(3),T \in {\mathbb{R}^{3}}} \rbrace
$$

SO(3)和SE(3)对加法不封闭，对于乘法封闭。我们知道乘法对应着旋转或变换的复合——两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个运算的集合，我们把它叫做群。

1. 群

   群是一种集合加上一种运算的代数结构。

   旋转矩阵集合和矩阵乘法构成群，变换矩阵和矩阵乘法也构成群。

   李群是指具有连续光滑性质的群。像整数群 Z 那样离散的群没有连续性质，所以不是李群。而 SO(n) 和 SE(n)，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。

   理论部分学习比较复杂。还需要后续复习，这里不做详细记录

6.2.2 指数与对数映射

6.2.3 李代数求导与扰动模型

6.3 相机与图像

6.3.1 相机模型

相机的针孔模型常用且有效。相机镜头透镜的存在，会使得光线投影到成像平面的过程中产生畸变。针孔模型 + 畸变模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数。

1. 针孔相机模型

针孔模型建模。小学的时候学过小孔成像模型，成像是倒像，但相机所展示的像是正的，所以我们可以根据相似三角形得出公式后并通过对称将成像倒过来，可以使公式更加简洁，那么就可以得到所成像坐标的公式：

$$
X^\prime=f \frac{X}{Z} \\ Y^\prime=f \frac{Y}{Z}
$$

上式描述了点与它的像之间的空间关系。但在相机中，我们最终获得的是像素，需要在物理成像平面固定一个像素平面，就可以得到像素平面上P^\prime的像素坐标[u,v]^T。

像素坐标系定义为：原点 O^\prime 位于图像的左上角，u轴向右与x轴平行，v轴向下与y轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。假设像素坐标在u轴上缩放了\alpha倍，在v上缩放了\beta倍。同时，原点平移了[c_x,c_y]^T。则可以得到点P^\prime的坐标与像素坐标[u,v]^T的关系为：

$$
\begin{cases} u=\alpha X^\prime + c_x \\ v=\beta Y^\prime +c_y \end{cases}
$$

代入可得：

$$
\begin{cases} u={f_x}{\frac{X}{Z}}+{c_x} \\ v={f_y}{\frac{Y}{Z}}+{c_y} \end{cases}
$$

其中，f_x=\alpha f，f_y=\beta f。将式子写为矩阵形式，可以得到相机的内参数矩阵。像素坐标转到世界坐标就需要已知内参矩阵，可以通过标定得到。

$$
\left( \matrix{ {f_x} & 0 & {c_x} \\ 0 & {f_y} & {c_y} \\ 0 & 0 & 1 } \right)
$$

外参就是我们之前说的位姿变化，即旋转和平移。已知了内外参，我们便可以表示如何从世界坐标下的一点通过位姿变化转到相机坐标然后再转到像素坐标。

$$
P_{uv}=K(R{P_w}+t)=KT{P_w}
$$

相机的位姿R,t称为相机的外参数。

我们可以将投影顺序归纳为：世界->相机->归一化->像素。

2. 畸变

为了更好的获得成像效果，我们会在相机中加入透镜，由透镜形状引起的畸变称为径向畸变。径向畸变又分为桶形畸变和枕形畸变。由于相机在组装中的误差引起的畸变叫做切向畸变。

我们考虑归一化平面上的一点 P，坐标为[x,y]^T，极坐标为[r,\theta]^T，r代表点P与坐标系原点间的距离，θ表示与水平轴的夹角。径向畸变可以看作坐标点沿着长度方向发生了变化，也就是距离原点的长度发生了变化，切向畸变可以看作是坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

对于径向畸变，可以采用多项式函数来描述畸变前后的坐标变化：

$$
x_{corrected}=x(1+{k_1}{r^2}+{k_2}{r^4}+{k_3}{r^6}) \\ y_{corrected}=y(1+{k_1}{r^2}+{k_2}{r^4}+{k_3}{r^6})
$$

其中[x,y]^T是未纠正的点的坐标，[x_{corrected},y_{corrected}]^T是纠正后的点的坐标，它们都是归一化平面上的点，不是像素平面上的点，式子本质上类似于泰勒展开。

对于切向畸变，可以这样表示：

$$
x_{corrected}=x+2{p_1}xy+{p_2}({r^2}+2{x^2}) \\ y_{corrected}=y+{p_1}(r^2+2{y^2})+2{p_2}xy
$$

所以，结合以上，可以通过五个畸变系数来找到相机坐标系中一点在像素平面上的正确位置：

1. 将三维空间点投影到归一化平面。设归一化坐标为[x,y]^T。

2. 对归一化平面的点进行径向畸变和切向畸变校正。

   $$
   \begin{cases} x_{corrected}=x(1+{k_1}{r^2}+{k_2}{r^4}+{k_3}{r^6})+2{p_1}xy+{p_2}({r^2}+2{x^2}) \\ y_{corrected}=y(1+{k_1}{r^2}+{k_2}{r^4}+{k_3}{r^6})+{p_1}(r^2+2{y^2})+2{p_2}xy \end{cases}
   $$

3. 将校正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

   $$
   \begin{cases} u={f_x}{x_{corrected}}+c_x \\ v={f_y}{y_{corrected}}+c_y \end{cases}
   $$

单目相机成像过程总结

1. 首先世界坐标系下有一个固定点 P，它的世界坐标为P_w

2. 通过位姿变换将点 P 的世界坐标转化到相机坐标系下：\widetilde{P_c}=R{P_w}+t。

3. 将相机坐标系坐标进行归一化处理，得到点 P 的归一化坐标：{P_c}=[X/Z,Y/Z,1]^T

4. 如果有畸变，将{P_c}进行去畸变处理，得到去畸变之后的归一化坐标。

5. 最终将归一化坐标经过内参后得到像素坐标：P_{uv}=K{P_c}

3. 双目相机模型

单目相机由于归一化的原因，并没有能够很好的获得深度信息。在 SLAM 中只有很好的知道深度信息，才能准确的估计位置。

双目相机成像原理，O_L和O_R为光心的位置，P_L和P_R为左右相机成像的位置，f为焦距，b为基线，表示双目相机中左眼相机与右眼相机的距离，根据相似三角形可得：

$$
\frac{z-f}{z}=\frac{b-{u_L}+{u_R}}{b}
$$

整理之后有：

$$
z=\frac{fb}{d},d={u_L}-{u_R}
$$

通过公式我们可以求出距离z，d我们称为视差，表示成像坐标之间的差值。可以看出视差和距离成反比，视差越大，距离越近，视差越小，距离越远。视差最小为一个像素，所以理论上距离有一个极限值，这个极限值由f_b确定，基线越大，可以测的距离越远，反之亦然。

4. RGB-D 相机模型

相比双目相机采用视差来测量距离，深度相机则是主动测量深度，目前深度相机按照原理可以分为：

1. 通过红外结构光原理测量距离

2. 通过飞行时间法原理测量距离

6.3.2 图像

图像可以用二维数组表示，但要注意，在图像中，数组的行数对应图像的高度，列数对应图像的宽度。

像素坐标系定义方式是，坐标系原点位于图像的左上角，X轴向右，Y轴向下，如果还有第三个轴，由右手法则，Z轴向前。