mirrors/opencv_contrib
1
0
Fork 0
Code Pull Requests Packages Releases Activity

vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e

Browse Source
This commit is contained in:
Gitea Mirror Bot
2026-08-22 00:11:13 +08:00
commit 12022378a3
3872 changed files with 2513409 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/rgbd/doc/dynafu_ICP.ipynb
+419
View File
@@ -0,0 +1,419 @@
{
"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
}
modules/rgbd/doc/kinfu_icp.markdown
+140
View File
@@ -0,0 +1,140 @@
# ICP point-to-plane odometry algorithm {#kinfu_icp}
This article describes an ICP algorithm used in depth fusion pipelines such as KinectFusion.
The goal of ICP is to align two point clouds, the old one (the existing points and normals in 3D model) and new one (new points and normals, what we want to integrate to the exising model). ICP returns rotation+translation transform between these two point clouds.
The Iterative Closest Point (ICP) minimizes the objective function which is the Point to Plane Distance (PPD) between the corresponding points in two point clouds:
<img src="https://render.githubusercontent.com/render/math?math=E=\sum_{i}\left\|ppd(p_{i}, q_{i}, n_{i})\right\|_{2}\rightarrow0">
### What is ppd(p, q, n)?
Specifically, for each corresponding points ***P*** and ***Q***, it is the distance from the point ***P*** to the plane determined by the point ***Q*** and the normal ***N*** located in the point ***Q***.
Two points ***P*** and ***Q*** are considered correspondent if given current camera pose they are projected in the same pixel.
***p*** - i'th point in the new point cloud
***q*** - i'th point in the old point cloud
***n*** - normal in the point ***q*** in the old point cloud
Therefore, ***ppd(...)*** can be expressed as the dot product of (difference between ***p*** and ***q***) and (***n***):
<img src="https://render.githubusercontent.com/render/math?math=dot(T_{p2q}(p)-q, n)=dot((R\cdot p %2b t)-q,n)=[(R\cdot p %2b t)-q]^{T}\cdot n">
***T(p)*** is a rigid transform of point ***p***:
<img src="https://render.githubusercontent.com/render/math?math=T_{p2q}(p) = (R \cdot p %2B t)">
where ***R*** - rotation, ***t*** - translation.
***T*** is the transform we search by ICP, its purpose is to bring each point ***p*** closer to the corresponding point ***q*** in terms of point to plane distance.
### How to minimize objective function?
We use the Gauss-Newton method for the function minimization.
In Gauss-Newton method we do sequential steps by changing ***R*** and ***t*** in the direction of the function E decrease, i.e. in the direction of its gradient:
1. At each step we approximate the function ***E*** linearly as its current value plus Jacobian matrix multiplied by ***delta x*** which is concatenated ***delta R*** and ***delta t*** vectors.
2. We find ***delta R*** and ***delta t*** by solving the equation ***E_approx(delta_x) = 0***
3. We apply ***delta R*** and ***delta t*** to current Rt transform and proceed to next iteration
### How to linearize E?
Let's approximate it in infinitesimal neighborhood.
Here's a formula we're going to minimize by changing ***R*** and ***t***:
<img src="https://render.githubusercontent.com/render/math?math=E=\sum\left\|[(R\cdot p %2B t)-q]^{T}\cdot n\right\|_{2}">
While the point to plane distance is linear to both ***R*** and ***t***, the rotation space is not linear by itself. You can see this in how ***R*** is generated from its rotation angles:
<img src="https://render.githubusercontent.com/render/math?math=%5Cdisplaystyle+R+%3D+R_%7Bz%7D%28%5Cgamma%29R_%7By%7D%28%5Cbeta+%29R_%7Bx%7D%28%5Calpha%29%3D%0A%5Cbegin%7Bbmatrix%7D%0Acos%28%5Cgamma%29+%26+-sin%28%5Cgamma%29+%26+0+%5C%5C%0Asin%28%5Cgamma%29+%26+cos%28%5Cgamma%29+%26+0%5C%5C%0A0+%26+0+%26+1%0A%5Cend%7Bbmatrix%7D%0A%5Cbegin%7Bbmatrix%7D%0Acos%28%5Cbeta%29+%26+0+%26+sin%28%5Cbeta%29%5C%5C%0A0+%26+1+%26+0%5C%5C%0A-sin%28%5Cbeta%29+%26+0+%26+cos%28%5Cbeta%29%0A%5Cend%7Bbmatrix%7D%0A%5Cbegin%7Bbmatrix%7D%0A1+%26+0+%26+0%5C%5C%0A0+%26+cos%28%5Calpha%29+%26+-sin%28%5Calpha%29%5C%5C%0A0+%26+sin%28%5Calpha%29+%26+cos%28%5Calpha%29%0A%5Cend%7Bbmatrix%7D%0A">
But since we have infinitesimal rotations, ***R*** can be approximated in the following form:
<img src="https://render.githubusercontent.com/render/math?math=R=I %2B Ad\theta">
where ***I*** - unit matrix, ***A*** - member of the three-dimensional special orthogonal group ***so(3)***.
By approaching all sin(t) and cos(t) terms to their limits where ***t --> 0*** we get the following representation:
<img src="https://render.githubusercontent.com/render/math?math=R = I %2B \begin{bmatrix}0 %26 -\gamma %26 \beta \\ \gamma %26 0 %26 -\alpha \\ -\beta %26 \alpha %26 0 \end{bmatrix} = I %2B skew(\begin{bmatrix} \alpha %26 \beta %26 \gamma \end{bmatrix}^{T}) = I %2B skew(R_{shift}) ">
Substituting the approximation of ***R*** back into ***E*** expression, we get:
<img src="https://render.githubusercontent.com/render/math?math=E_{approx}=\sum\left\|[(I %2B skew(R_{shift})) \cdot p %2B t - q]^{T} \cdot n \right \|_{2} ">
<img src="https://render.githubusercontent.com/render/math?math=E_{approx} = \sum \left \| [I \cdot p %2B skew(R_{shift}) \cdot p %2B t - q]^{T} \cdot n \right \|_{2} ">
<img src="https://render.githubusercontent.com/render/math?math=E_{approx} = \sum \left \| [skew(R_{shift}) \cdot p %2B t %2B p- q]^{T} \cdot n \right \|_{2} ">
Let's introduce a function f which approximates transform shift:
<img src="https://render.githubusercontent.com/render/math?math=f(x, p) = skew(R_{shift}) \cdot p %2B t">
<img src="https://render.githubusercontent.com/render/math?math=E_{approx} = \sum \left \| [f(x, p) %2B p- q]^{T} \cdot n \right \|_{2}">
### How to minimize _E_approx_?
***E_approx*** is minimal when its differential (i.e. derivative by argument increase) is zero, so let's find that differential:
<img src="https://render.githubusercontent.com/render/math?math=d(E_{approx}) = \sum_i d(\left \| ppd(T_{approx}(p_i), q_i, n_i) \right \|_2) = ">
<img src="https://render.githubusercontent.com/render/math?math=\sum_i d(ppd(T_{approx}(p_i), q_i, n_i)^2) =">
<img src="https://render.githubusercontent.com/render/math?math=\sum_i 2\cdot ppd(...)\cdot d(ppd(T_{approx}(p_i), q_i, n_i))">
Let's differentiate ***ppd***:
<img src="https://render.githubusercontent.com/render/math?math=d(ppd(T_{approx}(p_i), q_i, n_i)) = d([f(x, p_i) %2b p_i- q_i]^{T} \cdot n_i) = df(x, p_i)^{T} \cdot n_i = dx^T f'(x, p_i)^T \cdot n_i">
Here's what we get for all variables x_j from vector x:
<img src="https://render.githubusercontent.com/render/math?math=\frac{\partial E}{\partial x_{j}} = \sum [2 \cdot (f(x, p) %2B p - q)^{T} \cdot n] \cdot [f_{j}'(x, p)^{T} \cdot n] = 0">
<img src="https://render.githubusercontent.com/render/math?math=\sum [2 \cdot n^{T} \cdot (f(x, p) %2B p - q)] \cdot [n^{T} \cdot f{}'(x, p)] = 0 ">
Let new variable: <img src="https://render.githubusercontent.com/render/math?math=\triangle p = p - q">
<img src="https://render.githubusercontent.com/render/math?math=\sum [2 \cdot n^{T} \cdot (f(x, p) %2B \triangle p)] \cdot [n^{T} \cdot f{}'(x, p)] = 0">
<img src="https://render.githubusercontent.com/render/math?math=\sum [(f(x, p) %2B \triangle p)^{T} \cdot (n \cdot n^{T})] \cdot f{}'(x, p) = 0">
<img src="https://render.githubusercontent.com/render/math?math=\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p) %2B \triangle p] = 0">
***f(x, p)*** can be represented as a matrix-vector multiplication. To prove that, we have to remember that <img src="https://render.githubusercontent.com/render/math?math=cross(a, b) = skew(a) \cdot b = skew(b)^{T} \cdot a"> :
<img src="https://render.githubusercontent.com/render/math?math=f(x, p) = skew(R_{shift}) \cdot p %2B t_{shift} = skew(p)^T R_{shift} %2B t_{shift}">
<img src="https://render.githubusercontent.com/render/math?math=f(x, p) = \begin{bmatrix} skew(p)^{T} %26 I_{3\times 3}\end{bmatrix} \cdot \begin{bmatrix} \triangle R %26 \triangle t \end{bmatrix}^{T} = G(p) \cdot x">
***G(p)*** is introduced for simplification.
Since <img src="https://render.githubusercontent.com/render/math?math=d(f(x, p)) = G(p) \cdot dx = f'(x, p) \cdot dx"> we get <img src="https://render.githubusercontent.com/render/math?math=f'(x, p) = G(p)">.
<img src="https://render.githubusercontent.com/render/math?math=\sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [f(x, p)] = \sum f{}'(x, p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]">
<img src="https://render.githubusercontent.com/render/math?math=\sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [G(p) \cdot X] = \sum G(p)^{T} \cdot [n \cdot n^{T}] \cdot [- \triangle p]">
Let a new value:
<img src="https://render.githubusercontent.com/render/math?math=C = G(p)^{T} \cdot n">
<img src="https://render.githubusercontent.com/render/math?math=C^{T} = (G(p)^{T} \cdot n)^{T} = n^{T} \cdot G(p)">
Let's make a replacement:
<img src="https://render.githubusercontent.com/render/math?math=\sum C \cdot C^{T} \cdot X = \sum C \cdot n^{T} \cdot [- \triangle p]">
<img src="https://render.githubusercontent.com/render/math?math=\sum C\cdot C^{T}\cdot \begin{bmatrix} \triangle R\\ \triangle t \end{bmatrix} = \sum C \cdot n^{T} \cdot [- \triangle p]">
By solving this equation we get rigid transform shift for each Gauss-Newton iteration.
### How do we apply transform shift?
We generate rotation and translation matrix from the shift and then multiply the current pose matrix by the one we've got.
While the translational part of the shift contributes to the resulting matrix as-is, the rotational part is generated a bit trickier.
The rotation shift is converted from ***so(3)*** to ***SO(3)*** by exponentiation.
In fact, the 3-by-1 rshift vector represents rotation axis multiplied by the rotation angle. We use Rodrigues transform to get rotation matrix from that.
For more details, see [wiki page](https://en.wikipedia.org/wiki/3D_rotation_group).
modules/rgbd/doc/rgbd.bib
+7
View File
@@ -0,0 +1,7 @@
@inproceedings{dynamicfusion,
author = {Newcombe, Richard A. and Fox, Dieter and Seitz, Steven M.},
title = {DynamicFusion: Reconstruction and Tracking of Non-Rigid Scenes in Real-Time},
booktitle = {The IEEE Conference on Computer Vision and Pattern Recognition (CVPR)},
month = {June},
year = {2015}
}