文章的核心思想在于解决loop closure问题。

全局地图由一系列的submap构成，每个submap则由一系列的位姿节点及对应的scan数据构成。

文章的重点在第四部分和第五部分：

• 第四部分：local 2d slam，将scan与当前submap的匹配问题转化成一个最小二乘优化问题，由ceres来求解。参考文献《Many-to-Many Multi-Resolution Scan Matching 》
• 第五部分：closing loop，采用SPA进行后端loop closure，提出一种并行的scan与finished submaps匹配的方法BBS，大幅提高精度和速度。参考文献《Efficient Sparse Pose Adjustment for 2D Mapping(SPA)》、《Real-Time Correlative Scan Matching(BBS)》

0. 向量

• 坐标：首先确定一个坐标系，也就确定了一组基$(e_1, e_2, e_3)$，那么向量$a$的坐标为：

  $$a = [e_1, e_2, e_3] \begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix} = a_1e_1 + a_2e_2 + a_3e_3$$

• 内积：对向量$a, b \in R^3$，其内积为：

  $$a \cdot b = a^Tb = \Sigma a_ib_i = |a||b|cos<a,b>$$

  内积可以描述向量间的投影关系。

• 外积：

  $$a \times b = \begin{vmatrix} i & j & k\\ a_1 & a_2 & a_3\\ b_1 & b_2 & b_3 \end{vmatrix}= \begin{bmatrix} 0 & -a_3 & a_2\\ a_3 & 0 & -a_1\\ -a_2 & a_1 & 0 \end{bmatrix}b=a^{\wedge}b$$

  外积的方向垂直与这两个向量，大小为$|a||b|sin$。

  外积可以表示向量的旋转，向量$a$到$b$的旋转向量，外积的方向是旋转向量的方向，大小由夹角决定。

1. 旋转矩阵R与变换矩阵T

• 通常设置固定的世界坐标系$O_w$和运动的相机坐标系$O_c$，相机运动是刚体运动，两个坐标系之间的变换称为欧式变换。

