KinectFusion中相机位姿优化

point-plane energy 优化目标函数如下:

$$ \begin{equation}\label{loss_function} \boldsymbol{E}(T_{g,k}) = \sum_{\begin{array}{c} \boldsymbol{u} \in \mathcal{U} \\ \Omega_{k}(\boldsymbol{u}) \neq null \end{array}} \left\| \left(T_{g,k} \dot{V}_k(\boldsymbol{u}) - \hat{V}_{k-1}^g(\hat{\boldsymbol{u}}) \right)^T \hat{N}_{k-1}^g(\hat{\boldsymbol{u}}) \right\|_2 \end{equation} $$

修正过程\(\tilde{T}_{g,k}^z = \tilde{T}_{inc}^z \tilde{T}_{g,k}^{z-1}\)，其中位姿矩阵修正项\(\tilde{T}_{inc}^z\)如下:

$$ \tilde{T}_{inc}^z = \left[ \tilde{R^z} \big| \tilde{\boldsymbol{t}^z} \right] = \begin{bmatrix} 1 & \alpha & -\gamma & t_x \\ -\alpha & 1 & \beta & t_y \\ \gamma & -\beta & 1 & t_z \end{bmatrix} $$

修正过程:

$$ \begin{align} \tilde{V}_k^g(\boldsymbol u) & = \tilde{T}_{g,k}^{z-1} \dot{V}_k(\boldsymbol{u}) = \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} \\ \tilde{T}_{g,k}^z \dot{V}_k(\boldsymbol{u}) & = \tilde{R}^z\tilde{V}_k^g(\boldsymbol u) + \tilde{\boldsymbol{t}^z} \\ & = \begin{bmatrix} 1 & \alpha & -\gamma \\ -\alpha & 1 & \beta \\ \gamma & -\beta & 1 \end{bmatrix} \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} + \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix} \\ & = \begin{cases} p_x + \alpha p_y - \gamma p_z + t_x \\ -\alpha p_x + p_y + \beta p_z + t_y \\ \gamma p_x - \beta p_y + p_z + t_z \end{cases} \\ & = \left[\begin{matrix} 0 & -p_z & p_y \\ p_z & 0 & -p_x \\ -p_y & p_x & 0 \end{matrix}\right |\left . \begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}\right ] \begin{bmatrix} \beta \\ \gamma \\ \alpha \\ t_x \\ t_y \\ t_z \end{bmatrix} + \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} = G(\boldsymbol{u})\boldsymbol x + \tilde{V}_k^g(\boldsymbol u) \end{align} $$

其中：

$$ G(\boldsymbol{u}) = \begin{bmatrix} \begin{bmatrix} \tilde{V}_k^g(\boldsymbol u) \end{bmatrix}_{\times} \big| I_{3 \times 3}\end{bmatrix} $$

对式 \((\ref{loss_function})\)求导，有：

$$ E = \hat{N}_{k-1}^g(\hat{\boldsymbol{u}})^T \left( G(\boldsymbol{u}) \boldsymbol{x} + \tilde{V}_k^g(\boldsymbol u) - \hat{V}_{k-1}^g(\hat{\boldsymbol u}) \right) $$

令上式等于0，我们有

$$ \begin{align} A & = \hat{N}_{k-1}^g(\hat{\boldsymbol{u}})^T G(\boldsymbol{u}) \\ \boldsymbol{b} & = \hat{N}_{k-1}^g(\hat{\boldsymbol{u}})^T \left( \hat{V}_{k-1}^g(\hat{\boldsymbol u}) - \tilde{V}_k^g(\boldsymbol u) \right) \\ A \boldsymbol{x} & = \boldsymbol{b} \end{align} $$

之后，就是最小二乘的问题：

$$ \begin{equation} \sum_{\Omega_k(\boldsymbol{u}) \neq null} \left( A^T A \right) \boldsymbol{x} = \sum A^T \boldsymbol{b} \end{equation} $$