偏导数
优化方程稀疏性的解释
SLAM优化函数中，H矩阵和B矩阵是稀疏的，因此可以针对性的进行一些算法设计来加速求解。
H和B为什么是稀疏的，直观解释如下所示：
基于SVD分解计算旋转和平移向量
点云配准任务详见pcl中的点云配准
如果可以得到两个点云之间的同名点对，那么就可以计算出旋转和平移向量的闭式解，以二维点集之间的相对变换为例进行解释：
- 首先对原始的点云进行归一化操作，即减去对应质心点的坐标，得到\(\tilde{\mathbf{X}}, \tilde{\mathbf{Y}}\)；
\[ \tilde{x}=x_i-\bar{x},\tilde{y}=y_i-\bar{y},i=1,...,N \]
- 计算加权协方差矩阵；
\[ \mathbf{H}=\tilde{\mathbf{X}}^{T} \mathbf{E} \tilde{\mathbf{Y}} \]
其中\(\mathbf{E}=\mbox{diag}(e_1,e_2,...,e_N)\).
- 对\(\mathbf{H}\)做SVD分解即可计算从\(\mathbf{X}\)到\(\mathbf{Y}\)的旋转矩阵;
\[ \begin{align} [\mathbf{U},\mathbf{S},\mathbf{V}] &= \mbox{SVD}(\mathbf{H}) \\ \hat{\mathbf{R}} &= \mathbf{V} \left[\begin{array}{ccc} 1 &0 &0 \\ 0 &1 &0 \\ 0 &0 &\mbox{det}(\mathbf{V}\mathbf{U}^{T}) \end{array} \right] \mathbf{U}^{T} \end{align} \]
其中det()是行列式。
- 最后再计算平移向量；
\[ \hat{\mathbf{t}}=\bar{y}-\hat{\mathbf{R}\bar{x}} \]
至于背后的数学推导，见这里。
PCL中的代码解释，详见另外一篇博客，或者文章《Least-Squares Rigid Motion Using SVD》。
