我眼中的视觉SLAM

以下内容纯属个人观点！

---

  SLAM的本质就是一个状态估计问题，整个过程可以由一个运动方程和一个观测方程来描述：
$$\{\begin{array}{ll}{x}_k=f(x_{k-1},u_k)+w_k& \text{运动方程}\\ z_{k,j}=h(y_j,x_k)+v_{k,j}& \text{观测方程}\end{array}\text{\hspace{1em}(1)}$$

  输入变量$u_k$代表机器人或者相机自身的运动，$u_k$可由惯导装置测量获取。相机的运动$u_k$会使相机的位姿发生变化，当相机运动与相机位姿之间的函数关系$f(x_{k-1},u_k)$已知时，我们可以根据$u_k$实时估计相机当前的位姿$x_k$。通常，我们的运动模型$f(x_{k-1},u_k)$都不是十分精确的，并且测量数据$u_k$也是不精确的，这就是噪声$w_k$的来源。由带噪声的运动模型和测量数据估计出来的相机位姿$x_k$是存在误差的。

  输出变量（观测数据）$z_{k,j}$是第$k$帧图像中某些像素点的像素坐标，$z_{k,j}$可由特征点匹配（特征点法）或关键点跟踪（直接法）获得。$y_j$是$z_{k,j}$对应现实路标点在世界坐标系下的三维坐标，$y_j$是地图中的点，在单目SLAM中通过对关键帧之间相匹配的像素点（即$z_{k,j}$）进行三角化而获得。有了$y_j$和$z_{k,j}$，就可以利用PnP或ICP算法计算相机当前的位姿$x_k$。通常，图像与局部地图之间的特征点匹配或相邻两帧图像之间的关键点跟踪是不可能$100\%$准确的，相机的内参矩阵$K$也是不完全精确的，这些都是噪声$v_{k,j}$的来源。由带噪声的像素点坐标$z_{k,j}$和相机内参$K$计算出来的路标点$y_j$和相机位姿$x_k$是存在误差的。

  在纯粹的视觉SLAM问题中，式（1）中的运动方程是不存在的，因为此时除了相机没有其他任何传感器，我们只能得到观测数据$z_{k,j}$。所以，视觉SLAM是一个根据带有噪声的相机观测数据$z_{k,j}$估计路标点坐标$y_j$和相机位姿$x_k$的状态估计问题。这里的$x_k$与$y_j$之间存在一个先有鸡先有蛋的问题，它们两个互相依存（三角化两帧之间的匹配点更新局部地图是$y_j$对$x_k$的依赖，PnP、ICP方法求相机位姿是$x_k$对$y_j$的依赖），互相影响，很容易造成误差的累积。所以我们不得不利用针孔相机模型构造一个描述$y_j$到$z_{k,j}$投影过程的观测方程，采用最小二乘法对$x_k$和$y_j$这两个状态变量同时进行滤波或优化来得到它们的最优估计：
$$s{z}_{k,j}=K\text{exp}({\xi }_k^{\wedge })y_j\text{\hspace{1em}(2)}$$

其中，$s$为像素点的深度（距离），$K$为相机内参，${\xi }_k$为相机外参的李代数形式，相机外参就是相机与世界坐标系的位姿关系$x_k$。

  式（2）是视觉SLAM中观测方程参数化后的具体形式，理想情况下该式成立，但实际情况中$s$、$z_{k,j}$、$K$、${\xi }_k$、$y_j$都是不精确的。当然，这样说好像显得过于苛刻了，因为现实中根本不存在完全精确的数学模型，所以我们首先认为$K$是足够精确的，后期不再考虑对它的优化。而$z_{k,j}$是观测数据，不需要优化（无法优化）；$s$与${\xi }_k$和$y_j$有直接关系，也不需要优化；最后我们要优化的就只有${\xi }_k$（$x_k$）和$y_j$。

  状态估计问题本身并不是一个优化问题，优化只是一种手段。估计的最终目的是为了得到状态的最优估计，即把噪声对系统的影响降到最低，使下式最小：
$$e=s{z}_{k,j}-K\text{exp}({\xi }^{\wedge })y_j\text{\hspace{1em}(3)}$$

为了达到这个目的通常有两大手段：滤波和优化。

未完待续。。。

---

参考文献
[1] https://www.cnblogs.com/gaoxiang12/p/4395446.html
[2] https://blog.csdn.net/u79501/article/details/69787436