KinectFusion阅读笔记公式详解

青蛙球

已于 2022-11-15 17:15:24 修改

阅读量541

点赞数

分类专栏：三维场景 # 三维重建文章标签： ICP算法旋转矩阵状态估计误差函数迭代过程

于 2022-08-08 00:25:52 首次发布

本文链接：https://blog.csdn.net/littlefrogyq/article/details/126214708

版权

三维场景同时被 2 个专栏收录

10 篇文章 0 订阅

订阅专栏

三维重建

3 篇文章 0 订阅

订阅专栏

小白一枚，看这篇文章的时候有些痛苦，感觉这一篇论文公式字母都怪怪的乱乱的，然后还有一些不太知道的东西查了一下资料，现在整理一下。

论文地址：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6162880

公式上下标解释

字母上面有 $\cdot$ 代表其次坐标，例如
$\dot{\boldsymbol u}= \begin{pmatrix} \boldsymbol u \\ 1 \end{pmatrix}$
下标 $k$ 表示第 $k$ 帧时刻，例如
$\mathrm R_k 表示第k帧时刻深度相机的深度图$
上标和下标 $g$ 都代表的是世界坐标系，例如
$\boldsymbol T_{g,k}~~表示从世界坐标系到第k帧相机坐标系的转换\\ \boldsymbol V_k^g(\boldsymbol u)~~表示世界坐标系下的~~\boldsymbol V_k(\boldsymbol u)$
字母上面有 $~\hat{}~$ 代表预测值，没有的代表的是测量值。例如
$\hat{\boldsymbol V}_k,~\hat{\boldsymbol N}_k ~~ 分别表示\boldsymbol V_k,~\boldsymbol N_k的预测值$
字母上面有 ~ 代表还在迭代中的值，没有 ~ 就代表已经迭代完成的值，例如
$\widetilde{\boldsymbol T}_{g,k}^z=\widetilde{\boldsymbol T}_{inc}^z\widetilde{\boldsymbol T}_{g,k}^{z-1} ~~表示的是一个\mathsf{ICP}的迭代过程，上标z表示迭代次数$

公式(6)

