{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# DynaFu ICP Math\n", "## Differentiating and Linearising Rt matrices\n", "\n", "In dynafu, the warp function looks like the following for each node $i$:\n", "\n", "\n", "$\n", "\\begin{equation*}\n", "f_i(x_i, V_g) = T_{x_i} * V_g = R(x_i) * V_g + t(x_i)\n", "\\end{equation*}\n", "$\n", "\n", "where ${x_i}$ are the transformation parameters for node $i$ and the rotation is performed around the corresponding node (and not a global reference)\n", "\n", "For linearising a transform around the parameters $\\mathbf{x}$, we need to find the derivative\n", "\n", "$\n", "\\begin{equation*}\n", "\\displaystyle\n", "\\frac{\\partial f_i(\\mathbf{x} \\circ \\epsilon, V_g)}{\\partial \\epsilon} |_{\\epsilon = 0}\n", "\\end{equation*}\n", "$\n", "\n", "We calculate this as follows:\n", "\n", "$\n", "\\begin{equation*}\n", "f_i(\\mathbf{x} \\circ \\epsilon, V_g) = f_i(\\epsilon, V) = T_{inc} * V\n", "\\end{equation*}\n", "$ where $V = f_i(\\mathbf{x}, V_g)$ and $T_{inc}$ is the infinitesimal transform with parameters $\\epsilon$\n", "\n", "According to Lie algebra, each Rt matrix can be represented as $A = e^\\xi$ where $\\xi$ are the transform parameters. Therefore,\n", "\n", "\n", "$\n", "\\begin{equation*}\n", "f_i(\\mathbf{x}, V_g) = e^\\xi V\n", "\\end{equation*}\n", "$\n", "\n", "Therefore,\n", "\n", "$\n", "\\begin{equation*}\n", "\\displaystyle\n", "\\frac{\\partial f_i(\\mathbf{x} \\circ \\xi, V_g)}{\\partial \\xi} |_{\\xi = 0} =\n", "\\frac{\\partial e^\\xi V}{\\partial \\xi} |_{\\xi=0} = \n", "\\begin{pmatrix} -[V]_{\\times} & I_{3x3} \\end{pmatrix}_{3 \\times 6}\n", "\\end{equation*}\n", "$\n", "\n", "Let us denote $\\begin{pmatrix} -[V]_{\\times} & I_{3x3} \\end{pmatrix}$ as $G(V)$ from now on.\n", "\n", "This result is mentioned in [this manifold optimisation tutorial](http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf) (equation 10.23).\n", "\n", "With this result, we can now linearise our transformation around $\\mathbf{x}$:\n", "\n", "$\n", "\\begin{equation*}\n", "f_i(x_i, V_g) = G(V) * \\epsilon + V\n", "\\end{equation*}\n", "$\n", "\n", "\n", "I suppose the following is an equivalent excerpt from the dynafu paper (Section about efficient optimisation) that mentions this way of calculating derivatives:\n", "> We formulate compositional updates $\\hat x$ through the exponential map with a per-node twist $ξ_i ∈ se(3)$, requiring 6 variables per node transform, and perform linearisation around $ξ_i= 0$. \n", "\n", "As a side note, the derivative $\\large \\frac{\\partial e^\\xi}{\\partial \\xi}|_{\\xi=0}$ is called the tangent (esentially the derivative) to the SE(3) manifold (the space in which Rt matrix $T_{inc}$ exists) at identity ($\\xi = 0$)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Estimating Warp Field Parameters\n", "The total energy to be minimised is \n", "\n", "$\n", "E = E_{data} + \\lambda E_{reg}\n", "$\n", "\n", "#### Data term rearrangement \n", "$\n", "\\displaystyle\n", "E_{data} = \\sum_{u \\in \\Omega} \\rho_{Tukey}( (T_u N_g)^T ((T_u V_g) - V_c))\n", "$\n", "\n", "The quadcopter paper tells us that the following expression has the same minimiser, so we can use this instead:\n", "\n", "$\n", "\\displaystyle\n", "E_{data} = \\sum_{u \\in \\Omega} w_{Tukey}(r_u) \\cdot (r_u)^2\n", "$\n", "\n", "where $w_{Tukey}(x) = \\rho'(x)/x$ which behaves like a constant term and $r_u = N_g^T (V_g - T_u^{-1}\\cdot V_c)$\n", "\n", "#### Regularisation term rearrangement\n", "$\n", "\\begin{equation}\n", "\\displaystyle\n", "E_{reg} = \\sum_{i = 0}^n \\sum_{j \\in \\varepsilon(i)} \\alpha_{ij} \\rho_{Huber} (T_{i}V_g^j - T_{j}V_g^j)\n", "\\end{equation}\n", "$\n", "\n", "This needs to be changed to the form of weighted least squares to be useful. So incorporate the same rearrangement as the data term and sum over edges instead:\n", "\n", "$\n", "\\begin{equation}\n", "\\displaystyle\n", "E_{reg} = \\sum_{e \\in E} w_{Huber}(r_e) (r_e)^2\n", "\\end{equation}\n", "$\n", "\n", "Here $E$ is the set of the directed edges in the regularisation graph between all nodes from current level and the next coarser level. And $w_{Huber}(x) = \\alpha_x \\rho'(x)/x$\n", "\n", "#### Obtaining normal equation\n", "\n", "Therefore to solve an iteration, we equate the derivative with 0\n", "\n", "$\n", "\\begin{equation*}\n", "\\large\n", "\\frac{\\partial E_{data}}{\\partial \\xi} + \\lambda \\frac{\\partial E_{reg}}{\\partial \\xi} = 0\n", "\\end{equation*}\n", "$\n", "\n", "which gives us\n", "\n", "$\n", "\\begin{equation*}\n", "J_d^T W_d(r_d + J_d\\mathbf{\\hat x}) + \\lambda J_r^T W_r (r_r + J_r\\mathbf{\\hat x}) = 0\n", "\\end{equation*}\n", "$\n", "\n", "$\n", "(J_d^T W_d J_d + \\lambda J_r^T W_r J_r)\\mathbf{\\hat x} = -(J_d^T W_d r_d + \\lambda J_r^T W_r r_r)\n", "$\n", "\n", "Here $W_d$ and $W_r$ are the weight matrices as described in quadcopter paper. However for $W_r, \\alpha$ is also incorporated in this matrix" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Calculating Data Term Jacobian ($J_d$) \n", "\n", "We estimate the inverse of Rt matrices instead of the Rt matrices themselves. So firstly we have to write $T_u^{-1} V_g$ in terms of inverse matrices. However, I realised that\n", "\n", "$$\n", "\\begin{equation*}\n", "T_u^{-1} V_g \\ne \\sum_{k \\in N(V_u)} \\frac{w_k T_k^{-1}V_g}{w_k}\n", "\\end{equation*}\n", "$$\n", "\n", "Unfortunately, I could not find a representation of $T_u^{-1} V_g$ in terms of $T_k^{-1}V_g$ and got stuck here. Below is an approach without estimating the inverse Rt matrices, I think we can use that instead as the math is now fixed.\n", "\n", "### Alternative calculation for $J_d$\n", "The residual $r_u$ in the data term is \n", "\n", "$$\n", "r_u = (T_u N_g)^T (T_u V_g - V_c)\n", "$$\n", "\n", "Let $a, b$ be column vectors such that $a = T_u N_g$ and $b = (T_u V_g - V_c)$. Now we can state the residual as \n", "\n", "$$\n", "r_u = a^Tb\n", "$$\n", "\n", "Each entry in $J_d$ for node paramter $x_j$ associated with node $j$ is:\n", "\n", "$$\n", "(J_d)_{uj} = \\frac{\\partial r_u}{\\partial x_j} = \\frac{\\partial (a^Tb)}{\\partial x_j}\n", "$$\n", "\n", "**Please note that numerator layout is assumed in all the derivatives**\n", "\n", "Applying chain rule for multiple variables, we get\n", "\n", "$$\n", "\\begin{equation}\\begin{aligned}\n", "\\frac{\\partial (a^Tb)}{\\partial x_j} & = \n", "\\frac{\\partial (a^Tb)}{\\partial a} \\cdot \\frac{\\partial a}{\\partial x_j} +\n", "\\frac{\\partial (a^Tb)}{\\partial b} \\cdot \\frac{\\partial b}{\\partial x_j} \\\\\n", "& =\n", "\\frac{\\partial (a^Tb)}{\\partial a} \\cdot \\frac{\\partial a}{\\partial x_j} +\n", "\\frac{\\partial (b^Ta)}{\\partial b} \\cdot \\frac{\\partial b}{\\partial x_j} && \\text{Since $a^Tb = b^Ta$} \\\\\n", "& =\n", "b^T \\cdot \\frac{\\partial a}{\\partial x_j} +\n", "a^T \\cdot \\frac{\\partial b}{\\partial x_j} && \\text{Since $\\frac{\\partial x^TA}{\\partial x} = A^T$}\n", "\\end{aligned}\\end{equation}\\tag{1}\\label{1}\n", "$$\n", "\n", "The identity $\\frac{\\partial x^TA}{\\partial x} = A^T$ is mentioned in [this wikipedia page](https://en.wikipedia.org/wiki/Matrix_calculus#Vector-by-vector_identities). Now we calculate $\\frac{\\partial a}{\\partial x_j}$ and $\\frac{\\partial b}{\\partial x_j}$ as follows:\n", "\n", "$$\n", "\\begin{equation}\\begin{aligned}\n", "\\frac{\\partial a}{\\partial x_j} & = \\frac{\\partial (T_u N_g)}{\\partial x_j} \\\\\n", "& = \\sum_{k \\in N(V_u)} \\frac{w_k \\frac{\\partial T_k N_g}{\\partial x_j}}{w_k} \\\\\n", "& = \n", "\\begin{cases}\n", " \\frac{w_j \\frac{\\partial T_j N_g}{\\partial x_j}}{\\sum_{k \\in N(V_u)} w_k} && \\text{if $j \\in N(V_u)$} \\\\\n", " 0 && \\text{otherwise}\n", "\\end{cases} \\\\\n", "& = \n", "\\begin{cases}\n", " \\frac{w_j \\begin{pmatrix}-[T_j N_g]_\\times & I_{3\\times3}\\end{pmatrix}}{\\sum_{k \\in N(V_u)} w_k} && \\text{if $j \\in N(V_u)$} \\\\\n", " 0 && \\text{otherwise}\n", "\\end{cases}\n", "\\end{aligned}\\end{equation}\\tag{2}\\label{2}\n", "$$\n", "\n", "\n", "$$\n", "\\begin{equation}\\begin{aligned}\n", "\\frac{\\partial b}{\\partial x_j} & = \\frac{\\partial (T_uV_g - V_c)}{\\partial x_j} \\\\\n", "& = \\frac{\\partial T_uV_g}{\\partial x_j}\\\\\n", "& = \n", "\\begin{cases}\n", " \\frac{w_j \\begin{pmatrix}-[T_j V_g]_\\times & I_{3\\times3}\\end{pmatrix}}{\\sum_{k \\in N(V_u)} w_k} && \\text{if $j \\in N(V_u)$} \\\\\n", " 0 && \\text{otherwise}\n", "\\end{cases} && \\text{Calculated similarly to ($\\ref{2}$)}\n", "\\end{aligned}\\end{equation}\\tag{3}\\label{3}\n", "$$\n", "\n", "Substituting equations $(\\ref{2})$, $(\\ref{3})$ as well as the values of $a^T$ and $b^T$ in $(\\ref{1})$, we obtain the required result:\n", "\n", "$$\n", "(J_d)_{uj} = \n", "\\begin{cases}\n", "\\frac{w_j}{\\sum_{k \\in N(V_u)} w_k}\n", "\\left(\n", " (T_u V_g - V_c)^T\n", " \\begin{pmatrix}-[T_j N_g] & I_{3\\times3}\\end{pmatrix} +\n", " (T_u N_g)^T \\begin{pmatrix}-[T_j V_g] & I_{3\\times3}\\end{pmatrix}\n", "\\right) & \\text{if} j \\in N(V_u) \\\\\n", "0 & \\text{otherwise}\n", "\\end{cases}\n", "$$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We can simplify this expression further:\n", "\n", "$$\n", "\\begin{equation*}\n", "\\left(\n", " (T_u V_g - V_c)^T \\begin{pmatrix}-[T_j N_g] & I_{3\\times3}\\end{pmatrix} +\n", " (T_u N_g)^T \\begin{pmatrix}-[T_j V_g] & I_{3\\times3}\\end{pmatrix}\n", "\\right) = \\\\\n", "\\left [ \n", "(T_u V_g - V_c)^T \\left ( -[T_j N_g]_\\otimes \\right ) \\mid (T_u V_g - V_c)^T\n", " \\right ]\n", "+\n", "\\left [ \n", "(T_u N_g)^T \\left ( -[T_j V_g]_\\otimes \\right ) \\mid (T_u N_g)^T\n", " \\right ] = \\\\\n", "\\left [ \n", "(T_u V_g - V_c)^T \\left ( -[T_j N_g]_\\otimes \\right ) +\n", "(T_u N_g)^T \\left ( -[T_j V_g]_\\otimes \\right )\n", " \\mid (T_u V_g + T_u N_g - V_c )^T\n", "\\right ] = \\\\\n", "\\left [ \n", "(T_u V_g - V_c)^T ( [T_j N_g]_\\otimes )^T +\n", "(T_u N_g)^T ( [T_j V_g]_\\otimes )^T\n", " \\mid (T_u V_g + T_u N_g - V_c )^T\n", "\\right ] = \\\\\n", "\\begin{bmatrix}\n", "( [T_j N_g]_\\otimes )(T_u V_g - V_c) +\n", "( [T_j V_g]_\\otimes )(T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c\n", "\\end{bmatrix}^T = \\\\\n", "\\begin{bmatrix}\n", "( T_j N_g) \\times (T_u V_g - V_c) +\n", "( T_j V_g) \\times (T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c\n", "\\end{bmatrix}^T = \\\\\n", "\\begin{bmatrix}\n", "( T_j N_g) \\times (T_u V_g - V_c) +\n", "( T_j V_g) \\times (T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c \n", "\\end{bmatrix}^T\n", "\\end{equation*}\n", "$$\n", "\n", "So, we get the final expression as:\n", "$$\n", "\\begin{equation*}\n", "(J_d)_{uj} = \n", "\\begin{cases}\n", "\\frac{w_j}{\\sum_{k \\in N(V_u)} w_k}\n", "\\begin{bmatrix}\n", "( T_j N_g) \\times (T_u V_g - V_c) +\n", "( T_j V_g) \\times (T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c \n", "\\end{bmatrix}^T\n", "&\n", "\\text{if} j \\in N(V_u) \\\\\n", "0 & \\text{otherwise}\n", "\\end{cases}\n", "\\end{equation*}\n", "$$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Now that we have expression for the Jacobian, we can form the normal equation corresponding to the data term of the ICP\n", "\n", "$$\n", "\\begin{equation*}\n", "\\\\\n", "\\Omega \\text{ pixels, N nodes}\n", "\\\\\n", "J_d^T W_d J_d \\mathbf{\\hat x} = -J_d^T W_d r_d \\\\\n", "\\underbrace{J_d^T}_{6N \\times\\Omega} \\underbrace {W_d}_{\\Omega \\times \\Omega}\n", "\\underbrace{J_d}_{\\Omega \\times 6N} \\underbrace{\\mathbf{\\hat x}}_{6N \\times 1} =\n", "-J_d^T W_d \\underbrace{r_d}_{\\Omega \\times 1} \\\\\n", "\\underbrace{\\mathbf{A}_d}_{6N \\times 6N} \\mathbf{\\hat x} = \\underbrace{\\mathbf{b}_d}_{6N \\times 1} \\\\\n", "\\\\\n", "\\text {for each block (i, j) which are 6}\\times\\text{6:}\n", "\\\\\n", "(\\mathbf{A}_d)_{ij} = \\sum_{\\Omega} \\underbrace{w_d(\\Omega)}_{\\text{robust for pix}}\n", "\\left(\\frac{\\partial E_d}{\\partial x_i}\\right)^T_\\Omega \\left(\\frac{\\partial E_d}{\\partial x_j}\\right)_\\Omega \\\\\n", "\\\\\n", "\\text {for each block j which are 6}\\times\\text{1:}\n", "\\\\\n", "(\\mathbf{b}_d)_{j} = - \\sum_{\\Omega} \\underbrace{w_d(\\Omega)}_{\\text{robust for pix}} r_d(\\Omega)\n", "\\left(\\frac{\\partial E_d}{\\partial x_j}\\right)^T_\\Omega \\\\\n", "(\\mathbf{b}_d)_{j} = - \\sum_{\\Omega} \\underbrace{w_d(\\Omega)}_{\\text{robust for pix}} ((T_u N_g)^T (T_u V_g - V_c))_{\\Omega}\n", "\\left(\n", " \\frac{w_j \\text{ or 0} }{\\sum_{k \\in N(V_u)} w_k}\n", "\\begin{bmatrix}\n", "( T_j N_g) \\times (T_u V_g - V_c) +\n", "( T_j V_g) \\times (T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c \n", "\\end{bmatrix}\n", " \\right)_\\Omega \\\\\n", "(\\mathbf{A}_d)_{ij} = \\sum_{\\Omega} \\underbrace{w_d(\\Omega)}_{\\text{robust for pix}}\n", "\\left(\n", "\\frac{(w_i\\text{ or 0})(w_j\\text{ or 0})}{(\\sum_{k \\in N(V_u)} w_k)^2}\n", " \\underbrace{(A^T_{i} A_{j})}_{6 \\times 6}\n", "\\right)_{\\Omega}\n", "\\\\ \\\\\n", "A_{i} =\n", "\\begin{bmatrix}\n", "( T_i N_g) \\times (T_u V_g - V_c) +\n", "( T_i V_g) \\times (T_u N_g)\n", "\\\\ \n", "T_u V_g + T_u N_g - V_c \n", "\\end{bmatrix}\n", "\\end{equation*}\n", "$$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Calculating Regularisation Term Jacobian ($J_r$)\n", "\n", "Each row in $J_r$ corresponds to derivative of summand for each edge $e$ in the regularisation graph $\\epsilon$ and column $k$ corresponds to node $k$ with respect to which the derivative is calculated.\n", "\n", "$$\n", "\\begin{equation*}\n", "\\displaystyle\n", "(J_r)_{ek} = \n", "\\sum_{e_{ij} \\in \\epsilon} \\frac{\\partial ( T_iV_g^j - T_jV_g^j)}{\\partial x_k}\n", "=\n", "\\sum_{e_{ij} \\in \\epsilon} \\begin{cases}\n", "\\begin{pmatrix} -[T_iV_g^j] & I_{3x3} \\end{pmatrix} & \\text {if } i = k \\\\\n", "-\\begin{pmatrix} -[T_jV_g^j] & I_{3x3} \\end{pmatrix} & \\text {if } j = k \\\\\n", "0 & \\text {otherwise}\n", "\\end{cases}\n", "\\end{equation*}\n", "$$\n", "\n", "Using this Jacobian we can set up a normal equation corresponding to the regularisation term similarly to the data term as mentioned in the previous section" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.5.2" } }, "nbformat": 4, "nbformat_minor": 2 }