PL-ICP求解公式推导

最新推荐文章于 2022-11-01 16:57:02 发布

Jaky_wang

最新推荐文章于 2022-11-01 16:57:02 发布

阅读量1.4k

点赞数

分类专栏： ICP

本文链接：https://blog.csdn.net/WangQiang319670/article/details/108002398

版权

ICP 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

非线性最小化问题：

$\textup{\textup{\textup{}}}$ $\underset{t,\theta }{min}\underset{i}{\sum }\left \| \left (\mathbf{R}(\theta)\mathbf{p}_{i} + \mathbf{t} \right ) - \mathbf{\pi} _{i}\right \|_{\mathbf{C}_{i}}^{2}$

其中 $\boldsymbol{\mathbf{p}}^{_{i}}, \boldsymbol{\pi} _{i},\mathbf{t} \in \mathbb{R}^{2}$ , $\mathbf{R}\left ( \theta \right )$ 是2×2的旋转矩阵

范数 $\left \|\boldsymbol{a} \right \|_{\mathbf{C}}^{2} = \boldsymbol{a}^{T}\boldsymbol{C}\boldsymbol{a}$

对于PP-ICP来说， $\mathbf{C}_{i} = \omega _{i}\mathbf{I}_{2\times 2}$ ， $\omega _{i}$ 为权重

对于PL-ICP来说， $\mathbf{C}_{i} = \omega _{i}\mathbf{n}_{i}\mathbf{n}_{i}^{T}$

简化为最小二乘问题：

上述问题的解为 $\left ( t_{x}, t_{y}, \theta \right )$ ，用四维向量表示 $\boldsymbol{x} = \left [ x_1, x_2, x_3, x_4 \right ]^{T} \doteq \left [ t_x, t_y, cos\theta, sin\theta \right ]^{T}$ ，并且约束 $x_{3}^{2} + x_{4}^{2} = 1$ 。

对于给定点 $\mathbf{p}_i = (p_{i0}, p_{i1})$ , 定义矩阵 $\mathbf{M}_i$ ：

$\mathbf{M}_i = \begin{bmatrix} 1 &0 &p_i0 &-p_i1\\ 0 &1 &p_i1 &p_i0 \end{bmatrix} \qquad\qquad (17)$

上述最小化问题可转化为：

$\underset{\mathbf{x}}{min}\underset{i}{\sum }(\mathbf{M}_i\boldsymbol{x} - \pi_i)^{T}\mathbf{C}_i(\mathbf{M}_i\mathbf{x} - \pi_i) \qquad\qquad (18)$

展开有： $\sum_{i}(\mathbf{M}_i\mathbf{x} - \pi_i)^{T}\mathbf{C}_i(\mathbf{M}_i\mathbf{x} - \pi_i) = \sum_{i}(\mathbf{x}^{T}\mathbf{M}_i^{T}\mathbf{C}_i\mathbf{M}_i\mathbf{x}}} + \pi_i^{T}\mathbf{C}_i\pi_i - 2\pi_i^{T}\mathbf{C}_i\mathbf{M}_i\mathbf{x})$

忽略常数项： $\mathbf{x}^{T} \underset{\mathbf{M}}{\underbrace{\left ( \sum_{i}\mathbf{M}_i^{T}\mathbf{C}_i\mathbf{M}_i\right )}} \mathbf{x} + \underset{\mathbf{g}}{\underbrace{\left ( \sum_{i}-2\pi_i^{T}\mathbf{C}_i\mathbf{M}_i\right )}} \mathbf{x} \qquad\qquad (19)$

定义 $W = \begin{bmatrix} 0_{2\times 2} & 0_{2\times 2}\\ 0_{2\times 2}& \mathbf{I}_{2\times 2} \end{bmatrix}$ ，约束 $x_{3}^{2} + x_{4}^{2} = 1$ 可写成 $\mathbf{x}^{T}\mathbf{W}\mathbf{x} = 1$ 。

问题即可转化为：

$\underset{\mathbf{x}}{min}\mathbf{x}^{T}\mathbf{M}\mathbf{x} + \mathbf{g}^{T}\mathbf{x} \qquad\qquad (20)$

subject to $\mathbf{x}^{T}\mathbf{W}\mathbf{x} = 1 \qquad\qquad (21)$

使用拉格朗日乘子求解：

定义函数： $L(\mathbf{x}) = \mathbf{x}^{T}\mathbf{M}\mathbf{x} + \mathbf{g}^{T}\mathbf{x} + \lambda (\mathbf{x}^{T}\mathbf{W}\mathbf{x}-1) \qquad\qquad (22)$