• 旋转矩阵$R$：可以描述相机的旋转

  坐标系旋转前后同一个向量的坐标变换关系：

  $$a =\begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix}= \begin{bmatrix} e_1^T\\ e_2^T\\ e_3^T \end{bmatrix} \begin{bmatrix} e_1^{'} & e_2^{'}& e_3^{'} \end{bmatrix} \begin{bmatrix} a_1^{'}\\ a_2^{'}\\ a_3^{'} \end{bmatrix}= Ra^{'}$$

  不难验证旋转矩阵是行列式为1的正交矩阵，因此可以把旋转矩阵的集合特殊正交群定义如下：

  $$SO(n) = \{R \in R^{n*n} | RR^T=I, det(R)=1\}$$

  相反的旋转：

  $$a = Ra^{'}\\ a^{'} = R^{-1}a = R^Ta$$

• 欧式变换：包括旋转和平移

  $$a^{'} = Ra + t$$

• 齐次坐标：射影几何的概念，每个分量同乘一个非零常数仍然表示同一个点：

  $$\tilde{x} = [x,y,z,w]^T=[x/w, y/w, z/w, 1]^T$$

• 齐次变换矩阵$T$：使得欧式变换仍旧保持线性关系：

  $$\begin{bmatrix} a^{'}\\ 1 \end{bmatrix}= \begin{bmatrix} R & t\\ 0 &1 \end{bmatrix} \begin{bmatrix} a\\ 1 \end{bmatrix} =T \begin{bmatrix} a\\ 1 \end{bmatrix}$$

  变换矩阵的集合特殊欧式群：

  $$SE(3) = \left\{T= \begin{bmatrix} R & t\\ 0 & 1 \end{bmatrix} \in R^{4*4} | R\in SO(3), t \in R^3 \right\}$$

2. 旋转向量 Axis-Angle

一个旋转只有3个自由度，旋转矩阵R要用9的参数来描述，显然是冗余的。一种紧凑的方式——任何旋转都可以用一个旋转轴$n$和一个旋转角$\theta$来刻画：

旋转轴上的向量在旋转后不发生改变，因此有：

转轴$n$是旋转矩阵$R$的特征值1对应的特征向量，可以由此来计算转轴$n$。

3. 欧拉角 rpy

把旋转分解到3个轴上，rpy角的旋转顺序是ZYX：

• 首先绕物体的Z轴旋转，得到偏航角yaw
• 然后绕旋转之后的Y轴旋转，得到俯仰角pitch
• 绕旋转之后的X轴旋转，得到滚转角roll

万向锁问题：在俯仰角为$\pm 90^{\circ}$时，第一次和第三次旋转使用同一根轴，丢了自由度——奇异性问题。

4. 四元数 q

四元数是一种扩展的负数，由一个实部和三个虚部组成，可以把三个虚部脑补成空间中的三根轴：

• 乘以$i$对应着绕$i$轴旋转$180^{\circ}$

• 任意的旋转可以由两个互为相反数的四元数表示

• 与旋转向量$n = [n_x, n_y, n_z]^T, \theta$转换关系：

  $$q = [cos\frac{\theta}{2}, n_xsin\frac{\theta}{2}, n_ysin\frac{\theta}{2}, n_zsin\frac{\theta}{2}]^T\\ \left\{ \begin{split} & \theta = 2arccos q_0\\ & [n_x, n_y, n_z]^T = [q_1, q_2, q_3]^T/sin\frac{\theta}{2} \end{split} \right.$$

• 与旋转矩阵$R$的关系：

  $$R = \begin{bmatrix} 1-2q_2^2 - 2q_3^2 & 2q_1q_2-2q_0q_3 & 2q_1q_3+2q_0q_2\\ 2q_1q_2+2q_0q_3 & 1-2q_1^2 - 2q_3^2 & 2q_2q_3-2q_0q_1\\ 2q_1q_3-2q_0q_2 & 2q_2q_3+2q_0q_1 & 1-2q_1^2 - 2q_2^2 \end{bmatrix}\\ q_0 = \frac{\sqrt{tr(R)+1}}{2}, q_1 = \frac{R_{23}-R_{32}}{4q_0}, q_2 = \frac{R_{31}-R_{13}}{4q_0}, q_3 = \frac{R_{12}-R_{21}}{4q_0}$$

• 表示旋转：

  空间中点$p = [x, y,z]^T\in R^3$，已知旋转轴角$n,\theta$，旋转之后点坐标变为$p^{‘}$，如果用旋转矩阵描述：

  $$p^{'} = Rp$$

  四元数$q = [cos\frac{\theta}{2}, nsin\frac{\theta}{2}]$，那么旋转后的点$p^{‘}$可以表示为：

  $$p^{'} = qpq^{-1}$$

5. 李群

上面提到了旋转矩阵构成的特殊正交群$SO(3)$和由变换矩阵构成的特殊欧式群$SE(3)$：

• $SO(n)$和$SE(n)$对加法不封闭，对乘法是封闭的。
• 群是一种集合$A$加上一种运算$\ \cdot \ $的代数结构，记作$G = (A, \ \cdot \ )$，群内元素满足封闭性、结合律、幺元、可逆四个条件。
• 李群是指具有连续性质的群。刚体在空间中能够连续地运动，因此$SO(n)$和$SE(n)$是李群。

6. 李代数

6.1 引入

对任意旋转矩阵$R$，都满足$RR^T=I$。把它写成关于时间的函数$R(t)$有：

对等式两边求导：

可以看出$\dot R(t)R(t)^T $是一个反对称阵，对任意一个反对称阵，都可以找到一个与之对应的向量：

于是可以找到一个三维向量$\phi(t) \in R^3$与之对应：

可以看到，每对旋转矩阵求一次导数，只需左乘一个反对称阵$\phi(t)^{\wedge}$即可。

求解上面的微分方程，可以得到$R(t) = exp(\phi^{\wedge}t)$。也就是说$\phi$描述了$R$在局部的导数关系。

6.2 李代数

• 每个李群都有与之对应的李代数。李代数描述了李群的局部性质。

• 李代数由一个集合$V$，一个数域$F$，和一个二元运算李括号$[,]$组成，记作$( V, F, [,])$。李代数的元素满足封闭性、双线性、自反性、雅可比等价四条性质。

• 上一节的$\phi$就是$SO(3)$对应的李代数$so(3)$，两者的关系由指数映射给定：

  $$R = exp(\phi^{\wedge})\\ so(3) = \left\{ \phi \in R^3, \Phi = \phi^{\wedge} \in R^{3*3}\right\}$$

• $SE(3)$对应的李代数$se(3)$位于$R^6$空间中：

  $$se(3) = \left\{ \xi = \begin{bmatrix} \rho\\ \phi \end{bmatrix} \in R^6, \rho \in so(3), \xi^{\wedge} = \begin{bmatrix} \phi^{\wedge} & \rho\\ 0^T & 0 \end{bmatrix} \in R^{4*4} \right\}$$

• 指数映射

  由于$\phi$是一个三维向量，因此可以写作$\theta a$的形式，$a$是一个单位向量，因此具有以下性质：

  $$a^{\wedge}a^{\wedge} = aa^T-I\\ a^{\wedge}a^{\wedge}a^{\wedge} = -a^{\wedge}$$

  对$so(3)$李代数的指数映射做泰勒展开，可以得到：

  $$\begin{split} &R= exp(\phi^{\wedge}) = exp(\theta a^{\wedge})=\Sigma_{n=0}^{\infty}\frac{1}{n!} (\theta a^{\wedge})^n\\ & =cos\theta I + (1-cos\theta)aa^T+sin\theta a^{\wedge} \end{split}$$

  可以看到$so(3)$实际上就是旋转向量组成的空间，指数映射即是罗德里格斯公式。

  指数映射是一个满射，每个$SO(3)$中的元素，都可以找到至少一个$so(3)$元素与之对应（$\theta + 2k\pi$）。

  $se(3)$上的指数映射为：

  $$T = exp(\xi^{\wedge}) = \begin{bmatrix} R & J\rho\\ 0 &1 \end{bmatrix}\\ J = \frac{sin\theta}{\theta}I + (1-\frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\wedge}$$

6.3 李代数求导

• 两个李代数指数映射乘积的完整形式由BCH公式给出：

  $$ln(exp(A)exp(B)) = A+B + \frac{1}{2}[A, B] + \frac{1}{12}[A,[A,B]] + ...$$

• 对$ln(exp(\phi_1^{\wedge})exp(\phi_2^{\wedge}))^{\vee}$，当$\phi_1$或$\phi_2$为小量时，BCH公式给出了线性近似表达：

  $$ln(exp(\phi_1^{\wedge})exp(\phi_2^{\wedge}))^{\vee} = \left\{ \begin{split} J_l(\phi_2)^{-1}\phi_1 + \phi_2\ \ \ \ \ \ \phi_1为小量\\ J_r(\phi_1)^{-1}\phi_2 + \phi_1\ \ \ \ \ \ \phi_2为小量\\ \end{split} \right.$$

  BCH近似雅可比$J_l$就是上一节的$J$：

  $$J_l = J = \frac{sin\theta}{\theta}I + (1-\frac{sin\theta}{\theta})aa^T + \frac{1-cos\theta}{\theta}a^{\wedge}\\ J_l^{-1} = \frac{\theta}{2}cot\frac{\theta}{2}I + (1-\frac{\theta}{2}cot\frac{\theta}{2})aa^T - \frac{\theta}{2}a^{\wedge}\\ J_r(\phi) = J_l(-\phi)$$

  由以上公式说明了李群乘法和李代数加法的近似转换关系。

• 在$SO(3)、SE(3)$上没有良好定义的加法，而李代数由向量组成，有良好的加法运算。因此在计算位姿的导数时，通常使用李代数解决，李代数求导的两种思路：

  • 李代数求导$\delta \phi$：用李代数表示姿态，然后转化成对李代数求导$\phi + \delta \phi$
  $$\begin{split} &\frac{\partial (Rp)}{\partial R} = \frac{\partial(exp(\phi^{\wedge})p)}{\partial \phi}\\ &= lim \frac{exp((\phi+\delta\phi)^{\wedge})p-exp(\phi^{\wedge})p}{\partial \phi}\\ &=-(Rp)^{\wedge}J_l \end{split}$$
  • 扰动模型$\Delta R$：对$R$进行扰动，然后对扰动求导$\Delta R R$
  $$\begin{split} &\frac{\partial (Rp)}{\partial R} = lim \frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\partial \varphi}= -(Rp)^{\wedge}\\ & \frac{\partial Tp}{\partial \delta \xi} = \begin{bmatrix} I & -(Rp+t)^{\wedge}\\ 0 & 0 \end{bmatrix} = (Tp)^{\odot} \end{split}$$

最近开始着手写slam代码，看一些常用库源码的时候发现各种力不从心，一些c++11的骚操作竟然没见过，是时候完整撸一发c++ primer祭天了。

1. iostream

   • 标准输入：cin
   • 标准输出：cout、cerr、clog

   1 2 3 4 5 6

   #include <iostream> using namespace std; int v1=0, v2=0; cin >> v1 >> v2; cout << v1+v2 << endl; cerr << "This is nonsense." << endl;

   • << 和 >> 的方向表示了数据流的走向，也就是赋值的方向。cerr用来输出错误信息。

2. 控制流

   • while：每次执行循环之前先检查循环条件

   • do while：先执行循环体后检查条件

     1 2 3 4 5 6

     while (condition) statement do statement while (condition);

   • for：每次执行循环之前先检查循环条件，执行循环之后执行表达式

     1 2 3 4 5 6 7

     for (init-statement; condition; expression) { statemnt } // 范围for语句 for (declaration : expression) statement

   • switch：

     • case label：case标签必须是整形常量表达式
     • 如果某个case标签匹配成功，会往后顺序执行所有case分支，直到结尾或者遇到break
     • default标签

     1 2 3 4 5 6

     switch(ch) { case 'a': case 'b': case 'c': ++cnt; break; }

   • break：负责终止离他最近的while、do while、for或switch语句。

   • continue：负责终止离他最近的while、do while、for循环的当前迭代。

   • goto：无条件跳转到同一函数内的某个带标签语句。

     labeled statement: label: statement

   • 异常

     • throw：引发异常，后面紧随一个异常类型，终止当前函数，将控制权转移给能够处理该异常的代码。

       1 2 3

       #include <stdexcept> // runtime_error 标准库异常类型 throw runtime_error("Data must refer to same name");

     • try：处理异常，后面紧随一套catch子句用来处理异常。

       1 2 3 4 5 6 7

       try{ program statements } catch (exception-declaration) { handler-statements } catch (exception-declaration) { handler-statements } ...

       try语句块内声明的变量在块外无法访问，即使是catch语句。

       catch一旦完成，程序跳转到最后一个catch子句之后的语句。

3. 类

   类型 & 对象（实例），变量 & 行为（方法）。

   • 存在类内默认初始化

   • 类通常被定义在头文件中，头文件名字应与类的名字保持一致

     头文件通常包含只能被定义一次的实体，如类、const等。

     头文件保护符#ifndef系列，创建预处理变量，防止多次包含。

   • 构造函数初始值列表：冒号以及冒号和花括号之间的代码

     列表只说明用于初始化成员的值，而不限定初始化的具体顺序。

     成员的初始化顺序与它们在类定义中的出现顺序一致。

     1 2 3 4 5 6 7 8 9 10

     // 为类成员初始化 Sales_data(const string &s, unsigned n, double p) : bookNo(s), units_sold(n), revenue(p*n) {}  // 区别于赋值 Sales_data(const string &s, unsigned n, double p) { bookNo = s; ... }

   • 接口与封装：

     • 定义在private说明符之后的成员只能被类内成员函数访问，封装了类的实现细节。
     • 定义在public说明符之后的成员可以在整个程序内被访问，定义类的接口。

   • class和struct的区别：成员访问权限

     • struct：定义在第一个说明符之前的成员是public
     • class：定义在第一个说明符之前的成员是private

   • 友元：允许其他类或函数访问它的非公有成员，在类内添加以friend关键字开始的友元声明。

     友元的声明仅仅指定了访问权限，而非一个通常意义上的函数声明。

     1 2 3 4 5 6 7 8

     class Sales_data { // 友元声明 friend Sales_data add(const Sales_data&, const Sales_data&); // 非公有成员 private: string bookNo; double revenue = 0.0; };

   • 静态成员static：与类本身相关联，不属于任何一个对象，因此不是在创建类对象的时候被定义的，因此通常在类的外部定义和初始化，在类内部添加以static关键字开始的静态成员声明。

4. 内置类型

   • 内存中的一个地址对应一个字节
   • unsigned类型表示大于等于0的数（$[0, 2^{n}-1]$），被赋给一个超出表示范围的数时，自动取余，作为循环条件时当心进入无限循环
   • signed类型正负值范围平衡（$[-2^{n-1}, 2^{n-1}-1]$），被赋给一个超出表示范围的数时，结果未定义
   • 字符型char，单引号，一个字节
   • 字符串型，双引号，常量字符数组，结尾隐含空字符 ‘\0’
   • nullptr = 0（传统NULL包含在cstdlib头文件内）

5. 变量

   • 列表初始化，花括号

     1 2 3 4 5 6

     // 拷贝初始化 int x=0; int x={0}; // 直接初始化 int x{0}; int x(0);

   • 变量声明extern，源于分离式编译机制，一个变量只能被定义一次，可以声明多次

   • 作用域，嵌套作用域 & 内部重定义

6. 复合类型

   • 引用，typename &declaration，浅拷贝，绑定一个对象，引用不是对象

   • 指针，typename *declaration，存放对象地址

     1 2 3 4

     int a; int *p, *q=a; p = &a; p = q;

   • 取地址符&

     1 2 3

     int *p = a; int *p = &a; // a--->对象 &a--->地址

   • 解引用符*

     1 2 3 4 5 6

     int a; int *p; *p ---> undefined p = &a; *p = 10; // p--->指针 *p--->对象

   • void* 指针，可以指向任意类型的对象，但是不能进行对象操作

7. const限定符

   • 参与编译预处理

   • 要实现多个文件共享，必须在const变量定义之前加上extern关键字

     1 2 3 4

     // define extern const int bufferSize = fcn(); // declare extern const int bufferSize;

   • 允许任意表达式作为初始值（允许隐式类型转换）

   • 常量引用，允许非常量赋值，实际引用一个内存中的“临时值”

   • 指向常量的指针，允许非常量赋值，但是不能通过该指针修改对象

   • 常量指针，指针始终指向同一个对象

   • 常量表达式constexpr，表达式在编译过程中就能得到计算结果

8. 处理类型

   • 类型别名typedef & using

     1 2 3 4 5 6 7

     // 传统 typedef double base; typedef base *p; // p是double指针 base a; p p1=&a; // c++11 using base = double;

   • auto类型说明符，让编译器分析表达式所属类型并为变量赋值

     1 2	// 一条类型声明语句中所有变量的类型必须保持一致 auto i=0, *p=&i;

   • decltype类型指示符，仅分析表达式返回类型，不做赋值（因此不做实际计算）

     1	decltype(f()) a=x;

1. string

   • 读取，>>不读取空白，遇到空白符停止，getline保留空白符，遇到换行符停止。

   • 字符串字面值不是string对象，而是C风格字符串，c_str()成员函数能够将string对象转化成C风格字符串

   • 遍历，范围for语句，每次迭代declare的变量会被初始化为expression的下一个元素

     1 2 3 4 5 6 7 8 9

     for (declaration : expression) statement string str("some string"); // 赋值 for (auto c: str) cout << c << endl; // 引用 for (auto &c: str) c = toupper(c);

   • size()返回的类型是string::size_type，通常用auto

2. vector

   • 类模版，相同类型对象的集合，声明时必须提供元素类型vector<int>
   • 添加元素push_back()

3. 迭代器

   • 所有标准库容器都支持迭代器，只有少数支持下标访问
   • begin()返回指向第一个元素的迭代器，end()返回尾后元素的迭代器
   • cbegin()和cend()操作类似，返回值是const_iterator，不能修改对象
   • 迭代器的类型是container::iterator和container::const_iterator，通常用auto
   • 解引用迭代器得到对象
   • 箭头运算符->，结合解引用+成员访问两个操作
   • 迭代器失效：容器改变容量

4. 数组

   • 大小固定，编译的时候维度应该已知，因此必须是常量表达式
   • 不能用做拷贝和赋值

5. 表达式

   • 左值和右值

     ​ C语言中，左值指的是既能出现在等号左边也能出现在等号右边的变量或表达式，通常来说就是有名字的变量，而右值只能出现在等号右侧，通常就是一些没有名字也取不到地址的中间结果。

     继承到C++中归纳来讲就是：当一个对象被用作右值的时候，用的是对象的值（内容），当被用作左值的时候，用的是对象的身份（在内存中的位置）。

   • 求值顺序

     有四种运算符明确规定了求值顺序，逻辑与（&&）、逻辑或（||）、条件（?:）、逗号（,）运算符。

     1 2	int i = 0; cout << i << ++i << endl;

   • 前置版本和后置版本的递增递减

     用于复合运算中时，

     前置版本首先修改对象，然后将对象本身作为左值返回。

     后置版本将对象原始值的副本作为右值返回。

   • 位运算

     • 整形提升，char8->int32
     • 添0，越界丢弃

   • 逗号运算符：含有两个运算对象，首先对左表达式求值，然后将求值结果丢弃掉，最右边的表达式的值将作为整个逗号表达式的值。本质上，逗号的作用是导致一些列运算被顺序执行。

     1 2	// 分别对逗号表达式内对象赋值，然后返回最右cnt的值 var = (count=19, incr=10, cnt++)

6. 函数

• 局部静态对象static：首次调用时被初始化，直到程序终止才被销毁。

  1 2 3 4 5 6

  int f() { // 只初始化一次，函数调用结束以后这个值仍有效 static cnt = 0; return ++cnt; }

• 参数传递：如果形参被声明为引用类型，它将绑定到对应的实参上（传引用调用），否则将实参的值拷贝后赋给形参（传值调用）。

• 含有可变形参的函数

  • 所有实参类型相同，可以使用initializer_list模版类型的形参，列表中元素是const。

    1 2	initializer_list<T> lst; initializer_list<T> lst{a, b, c, ...};

  • 编写可变参数模版

  • 省略符形参：对应的实参无需类型检查

    1 2 3 4

    // 带部分形参类型 void foo(parm_list, ...); void foo(...);

• 内联函数incline：避免函数调用开销

• 调试帮助

  • NDEBUG预处理变量：用于关闭调试状态，assert将跳过不执行。
  • assert (expr) 预处理宏：如果表达式为假，assert输出信息并终止程序。

  预处理名字由预处理而非编译器管理，因此可以直接使用名字而无须提供using声明。

1. static_cast和dynamic_cast强制类型转换
   • static_cast \ (expression)：暴力类型转换，不运行类型检查。
   • dynamic_cast\ (expression)：运行类型检查，下行转换安全。
2. new & delete：new [] 要和 delete []对应上。
3. c++的oop特性（private public…）只在编译时刻有意义。同一类的对象可以互相访问私有成员。
4. firend：注意方向是give acess to，授权friend访问自己的private。编译时刻检查。
5. composition：组合，用一系列对象构造对象。
6. inheritance：继承，用一些类来构造新的类。

1
2
3
4

class A;
class B : public A{
    ....
};

构造：子类构造的时候要先构造父类，析构的时候反过来，先析构子类。

重名：name hiding，special for c++。

1. ​ protected：designed for sub class。子类可以直接访问。其他类看不到。
2. overload：参数表必须不同，否则编译器无法识别。
3. default argument：defaults must be added from right to left。must be declared in .h files。发生在编译时刻。
4. inline：不用真正调用函数，而是直接插入汇编代码段。tradeoff between space and time consuming。区别于宏，宏是没有类型检查的。

5. const

   • declare a variable：是变量，而不是常数
6. 变量的生存期限
   • Automatic：local variables，存活在{}函数定义里面
   • dynamic：controlled by special instructions，比如main里面实例化的变量/类/内存，最后delete/free它
   • thread：
   • static：程序运行时间内有效
7. 存储
   • stack：快，小，用于Automatic
   • heap：慢，大，用于dynamic
8. 指针
   • 比引用灵活：可以在程序存活期间改指别的目标
   • 用于支持底层的各类不连续存储：dynamic memory各种链表
   • 智能指针
     • std::unique_pointer：每个object只能有一个unique pointer，pointer和target的生存时间相同
     • std::shared_pointer：每个object可以有多个，只要pointer存在target就存在
     • std::weak_pointer：同上
     • c++11里面是shared_ptr、unique_ptr 以及 weak_ptr
   • nullptr：可以当false用，free memory，所以不能解引用，*p会segmentation fault
   • const
     • T*：自由指针
     • T const*：用read-only way指向value
     • T* const：常数指针，不能改指
     • T const* const：即不能改指，也不能改数
   • this指针
     • available inside member functions：类/函数等{}里面
     • 返回object的地址
     • this->能够访问成员
     • *this表示object本身
9. unique
   • java里面有garbage collector，扫描那种没有指针的内存地址，然后销毁，但是不及时
   • c++里面关键字类型可以自动回收
   • unique_ptr保存在stack里面，但是它创建的对象在heap里面，unique_ptr失效的时候heap里的object也销毁掉
   • move可以给object切换指针

0. 变量

变量使用 ${ } 的方式取值，但是在if控制语句中直接使用变量名。

1. project

project ( project_name [CXX] [C] [Java] )

用来指定工程名称和工程语言（可省略），指令隐式定义了projectname_BINARY_DIR和projectname_SOURCE_DIR两个变量（写在cmake_cache里面），指的是编译发生的当前目录。

2. set

set ( VAR [VALUE] )

用来显式定义变量，如set (SRC_LIST main.c t1.c t2.c) 。（竟然不用套括号？）

3. message

message ( [SEND_ERROR | STATUS | FATAL_ERROR] “message to display” VAR )

用来向终端输出用户定义的信息。

4. add_executable

add_executable ( executable_filename [source_filename] )

生成名字为executable_filename的可执行文件，相关的源文件 [source_filename] 可以是一个源文件列表。

5. 清理构建结果

make clean

对构建出的可执行文件进行清理。

6. 外部构建

1
2
3
4

mkdir build
cd build
cmake ..
make

所有编译动作发生在编译目录，对原有工程没有任何影响。

7. add_subdirectory

add_subdirectory ( source_dir [binary_dir] [EXCLUDE_FROM_ALL] )

向当前工程目录添加存放源文件的子目录source_dir，并指定存放中间二进制文件和目标二进制文件的位置binary_dir。指令隐式修改 EXECUTABLE_OUTPUT_PATH 和 LIBRARY_OUTPUT_PATH 两个变量。

8. 更加像一个工程

• 创建工程根目录，创建CMakeLists.txt。

1
2
3
4
5
6
7

# 指定最低编译版本
cmake_minimum_required(VERSION 3.7)
# 指定工程名字
PROJECT(HELLO)
# 测试类打印信息
MESSAGE(STATUS "This is BINARY dir " ${HELLO_BINARY_DIR})
MESSAGE(STATUS "This is SOURCE dir " ${HELLO_SOURCE_DIR})

• 添加子目录src，用来存放源文件，为子目录创建CMakeLists.txt。

1
2
3

# 在根目录CMakeLists.txt中添加子目录声明
add_subdirectory(src bin)
# 编译产生的中间文件以及目标文件将保存在编译文件夹的bin子目录下

1
2
3
4
5

# 编写当前子目录的CMakeLists.txt
add_executable(hello main.c)
# 修改最终生成的可执行文件以及库的路径，这两个指令要追随对应的add_executable()和add_library()指令
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_PATH}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib)

• 添加子目录build，作为外部编译文件夹（ ${PROJECT_BINARY_DIR} ），存放编译的过程和目标文件。

1
2
3

cd build
cmake ..
make

• 添加子目录doc，用来存放工程文档hello.txt。
• 添加文本文件README，COPYRIGHT。
• 添加runhello.sh脚本，用来调用可执行文件hello。

9. 打包安装

• 在根目录的CMakeList.txt中添加安装信息

1
2
3
4
5
6

# 安装COPYRIGHT/README到<prefix>/share/doc/cmake/t2
INSTALL(FILES COPYRIGHT README DESTINATION share/doc/cmake/t2)
# 安装runhello.sh到<prefix>/bin
INSTALL(PROGRAMS runhello.sh DESTINATION bin)
# 安装工程文档到<prefix>/share/doc/cmake/t2
INSTALL(DIRECTORY doc/ DESTINATION share/doc/cmake/t2)

• 在子目录的CMakeList.txt中添加安装信息

1
2
3
4

# 安装脚本要调用的可执行文件hello到<prefix>/bin，
# 注意install(targets)指令也要追随对应add_executable()和add_library()指令的路径
INSTALL(TARGETS hello
		RUNTIME DESTINATION bin)

• 安装程序包

1
2
3
4
5
6
7
8
9
10

cd build
# 在cmake命令中指明安装目录的前缀<prefix>
# CMAKE_INSTALL_PREFIX 默认是/usr/local
cmake -DCMAKE_INSTALL_PREFIX=/Users/carrol/tmp ..
make
make install

# 查看目标文件夹
j tmp
tree -a

10. add_library

add_library ( name [SHARED | STATIC | MODULE] [source_filename] )

生成名字为libname.X的库文件。

• SHARED，动态库，libname.dylib
• STATIC，静态库，libname.a

设置目标动态库和静态库同名 set_target_properties

1
2
3
4
5

# 设置目标动静态库同名
add_library(hello SHARED hello.c)
add_library(hello_static hello.c)
set_target_properties(hello_static
                      PROPERTIES OUTPUT_NAME hello)

防止构建中清理同名文件 set_target_properties

cmake在构建一个target时，会尝试清理掉其他使用这个名字的库——在构建libhello.a时会清理掉libhello.dylib。

我实际操作时候会保留两个库文件，但是在作为第三方被引用的时候会报错：

dyld: Library not loaded: libhello.dylib

Reason: image not found

1
2
3
4

SET_TARGET_PROPERTIES(hello 
                      PROPERTIES CLEAN_DIRECT_OUTPUT 1)
SET_TARGET_PROPERTIES(hello_static 
                      PROPERTIES CLEAN_DIRECT_OUTPUT 1)

设置动态版本号 set_target_properties

1
2
3
4

# 设置动态库版本号
set_target_properties(hello
                      PROPERTIES VERSION 1.2
                      SOVERSION 1)

编译文件夹下生成了libhello.1.2.dylib、libhello.1.dylib、libhello.dylib三个动态库文件，只有一个是真的，另外两个是替身。

安装共享库和头文件

修改库的源文件夹下的CMakeLIsts.txt

1
2
3
4
5
6

# 库文件
install(TARGETS hello hello_static
        ARCHIVE DESTINATION lib   //静态库
        LIBRARY DESTINATION lib)  //动态库
# 头文件
install(FILES hello.h DESTINATION include/hello)

11. include_directories

include_directories( dir1 dir2 … )

用来向工程添加多个特定的头文件搜索路径

12. link_directories & target_link_libraries

link_directories( dir1 dir2 … )

添加非标准的共享库搜索路径

target_link_libraries( target lib1 lib2 … )

用来为目标target添加需要链接的共享库，target可以是一个可执行文件，也可以是一个库文件。

查看生成目标的库依赖情况

1
2
3
4
5

# 生成的目标可执行文件为main
# for OSX
otool -L main
# for linux
ldd main

只能列出动态库。

13. 常用变量

PROJECT_BINARY_DIR：编译发生的目录

PROJECT_SOURCE_DIR：工程顶层目录

CMAKE_CURRENT_SOURCE_DIR：当前CMakeLists.txt所在目录

CMAKE_MODULE_PATH：自定义的cmake模块所在路径

LIBRARY_OUTPUT_PATH：重定义目标库文件存放目录

EXECUTABLE_OUTPUT_PATH：重定义目标可执行文件存放目录

14. findNAME.cmake模块

1. 在工程目录中创建cmake文件夹，并创建FindHELLO.cmake模块：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

# 示例
FIND_PATH(HELLO_INCLUDE_DIR hello.h /usr/local/include/hello)
FIND_LIBRARY(HELLO_LIBRARY hello /usr/local/lib)
IF (HELLO_INCLUDE_DIR AND HELLO_LIBRARY)
	SET(HELLO_FOUND TRUE)
ENDIF (HELLO_INCLUDE_DIR AND HELLO_LIBRARY)
IF (HELLO_FOUND)
	IF (NOT HELLO_FIND_QUIETLY)
		MESSAGE(STATUS "Found Hello: ${HELLO_LIBRARY}")
	ENDIF (NOT HELLO_FIND_QUIETLY)
ELSE (HELLO_FOUND)
	IF (HELLO_FIND_REQUIRED)
		MESSAGE(FATAL_ERROR "Could not find hello library")
	ENDIF (HELLO_FIND_REQUIRED)
ENDIF (HELLO_FOUND)

1. 在主目录CMakeLists.txt中添加cmake模块所在路径：

1
2

# 为find_package()指令成功执行
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

1. 然后就可以在源文件CMakeLists.txt中调用 find_package：

find_package ( name [QUIET] [REQUIRED] )

用来调用预定义在CMAKE_MODULE_PATH下的Find\.cmake模块。

每一个模块都会定义以下几个变量：

• NAME_FOUND
• NAME_INCLUDE_DIR or NAME_INCLUDES
• NAME_LIBRARY or NAME_LIBRARIES

根据指令后面的参数还会有以下变量：

• NAME_FIND_QUIETLY，如果指定了QUIET参数，就不会执行如下语句：

  1	MESSAGE(STATUS "Found Hello: ${NAME_LIBRARY}")

• NAME_FIND_REQUIRED，如果指定了REQUIRED参数，就是指这个共享库是工程必须的，如果找不到，工程就不能编译，对应地会执行如下语句：

  1	MESSAGE(FATAL_ERROR "Could not find NAME library")

可以通过\_FOUND判断模块是否被找到，并执行不同的操作（如添加非标准路径、输出错误信息等）。

15. find_指令

• find_path

  find_path ( VAR name1 path1 path2 … )

  VAR变量代表包含name1文件的路径——路径。

• find_library

  find_library ( VAR name1 path1 path2 …)

  VAR变量包含找到的库的全路径，包括库文件名——路径下的所有文件。

1 基本实现

数据点云配准，最经典的方法就是ICP迭代最近点法。

• 最近点：欧几里得意义上距离最近的点。
• 迭代：迭代目标是通过不断更新运动参数，使得两个点云的重叠部分充分吻合。

ICP的求解分为两种方式：

• 利用线性代数求解（SVD），在给定了匹配的情况下，最小二乘问题实际上具有解析解。
• 利用非线性优化方式求解，类似于BA方法，适用于匹配未知的情况。

2 SVD方法求解

算法推导如下：

1. 首先将点云文件进行粗匹配，如ORB特征点匹配。

2. 从点集$P={\overrightarrow{p_1}, \overrightarrow{p_2}, …, \overrightarrow{p_n}}$中随机选取指定数量的点$\{\overrightarrow{p_t}\}$作为参考点，参考点的数量决定了ICP算法的计算效率和配准精度。

3. 在另一个点集$Q={\overrightarrow{q_1}, \overrightarrow{q_2}, …, \overrightarrow{q_m}}$是待匹配的点query points，那么想要找到一个欧式变换$R, t$，使得$\forall i, p_i = Rq_i + t$。

4. 求解欧式变换$T^k$，使得$E^k=\Sigma| \overrightarrow{p_t} - T^k \overrightarrow{q_t}|^2$最小化。 将空间变换分解为旋转和平移两部分，首先定义两个点云的质心：

   $$\overrightarrow{p} = \frac{1}{n} \Sigma \overrightarrow{p_t}, \ \ \overrightarrow{q} = \frac{1}{n} \Sigma \overrightarrow{q_t}，质心,\ 用于描述平移\\ \overrightarrow p_i = \overrightarrow{p_t} - \overrightarrow{p}, \ \ \overrightarrow q_i = \overrightarrow{q_t} - \overrightarrow p，中心化点云,\ 用于描述旋转\\$$

   于是有目标函数：

   $$\begin{split} E^k & = \Sigma|\overrightarrow{p_t} - T^k \overrightarrow{q_t}|^2 = \Sigma|(p+p_i) -T (q+q_i)|^2\\ & = \Sigma|(p+p_i) -R (q+q_i) -t|^2\\ & = \Sigma |(p_i - Rq_i) + (p - Rq -t)|^2\\ & = \Sigma( |p_i - Rq_i|^2 + |p - Rq -t|^2)\\ & = \Sigma( |p_i - Rq_i|^2\\ J &= \frac{1}{2} \sum e_i = \frac{1}{2} E^k \end{split}$$

   对目标函数展开，而且已知旋转矩阵是正交阵，$R^TR=I​$，所以目标函数的前两项都与$R​$无关：

   $$R^* = argmin_R J = \frac{1}{2}\sum p_i^Tp_i + q_i^TR^TRq_i - 2p_i^TRq_i$$

   只有最后一项与$R$有关，于是得到关于$R$的目标函数：

   $$J(R) = \sum_{unrelated} -\ p_i^TRq_i = \sum - \ tr(Rq_ip_i^T) = -tr(R\sum_{i=1}^nq_ip_i^T)$$

   然后通过SVD奇异值分解求解上述问题的最优$R$，首先定义$W = \sum_1^n pq^T$，当$W$满秩时：

   $$W = \sum_{i=1}^{n} \overrightarrow{p_i}*\overrightarrow{q_i^T} = U \begin{bmatrix} \sigma1 & 0 & 0 \\ 0 & \sigma2 & 0 \\ 0 & 0 & \sigma3 \end{bmatrix} V^T\\ R = UV^T$$

   然后间接得到平移$t$：

   $$t = {p} - R{q}$$

代码实现如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

void pose_estimation_3d3d(
        const vector<Point3f>& pts1,         // point cloud 1
        const vector<Point3f>& pts2,         // point cloud 2
        Mat& R, Mat& t,
        Eigen::Matrix3d& R_, Eigen::Vector3d& t_
        )
{
    Point3f p1, p2;         // center of Mass
    int N = pts1.size();
    for(int i=0; i<N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 /= N;
    p2 /= N;

    vector<Point3f> q1(N), q2(N);      // remove the COM
    for(int i=0; i<N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();       // calculate W matrix
    for(int i=0; i<N; i++)
    {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }

    // SVD decomposition
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);         // SVD
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // calculate R,t
    R_ = U * V.transpose();
    t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

3 非线性优化方法

另一种方式是通过迭代的方式来寻找最优值，误差项的表示与上一节相同，用李代数来表达位姿，旋转和平移不用再解耦表示，目标函数为：

单个误差项关于位姿的导数可以使用李代数扰动模型来描述：

其中$p_i$作为参考点，对扰动的导数为0，因此：

将最小二乘问题进行图描述：优化变量为李代数表达的位姿$\xi$，因此图中只有一个节点，误差项为一元边（从当前节点指向当前节点），对误差项做线性展开：

其中的雅可比矩阵也就是上面说的，单个误差项关于位姿的一阶导数。

4算法优化

• 删除点云数据采集中产生的噪声及异常值。
• 查找最近点的过程采用KD-Tree数据结构，减少时间复杂度。

1 cmake

详见cmake for record。

2 简单配置

主要就是keymap很不适应，基本上删除了大部分editing的配置，因为可以用vim。剩下的修改主要延续sublime和OSX的习惯。

2.1 搜索

全局搜索：cmd + F

剩下的交给vim。

2.2 导航

search for file：cmd + O

search for class：opt + cmd + O

search for symbol：shift + cmd + O

go to line：cmd + G

back：ctrl + cmd + left

forward：ctrl + cmd + right

剩下的交给vim。

2.3 注释

代码块注释：shift + cmd + ／

2.4 智能提示

看见小灯泡就：opt + enter

2.5 run & build

run：cmd + R

build：cmd + B

2.6 代码生成

insert：cmd + J

最近在熟悉Eigen库，经常要打印东西，加了一个split模版快速分割代码片段。

generate：cmd + N

还有一些vim与ide冲突的键，可以手动选择是服从ide还是vim。

1 综述

基于图优化的slam主要分为以下三个部分：

• 前端：基于传感器数据建图，匹配相邻帧，添加节点和边（raw graph），节点表示机器人的位姿，边表示节点之间的位姿联系。位姿信息可以来自里程计计算，可以来自ICP激光点云匹配，也可以来自闭环检测反馈。
• 后端：优化图，基于历史信息的约束，调整新加入的机器人位姿顶点使其尽量满足边的约束（optimized graph）。
• 宏观的闭环检测：根据闭环信息优化矫正整个拓扑图。

这里面涉及到了两个优化，一个是后端局部优化，一个是全局闭环优化，两者计算的思路是一样的。

2 优化

2.1 全局闭环优化，用于矫正整个拓扑图

前端后端完成的事情是探索并创建新的节点，获得新的测量值，添加新的位姿关系方程：

而全局闭环检测添加已知节点之间的位姿约束关系：

再添加一个初始条件（不是必须的，但是后面实验表明固定一个顶点比不固定效果要好——相当于有一个明确可信的基准）：

• 以上线性方程组中，闭环检测部分的方程中的两个结点都在前面出现过，因此不增加矩阵的秩，因此最终要求解包含$k$个方程$k+1$个未知数的线性方程组。
• 闭环的关键性：如果没有闭环条件，方程组$Ax=b$左右两边秩是相等的——有唯一解，添加了闭环条件以后，相当于方程组左侧$A$的秩不变，但是右侧$b$的秩则增加了，$rank(A) < rank(A, b)$——没有解析解，只有最优。
• 实际上状态$\textbf x$是一个包含夹角$\theta$的向量$[x, y, \theta]$，实际相对位姿的计算并非简单的线性叠加：

举个栗子：

机器人从起始位置$x_0=0$处出发，里程计测得它向前移动了1m，到达$x_1$，接着测得它向后移动了0.8m，到达$x_2$，这时通过闭环检测，发现他回到了起始位置。

首先根据给出信息构建图：

然后根据闭环条件添加约束：

补充初始条件：

使用最小二乘法求上述方程组的最优解，首先构建残差平方和函数：

然后对每个参数求偏导：

解得$x_1 = 0.93, x_2 = 0.07$，可以看到闭环矫正了所有节点的位姿，优化了整个拓扑图。

2.2 后端局部优化，用于矫正局部地图

再举个栗子：

机器人从起始位置$x_0=0$处出发，并观测到其正前方2m处有一个路标$l_0$，里程计测得它向前移动了1m，到达$x_1$，这时观测到路标在其正前方0.8m处。

首先根据前端信息建图 raw graph（这样建图明显是存在累积误差的）：

然后添加闭环约束：

初始条件：

构建残差平方和：

求偏导求解：$x_1 = 1.07, l_0 = 1.93$，可以看到后端是对前端新添加进来的节点位姿做了矫正，消除部分测量误差。

这里面涉及到两种传感器信息——里程计和激光雷达，两种传感器的精度是有差别的，我们对其的信任程度也应该不同，反映到公式中就是要为不同传感器信息赋予不同的权重。假设编码器信息更准确，那么：

调整权重之后解得：$x_1 = 1.01, l_0 = 1.9$，可以看到计算结果会向着更信任的传感器的测量结果靠近。

2.3 严格推导

2.3.1 信息矩阵（误差权重矩阵)

图优化问题转化为最小二乘问题，首先是带权重的残差平方和函数的一般形式：

其中的$\Omega_{i,j}$项就是上文提到的误差权重矩阵，它的正式名字叫信息矩阵。

传感器的测量值，可以看作是以真值为中心的多元高斯分布：

协方差矩阵$\Sigma$对角线上的值表示每一维对应的方差，该方差值越大表示该维度不确定性越大，对应的信息权重应该越小。实际上拓扑图上每条边对应的信息矩阵就是对应测量协方差矩阵的逆。

2.3.2 非线性

上文已经提到，位姿变化非线性——非线性最小二乘问题，要采用迭代法求解。迭代法需要有一个好的初始假设值，然后在这个值附近增量式迭代寻找最优解。

首先要将非线性函数转化成关于增量$\Delta x$的线性函数——泰勒展开，根据具体的展开形式又分为：

• 一阶、二阶梯度法

  直接对目标函数在$x$附近进行泰勒展开：

  $$||f(x+\Delta x)||_2^2 \approx ||f(x)||_2^2 +J(x) \Delta x = \frac{1}{2} \Delta x^T H \Delta x$$

  一阶梯度法（最速下降法）：只保留一阶梯度，并引入步长$\lambda$：

  $$\Delta x^* = -\lambda J^T(x)$$

  二阶梯度法（牛顿法）：保留一阶和二阶梯度信息

  $$J^T(x)+H\Delta x^*=0$$

  最速下降法过于贪心，容易走出锯齿路线，增加迭代次数。牛顿法需要计算目标函数的二阶导数（Hessian矩阵），计算困难。

• 高斯牛顿法

  对$f(x)$而不是目标函数$f(x)^2$在$x$附近进行一阶泰勒展开：

  $$f(x+\Delta x) \approx f(x) + J(x) \Delta x$$

  对应每一个误差函数$e_{ij}$：

  $$\begin{split} & e_{ij}(x_i+\Delta x_i, x_j+\Delta x_j) = e_{i,j}(x+\Delta x) \\ &\approx e_{ij} +\frac{\partial e_{ij}}{\partial x}\Delta x = e_{ij} + J_{ij}\Delta x \end{split}$$

  ​ 其中$J_{ij}$为初始值附近的雅可比矩阵（定义见卡尔曼滤波）。

  带入目标函数得到近似二阶展开：

  $$\begin{split} & F_{ij}(x+\Delta x) = e_{ij}(x+\Delta x)^T \Omega_{ij}e_{ij}(x+\Delta x)\\ & \approx (e_{ij} + J_{ij}\Delta x)^T \Omega_{ij}(e_{ij} + J_{ij}\Delta x)\\ & = \underbrace{e_{ij}^T\Omega_{ij}e_{ij}}_{c_{ij}} + 2\underbrace{e_{ij}^T\Omega_{ij}J_{ij}}_{b_{ij}^T}\Delta x + \Delta x^T \underbrace{J_{ij}^T\Omega_{ij}J_{ij}}_{H_{ij}}\Delta x\\ & = c_{ij} + 2b_{ij}^T\Delta x + \Delta x^T H_{ij}\Delta x \end{split}$$

  求解增量$\Delta x$：

  $$2b + 2H\Delta x^* = 0\\ 高斯牛顿方程：H\Delta x^* = -b\\$$

  对比牛顿法可见，高斯牛顿法用$J^TJ$作为二阶Hessian矩阵的近似，简化了计算。

  上述算法要求近似$H$矩阵是正定且可逆的，实际数据很难满足，因而在使用高斯牛顿算法时可能出现$H$为奇异矩阵或病态的情况，增量稳定性较差，导致算法不收敛。

  图形上来思考，就是近似后的梯度方向不再是梯度变化最快的方向，可能引起不稳定。

• 列文伯格—马夸尔特法

  为$\Delta x$添加一个信赖区域，不让它因为过大而使得近似$f(x+\Delta x) = f(x) + J(x)\Delta x$不准确。

  $$\rho = \frac{f(x+\Delta x) - f(x)}{J(x) \Delta x}$$

  可以看到如果$\rho$接近1，说明近似比较好。如果$\rho$比较大，说明实际减小的值远大于估计减小的值，需要放大近似范围，反之你懂的。

  $$||D\Delta x_k||_2^2 \leq \mu$$

  将每次迭代得到的$\Delta x$限定在一个半径为信赖区域的椭球中，根据$\rho$的大小修改信赖区域。于是问题转化成为了带不等式约束的优化问题：

  $$min_\Delta x \frac{1}{2}||f(x) + J(x)\Delta x||^2_2, \ \ s.t. ||D \Delta x ||^2 = \mu$$

  用拉格朗日乘子转化成无约束问题：

  $$min_\Delta x \frac{1}{2}||f(x) + J(x)\Delta x||^2_2 + \frac{\lambda}{2}||D \Delta x ||^2$$

  展开后得到如下形式：

  $$(H + \lambda D^TD)\Delta x^* = -b$$

  通常把$D$取值为单位阵$I$，得到更简化形式：

  $$(H + \lambda I)\Delta x^* = -b$$

  当$\lambda$较小时，$H$占主要地位，说明二次近似模型较好，LM算法更接近高斯牛顿法。当$\lambda$较大时，$\lambda I$占主要地位，LM算法更接近一阶梯度法。修正了线性方程组矩阵的病态问题，比高斯牛顿法更加健壮，但是收敛速度也更慢。

  图形上思考，LM算法修正了高斯牛顿法得到的梯度，以此固定一个搜索区域，在区域内寻找最优。

2.3.3 稀疏矩阵

对于误差函数$e_{ij}$，它只和$e_i$和$e_j$有关，因此它的雅可比矩阵有如下结构（行数是$x$的维度，列数是拓扑图中节点映射关系的数目）：

相应地$b_{ij}$是一个包含很多0的列向量：

$b = \Sigma_{ij} b_{ij}$：

$H_{ij}$是一个包含很多0的对称阵：

$H=\Sigma_{ij}H_{ij}$：

梳理一下计算流程：$e_{ij} \to J_{ij} \to A_{ij}, B_{ij} \to b_{ij}, H_{ij} \to b, H \to \Delta x^* \to x$

2.3.4 误差函数

前面定义过位姿的非线性叠加，显然位姿误差也不是简单的线性加减关系：

其中的$Z_{ij}$、$X_i$、$X_j$都是矩阵形式。$X_i^{-1}X_j$表示节点j到节点i之间的位姿差异$\hat Z_{ij}$，假设这个转移矩阵形式如下：

假设测量值$Z_{ij}$形式如下：

分块矩阵的求逆过程如下：

所以误差$e_{ij}$计算如下：

求解雅可比矩阵$J_{ij}$：

累加$b$和$H$矩阵：

1 引言

频繁项集：集合，${a, b, c}$

关联规则：映射，$a\to b$

支持度：针对某个频繁项集，$support(频繁项集a) = \frac{freq(频繁项集a)}{freq(所有项集)}$

可信度：衡量某条关联规则，$confidence(a\to b) = \frac{support(a|b)}{support(a)}$

对于包含N个元素的数据集，可能的集合有$2^N - 1$种，暴力遍历显然药丸，因此引入Apriori原理。

Apriori原理：减少可能的项集，避免指数增长。

• backwards：如果某个项集是频繁的，那么它的所有子集也是频繁的。
• forwards：如果一个项集是非频繁项集，那么它的所有超集也是非频繁的。

2 Apriori算法

1

def apriori(dataSet, minSupport=0.5):

算法思路：从单个项集开始检查，去掉那些不满足最小支持度的项集，然后对剩下的集合进行组合，得到包含两个元素的项集，重复扫描，然后将剩余项集组合成包含三个元素的集合，依次类推，直到所有项集都被去掉。

• 为啥最后会得到空集：因为包含所有元素的项集一定不是频繁项集，否则根据Apriori原理，它的全部子集都是频繁项集。
• 如何从包含k个元素的项集集合生成包含k+1个元素的项集集合：从k个元素的项集到k+1个元素项集的扩充，只允许有一个元素的不同，算法中为了避免重复结果，只对前k-1个元素相同的两个项集求并集。

代码实现过程中发现了几个知识记录一下：

• map函数的返回值：python2下直接返回列表，python3下返回的是迭代器：

1
2
3
4
5

map(frozenset, C1)
# 返回 <map object at 0x101e78940> 

list(map(frozenset, C1))
# 返回 list[frozenset1(), frozenset2(), ...]

• 字典的update方法：

1
2

# 将dict2的键值添加到dict1中，在涉及迭代操作时可以省略传递中间值
dict1.update(dict2)

• set & frozenset：set无排序且不重复，并且可变，因此unhashable。frozenset不可变，可以用作字典的key。

3 关联规则

对一个包含k个元素的频繁项集，其中可能的关联规则有：

暴力遍历肯定又药丸，因此延续Apriori的思路，关联规则也有一条类似的属性：

• 如果某条规则的前件不满足最小可信度要求，那么它的所有子集也不满足最小可信度要求。
• 对应的，如果某条规则的后件不满足最小可信度要求，那么它的所有超集也不满足。

算法思路：对每个至少包含两个元素的频繁项集，从后部只包含一个元素的规则开始，对这些规则进行测试，接下来对所有剩余规则的后件进行组合，得到包含两个元素的后件（对应的补集就是前件），依次类推，直到测试完所有可能的后件。

• 为啥只检查前后件互补的规则：因为一个频繁项集的所有子集也都是频繁项集，所以一个频繁项集中不互补的规则将是该频繁项集的某个子集的互补规则。

4 FP-growth算法

Apriori算法避免了暴力遍历子项集的指数式增长，但是对每一个新生成的频繁项集，都要扫描整个数据集，当数据集很大时，这种抛物线式增长的时间复杂度也不太令人满意。

FP-growth算法借助一种称为FP树的数据结构存储数据，来抽象原始数据集：

• 项集以路径的方式存储在树中
• 相似项之间相连接成链表

• 一个元素项可以在FP树中出现多次
• FP树存储的是元素的出现频率
• 项集完全不同时，树才会分叉，否则会有复用路径

​算法思路：首先遍历一遍原始数据集，记录元素的出现频率，去掉不满足最小支持度的元素。然后再遍历一遍剩下的集合元素，构建FP树。然后就可以通过FP树挖掘频繁项集。

构建FP树：依次遍历每一个项集，首先将其中的非频繁项移除，并按照元素出现频率对过滤后的元素进行重排序。对过滤、排序后的集合，如果树中已存在现有元素，则增加现有元素的计数值，如果不存在，则向树中添加一个分支，新增节点的同时还要更新链表元素。主要就涉及两个数据结构：

1
2
3
4
5
6
7
8
9
10
11
12
13

# 自定义节点数据结构
class treeNode:
    def __init__(self, nameValue, numOccur, parentNode):
        self.name
        self.count
        self.nodeLink   # 链表信息，指向下一个相似项
        self.parent
        self.children
        
        
# 用于存储元素frequency以及链接相似项的字典数据结构
freq = {}
freq[node_name] = [frequency, node1, node2, ...]

• 因为集合中元素的出现频率可能相等，因此过滤排序的结果不唯一，生成的树结构也会有差异。
• 第一次遍历删除非频繁元素时，发现字典在迭代过程中不能删除item，我转化成list暴力解决了，不知道有没有什么优雅的方式。

1
2
3
4
5
6

del freq[item]
# 返回 RuntimeError: dictionary changed size during iteration

for item in list(freq.keys()):
    if freq[item] < minSupport:
        del(freq[item])

挖掘频繁项集：首先创建条件模式基，然后利用条件模式基，构建条件FP树。

1 条件模式基：以所查找元素项为结尾的前缀路径集合，并且每条前缀路径都与起始元素项的计数值相关联。（这里面用到了前面定义的parent和nodeLink属性）

2 构造条件FP树：与构造树的过程相同，使用的dataSet换成了条件模式基而已，函数参数count就是预留彩蛋。这样得到的就是指定频繁项的条件FP树。

1
2
3
4
5

def updateTree(cond_set, myTree, freq_header, count):
# cond_set: 一条path
# myTree: 根节点
# freq_header: dict[node_name] = [frequency, head_node]
# count: path对应的count

• 构造的条件FP树过滤掉了条件模式基中的一些元素：这些元素本身是频繁项，但是与指定元素组合的集合不是频繁的。
• 相应地，条件树中剩余元素与指定频繁项组合的集合是频繁的。

3 迭代：从生成的条件FP树中，可以得到更复杂的频繁项。求解复杂频繁项的条件模式基，进而生成对应的条件FP树，就能得到更复杂的频繁项，依次类推进行迭代，直到FP树为空。

​

最近不知道sublime3抽什么风，突然开始验证注册码了，输入一个valid code分分钟给你移除。

收藏一个解决办法，有效性待验证：

1
2
3
4

# add the following to your host file(/private/etc/hosts)
127.0.0.1 license.sublimehq.com
127.0.0.1 45.55.255.55
127.0.0.1 45.55.41.223

报错原因是通过IDE编译而不是直接通过cmake，因此要添加如下代码段：

1
2
3

#include "vtkAutoInit.h"   
VTK_MODULE_INIT(vtkRenderingOpenGL2); 
VTK_MODULE_INIT(vtkInteractionStyle);

先记录解决办法，more details 留到以后。

基础测试：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47

#include "vtkAutoInit.h"
VTK_MODULE_INIT(vtkRenderingOpenGL2); // VTK was built with vtkRenderingOpenGL2
VTK_MODULE_INIT(vtkInteractionStyle);

#include <vtkSphereSource.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkRenderWindow.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>

int main(int, char *[])
{
  // Create a sphere
  vtkSmartPointer<vtkSphereSource> sphereSource =
          vtkSmartPointer<vtkSphereSource>::New();
  sphereSource->SetCenter(0.0, 0.0, 0.0);
  sphereSource->SetRadius(5.0);

  //mapper
  vtkSmartPointer<vtkPolyDataMapper> mapper =
          vtkSmartPointer<vtkPolyDataMapper>::New();
  mapper->SetInputConnection(sphereSource->GetOutputPort());

  //actor
  vtkSmartPointer<vtkActor> actor =
          vtkSmartPointer<vtkActor>::New();
  actor->SetMapper(mapper);

  //renderer ,renderWindow, renderWindowInteractor.
  vtkSmartPointer<vtkRenderer> renderer =
          vtkSmartPointer<vtkRenderer>::New();
  vtkSmartPointer<vtkRenderWindow> renderWindow =
          vtkSmartPointer<vtkRenderWindow>::New();
  renderWindow->AddRenderer(renderer);
  vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
          vtkSmartPointer<vtkRenderWindowInteractor>::New();
  renderWindowInteractor->SetRenderWindow(renderWindow);

  renderer->AddActor(actor);
  renderer->SetBackground(.3, .6, .3); // Background color green
  renderWindow->Render();
  renderWindowInteractor->Start();
  return EXIT_SUCCESS;
}