$\mathrm F_{\mathrm R_k}(\boldsymbol p) = \Psi\Big(\lambda^{-1} {\color{#009900} \|\boldsymbol t_{g,k}-\boldsymbol p\|_2} - \mathrm R_k(\boldsymbol x)\Big) \tag{6}$

这个其实就是上面 $|r-\lambda\mathrm R_k(\boldsymbol u)|\leq\mu$ 的变体
$r=\|\boldsymbol p_k\|_2 =\| \boldsymbol R_{g,k}\boldsymbol p_k \|_2 ={\color{#009900} \| \boldsymbol p-\boldsymbol t_{g.k} \|_2 } \\ {\color{#BBBBBB}(~\because \boldsymbol p= \boldsymbol R_{g,k}\boldsymbol p_k+\boldsymbol t_{g.k}~)}$
而
$|~r-\lambda\mathrm R_k(\boldsymbol u)~|\leq\mu \\ 即， |~\lambda^{-1}r-\mathrm R_k(\boldsymbol u)~|\leq\mu', ~~~\lambda\geq0$
就得到了上式

公式(16)

$\tag{16} \sum_{\boldsymbol u\in\mathscr U \atop \Omega_k(\boldsymbol u)\neq null} \Bigg\|\Big(\boldsymbol T_{g,k}\dot{\boldsymbol V}_k(\boldsymbol u)-\hat{\boldsymbol V}_{k-1}^g(\hat{\boldsymbol u})\Big)^\mathsf T\hat{\boldsymbol N}_{k-1}^g(\hat{\boldsymbol u})\Bigg\|_2$

这公式其实就是 ICP 中的 Point-to-Plane 方式构造的误差函数。
不了解ICP的，可以看我的这篇 ICP算法思想与推导详解

公式(18)

$\widetilde{\boldsymbol T}_{inc}^z=\Big[ \widetilde{\boldsymbol R}^z \Big| \widetilde{\boldsymbol t}^z \Big]= \left[\begin{array}{ccc|c} 1 & \alpha & -\gamma & t_x \\ -\alpha & 1 & \beta & t_y \\ \gamma & -\beta & 1 & t_z \end{array}\right] \tag{18}$

这里旋转矩阵 $\boldsymbol R$ 为什么可以这么表示？

它这里绕三个轴旋转的角度分别是 $\color{#FFAA00}\beta,\gamma,\alpha$ ，然后这里的 $\widetilde{\boldsymbol T}_{inc}^z$ 表示的是相机量帧之间的变化，这个变化非常小，所以，

解释一：
我们知道，按照1-2-3顺序旋转的旋转矩阵的表达式为：
$\boldsymbol R= \begin{bmatrix} c_\gamma c_\alpha & c_\beta s_\alpha + s_\beta s_\gamma c_\alpha & s_\beta s_\alpha - c_\beta s_\gamma c_\alpha \\ -c_\gamma c_\alpha & c_\beta c_\alpha - s_\beta s_\gamma s_\alpha & s_\beta c_\alpha + c_\beta s_\gamma s_\alpha \\ s_\gamma & -s_\beta c_\gamma & c_\beta c_\gamma \end{bmatrix}$
（这里 $s$ 表示 $\sin$ ， $c$ 表示 $\cos$ ）
此时 $\alpha,\beta,\gamma \rightarrow 0$ ，那么，
$c_\alpha,c_\beta,c_\gamma \rightarrow 1$ ，
$s_\alpha,s_\beta,s_\gamma \rightarrow 0$ ， $s_\alpha\sim\alpha,s_\beta\sim\beta,s_\gamma\sim\gamma$ ，
并舍去 $\sin\cdot\sin$ 高阶项：
$\boldsymbol R ≈ \begin{bmatrix} 1 & \alpha & -\gamma \\ -\alpha & 1 & \beta \\ \gamma & -\beta & 1 \end{bmatrix}$
解释二：
使用罗德里格斯公式来解释，可看我这篇的1.3节

公式(20)

$\widetilde{\boldsymbol T}_{g,k}^z \dot{\boldsymbol V}_k(\boldsymbol u)= \widetilde{\boldsymbol R}^z \widetilde{\boldsymbol V}_k^g(\boldsymbol u)+\widetilde{\boldsymbol t}^z= \boldsymbol G(\boldsymbol u)\boldsymbol x+\widetilde{\boldsymbol V}_k^g(\boldsymbol u) \tag{20}$
其中
$\boldsymbol x=\big( {\color{#FFAA00}\beta, \gamma, \alpha}, t_x, t_y, t_z \big)^\mathsf T \\ \boldsymbol G(\boldsymbol u)=\Bigg(\Big[ \widetilde{\boldsymbol V}_k^g(\boldsymbol u)\Big]_\times~ \Bigg|~\boldsymbol I_{3\times 3}\Bigg) \\ ~[~\cdot ~]_\times表示向量的叉积矩阵$

左边一个等号很简单：
$\begin{aligned} \widetilde{\boldsymbol T}_{g,k}^z \dot{\boldsymbol V}_k(\boldsymbol u) &=\widetilde{\boldsymbol T}_{inc}^z\widetilde{\boldsymbol T}_{g,k}^{z-1} \dot{\boldsymbol V}_k(\boldsymbol u) \\ &=\widetilde{\boldsymbol T}_{inc}^z\widetilde{\boldsymbol V}_k^g(\boldsymbol u) \\ &=\widetilde{\boldsymbol R}^z \widetilde{\boldsymbol V}_k^g(\boldsymbol u) +\widetilde{\boldsymbol t}^z \end{aligned}$

再看右边一个等号
对于 $\boldsymbol G(\boldsymbol u)\boldsymbol x$ 部分，
我们把 $\boldsymbol x$ 拆开一下，记：
${\color{#00AAFF}\boldsymbol \phi} = ({\color{#FFAA00}\beta, \gamma, \alpha})^\mathsf T$
那么
$\boldsymbol x= \begin{pmatrix} {\color{#00AAFF}\boldsymbol \phi} \\ \widetilde{\boldsymbol t}^z \end{pmatrix}= ( {\color{#00AAFF}\boldsymbol \phi}^\mathsf T, {\widetilde{\boldsymbol t}^z}^\mathsf T)^\mathsf T \\$
根据分块矩阵乘法， $\boldsymbol G(\boldsymbol u)\cdot \boldsymbol x$ 实际上就是：
$\boldsymbol G(\boldsymbol u)\cdot \boldsymbol x= \Big[ \widetilde{\boldsymbol V}_k^g(\boldsymbol u)\Big]_\times\cdot{\color{#00AAFF}\boldsymbol \phi}+\widetilde{\boldsymbol t}^z$

上面根据公式(18)的结果可知：
$\widetilde{\boldsymbol R}^z ≈ \begin{bmatrix} 1 & \alpha & -\gamma \\ -\alpha & 1 & \beta \\ \gamma & -\beta & 1 \end{bmatrix} =\boldsymbol I-\Big[({\color{#FFAA00}\beta, \gamma, \alpha})^\mathsf T\Big]_\times =\boldsymbol I-[{\color{#00AAFF}\boldsymbol\phi}]_\times$
根据叉积性质我们知道：
$\begin{aligned} \widetilde{\boldsymbol R}^z \widetilde{\boldsymbol V}_k^g(\boldsymbol u) &=\big(\boldsymbol I-[{\color{#00AAFF}\boldsymbol\phi}]_\times\big) \cdot\widetilde{\boldsymbol V}_k^g(\boldsymbol u) \\ &=\widetilde{\boldsymbol V}_k^g(\boldsymbol u)-{\color{#00AAFF}\boldsymbol\phi} \times \widetilde{\boldsymbol V}_k^g(\boldsymbol u)= \widetilde{\boldsymbol V}_k^g(\boldsymbol u)+\widetilde{\boldsymbol V}_k^g(\boldsymbol u) \times {\color{#00AAFF}\boldsymbol\phi} \\ &=\widetilde{\boldsymbol V}_k^g(\boldsymbol u)+\Big[ \widetilde{\boldsymbol V}_k^g(\boldsymbol u)\Big]_\times\cdot{\color{#00AAFF}\boldsymbol \phi} \end{aligned}$
那么就得出：
$\begin{aligned} \widetilde{\boldsymbol R}^z \widetilde{\boldsymbol V}_k^g(\boldsymbol u) +\widetilde{\boldsymbol t}^z &=\widetilde{\boldsymbol V}_k^g(\boldsymbol u)+ \Big[ \widetilde{\boldsymbol V}_k^g(\boldsymbol u)\Big]_\times\cdot{\color{#00AAFF}\boldsymbol \phi}+\widetilde{\boldsymbol t}^z\\ &=\widetilde{\boldsymbol V}_k^g(\boldsymbol u)+\boldsymbol G(\boldsymbol u)\cdot \boldsymbol x \end{aligned}$