最优性条件 $\partial L/\partial \boldsymbol{x} = 0^{T}$ :

$2\mathbf{x}^{T}\mathbf{M} + \mathbf{g}^{T} + 2\lambda \mathbf{x}^{T}\mathbf{W} = \mathbf{0}^{T} \qquad\qquad (23)$

解为： $\mathbf{x} = -(2\mathbf{M} + 2\lambda \mathbf{W})^{-T}\mathbf{g} \qquad\qquad(24)$

回代到约束中有： $\mathbf{g}^{T}(2\mathbf{M} + 2\lambda \mathbf{W})^{-1}\mathbf{\mathbf{W}}(2\mathbf{M} + 2\lambda \mathbf{W})^{-T}\mathbf{g} = 1 \qquad\qquad (25)$

上式为 $\lambda$ 的四次多项式。

将 $(2\mathbf{M}+2\lambda \mathbf{W})$ 分成四块： $2\mathbf{M}+2\lambda \mathbf{W} = \begin{bmatrix} \mathbf{A} & \mathbf{B} \\ \mathbf{B}^{T } & \mathbf{D} + 2\lambda \mathbf{I} \end{bmatrix} \qquad\qquad (26)$

由于（25）中的 $\mathbf{W}$ 稀疏的，所以仅需计算 $(2\mathbf{M}+2\lambda \mathbf{W})^{-1}$ 的最后一列。

$(2M+2\lambda W)^{-1} = \begin{bmatrix} \mathbf{A} & \mathbf{B} \\ \mathbf{B}^{T} & (\mathbf{D}+2\lambda \mathbf{I}) \end{bmatrix}^{-1} = \begin{bmatrix} * & -\mathbf{A}^{-1}\mathbf{B}\mathbf{Q}^{-1} \\ * & \mathbf{Q}^{-1} \end{bmatrix} \qquad\qquad (27)$

其中 $\mathbf{Q} = (\mathbf{D} - \mathbf{B}^{T}\mathbf{A}^{-1}\mathbf{B} + 2\lambda \mathbf{I}) \doteq (\mathbf{S} + 2\lambda \mathbf{I})$

代入(25)中，得到

$\mathbf{g}^{T} \begin{bmatrix} \mathbf{A}^{-1}\mathbf{B}\mathbf{Q}^{-1}\mathbf{Q}^{-T}\mathbf{B}^{T}\mathbf{A}^{-T} & -\mathbf{A}^{-1}\mathbf{B}\mathbf{Q}^{-1}\mathbf{Q}^{-T} \\ -\mathbf{Q}^{-1}\mathbf{Q}^{-T}\mathbf{B}^{T}\mathbf{A}^{-T} & \mathbf{Q}^{-1}\mathbf{Q}^{-T} \end{bmatrix} \mathbf{g} = 1 \qquad\qquad (28)$

将 $\mathbf{Q}^{-1}$ 写成如下形式： $\mathbf{Q}^{-1} = (\mathbf{S} + 2\lambda \mathbf{I})^{-1} = \frac{\mathbf{S}^{A} + 2\lambda \mathbf{I}}{p(\lambda )} \qquad\qquad (29)$

其中： $\mathbf{S}^{A} = det(\mathbf{S}) \cdot \mathbf{S}^{-1}, \qquad\qquad p(\lambda ) = det(\mathbf{S} + 2\lambda \mathbf{I})$

由于

$\mathbf{Q}^{-1}\mathbf{Q}^{-T} = \frac{(\mathbf{S}^{A} + 2\lambda \mathbf{I })(\mathbf{S}^{A} + 2\lambda \mathbf{I} )^{T}}{p(\lambda )^{2}} = \frac{\mathbf{S}^{A}\mathbf{S}^{A^{T}} + 4\lambda ^{2} \mathbf{I} + 4\lambda \mathbf{S}^{A}}{p(\lambda )^{2}} \qquad\qquad (30)$

最终可得到：

