无IMU纯双目SLAM之求解相机位姿状态

1. 双目估计深度；
2. 基于深度得到三维点云XYZ；
3. 基于单目相邻帧求解摄像头位姿：

• 创建样本集合：X,Y。
  \( X^T = \left[\begin{aligned}x_1\\y_1\\z_1\\1\end{aligned}\right] \\ Y^T = \left[\begin{aligned}x_2\\y_2\\z_2\end{aligned}\right] \\ AX^T=Y^T \rightarrow XA^T=Y\rightarrow (X^TX)A^T=X^TY\rightarrow A^T=(X^TX)^{-1}X^TY\\ \text{let } P = {(X^TX)}^{-1}X^T\\ A = (PY)^T=({(X^TX)}^{-1}X^TY)^T.\\ \text{let } S = \left[ \begin{aligned} A && \mathbf{0}\\ \mathbf{0} && I \end{aligned} \right] \)

4. 对齐坐标系到初始启动位姿：
   \( S_0\leftarrow S\cdot S_0 \)
5. 为了实时画出摄像头的轨迹，需要获得摄像头的位姿相对于初始坐标系的状态：
   \( \text{let }\mathbb{N} \text{ be the basis of camera under coordinate of } A_0 \\ \mathbb{N}_0 = \left[\begin{aligned} 1 && 0 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \\ 1 && 1 && 1 \end{aligned}\right]\\ \text{then }\\ \quad \mathbb{N} = {S_0}^{-1}\mathbb{N}_0 \)