$\lambda ^{2} \cdot 4\mathbf{g}^{T} \begin{bmatrix} \mathbf{A}^{-1}\mathbf{B}\mathbf{B}^{T}\mathbf{A}^{-T} & -\mathbf{A}^{-1}\mathbf{B}\\ (symm) & \mathbf{I} \end{bmatrix} \mathbf{g} + \\ \lambda \cdot 4\mathbf{g}^{T} \begin{bmatrix} \mathbf{A}^{-1}\mathbf{B}\mathbf{S}^{A}\mathbf{B}^{T}\mathbf{A}^{-T} & -\mathbf{A}^{-1}\mathbf{B}\mathbf{S}^{A}\\ (symm) & \mathbf{S}^{A} \end{bmatrix} \mathbf{g} + \\ \mathbf{g}^{T} \begin{bmatrix} \mathbf{A}^{-1}\mathbf{B}\mathbf{S}^{A^{T}}\mathbf{S}^{A}\mathbf{B}^{T}\mathbf{A}^{-T} & -\mathbf{A}^{-1}\mathbf{B}\mathbf{S}^{A^{T}}\mathbf{S}^{A}\\ (symm) & \mathbf{S}^{A^{T}}\mathbf{S}^{A} \end{bmatrix} \mathbf{g} = [p(\lambda )]^{2} \qquad\qquad (31)$

求解方程（31），参照gpc_solve（）函数：(偷懒，矩阵很向量写法不规范)

令:

$m_1^T = g_1^T \cdot A^{-1}B \\ m_2^T = m_2^T * S^A = g_1^TA^{-1}BS^A \\ m_3^T = g_2^TS^A$

式(31)展开：

$\lambda ^{2}\cdot 4\begin{bmatrix} g_{1}^{T} & g_{2}^{T} \end{bmatrix}\begin{bmatrix} A^{-1}BB^{T}A{-T} & -A^{-1}B \\ symm & I \end{bmatrix}\begin{bmatrix} g_{1} \\ g_{2} \end{bmatrix} \\ = 4\alpha \lambda ^{2} ( g_{1}^{T}A^{-1}BB^{T}A^{-T}g_{1} - 2g_{1}^{T}A_{-1}Bg_{2} + g_{2}^{T}g_{2}) \\ = 4\lambda ^2(m_1^Tm_1 - 2m_1^Tg_2 + g_2^Tg_2)$

$\lambda \cdot 4\begin{bmatrix} g_{1}^{T} & g_{2}^{T} \end{bmatrix}\begin{bmatrix} A^{-1}BS^{A}B^{T}A^{-T} & -A^{-1}BS^{A} \\ symm & S^{A} \end{bmatrix}\begin{bmatrix} g_{1} \\ g_{2} \end{bmatrix} \\ = 4\lambda (g_{1}^{T}A^{-1}BS^{A}B^{T}A^{-T}g_{1} - 2g_{1}^{T}A^{-1}BS^{A}g_{2} + g_{2}^{T}S^{A}g_{2}) \\ = 4\lambda (m_2^Tm_1 - 2m_2^Tg_2 + g_2^Tm_3)$

$\begin{bmatrix} g_{1}^{T} & g_{2}^{T} \end{bmatrix}\begin{bmatrix} A^{-1}BS^{A^{T}}S^{A}B^{T}A^{-T} & -A^{-1}BS^{A^{T}}S^{A} \\ symm & S^{A^{T}}S^{A} \end{bmatrix}\begin{bmatrix} g_{1} \\ g_{2} \end{bmatrix} \\ = g_{1}^{T}A^{-1}BS^{A^{T}}S^{A}B^{T}A^{-T}g_{1} - 2g_{1}^{T}A^{-1}BS^{A^{T}}S^{A}g_{2} + g_{2}^{T}S^{A^T}S^Ag_2 \\ = m_2^Tm_2 - 2m_2^Tm_3 + m_3^Tm_3$

令：

$S = \begin{bmatrix} s_{00} & s_{01} \\ s_{10} & s_{11} \end{bmatrix}$

$p(\lambda ) = det(\mathbf{S} + 2\lambda \mathbf{I}) = \begin{vmatrix} s_{00} + 2\lambda & s_{01} \\ s_{10} & s_{11} + 2\lambda \end{vmatrix} \\ \qquad \qquad= 4\lambda ^{2} + 2 * (s_{00} + s_{11})\lambda + s_{00}s_{11} - s_{01} s_{10} = 4\lambda ^{2} + 2 * (s_{00} + s_{11})\lambda + det(\mathbf{S})$

各个系数整合 /* q = p - l^2 */ ，得到5个系数项。然后用Eigen::PolynomialSolver求解得到 $\lambda$

Jaky_wang

关注

0
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
PL-ICP求解公式推导

非线性最小化问题：其中 ,是2×2的旋转矩阵范数对于PP-ICP来说，，为权重对于PL-ICP来说，简化为最小二乘问题：上述问题的解为，用四维向量表示，并且约束。对于给定点, 定义矩阵：上述最小化问题可转化为：展开有：忽略常数项：定义，约束可写成。问题即可转化为： subject to 使用拉格...
复制链接

扫一扫

专栏目录