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/CMakeLists.txt
+7
View File
@@ -0,0 +1,7 @@
set(the_description "RGBD algorithms")
ocv_define_module(rgbd opencv_core opencv_geometry opencv_imgproc opencv_ptcloud OPTIONAL opencv_viz WRAP python)
if(HAVE_OPENGL)
ocv_target_link_libraries(${the_module} PRIVATE "${OPENGL_LIBRARIES}")
endif()
modules/rgbd/LICENSE_KinectFusion.md
+27
View File
@@ -0,0 +1,27 @@
Copyright (c) 2012, Anatoly Baksheev
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the {organization} nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
modules/rgbd/README.md
+19
View File
@@ -0,0 +1,19 @@
RGB-Depth Processing module
============================
Contains a collection of depth processing algorithms:
* Linemod 3D object recognition
* Fast surface normals and 3D plane finding
* 3D visual odometry
* KinectFusion
Note that the KinectFusion algorithm was patented and its use may be restricted by following (but not limited to) list of patents:
* _US20120196679A1_ Real-Time Camera Tracking Using Depth Maps
* _US20120194644A1_ Mobile Camera Localization Using Depth Maps
* _US20120194516A1_ Three-Dimensional Environment Reconstruction
* _US8401225B2_ Moving object segmentation using depth images
Since OpenCV's license imposes different restrictions on usage please consult a legal before using this algorithm any way.
That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
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}
}
modules/rgbd/include/opencv2/rgbd.hpp
+25
View File
@@ -0,0 +1,25 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
#ifndef __OPENCV_RGBD_HPP__
#define __OPENCV_RGBD_HPP__
#include "opencv2/rgbd/dynafu.hpp"
#include "opencv2/rgbd/kinfu.hpp"
#include "opencv2/rgbd/colored_kinfu.hpp"
#include "opencv2/rgbd/large_kinfu.hpp"
#include "opencv2/rgbd/linemod.hpp"
/** @defgroup rgbd RGB-Depth Processing
@ref kinfu_icp
*/
#endif
/* End of file. */
modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp
+273
View File
@@ -0,0 +1,273 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#ifndef __OPENCV_RGBD_COLORED_KINFU_HPP__
#define __OPENCV_RGBD_COLORED_KINFU_HPP__
#include "opencv2/core.hpp"
#include "opencv2/core/affine.hpp"
#include <opencv2/ptcloud.hpp>
namespace cv {
namespace colored_kinfu {
//! @addtogroup kinect_fusion
//! @{
struct CV_EXPORTS_W Params
{
CV_WRAP Params()
{
setInitialVolumePose(Matx44f::eye());
}
/**
* @brief Constructor for Params
* Sets the initial pose of the TSDF volume.
* @param volumeInitialPoseRot rotation matrix
* @param volumeInitialPoseTransl translation vector
*/
CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl)
{
setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl);
}
/**
* @brief Constructor for Params
* Sets the initial pose of the TSDF volume.
* @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
*/
CV_WRAP Params(Matx44f volumeInitialPose)
{
setInitialVolumePose(volumeInitialPose);
}
/**
* @brief Set Initial Volume Pose
* Sets the initial pose of the TSDF volume.
* @param R rotation matrix
* @param t translation vector
*/
CV_WRAP void setInitialVolumePose(Matx33f R, Vec3f t);
/**
* @brief Set Initial Volume Pose
* Sets the initial pose of the TSDF volume.
* @param homogen_tf 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
*/
CV_WRAP void setInitialVolumePose(Matx44f homogen_tf);
/**
* @brief Default parameters
* A set of parameters which provides better model quality, can be very slow.
*/
CV_WRAP static Ptr<Params> defaultParams();
/** @brief Coarse parameters
A set of parameters which provides better speed, can fail to match frames
in case of rapid sensor motion.
*/
CV_WRAP static Ptr<Params> coarseParams();
/** @brief HashTSDF parameters
A set of parameters suitable for use with HashTSDFVolume
*/
CV_WRAP static Ptr<Params> hashTSDFParams(bool isCoarse);
/** @brief ColoredTSDF parameters
A set of parameters suitable for use with HashTSDFVolume
*/
CV_WRAP static Ptr<Params> coloredTSDFParams(bool isCoarse);
/** @brief frame size in pixels */
CV_PROP_RW Size frameSize;
/** @brief rgb frame size in pixels */
CV_PROP_RW Size rgb_frameSize;
CV_PROP_RW VolumeType volumeKind;
/** @brief camera intrinsics */
CV_PROP_RW Matx33f intr;
/** @brief rgb camera intrinsics */
CV_PROP_RW Matx33f rgb_intr;
/** @brief pre-scale per 1 meter for input values
Typical values are:
* 5000 per 1 meter for the 16-bit PNG files of TUM database
* 1000 per 1 meter for Kinect 2 device
* 1 per 1 meter for the 32-bit float images in the ROS bag files
*/
CV_PROP_RW float depthFactor;
/** @brief Depth sigma in meters for bilateral smooth */
CV_PROP_RW float bilateral_sigma_depth;
/** @brief Spatial sigma in pixels for bilateral smooth */
CV_PROP_RW float bilateral_sigma_spatial;
/** @brief Kernel size in pixels for bilateral smooth */
CV_PROP_RW int bilateral_kernel_size;
/** @brief Number of pyramid levels for ICP */
CV_PROP_RW int pyramidLevels;
/** @brief Resolution of voxel space
Number of voxels in each dimension.
*/
CV_PROP_RW Vec3i volumeDims;
/** @brief Size of voxel in meters */
CV_PROP_RW float voxelSize;
/** @brief Minimal camera movement in meters
Integrate new depth frame only if camera movement exceeds this value.
*/
CV_PROP_RW float tsdf_min_camera_movement;
/** @brief initial volume pose in meters */
CV_PROP_RW Matx44f volumePose;
/** @brief distance to truncate in meters
Distances to surface that exceed this value will be truncated to 1.0.
*/
CV_PROP_RW float tsdf_trunc_dist;
/** @brief max number of frames per voxel
Each voxel keeps running average of distances no longer than this value.
*/
CV_PROP_RW int tsdf_max_weight;
/** @brief A length of one raycast step
How much voxel sizes we skip each raycast step
*/
CV_PROP_RW float raycast_step_factor;
// gradient delta in voxel sizes
// fixed at 1.0f
// float gradient_delta_factor;
/** @brief light pose for rendering in meters */
CV_PROP_RW Vec3f lightPose;
/** @brief distance theshold for ICP in meters */
CV_PROP_RW float icpDistThresh;
/** angle threshold for ICP in radians */
CV_PROP_RW float icpAngleThresh;
/** number of ICP iterations for each pyramid level */
CV_PROP_RW std::vector<int> icpIterations;
/** @brief Threshold for depth truncation in meters
All depth values beyond this threshold will be set to zero
*/
CV_PROP_RW float truncateThreshold;
};
/** @brief KinectFusion implementation
This class implements a 3d reconstruction algorithm described in
@cite kinectfusion paper.
It takes a sequence of depth images taken from depth sensor
(or any depth images source such as stereo camera matching algorithm or even raymarching renderer).
The output can be obtained as a vector of points and their normals
or can be Phong-rendered from given camera pose.
An internal representation of a model is a voxel cuboid that keeps TSDF values
which are a sort of distances to the surface (for details read the @cite kinectfusion article about TSDF).
There is no interface to that representation yet.
KinFu uses OpenCL acceleration automatically if available.
To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL().
This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake).
Note that the KinectFusion algorithm was patented and its use may be restricted by
the list of patents mentioned in README.md file in this module directory.
That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
*/
class CV_EXPORTS_W ColoredKinFu
{
public:
CV_WRAP static Ptr<ColoredKinFu> create(const Ptr<Params>& _params);
virtual ~ColoredKinFu();
/** @brief Get current parameters */
virtual const Params& getParams() const = 0;
/** @brief Renders a volume into an image
Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat.
Light pose is fixed in KinFu params.
@param image resulting image
*/
CV_WRAP virtual void render(OutputArray image) const = 0;
/** @brief Renders a volume into an image
Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat.
Light pose is fixed in KinFu params.
@param image resulting image
@param cameraPose pose of camera to render from. If empty then render from current pose
which is a last frame camera pose.
*/
CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose) const = 0;
/** @brief Gets points and normals of current 3d mesh
The order of normals corresponds to order of points.
The order of points is undefined.
@param points vector of points which are 4-float vectors
@param normals vector of normals which are 4-float vectors
*/
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
/** @brief Gets points of current 3d mesh
The order of points is undefined.
@param points vector of points which are 4-float vectors
*/
CV_WRAP virtual void getPoints(OutputArray points) const = 0;
/** @brief Calculates normals for given points
@param points input vector of points which are 4-float vectors
@param normals output vector of corresponding normals which are 4-float vectors
*/
CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0;
/** @brief Resets the algorithm
Clears current model and resets a pose.
*/
CV_WRAP virtual void reset() = 0;
/** @brief Get current pose in voxel space */
virtual Affine3f getPose() const = 0;
/** @brief Process next depth frame
@param depth input Mat of depth frame
@param rgb input Mat of rgb (colored) frame
@return true if succeeded to align new frame with current scene, false if opposite
*/
CV_WRAP virtual bool update(InputArray depth, InputArray rgb) = 0;
};
//! @}
}
}
#endif
modules/rgbd/include/opencv2/rgbd/dynafu.hpp
+119
View File
@@ -0,0 +1,119 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#ifndef __OPENCV_RGBD_DYNAFU_HPP__
#define __OPENCV_RGBD_DYNAFU_HPP__
#include "opencv2/core.hpp"
#include "opencv2/core/affine.hpp"
#include "kinfu.hpp"
namespace cv {
namespace dynafu {
/** @brief DynamicFusion implementation
This class implements a 3d reconstruction algorithm as described in @cite dynamicfusion.
It takes a sequence of depth images taken from depth sensor
(or any depth images source such as stereo camera matching algorithm or even raymarching renderer).
The output can be obtained as a vector of points and their normals
or can be Phong-rendered from given camera pose.
It extends the KinectFusion algorithm to handle non-rigidly deforming scenes by maintaining a sparse
set of nodes covering the geometry such that each node contains a warp to transform it from a canonical
space to the live frame.
An internal representation of a model is a voxel cuboid that keeps TSDF values
which are a sort of distances to the surface (for details read the @cite kinectfusion article about TSDF).
There is no interface to that representation yet.
Note that DynamicFusion is based on the KinectFusion algorithm which is patented and its use may be
restricted by the list of patents mentioned in README.md file in this module directory.
That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use DynamicFusion.
*/
/** Backwards compatibility for old versions */
using Params = kinfu::Params;
class CV_EXPORTS_W DynaFu
{
public:
CV_WRAP static Ptr<DynaFu> create(const Ptr<kinfu::Params>& _params);
virtual ~DynaFu();
/** @brief Get current parameters */
virtual const kinfu::Params& getParams() const = 0;
/** @brief Renders a volume into an image
Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat.
Light pose is fixed in DynaFu params.
@param image resulting image
@param cameraPose pose of camera to render from. If empty then render from current pose
which is a last frame camera pose.
*/
CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose = Matx44f::eye()) const = 0;
/** @brief Gets points and normals of current 3d mesh
The order of normals corresponds to order of points.
The order of points is undefined.
@param points vector of points which are 4-float vectors
@param normals vector of normals which are 4-float vectors
*/
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
/** @brief Gets points of current 3d mesh
The order of points is undefined.
@param points vector of points which are 4-float vectors
*/
CV_WRAP virtual void getPoints(OutputArray points) const = 0;
/** @brief Calculates normals for given points
@param points input vector of points which are 4-float vectors
@param normals output vector of corresponding normals which are 4-float vectors
*/
CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0;
/** @brief Resets the algorithm
Clears current model and resets a pose.
*/
CV_WRAP virtual void reset() = 0;
/** @brief Get current pose in voxel space */
virtual Affine3f getPose() const = 0;
/** @brief Process next depth frame
Integrates depth into voxel space with respect to its ICP-calculated pose.
Input image is converted to CV_32F internally if has another type.
@param depth one-channel image which size and depth scale is described in algorithm's parameters
@return true if succeeded to align new frame with current scene, false if opposite
*/
CV_WRAP virtual bool update(InputArray depth) = 0;
virtual std::vector<Point3f> getNodesPos() const = 0;
virtual void marchCubes(OutputArray vertices, OutputArray edges) const = 0;
virtual void renderSurface(OutputArray depthImage, OutputArray vertImage, OutputArray normImage, bool warp=true) = 0;
};
} // dynafu::
} // cv::
#endif // __OPENCV_RGBD_DYNAFU_HPP__
modules/rgbd/include/opencv2/rgbd/kinfu.hpp
+328
View File
@@ -0,0 +1,328 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#ifndef __OPENCV_RGBD_KINFU_HPP__
#define __OPENCV_RGBD_KINFU_HPP__
#include "opencv2/core.hpp"
#include "opencv2/core/affine.hpp"
#include <opencv2/geometry.hpp>
#include <opencv2/ptcloud.hpp>
namespace cv {
namespace kinfu {
//! @addtogroup kinect_fusion
//! @{
struct CV_EXPORTS_W VolumeParams
{
/** @brief Kind of Volume
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
*/
CV_PROP_RW VolumeType kind = VolumeType::TSDF;
/** @brief Resolution of voxel space
Number of voxels in each dimension.
Applicable only for TSDF Volume.
HashTSDF volume only supports equal resolution in all three dimensions
*/
CV_PROP_RW int resolutionX = 128;
CV_PROP_RW int resolutionY = 128;
CV_PROP_RW int resolutionZ = 128;
/** @brief Resolution of volumeUnit in voxel space
Number of voxels in each dimension for volumeUnit
Applicable only for hashTSDF.
*/
CV_PROP_RW int unitResolution = 0;
/** @brief Size of volume in meters */
CV_PROP_RW float volumSize = 3.f;
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
CV_PROP_RW Matx44f pose = Affine3f().translate(Vec3f(-volumSize / 2.f, -volumSize / 2.f, 0.5f)).matrix;
/** @brief Length of voxels in meters */
CV_PROP_RW float voxelSize = volumSize / 512.f;
/** @brief TSDF truncation distance
Distances greater than value from surface will be truncated to 1.0
*/
CV_PROP_RW float tsdfTruncDist = 7.f * voxelSize;
/** @brief Max number of frames to integrate per voxel
Represents the max number of frames over which a running average
of the TSDF is calculated for a voxel
*/
CV_PROP_RW int maxWeight = 64;
/** @brief Threshold for depth truncation in meters
Truncates the depth greater than threshold to 0
*/
CV_PROP_RW float depthTruncThreshold = 0.f;
/** @brief Length of single raycast step
Describes the percentage of voxel length that is skipped per march
*/
CV_PROP_RW float raycastStepFactor = 0.25f;
};
struct CV_EXPORTS_W Params
{
CV_WRAP Params()
{
setInitialVolumePose(Matx44f::eye());
}
/**
* @brief Constructor for Params
* Sets the initial pose of the TSDF volume.
* @param volumeInitialPoseRot rotation matrix
* @param volumeInitialPoseTransl translation vector
*/
CV_WRAP Params(Matx33f volumeInitialPoseRot, Vec3f volumeInitialPoseTransl)
{
setInitialVolumePose(volumeInitialPoseRot,volumeInitialPoseTransl);
}
/**
* @brief Constructor for Params
* Sets the initial pose of the TSDF volume.
* @param volumeInitialPose 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
*/
CV_WRAP Params(Matx44f volumeInitialPose)
{
setInitialVolumePose(volumeInitialPose);
}
/**
* @brief Set Initial Volume Pose
* Sets the initial pose of the TSDF volume.
* @param R rotation matrix
* @param t translation vector
*/
CV_WRAP void setInitialVolumePose(Matx33f R, Vec3f t);
/**
* @brief Set Initial Volume Pose
* Sets the initial pose of the TSDF volume.
* @param homogen_tf 4 by 4 Homogeneous Transform matrix to set the intial pose of TSDF volume
*/
CV_WRAP void setInitialVolumePose(Matx44f homogen_tf);
/**
* @brief Default parameters
* A set of parameters which provides better model quality, can be very slow.
*/
CV_WRAP static Ptr<Params> defaultParams();
/** @brief Coarse parameters
A set of parameters which provides better speed, can fail to match frames
in case of rapid sensor motion.
*/
CV_WRAP static Ptr<Params> coarseParams();
/** @brief HashTSDF parameters
A set of parameters suitable for use with HashTSDFVolume
*/
CV_WRAP static Ptr<Params> hashTSDFParams(bool isCoarse);
/** @brief ColoredTSDF parameters
A set of parameters suitable for use with ColoredTSDFVolume
*/
CV_WRAP static Ptr<Params> coloredTSDFParams(bool isCoarse);
/** @brief frame size in pixels */
CV_PROP_RW Size frameSize;
/** @brief Volume kind */
VolumeType volumeKind;
/** @brief camera intrinsics */
CV_PROP_RW Matx33f intr;
/** @brief rgb camera intrinsics */
CV_PROP_RW Matx33f rgb_intr;
/** @brief pre-scale per 1 meter for input values
Typical values are:
* 5000 per 1 meter for the 16-bit PNG files of TUM database
* 1000 per 1 meter for Kinect 2 device
* 1 per 1 meter for the 32-bit float images in the ROS bag files
*/
CV_PROP_RW float depthFactor;
/** @brief Depth sigma in meters for bilateral smooth */
CV_PROP_RW float bilateral_sigma_depth;
/** @brief Spatial sigma in pixels for bilateral smooth */
CV_PROP_RW float bilateral_sigma_spatial;
/** @brief Kernel size in pixels for bilateral smooth */
CV_PROP_RW int bilateral_kernel_size;
/** @brief Number of pyramid levels for ICP */
CV_PROP_RW int pyramidLevels;
/** @brief Resolution of voxel space
Number of voxels in each dimension.
*/
CV_PROP_RW Vec3i volumeDims;
/** @brief Size of voxel in meters */
CV_PROP_RW float voxelSize;
/** @brief Minimal camera movement in meters
Integrate new depth frame only if camera movement exceeds this value.
*/
CV_PROP_RW float tsdf_min_camera_movement;
/** @brief initial volume pose in meters */
CV_PROP_RW Matx44f volumePose;
/** @brief distance to truncate in meters
Distances to surface that exceed this value will be truncated to 1.0.
*/
CV_PROP_RW float tsdf_trunc_dist;
/** @brief max number of frames per voxel
Each voxel keeps running average of distances no longer than this value.
*/
CV_PROP_RW int tsdf_max_weight;
/** @brief A length of one raycast step
How much voxel sizes we skip each raycast step
*/
CV_PROP_RW float raycast_step_factor;
// gradient delta in voxel sizes
// fixed at 1.0f
// float gradient_delta_factor;
/** @brief light pose for rendering in meters */
CV_PROP_RW Vec3f lightPose;
/** @brief distance theshold for ICP in meters */
CV_PROP_RW float icpDistThresh;
/** angle threshold for ICP in radians */
CV_PROP_RW float icpAngleThresh;
/** number of ICP iterations for each pyramid level */
CV_PROP_RW std::vector<int> icpIterations;
/** @brief Threshold for depth truncation in meters
All depth values beyond this threshold will be set to zero
*/
CV_PROP_RW float truncateThreshold;
};
/** @brief KinectFusion implementation
This class implements a 3d reconstruction algorithm described in
@cite kinectfusion paper.
It takes a sequence of depth images taken from depth sensor
(or any depth images source such as stereo camera matching algorithm or even raymarching renderer).
The output can be obtained as a vector of points and their normals
or can be Phong-rendered from given camera pose.
An internal representation of a model is a voxel cuboid that keeps TSDF values
which are a sort of distances to the surface (for details read the @cite kinectfusion article about TSDF).
There is no interface to that representation yet.
KinFu uses OpenCL acceleration automatically if available.
To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL().
This implementation is based on [kinfu-remake](https://github.com/Nerei/kinfu_remake).
Note that the KinectFusion algorithm was patented and its use may be restricted by
the list of patents mentioned in README.md file in this module directory.
That's why you need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
*/
class CV_EXPORTS_W KinFu
{
public:
CV_WRAP static Ptr<KinFu> create(const Ptr<Params>& _params);
virtual ~KinFu();
/** @brief Get current parameters */
virtual const Params& getParams() const = 0;
/** @brief Renders a volume into an image
Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat.
Light pose is fixed in KinFu params.
@param image resulting image
*/
CV_WRAP virtual void render(OutputArray image) const = 0;
/** @brief Renders a volume into an image
Renders a 0-surface of TSDF using Phong shading into a CV_8UC4 Mat.
Light pose is fixed in KinFu params.
@param image resulting image
@param cameraPose pose of camera to render from. If empty then render from current pose
which is a last frame camera pose.
*/
CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose) const = 0;
/** @brief Gets points and normals of current 3d mesh
The order of normals corresponds to order of points.
The order of points is undefined.
@param points vector of points which are 4-float vectors
@param normals vector of normals which are 4-float vectors
*/
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
/** @brief Gets points of current 3d mesh
The order of points is undefined.
@param points vector of points which are 4-float vectors
*/
CV_WRAP virtual void getPoints(OutputArray points) const = 0;
/** @brief Calculates normals for given points
@param points input vector of points which are 4-float vectors
@param normals output vector of corresponding normals which are 4-float vectors
*/
CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0;
/** @brief Resets the algorithm
Clears current model and resets a pose.
*/
CV_WRAP virtual void reset() = 0;
/** @brief Get current pose in voxel space */
virtual Affine3f getPose() const = 0;
/** @brief Process next depth frame
Integrates depth into voxel space with respect to its ICP-calculated pose.
Input image is converted to CV_32F internally if has another type.
@param depth one-channel image which size and depth scale is described in algorithm's parameters
@return true if succeeded to align new frame with current scene, false if opposite
*/
CV_WRAP virtual bool update(InputArray depth) = 0;
};
//! @}
}
}
#endif
modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp
+218
View File
@@ -0,0 +1,218 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
// module's directory
#ifndef __OPENCV_RGBD_LARGEKINFU_HPP__
#define __OPENCV_RGBD_LARGEKINFU_HPP__
#include <opencv2/geometry.hpp>
#include "opencv2/core.hpp"
#include "opencv2/core/affine.hpp"
namespace cv
{
namespace large_kinfu
{
struct CV_EXPORTS_W VolumeParams
{
/** @brief Kind of Volume
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
*/
CV_PROP_RW VolumeType kind = VolumeType::TSDF;
/** @brief Resolution of voxel space
Number of voxels in each dimension.
Applicable only for TSDF Volume.
HashTSDF volume only supports equal resolution in all three dimensions
*/
CV_PROP_RW int resolutionX = 128;
CV_PROP_RW int resolutionY = 128;
CV_PROP_RW int resolutionZ = 128;
/** @brief Resolution of volumeUnit in voxel space
Number of voxels in each dimension for volumeUnit
Applicable only for hashTSDF.
*/
CV_PROP_RW int unitResolution = 0;
/** @brief Size of volume in meters */
CV_PROP_RW float volumSize = 3.f;
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
CV_PROP_RW Matx44f pose = Affine3f().translate(Vec3f(-volumSize / 2.f, -volumSize / 2.f, 0.5f)).matrix;
/** @brief Length of voxels in meters */
CV_PROP_RW float voxelSize = volumSize / 512.f;
/** @brief TSDF truncation distance
Distances greater than value from surface will be truncated to 1.0
*/
CV_PROP_RW float tsdfTruncDist = 7.f * voxelSize;
/** @brief Max number of frames to integrate per voxel
Represents the max number of frames over which a running average
of the TSDF is calculated for a voxel
*/
CV_PROP_RW int maxWeight = 64;
/** @brief Threshold for depth truncation in meters
Truncates the depth greater than threshold to 0
*/
CV_PROP_RW float depthTruncThreshold = 0.f;
/** @brief Length of single raycast step
Describes the percentage of voxel length that is skipped per march
*/
CV_PROP_RW float raycastStepFactor = 0.25f;
/** @brief Default set of parameters that provide higher quality reconstruction at the cost of slow performance.*/
CV_WRAP static Ptr<VolumeParams> defaultParams(VolumeType volumeType);
/** @brief Coarse set of parameters that provides relatively higher performance at the cost of reconstrution quality.*/
CV_WRAP static Ptr<VolumeParams> coarseParams(VolumeType volumeType);
};
struct CV_EXPORTS_W Params
{
/** @brief Default parameters
A set of parameters which provides better model quality, can be very slow.
*/
CV_WRAP static Ptr<Params> defaultParams();
/** @brief Coarse parameters
A set of parameters which provides better speed, can fail to match frames
in case of rapid sensor motion.
*/
CV_WRAP static Ptr<Params> coarseParams();
/** @brief HashTSDF parameters
A set of parameters suitable for use with HashTSDFVolume
*/
CV_WRAP static Ptr<Params> hashTSDFParams(bool isCoarse);
/** @brief frame size in pixels */
CV_PROP_RW Size frameSize;
/** @brief camera intrinsics */
CV_PROP_RW Matx33f intr;
/** @brief rgb camera intrinsics */
CV_PROP_RW Matx33f rgb_intr;
/** @brief pre-scale per 1 meter for input values
Typical values are:
* 5000 per 1 meter for the 16-bit PNG files of TUM database
* 1000 per 1 meter for Kinect 2 device
* 1 per 1 meter for the 32-bit float images in the ROS bag files
*/
CV_PROP_RW float depthFactor;
/** @brief Depth sigma in meters for bilateral smooth */
CV_PROP_RW float bilateral_sigma_depth;
/** @brief Spatial sigma in pixels for bilateral smooth */
CV_PROP_RW float bilateral_sigma_spatial;
/** @brief Kernel size in pixels for bilateral smooth */
CV_PROP_RW int bilateral_kernel_size;
/** @brief Number of pyramid levels for ICP */
CV_PROP_RW int pyramidLevels;
/** @brief Minimal camera movement in meters
Integrate new depth frame only if camera movement exceeds this value.
*/
CV_PROP_RW float tsdf_min_camera_movement;
/** @brief light pose for rendering in meters */
CV_PROP_RW Vec3f lightPose;
/** @brief distance theshold for ICP in meters */
CV_PROP_RW float icpDistThresh;
/** @brief angle threshold for ICP in radians */
CV_PROP_RW float icpAngleThresh;
/** @brief number of ICP iterations for each pyramid level */
CV_PROP_RW std::vector<int> icpIterations;
/** @brief Threshold for depth truncation in meters
All depth values beyond this threshold will be set to zero
*/
CV_PROP_RW float truncateThreshold;
/** @brief Volume parameters
*/
VolumeParams volumeParams;
};
/** @brief Large Scale Dense Depth Fusion implementation
This class implements a 3d reconstruction algorithm for larger environments using
Spatially hashed TSDF volume "Submaps".
It also runs a periodic posegraph optimization to minimize drift in tracking over long sequences.
Currently the algorithm does not implement a relocalization or loop closure module.
Potentially a Bag of words implementation or RGBD relocalization as described in
Glocker et al. ISMAR 2013 will be implemented
It takes a sequence of depth images taken from depth sensor
(or any depth images source such as stereo camera matching algorithm or even raymarching
renderer). The output can be obtained as a vector of points and their normals or can be
Phong-rendered from given camera pose.
An internal representation of a model is a spatially hashed voxel cube that stores TSDF values
which represent the distance to the closest surface (for details read the @cite kinectfusion article
about TSDF). There is no interface to that representation yet.
For posegraph optimization, a Submap abstraction over the Volume class is created.
New submaps are added to the model when there is low visibility overlap between current viewing frustrum
and the existing volume/model. Multiple submaps are simultaneously tracked and a posegraph is created and
optimized periodically.
LargeKinfu does not use any OpenCL acceleration yet.
To enable or disable it explicitly use cv::setUseOptimized() or cv::ocl::setUseOpenCL().
This implementation is inspired from Kintinuous, InfiniTAM and other SOTA algorithms
You need to set the OPENCV_ENABLE_NONFREE option in CMake to use KinectFusion.
*/
class CV_EXPORTS_W LargeKinfu
{
public:
CV_WRAP static Ptr<LargeKinfu> create(const Ptr<Params>& _params);
virtual ~LargeKinfu() = default;
virtual const Params& getParams() const = 0;
CV_WRAP virtual void render(OutputArray image) const = 0;
CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose) const = 0;
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
CV_WRAP virtual void getPoints(OutputArray points) const = 0;
CV_WRAP virtual void getNormals(InputArray points, OutputArray normals) const = 0;
CV_WRAP virtual void reset() = 0;
virtual Affine3f getPose() const = 0;
CV_WRAP virtual bool update(InputArray depth) = 0;
};
} // namespace large_kinfu
} // namespace cv
#endif
modules/rgbd/include/opencv2/rgbd/linemod.hpp
+435
View File
@@ -0,0 +1,435 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
#ifndef __OPENCV_RGBD_LINEMOD_HPP__
#define __OPENCV_RGBD_LINEMOD_HPP__
#include "opencv2/core.hpp"
#include <map>
/****************************************************************************************\
* LINE-MOD *
\****************************************************************************************/
namespace cv {
namespace linemod {
//! @addtogroup rgbd
//! @{
/**
* \brief Discriminant feature described by its location and label.
*/
struct CV_EXPORTS_W_SIMPLE Feature
{
CV_PROP_RW int x; ///< x offset
CV_PROP_RW int y; ///< y offset
CV_PROP_RW int label; ///< Quantization
CV_WRAP Feature() : x(0), y(0), label(0) {}
CV_WRAP Feature(int x, int y, int label);
void read(const FileNode& fn);
void write(FileStorage& fs) const;
};
inline Feature::Feature(int _x, int _y, int _label) : x(_x), y(_y), label(_label) {}
struct CV_EXPORTS_W_SIMPLE Template
{
CV_PROP int width;
CV_PROP int height;
CV_PROP int pyramid_level;
CV_PROP std::vector<Feature> features;
void read(const FileNode& fn);
void write(FileStorage& fs) const;
};
/**
* \brief Represents a modality operating over an image pyramid.
*/
class CV_EXPORTS_W QuantizedPyramid
{
public:
// Virtual destructor
virtual ~QuantizedPyramid() {}
/**
* \brief Compute quantized image at current pyramid level for online detection.
*
* \param[out] dst The destination 8-bit image. For each pixel at most one bit is set,
* representing its classification.
*/
CV_WRAP virtual void quantize(CV_OUT Mat& dst) const =0;
/**
* \brief Extract most discriminant features at current pyramid level to form a new template.
*
* \param[out] templ The new template.
*/
CV_WRAP virtual bool extractTemplate(CV_OUT Template& templ) const =0;
/**
* \brief Go to the next pyramid level.
*
* \todo Allow pyramid scale factor other than 2
*/
CV_WRAP virtual void pyrDown() =0;
protected:
/// Candidate feature with a score
struct Candidate
{
Candidate(int x, int y, int label, float score);
/// Sort candidates with high score to the front
bool operator<(const Candidate& rhs) const
{
return score > rhs.score;
}
Feature f;
float score;
};
/**
* \brief Choose candidate features so that they are not bunched together.
*
* \param[in] candidates Candidate features sorted by score.
* \param[out] features Destination vector of selected features.
* \param[in] num_features Number of candidates to select.
* \param[in] distance Hint for desired distance between features.
*/
static void selectScatteredFeatures(const std::vector<Candidate>& candidates,
std::vector<Feature>& features,
size_t num_features, float distance);
};
inline QuantizedPyramid::Candidate::Candidate(int x, int y, int label, float _score) : f(x, y, label), score(_score) {}
/**
* \brief Interface for modalities that plug into the LINE template matching representation.
*
* \todo Max response, to allow optimization of summing (255/MAX) features as uint8
*/
class CV_EXPORTS_W Modality
{
public:
// Virtual destructor
virtual ~Modality() {}
/**
* \brief Form a quantized image pyramid from a source image.
*
* \param[in] src The source image. Type depends on the modality.
* \param[in] mask Optional mask. If not empty, unmasked pixels are set to zero
* in quantized image and cannot be extracted as features.
*/
CV_WRAP Ptr<QuantizedPyramid> process(const Mat& src,
const Mat& mask = Mat()) const
{
return processImpl(src, mask);
}
CV_WRAP virtual String name() const =0;
CV_WRAP virtual void read(const FileNode& fn) =0;
virtual void write(FileStorage& fs) const =0;
/**
* \brief Create modality by name.
*
* The following modality types are supported:
* - "ColorGradient"
* - "DepthNormal"
*/
CV_WRAP static Ptr<Modality> create(const String& modality_type);
/**
* \brief Load a modality from file.
*/
CV_WRAP static Ptr<Modality> create(const FileNode& fn);
protected:
// Indirection is because process() has a default parameter.
virtual Ptr<QuantizedPyramid> processImpl(const Mat& src,
const Mat& mask) const =0;
};
/**
* \brief Modality that computes quantized gradient orientations from a color image.
*/
class CV_EXPORTS_W ColorGradient : public Modality
{
public:
/**
* \brief Default constructor. Uses reasonable default parameter values.
*/
ColorGradient();
/**
* \brief Constructor.
*
* \param weak_threshold When quantizing, discard gradients with magnitude less than this.
* \param num_features How many features a template must contain.
* \param strong_threshold Consider as candidate features only gradients whose norms are
* larger than this.
*/
ColorGradient(float weak_threshold, size_t num_features, float strong_threshold);
CV_WRAP static Ptr<ColorGradient> create(float weak_threshold, size_t num_features, float strong_threshold);
virtual String name() const CV_OVERRIDE;
virtual void read(const FileNode& fn) CV_OVERRIDE;
virtual void write(FileStorage& fs) const CV_OVERRIDE;
CV_PROP float weak_threshold;
CV_PROP size_t num_features;
CV_PROP float strong_threshold;
protected:
virtual Ptr<QuantizedPyramid> processImpl(const Mat& src,
const Mat& mask) const CV_OVERRIDE;
};
/**
* \brief Modality that computes quantized surface normals from a dense depth map.
*/
class CV_EXPORTS_W DepthNormal : public Modality
{
public:
/**
* \brief Default constructor. Uses reasonable default parameter values.
*/
DepthNormal();
/**
* \brief Constructor.
*
* \param distance_threshold Ignore pixels beyond this distance.
* \param difference_threshold When computing normals, ignore contributions of pixels whose
* depth difference with the central pixel is above this threshold.
* \param num_features How many features a template must contain.
* \param extract_threshold Consider as candidate feature only if there are no differing
* orientations within a distance of extract_threshold.
*/
DepthNormal(int distance_threshold, int difference_threshold, size_t num_features,
int extract_threshold);
CV_WRAP static Ptr<DepthNormal> create(int distance_threshold, int difference_threshold,
size_t num_features, int extract_threshold);
virtual String name() const CV_OVERRIDE;
virtual void read(const FileNode& fn) CV_OVERRIDE;
virtual void write(FileStorage& fs) const CV_OVERRIDE;
CV_PROP int distance_threshold;
CV_PROP int difference_threshold;
CV_PROP size_t num_features;
CV_PROP int extract_threshold;
protected:
virtual Ptr<QuantizedPyramid> processImpl(const Mat& src,
const Mat& mask) const CV_OVERRIDE;
};
/**
* \brief Debug function to colormap a quantized image for viewing.
*/
CV_EXPORTS_W void colormap(const Mat& quantized, CV_OUT Mat& dst);
/**
* \brief Debug function to draw linemod features
* @param img
* @param templates see @ref Detector::addTemplate
* @param tl template bbox top-left offset see @ref Detector::addTemplate
* @param size marker size see @ref cv::drawMarker
*/
CV_EXPORTS_W void drawFeatures(InputOutputArray img, const std::vector<Template>& templates, const Point2i& tl, int size = 10);
/**
* \brief Represents a successful template match.
*/
struct CV_EXPORTS_W_SIMPLE Match
{
CV_WRAP Match()
{
}
CV_WRAP Match(int x, int y, float similarity, const String& class_id, int template_id);
/// Sort matches with high similarity to the front
bool operator<(const Match& rhs) const
{
// Secondarily sort on template_id for the sake of duplicate removal
if (similarity != rhs.similarity)
return similarity > rhs.similarity;
else
return template_id < rhs.template_id;
}
bool operator==(const Match& rhs) const
{
return x == rhs.x && y == rhs.y && similarity == rhs.similarity && class_id == rhs.class_id;
}
CV_PROP_RW int x;
CV_PROP_RW int y;
CV_PROP_RW float similarity;
CV_PROP_RW String class_id;
CV_PROP_RW int template_id;
};
inline
Match::Match(int _x, int _y, float _similarity, const String& _class_id, int _template_id)
: x(_x), y(_y), similarity(_similarity), class_id(_class_id), template_id(_template_id)
{}
/**
* \brief Object detector using the LINE template matching algorithm with any set of
* modalities.
*/
class CV_EXPORTS_W Detector
{
public:
/**
* \brief Empty constructor, initialize with read().
*/
CV_WRAP Detector();
/**
* \brief Constructor.
*
* \param modalities Modalities to use (color gradients, depth normals, ...).
* \param T_pyramid Value of the sampling step T at each pyramid level. The
* number of pyramid levels is T_pyramid.size().
*/
CV_WRAP Detector(const std::vector< Ptr<Modality> >& modalities, const std::vector<int>& T_pyramid);
/**
* \brief Detect objects by template matching.
*
* Matches globally at the lowest pyramid level, then refines locally stepping up the pyramid.
*
* \param sources Source images, one for each modality.
* \param threshold Similarity threshold, a percentage between 0 and 100.
* \param[out] matches Template matches, sorted by similarity score.
* \param class_ids If non-empty, only search for the desired object classes.
* \param[out] quantized_images Optionally return vector<Mat> of quantized images.
* \param masks The masks for consideration during matching. The masks should be CV_8UC1
* where 255 represents a valid pixel. If non-empty, the vector must be
* the same size as sources. Each element must be
* empty or the same size as its corresponding source.
*/
CV_WRAP void match(const std::vector<Mat>& sources, float threshold, CV_OUT std::vector<Match>& matches,
const std::vector<String>& class_ids = std::vector<String>(),
OutputArrayOfArrays quantized_images = noArray(),
const std::vector<Mat>& masks = std::vector<Mat>()) const;
/**
* \brief Add new object template.
*
* \param sources Source images, one for each modality.
* \param class_id Object class ID.
* \param object_mask Mask separating object from background.
* \param[out] bounding_box Optionally return bounding box of the extracted features.
*
* \return Template ID, or -1 if failed to extract a valid template.
*/
CV_WRAP int addTemplate(const std::vector<Mat>& sources, const String& class_id,
const Mat& object_mask, CV_OUT Rect* bounding_box = NULL);
/**
* \brief Add a new object template computed by external means.
*/
CV_WRAP int addSyntheticTemplate(const std::vector<Template>& templates, const String& class_id);
/**
* \brief Get the modalities used by this detector.
*
* You are not permitted to add/remove modalities, but you may dynamic_cast them to
* tweak parameters.
*/
CV_WRAP const std::vector< Ptr<Modality> >& getModalities() const { return modalities; }
/**
* \brief Get sampling step T at pyramid_level.
*/
CV_WRAP int getT(int pyramid_level) const { return T_at_level[pyramid_level]; }
/**
* \brief Get number of pyramid levels used by this detector.
*/
CV_WRAP int pyramidLevels() const { return pyramid_levels; }
/**
* \brief Get the template pyramid identified by template_id.
*
* For example, with 2 modalities (Gradient, Normal) and two pyramid levels
* (L0, L1), the order is (GradientL0, NormalL0, GradientL1, NormalL1).
*/
CV_WRAP const std::vector<Template>& getTemplates(const String& class_id, int template_id) const;
CV_WRAP int numTemplates() const;
CV_WRAP int numTemplates(const String& class_id) const;
CV_WRAP int numClasses() const { return static_cast<int>(class_templates.size()); }
CV_WRAP std::vector<String> classIds() const;
CV_WRAP void read(const FileNode& fn);
void write(FileStorage& fs) const;
String readClass(const FileNode& fn, const String &class_id_override = "");
void writeClass(const String& class_id, FileStorage& fs) const;
CV_WRAP void readClasses(const std::vector<String>& class_ids,
const String& format = "templates_%s.yml.gz");
CV_WRAP void writeClasses(const String& format = "templates_%s.yml.gz") const;
protected:
std::vector< Ptr<Modality> > modalities;
int pyramid_levels;
std::vector<int> T_at_level;
typedef std::vector<Template> TemplatePyramid;
typedef std::map<String, std::vector<TemplatePyramid> > TemplatesMap;
TemplatesMap class_templates;
typedef std::vector<Mat> LinearMemories;
// Indexed as [pyramid level][modality][quantized label]
typedef std::vector< std::vector<LinearMemories> > LinearMemoryPyramid;
void matchClass(const LinearMemoryPyramid& lm_pyramid,
const std::vector<Size>& sizes,
float threshold, std::vector<Match>& matches,
const String& class_id,
const std::vector<TemplatePyramid>& template_pyramids) const;
};
/**
* \brief Factory function for detector using LINE algorithm with color gradients.
*
* Default parameter settings suitable for VGA images.
*/
CV_EXPORTS_W Ptr<linemod::Detector> getDefaultLINE();
/**
* \brief Factory function for detector using LINE-MOD algorithm with color gradients
* and depth normals.
*
* Default parameter settings suitable for VGA images.
*/
CV_EXPORTS_W Ptr<linemod::Detector> getDefaultLINEMOD();
//! @}
} // namespace linemod
} // namespace cv
#endif // __OPENCV_OBJDETECT_LINEMOD_HPP__
modules/rgbd/misc/python/pyopencv_linemod.hpp
+60
View File
@@ -0,0 +1,60 @@
#ifdef HAVE_OPENCV_RGBD
#include "opencv2/core/saturate.hpp"
template<> struct pyopencvVecConverter<linemod::Match>
{
static bool to(PyObject* obj, std::vector<linemod::Match>& value, const ArgInfo& info)
{
return pyopencv_to_generic_vec(obj, value, info);
}
static PyObject* from(const std::vector<linemod::Match>& value)
{
return pyopencv_from_generic_vec(value);
}
};
template<> struct pyopencvVecConverter<linemod::Template>
{
static bool to(PyObject* obj, std::vector<linemod::Template>& value, const ArgInfo& info)
{
return pyopencv_to_generic_vec(obj, value, info);
}
static PyObject* from(const std::vector<linemod::Template>& value)
{
return pyopencv_from_generic_vec(value);
}
};
template<> struct pyopencvVecConverter<linemod::Feature>
{
static bool to(PyObject* obj, std::vector<linemod::Feature>& value, const ArgInfo& info)
{
return pyopencv_to_generic_vec(obj, value, info);
}
static PyObject* from(const std::vector<linemod::Feature>& value)
{
return pyopencv_from_generic_vec(value);
}
};
template<> struct pyopencvVecConverter<Ptr<linemod::Modality> >
{
static bool to(PyObject* obj, std::vector<Ptr<linemod::Modality> >& value, const ArgInfo& info)
{
return pyopencv_to_generic_vec(obj, value, info);
}
static PyObject* from(const std::vector<Ptr<linemod::Modality> >& value)
{
return pyopencv_from_generic_vec(value);
}
};
typedef std::vector<linemod::Match> vector_Match;
typedef std::vector<linemod::Template> vector_Template;
typedef std::vector<linemod::Feature> vector_Feature;
typedef std::vector<Ptr<linemod::Modality> > vector_Ptr_Modality;
#endif
modules/rgbd/samples/colored_kinfu_demo.cpp
+276
View File
@@ -0,0 +1,276 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include <iostream>
#include <fstream>
#include <opencv2/imgproc.hpp>
#include <opencv2/geometry.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/rgbd/colored_kinfu.hpp>
#include "io_utils.hpp"
using namespace cv;
using namespace cv::kinfu;
using namespace cv::colored_kinfu;
using namespace cv::io_utils;
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif
#ifdef HAVE_OPENCV_VIZ
const std::string vizWindowName = "cloud";
struct PauseCallbackArgs
{
PauseCallbackArgs(ColoredKinFu& _kf) : kf(_kf)
{ }
ColoredKinFu& kf;
};
void pauseCallback(const viz::MouseEvent& me, void* args);
void pauseCallback(const viz::MouseEvent& me, void* args)
{
if(me.type == viz::MouseEvent::Type::MouseMove ||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
me.type == viz::MouseEvent::Type::MouseScrollUp)
{
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
viz::Viz3d window(vizWindowName);
UMat rendered;
pca.kf.render(rendered, window.getViewerPose().matrix);
imshow("render", rendered);
waitKey(1);
}
}
#endif
static const char* keys =
{
"{help h usage ? | | print this message }"
"{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb images }"
"{camera |0| Index of depth camera to be used as a depth source }"
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
" in coarse mode points and normals are displayed }"
"{idle | | Do not run KinFu, just display depth frames }"
"{record | | Write depth frames to specified file list"
" (the same format as for the 'depth' key) }"
};
static const std::string message =
"\nThis demo uses live depth input or RGB-D dataset taken from"
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
"\nto demonstrate KinectFusion implementation \n";
int main(int argc, char **argv)
{
bool coarse = false;
bool idle = false;
std::string recordPath;
CommandLineParser parser(argc, argv, keys);
parser.about(message);
if(!parser.check())
{
parser.printMessage();
parser.printErrors();
return -1;
}
if(parser.has("help"))
{
parser.printMessage();
return 0;
}
if(parser.has("coarse"))
{
coarse = true;
}
if(parser.has("record"))
{
recordPath = parser.get<String>("record");
}
if(parser.has("idle"))
{
idle = true;
}
Ptr<DepthSource> ds;
Ptr<RGBSource> rgbs;
if (parser.has("depth"))
ds = makePtr<DepthSource>(parser.get<String>("depth") + "/depth.txt");
else
ds = makePtr<DepthSource>(parser.get<int>("camera"));
//TODO: intrinsics for camera
rgbs = makePtr<RGBSource>(parser.get<String>("depth") + "/rgb.txt");
if (ds->empty())
{
std::cerr << "Failed to open depth source" << std::endl;
parser.printMessage();
return -1;
}
Ptr<DepthWriter> depthWriter;
Ptr<RGBWriter> rgbWriter;
if (!recordPath.empty())
{
depthWriter = makePtr<DepthWriter>(recordPath);
rgbWriter = makePtr<RGBWriter>(recordPath);
}
Ptr<colored_kinfu::Params> params;
Ptr<ColoredKinFu> kf;
params = colored_kinfu::Params::coloredTSDFParams(coarse);
// These params can be different for each depth sensor
ds->updateParams(*params);
rgbs->updateParams(*params);
// Enables OpenCL explicitly (by default can be switched-off)
cv::setUseOptimized(false);
// Scene-specific params should be tuned for each scene individually
//float cubeSize = 1.f;
//params->voxelSize = cubeSize/params->volumeDims[0]; //meters
//params->tsdf_trunc_dist = 0.01f; //meters
//params->icpDistThresh = 0.01f; //meters
//params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
//params->tsdf_max_weight = 16;
if(!idle)
kf = ColoredKinFu::create(params);
#ifdef HAVE_OPENCV_VIZ
cv::viz::Viz3d window(vizWindowName);
window.setViewerPose(Affine3f::Identity());
bool pause = false;
#endif
UMat rendered;
UMat points;
UMat normals;
int64 prevTime = getTickCount();
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
{
if(depthWriter)
depthWriter->append(frame);
UMat rgb_frame = rgbs->getRGB();
#ifdef HAVE_OPENCV_VIZ
if(pause)
{
// doesn't happen in idle mode
kf->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(kf->getParams().volumePose));
PauseCallbackArgs pca(*kf);
window.registerMouseCallback(pauseCallback, (void*)&pca);
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
"Close the window or press Q to resume"), Point()));
window.spin();
window.removeWidget("text");
window.removeWidget("cloud");
window.removeWidget("normals");
window.registerMouseCallback(0);
}
pause = false;
}
else
#endif
{
UMat cvt8;
float depthFactor = params->depthFactor;
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
if(!idle)
{
imshow("depth", cvt8);
imshow("rgb", rgb_frame);
if(!kf->update(frame, rgb_frame))
{
kf->reset();
}
#ifdef HAVE_OPENCV_VIZ
else
{
if(coarse)
{
kf->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
}
}
//window.showWidget("worldAxes", viz::WCoordinateSystem());
Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(kf->getParams().volumePose));
window.setViewerPose(kf->getPose());
window.spinOnce(1, true);
}
#endif
kf->render(rendered);
}
else
{
rendered = cvt8;
}
}
int64 newTime = getTickCount();
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
(int)(getTickFrequency()/(newTime - prevTime))),
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
prevTime = newTime;
imshow("render", rendered);
int c = waitKey(1);
switch (c)
{
case 'r':
if(!idle)
kf->reset();
break;
case 'q':
return 0;
#ifdef HAVE_OPENCV_VIZ
case 'p':
if(!idle)
pause = true;
#endif
default:
break;
}
}
return 0;
}
modules/rgbd/samples/dynafu_demo.cpp
+304
View File
@@ -0,0 +1,304 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#define CV_LOG_STRIP_LEVEL CV_LOG_LEVEL_VERBOSE
#include <iostream>
#include <fstream>
#include <opencv2/imgproc.hpp>
#include <opencv2/geometry.hpp>
#include <opencv2/ptcloud.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/rgbd.hpp>
#include "io_utils.hpp"
using namespace cv;
using namespace cv::dynafu;
using namespace cv::io_utils;
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif
#ifdef HAVE_OPENCV_VIZ
const std::string vizWindowName = "cloud";
struct PauseCallbackArgs
{
PauseCallbackArgs(DynaFu& _df) : df(_df)
{ }
DynaFu& df;
};
void pauseCallback(const viz::MouseEvent& me, void* args);
void pauseCallback(const viz::MouseEvent& me, void* args)
{
if(me.type == viz::MouseEvent::Type::MouseMove ||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
me.type == viz::MouseEvent::Type::MouseScrollUp)
{
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
viz::Viz3d window(vizWindowName);
UMat rendered;
pca.df.render(rendered, window.getViewerPose().matrix);
imshow("render", rendered);
waitKey(1);
}
}
#endif
static const char* keys =
{
"{help h usage ? | | print this message }"
"{depth | | Path to depth.txt file listing a set of depth images }"
"{camera |0| Index of depth camera to be used as a depth source }"
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
" in coarse mode points and normals are displayed }"
"{idle | | Do not run DynaFu, just display depth frames }"
"{record | | Write depth frames to specified file list"
" (the same format as for the 'depth' key) }"
};
static const std::string message =
"\nThis demo uses live depth input or RGB-D dataset taken from"
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
"\nto demonstrate KinectFusion implementation \n";
int main(int argc, char **argv)
{
bool coarse = false;
bool idle = false;
std::string recordPath;
CommandLineParser parser(argc, argv, keys);
parser.about(message);
if(!parser.check())
{
parser.printMessage();
parser.printErrors();
return -1;
}
if(parser.has("help"))
{
parser.printMessage();
return 0;
}
if(parser.has("coarse"))
{
coarse = true;
}
if(parser.has("record"))
{
recordPath = parser.get<String>("record");
}
if(parser.has("idle"))
{
idle = true;
}
Ptr<DepthSource> ds;
if (parser.has("depth"))
ds = makePtr<DepthSource>(parser.get<String>("depth"));
else
ds = makePtr<DepthSource>(parser.get<int>("camera"));
if (ds->empty())
{
std::cerr << "Failed to open depth source" << std::endl;
parser.printMessage();
return -1;
}
Ptr<DepthWriter> depthWriter;
if(!recordPath.empty())
depthWriter = makePtr<DepthWriter>(recordPath);
Ptr<kinfu::Params> params;
Ptr<DynaFu> df;
if(coarse)
params = kinfu::Params::coarseParams();
else
params = kinfu::Params::defaultParams();
// These params can be different for each depth sensor
ds->updateParams(*params);
// Enables OpenCL explicitly (by default can be switched-off)
cv::setUseOptimized(false);
// Scene-specific params should be tuned for each scene individually
//params->volumePose = params->volumePose.translate(Vec3f(0.f, 0.f, 0.5f));
//params->tsdf_max_weight = 16;
namedWindow("OpenGL Window", WINDOW_OPENGL);
resizeWindow("OpenGL Window", 1, 1);
if(!idle)
df = DynaFu::create(params);
#ifdef HAVE_OPENCV_VIZ
cv::viz::Viz3d window(vizWindowName);
window.setViewerPose(Affine3f::Identity());
bool pause = false;
#endif
UMat rendered;
UMat points;
UMat normals;
int64 prevTime = getTickCount();
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
{
Mat depthImg, vertImg, normImg;
setOpenGlContext("OpenGL Window");
df->renderSurface(depthImg, vertImg, normImg);
if(!depthImg.empty())
{
UMat depthCvt8, vertCvt8, normCvt8;
convertScaleAbs(depthImg, depthCvt8, 0.33*255);
vertImg.convertTo(vertCvt8, CV_8UC3, 255);
normImg.convertTo(normCvt8, CV_8UC3, 255);
imshow("Surface prediction", depthCvt8);
imshow("vertex prediction", vertCvt8);
imshow("normal prediction", normCvt8);
}
if(depthWriter)
depthWriter->append(frame);
#ifdef HAVE_OPENCV_VIZ
if(pause)
{
// doesn't happen in idle mode
df->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
Vec3d volSize = df->getParams().voxelSize*Vec3d(df->getParams().volumeDims);
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(df->getParams().volumePose));
PauseCallbackArgs pca(*df);
window.registerMouseCallback(pauseCallback, (void*)&pca);
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
"Close the window or press Q to resume"), Point()));
window.spin();
window.removeWidget("text");
//window.removeWidget("cloud");
//window.removeWidget("normals");
window.registerMouseCallback(0);
}
pause = false;
}
else
#endif
{
UMat cvt8;
float depthFactor = params->depthFactor;
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
if(!idle)
{
imshow("depth", cvt8);
if(!df->update(frame))
{
df->reset();
std::cout << "reset" << std::endl;
}
#ifdef HAVE_OPENCV_VIZ
else
{
Mat meshCloud, meshEdges, meshPoly;
df->marchCubes(meshCloud, meshEdges);
for(int i = 0; i < meshEdges.size().height; i += 3)
{
meshPoly.push_back<int>(3);
meshPoly.push_back<int>(meshEdges.at<int>(i, 0));
meshPoly.push_back<int>(meshEdges.at<int>(i+1, 0));
meshPoly.push_back<int>(meshEdges.at<int>(i+2, 0));
}
viz::WMesh mesh(meshCloud.t(), meshPoly);
window.showWidget("mesh", mesh);
if(coarse)
{
df->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
//window.showWidget("cloud", cloudWidget);
//window.showWidget("normals", cloudNormals);
if(!df->getNodesPos().empty())
{
viz::WCloud nodeCloud(df->getNodesPos(), viz::Color::red());
nodeCloud.setRenderingProperty(viz::POINT_SIZE, 4);
window.showWidget("nodes", nodeCloud);
}
}
}
//window.showWidget("worldAxes", viz::WCoordinateSystem());
Vec3d volSize = df->getParams().voxelSize*df->getParams().volumeDims;
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(df->getParams().volumePose));
window.setViewerPose(df->getPose());
window.spinOnce(1, true);
}
#endif
df->render(rendered);
}
else
{
rendered = cvt8;
}
}
int64 newTime = getTickCount();
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
(int)(getTickFrequency()/(newTime - prevTime))),
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
prevTime = newTime;
imshow("render", rendered);
int c = waitKey(1);
switch (c)
{
case 'r':
if(!idle)
df->reset();
break;
case 'q':
return 0;
#ifdef HAVE_OPENCV_VIZ
case 'p':
if(!idle)
pause = true;
#endif
default:
break;
}
}
return 0;
}
modules/rgbd/samples/io_utils.hpp
+629
View File
@@ -0,0 +1,629 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef OPENCV_RGBS_IO_UTILS_HPP
#define OPENCV_RGBS_IO_UTILS_HPP
#include <fstream>
#include <iostream>
#include <opencv2/geometry.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/rgbd/kinfu.hpp>
#include <opencv2/rgbd/large_kinfu.hpp>
#include <opencv2/rgbd/colored_kinfu.hpp>
namespace cv
{
namespace io_utils
{
static std::vector<std::string> readDepth(const std::string& fileList)
{
std::vector<std::string> v;
std::fstream file(fileList);
if (!file.is_open())
throw std::runtime_error("Failed to read depth list");
std::string dir;
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
while (!file.eof())
{
std::string s, imgPath;
std::getline(file, s);
if (s.empty() || s[0] == '#')
continue;
std::stringstream ss;
ss << s;
double thumb;
ss >> thumb >> imgPath;
v.push_back(dir + '/' + imgPath);
}
return v;
}
struct DepthWriter
{
DepthWriter(std::string fileList) : file(fileList, std::ios::out), count(0), dir()
{
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
if (!file.is_open())
throw std::runtime_error("Failed to write depth list");
file << "# depth maps saved from device" << std::endl;
file << "# useless_number filename" << std::endl;
}
void append(InputArray _depth)
{
Mat depth = _depth.getMat();
std::string depthFname = cv::format("%04d.png", count);
std::string fullDepthFname = dir + '/' + depthFname;
if (!imwrite(fullDepthFname, depth))
throw std::runtime_error("Failed to write depth to file " + fullDepthFname);
file << count++ << " " << depthFname << std::endl;
}
std::fstream file;
int count;
std::string dir;
};
namespace Kinect2Params
{
static const Size depth_frameSize = Size(512, 424);
// approximate values, no guarantee to be correct
static const float depth_focal = 366.1f;
static const float depth_cx = 258.2f;
static const float depth_cy = 204.f;
static const float depth_k1 = 0.12f;
static const float depth_k2 = -0.34f;
static const float depth_k3 = 0.12f;
static const Size rgb_frameSize = Size(640, 480);
static const float rgb_focal = 525.0f;
static const float rgb_cx = 319.5f;
static const float rgb_cy = 239.5f;
static const float rgb_k1 = 0.0f;
static const float rgb_k2 = 0.0f;
static const float rgb_k3 = 0.0f;
}; // namespace Kinect2Params
namespace AstraParams
{
static const Size depth_frameSize = Size(640, 480);
// approximate values, no guarantee to be correct
static const float depth_fx = 535.4f;
static const float depth_fy = 539.2f;
static const float depth_cx = 320.1f;
static const float depth_cy = 247.6f;
static const float depth_k1 = 0.0f;
static const float depth_k2 = 0.0f;
static const float depth_k3 = 0.0f;
static const Size rgb_frameSize = Size(640, 480);
static const float rgb_focal = 525.0f;
static const float rgb_cx = 319.5f;
static const float rgb_cy = 239.5f;
static const float rgb_k1 = 0.0f;
static const float rgb_k2 = 0.0f;
static const float rgb_k3 = 0.0f;
}; // namespace Kinect2Params
struct DepthSource
{
public:
enum Type
{
DEPTH_LIST,
DEPTH_KINECT2_LIST,
DEPTH_KINECT2,
DEPTH_REALSENSE,
DEPTH_ASTRA
};
DepthSource(int cam) : DepthSource("", cam) {}
DepthSource(String fileListName) : DepthSource(fileListName, -1) {}
DepthSource(String fileListName, int cam)
: depthFileList(fileListName.empty() ? std::vector<std::string>()
: readDepth(fileListName)),
frameIdx(0),
undistortMap1(),
undistortMap2()
{
if (cam >= 0)
{
vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
if (vc.isOpened())
{
if(cam == 20)
sourceType = Type::DEPTH_ASTRA;
else
sourceType = Type::DEPTH_KINECT2;
}
else
{
vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
if (vc.isOpened())
{
sourceType = Type::DEPTH_REALSENSE;
}
}
}
else
{
vc = VideoCapture();
sourceType = Type::DEPTH_KINECT2_LIST;
}
}
UMat getDepth()
{
UMat out;
if (!vc.isOpened())
{
if (frameIdx < depthFileList.size())
{
Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH);
f.copyTo(out);
}
else
{
return UMat();
}
}
else
{
vc.grab();
switch (sourceType)
{
case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break;
case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break;
default:
// unknown depth source
vc.retrieve(out);
}
// workaround for Kinect 2
if (sourceType == Type::DEPTH_KINECT2)
{
out = out(Rect(Point(), Kinect2Params::depth_frameSize));
UMat outCopy;
// linear remap adds gradient between valid and invalid pixels
// which causes garbage, use nearest instead
remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST);
cv::flip(outCopy, out, 1);
}
}
if (out.empty())
throw std::runtime_error("Matrix is empty");
return out;
}
bool empty() { return depthFileList.empty() && !(vc.isOpened()); }
void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor)
{
if (vc.isOpened())
{
// this should be set in according to user's depth sensor
int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH);
int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT);
// it's recommended to calibrate sensor to obtain its intrinsics
float fx, fy, cx, cy;
float depthFactor = 1000.f;
Size frameSize;
if (sourceType == Type::DEPTH_KINECT2)
{
fx = fy = Kinect2Params::depth_focal;
cx = Kinect2Params::depth_cx;
cy = Kinect2Params::depth_cy;
frameSize = Kinect2Params::depth_frameSize;
}
else if (sourceType == Type::DEPTH_ASTRA)
{
fx = AstraParams::depth_fx;
fy = AstraParams::depth_fy;
cx = AstraParams::depth_cx;
cy = AstraParams::depth_cy;
frameSize = AstraParams::depth_frameSize;
}
else
{
if (sourceType == Type::DEPTH_REALSENSE)
{
fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ);
fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT);
depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE);
}
else
{
fx = fy =
(float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);
}
cx = w / 2 - 0.5f;
cy = h / 2 - 0.5f;
frameSize = Size(w, h);
}
Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1);
_intrinsics = camMatrix;
_frameSize = frameSize;
_depthFactor = depthFactor;
}
}
void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist,
Affine3f& _volumePose, float& _depthTruncateThreshold)
{
float volumeSize = 3.0f;
_depthTruncateThreshold = 0.0f;
// RealSense has shorter depth range, some params should be tuned
if (sourceType == Type::DEPTH_REALSENSE)
{
volumeSize = 1.f;
_voxelSize = volumeSize / _resolution[0];
_tsdfTruncDist = 0.01f;
_depthTruncateThreshold = 2.5f;
}
_volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.05f));
}
void updateICPParams(float& _icpDistThresh, float& _bilateralSigmaDepth)
{
_icpDistThresh = 0.1f;
_bilateralSigmaDepth = 0.04f;
// RealSense has shorter depth range, some params should be tuned
if (sourceType == Type::DEPTH_REALSENSE)
{
_icpDistThresh = 0.01f;
_bilateralSigmaDepth = 0.01f;
}
}
void updateParams(large_kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
auto& volParams = params.volumeParams;
Vec3i volResolution(volParams.resolutionX,
volParams.resolutionY,
volParams.resolutionZ);
Affine3f volPose(Matx44f(volParams.pose));
updateVolumeParams(volResolution, volParams.voxelSize, volParams.tsdfTruncDist, volPose,
params.truncateThreshold);
volParams.pose = Mat(volPose.matrix);
updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth);
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
void updateParams(kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
Affine3f volumePose(params.volumePose);
updateVolumeParams(params.volumeDims, params.voxelSize,
params.tsdf_trunc_dist, volumePose, params.truncateThreshold);
params.volumePose = volumePose.matrix;
updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth);
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
void updateParams(colored_kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
Affine3f volumePose(params.volumePose);
updateVolumeParams(params.volumeDims, params.voxelSize,
params.tsdf_trunc_dist, volumePose, params.truncateThreshold);
params.volumePose = volumePose.matrix;
updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth);
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
std::vector<std::string> depthFileList;
size_t frameIdx;
VideoCapture vc;
UMat undistortMap1, undistortMap2;
Type sourceType;
};
static std::vector<std::string> readRGB(const std::string& fileList)
{
std::vector<std::string> v;
std::fstream file(fileList);
if (!file.is_open())
throw std::runtime_error("Failed to read rgb list");
std::string dir;
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
while (!file.eof())
{
std::string s, imgPath;
std::getline(file, s);
if (s.empty() || s[0] == '#')
continue;
std::stringstream ss;
ss << s;
double thumb;
ss >> thumb >> imgPath;
v.push_back(dir + '/' + imgPath);
}
return v;
}
struct RGBWriter
{
RGBWriter(std::string fileList) : file(fileList, std::ios::out), count(0), dir()
{
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
if (!file.is_open())
throw std::runtime_error("Failed to write rgb list");
file << "# rgb maps saved from device" << std::endl;
file << "# useless_number filename" << std::endl;
}
void append(InputArray _rgb)
{
Mat rgb = _rgb.getMat();
std::string rgbFname = cv::format("%04d.png", count);
std::string fullRGBFname = dir + '/' + rgbFname;
if (!imwrite(fullRGBFname, rgb))
throw std::runtime_error("Failed to write rgb to file " + fullRGBFname);
file << count++ << " " << rgbFname << std::endl;
}
std::fstream file;
int count;
std::string dir;
};
struct RGBSource
{
public:
enum Type
{
RGB_LIST,
RGB_KINECT2_LIST,
RGB_KINECT2,
RGB_REALSENSE,
RGB_ASTRA
};
RGBSource(int cam) : RGBSource("", cam) {}
RGBSource(String fileListName) : RGBSource(fileListName, -1) {}
RGBSource(String fileListName, int cam)
: rgbFileList(fileListName.empty() ? std::vector<std::string>()
: readRGB(fileListName)),
frameIdx(0),
undistortMap1(),
undistortMap2()
{
if (cam >= 0)
{
vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
if (vc.isOpened())
{
if(cam == 20)
sourceType = Type::RGB_ASTRA;
else
sourceType = Type::RGB_KINECT2;
}
else
{
vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
if (vc.isOpened())
{
sourceType = Type::RGB_REALSENSE;
}
}
}
else
{
vc = VideoCapture();
sourceType = Type::RGB_KINECT2_LIST;
}
}
UMat getRGB()
{
UMat out;
if (!vc.isOpened())
{
if (frameIdx < rgbFileList.size())
{
Mat f = cv::imread(rgbFileList[frameIdx++], IMREAD_COLOR);
f.copyTo(out);
}
else
{
return UMat();
}
}
else
{
vc.grab();
switch (sourceType)
{
case Type::RGB_KINECT2: vc.retrieve(out, CAP_OPENNI_BGR_IMAGE); break;
case Type::RGB_REALSENSE: vc.retrieve(out, CAP_INTELPERC_IMAGE); break;
default:
// unknown rgb source
vc.retrieve(out);
}
// workaround for Kinect 2
if (sourceType == Type::RGB_KINECT2)
{
out = out(Rect(Point(), Kinect2Params::rgb_frameSize));
UMat outCopy;
// linear remap adds gradient between valid and invalid pixels
// which causes garbage, use nearest instead
remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST);
cv::flip(outCopy, out, 1);
}
}
if (out.empty())
throw std::runtime_error("Matrix is empty");
return out;
}
bool empty() { return rgbFileList.empty() && !(vc.isOpened()); }
void updateIntrinsics(Matx33f& _rgb_intrinsics, Size& _rgb_frameSize)
{
if (vc.isOpened())
{
// this should be set in according to user's rgb sensor
int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH);
int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT);
// it's recommended to calibrate sensor to obtain its intrinsics
float rgb_fx, rgb_fy, rgb_cx, rgb_cy;
Size rgb_frameSize;
if (sourceType == Type::RGB_KINECT2)
{
rgb_fx = rgb_fy = Kinect2Params::rgb_focal;
rgb_cx = Kinect2Params::rgb_cx;
rgb_cy = Kinect2Params::rgb_cy;
rgb_frameSize = Kinect2Params::rgb_frameSize;
}
else if (sourceType == Type::RGB_ASTRA)
{
rgb_fx = rgb_fy = AstraParams::rgb_focal;
rgb_cx = AstraParams::rgb_cx;
rgb_cy = AstraParams::rgb_cy;
rgb_frameSize = AstraParams::rgb_frameSize;
}
else
{
// TODO: replace to rgb types
rgb_fx = rgb_fy = Kinect2Params::rgb_focal;
rgb_cx = Kinect2Params::rgb_cx;
rgb_cy = Kinect2Params::rgb_cy;
rgb_frameSize = Size(w, h);
}
Matx33f rgb_camMatrix = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1);
_rgb_intrinsics = rgb_camMatrix;
_rgb_frameSize = rgb_frameSize;
}
}
void updateVolumeParams(const Vec3i&, float&, float&, Affine3f&)
{
// TODO: do this settings for rgb image
}
void updateICPParams(float&)
{
// TODO: do this settings for rgb image icp
}
void updateParams(colored_kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.rgb_intr, params.rgb_frameSize);
Affine3f volumePose(params.volumePose);
updateVolumeParams(params.volumeDims, params.voxelSize,
params.tsdf_trunc_dist, volumePose);
params.volumePose = volumePose.matrix;
updateICPParams(params.icpDistThresh);
if (sourceType == Type::RGB_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::rgb_k1;
distCoeffs(1) = Kinect2Params::rgb_k2;
distCoeffs(4) = Kinect2Params::rgb_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
std::vector<std::string> rgbFileList;
size_t frameIdx;
VideoCapture vc;
UMat undistortMap1, undistortMap2;
Type sourceType;
};
} // namespace io_utils
} // namespace cv
#endif /* ifndef OPENCV_RGBS_IO_UTILS_HPP */
modules/rgbd/samples/kinfu_demo.cpp
+277
View File
@@ -0,0 +1,277 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include <iostream>
#include <fstream>
#include <opencv2/imgproc.hpp>
#include <opencv2/geometry.hpp>
#include <opencv2/ptcloud.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/rgbd/kinfu.hpp>
#include "io_utils.hpp"
using namespace cv;
using namespace cv::kinfu;
using namespace cv::io_utils;
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif
#ifdef HAVE_OPENCV_VIZ
const std::string vizWindowName = "cloud";
struct PauseCallbackArgs
{
PauseCallbackArgs(KinFu& _kf) : kf(_kf)
{ }
KinFu& kf;
};
void pauseCallback(const viz::MouseEvent& me, void* args);
void pauseCallback(const viz::MouseEvent& me, void* args)
{
if(me.type == viz::MouseEvent::Type::MouseMove ||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
me.type == viz::MouseEvent::Type::MouseScrollUp)
{
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
viz::Viz3d window(vizWindowName);
UMat rendered;
pca.kf.render(rendered, window.getViewerPose().matrix);
imshow("render", rendered);
waitKey(1);
}
}
#endif
static const char* keys =
{
"{help h usage ? | | print this message }"
"{depth | | Path to depth.txt file listing a set of depth images }"
"{camera |0| Index of depth camera to be used as a depth source }"
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
" in coarse mode points and normals are displayed }"
"{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}"
"{idle | | Do not run KinFu, just display depth frames }"
"{record | | Write depth frames to specified file list"
" (the same format as for the 'depth' key) }"
};
static const std::string message =
"\nThis demo uses live depth input or RGB-D dataset taken from"
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
"\nto demonstrate KinectFusion implementation \n";
int main(int argc, char **argv)
{
bool coarse = false;
bool idle = false;
bool useHashTSDF = false;
std::string recordPath;
CommandLineParser parser(argc, argv, keys);
parser.about(message);
if(!parser.check())
{
parser.printMessage();
parser.printErrors();
return -1;
}
if(parser.has("help"))
{
parser.printMessage();
return 0;
}
if(parser.has("coarse"))
{
coarse = true;
}
if(parser.has("record"))
{
recordPath = parser.get<String>("record");
}
if(parser.has("useHashTSDF"))
{
useHashTSDF = true;
}
if(parser.has("idle"))
{
idle = true;
}
Ptr<DepthSource> ds;
if (parser.has("depth"))
ds = makePtr<DepthSource>(parser.get<String>("depth"));
else
ds = makePtr<DepthSource>(parser.get<int>("camera"));
if (ds->empty())
{
std::cerr << "Failed to open depth source" << std::endl;
parser.printMessage();
return -1;
}
Ptr<DepthWriter> depthWriter;
if(!recordPath.empty())
depthWriter = makePtr<DepthWriter>(recordPath);
Ptr<Params> params;
Ptr<KinFu> kf;
if(coarse)
params = Params::coarseParams();
else
params = Params::defaultParams();
if(useHashTSDF)
params = Params::hashTSDFParams(coarse);
// These params can be different for each depth sensor
ds->updateParams(*params);
// Enables OpenCL explicitly (by default can be switched-off)
cv::setUseOptimized(true);
// Scene-specific params should be tuned for each scene individually
//float cubeSize = 1.f;
//params->voxelSize = cubeSize/params->volumeDims[0]; //meters
//params->tsdf_trunc_dist = 0.01f; //meters
//params->icpDistThresh = 0.01f; //meters
//params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
//params->tsdf_max_weight = 16;
if(!idle)
kf = KinFu::create(params);
#ifdef HAVE_OPENCV_VIZ
cv::viz::Viz3d window(vizWindowName);
window.setViewerPose(Affine3f::Identity());
bool pause = false;
#endif
UMat rendered;
UMat points;
UMat normals;
int64 prevTime = getTickCount();
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
{
if(depthWriter)
depthWriter->append(frame);
#ifdef HAVE_OPENCV_VIZ
if(pause)
{
// doesn't happen in idle mode
kf->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(kf->getParams().volumePose));
PauseCallbackArgs pca(*kf);
window.registerMouseCallback(pauseCallback, (void*)&pca);
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
"Close the window or press Q to resume"), Point()));
window.spin();
window.removeWidget("text");
window.removeWidget("cloud");
window.removeWidget("normals");
window.registerMouseCallback(0);
}
pause = false;
}
else
#endif
{
UMat cvt8;
float depthFactor = params->depthFactor;
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
if(!idle)
{
imshow("depth", cvt8);
if(!kf->update(frame))
{
kf->reset();
std::cout << "reset" << std::endl;
}
#ifdef HAVE_OPENCV_VIZ
else
{
if(coarse)
{
kf->getCloud(points, normals);
if(!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
}
}
//window.showWidget("worldAxes", viz::WCoordinateSystem());
Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
window.showWidget("cube", viz::WCube(Vec3d::all(0),
volSize),
Affine3f(kf->getParams().volumePose));
window.setViewerPose(kf->getPose());
window.spinOnce(1, true);
}
#endif
kf->render(rendered);
}
else
{
rendered = cvt8;
}
}
int64 newTime = getTickCount();
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
(int)(getTickFrequency()/(newTime - prevTime))),
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
prevTime = newTime;
imshow("render", rendered);
int c = waitKey(1);
switch (c)
{
case 'r':
if(!idle)
kf->reset();
break;
case 'q':
return 0;
#ifdef HAVE_OPENCV_VIZ
case 'p':
if(!idle)
pause = true;
#endif
default:
break;
}
}
return 0;
}
modules/rgbd/samples/kinfu_demo.py
+58
View File
@@ -0,0 +1,58 @@
import numpy as np
import cv2 as cv
import sys
from argparse import ArgumentParser
def get_depth_list(folder):
f = open(folder + '/depth.txt', 'r')
rgb = [folder + '/' + s for s in f.read().split() if s.endswith('.png')]
return rgb
def kinfu_demo():
parser = ArgumentParser()
parser.add_argument(
"-i", "--input", help="Required. Path to folder with a input image file", required=True, type=str)
parser.add_argument(
"-t", "--large_kinfu", help="Required. Name of KinFu type", required=False, type=str)
parser.add_argument(
"-ocl", "--use_opencl", help="Required. Flag of OpenCL use", required=False, type=int, default=1)
args = parser.parse_args()
print("Args: ", args)
cv.ocl.setUseOpenCL(args.use_opencl)
if (args.large_kinfu == None or args.large_kinfu == "0"):
params = cv.kinfu_Params.defaultParams()
kf = cv.kinfu_KinFu.create(params)
elif (args.large_kinfu == "1"):
params = cv.kinfu_Params.hashTSDFParams(False)
kf = cv.kinfu_KinFu.create(params)
else:
raise ValueError("Incorrect kinfu type name")
depth_list = get_depth_list(args.input)
for path in depth_list:
image = cv.imread(path, cv.IMREAD_ANYDEPTH)
(height, width) = image.shape
cv.imshow('input', image)
size = height, width, 4
cvt8 = np.zeros(size, dtype=np.uint8)
if not kf.update(image):
kf.reset()
else:
kf.render(cvt8)
cv.imshow('render', cvt8)
cv.pollKey()
cv.waitKey(0)
if __name__ == '__main__':
print(__doc__)
kinfu_demo()
cv.destroyAllWindows()
modules/rgbd/samples/large_kinfu_demo.cpp
+263
View File
@@ -0,0 +1,263 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
// module's directory
#include <fstream>
#include <iostream>
#include <opencv2/geometry.hpp>
#include <opencv2/ptcloud.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/rgbd/large_kinfu.hpp>
#include "io_utils.hpp"
using namespace cv;
using namespace cv::kinfu;
using namespace cv::large_kinfu;
using namespace cv::io_utils;
#ifdef HAVE_OPENCV_VIZ
#include <opencv2/viz.hpp>
#endif
#ifdef HAVE_OPENCV_VIZ
const std::string vizWindowName = "cloud";
struct PauseCallbackArgs
{
PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {}
LargeKinfu& largeKinfu;
};
void pauseCallback(const viz::MouseEvent& me, void* args);
void pauseCallback(const viz::MouseEvent& me, void* args)
{
if (me.type == viz::MouseEvent::Type::MouseMove ||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
me.type == viz::MouseEvent::Type::MouseScrollUp)
{
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
viz::Viz3d window(vizWindowName);
UMat rendered;
pca.largeKinfu.render(rendered, window.getViewerPose().matrix);
imshow("render", rendered);
waitKey(1);
}
}
#endif
static const char* keys = {
"{help h usage ? | | print this message }"
"{depth | | Path to depth.txt file listing a set of depth images }"
"{camera |0| Index of depth camera to be used as a depth source }"
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
" in coarse mode points and normals are displayed }"
"{idle | | Do not run LargeKinfu, just display depth frames }"
"{record | | Write depth frames to specified file list"
" (the same format as for the 'depth' key) }"
};
static const std::string message =
"\nThis demo uses live depth input or RGB-D dataset taken from"
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
"\nto demonstrate Submap based large environment reconstruction"
"\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger "
"reconstructions by default\n";
int main(int argc, char** argv)
{
bool coarse = false;
bool idle = false;
std::string recordPath;
CommandLineParser parser(argc, argv, keys);
parser.about(message);
if (!parser.check())
{
parser.printMessage();
parser.printErrors();
return -1;
}
if (parser.has("help"))
{
parser.printMessage();
return 0;
}
if (parser.has("coarse"))
{
coarse = true;
}
if (parser.has("record"))
{
recordPath = parser.get<String>("record");
}
if (parser.has("idle"))
{
idle = true;
}
Ptr<DepthSource> ds;
if (parser.has("depth"))
ds = makePtr<DepthSource>(parser.get<String>("depth"));
else
ds = makePtr<DepthSource>(parser.get<int>("camera"));
if (ds->empty())
{
std::cerr << "Failed to open depth source" << std::endl;
parser.printMessage();
return -1;
}
Ptr<DepthWriter> depthWriter;
if (!recordPath.empty())
depthWriter = makePtr<DepthWriter>(recordPath);
Ptr<large_kinfu::Params> params;
Ptr<LargeKinfu> largeKinfu;
params = large_kinfu::Params::hashTSDFParams(coarse);
// These params can be different for each depth sensor
ds->updateParams(*params);
cv::setUseOptimized(true);
if (!idle)
largeKinfu = LargeKinfu::create(params);
const auto& volParams = largeKinfu->getParams().volumeParams;
#ifdef HAVE_OPENCV_VIZ
cv::viz::Viz3d window(vizWindowName);
window.setViewerPose(Affine3f::Identity());
bool pause = false;
#endif
UMat rendered;
UMat points;
UMat normals;
int64 prevTime = getTickCount();
for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
{
if (depthWriter)
depthWriter->append(frame);
Vec3i volResolution(volParams.resolutionX,
volParams.resolutionY,
volParams.resolutionZ);
Affine3f volPose(Matx44f(volParams.pose));
#ifdef HAVE_OPENCV_VIZ
if (pause)
{
// doesn't happen in idle mode
largeKinfu->getCloud(points, normals);
if (!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05,
viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
Vec3d volSize = volParams.voxelSize * Vec3d(volResolution);
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
PauseCallbackArgs pca(*largeKinfu);
window.registerMouseCallback(pauseCallback, (void*)&pca);
window.showWidget("text",
viz::WText(cv::String("Move camera in this window. "
"Close the window or press Q to resume"),
Point()));
window.spin();
window.removeWidget("text");
window.removeWidget("cloud");
window.removeWidget("normals");
window.registerMouseCallback(0);
}
pause = false;
}
else
#endif
{
UMat cvt8;
float depthFactor = params->depthFactor;
convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor);
if (!idle)
{
imshow("depth", cvt8);
if (!largeKinfu->update(frame))
{
largeKinfu->reset();
std::cout << "reset" << std::endl;
}
#ifdef HAVE_OPENCV_VIZ
else
{
if (coarse)
{
largeKinfu->getCloud(points, normals);
if (!points.empty() && !normals.empty())
{
viz::WCloud cloudWidget(points, viz::Color::white());
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1,
/*scale*/ 0.05, viz::Color::gray());
window.showWidget("cloud", cloudWidget);
window.showWidget("normals", cloudNormals);
}
}
// window.showWidget("worldAxes", viz::WCoordinateSystem());
Vec3d volSize = volParams.voxelSize * volResolution;
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
window.setViewerPose(largeKinfu->getPose());
window.spinOnce(1, true);
}
#endif
largeKinfu->render(rendered);
}
else
{
rendered = cvt8;
}
}
int64 newTime = getTickCount();
putText(rendered,
cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
(int)(getTickFrequency() / (newTime - prevTime))),
Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
prevTime = newTime;
imshow("render", rendered);
int c = waitKey(1);
switch (c)
{
case 'r':
if (!idle)
largeKinfu->reset();
break;
case 'q': return 0;
#ifdef HAVE_OPENCV_VIZ
case 'p':
if (!idle)
pause = true;
#endif
default: break;
}
}
return 0;
}
modules/rgbd/src/colored_kinfu.cpp
+409
View File
@@ -0,0 +1,409 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "precomp.hpp"
#include "opencv2/geometry.hpp"
namespace cv {
namespace colored_kinfu {
using namespace kinfu;
void Params::setInitialVolumePose(Matx33f R, Vec3f t)
{
setInitialVolumePose(Affine3f(R,t).matrix);
}
void Params::setInitialVolumePose(Matx44f homogen_tf)
{
volumePose = homogen_tf;
}
Ptr<Params> Params::defaultParams()
{
Params p;
p.frameSize = Size(640, 480);
p.volumeKind = VolumeType::TSDF;
float fx, fy, cx, cy;
fx = fy = 525.f;
cx = p.frameSize.width/2 - 0.5f;
cy = p.frameSize.height/2 - 0.5f;
p.intr = Matx33f(fx, 0, cx,
0, fy, cy,
0, 0, 1);
float rgb_fx, rgb_fy, rgb_cx, rgb_cy;
rgb_fx = 515.0f;
rgb_fy = 550.0f;
rgb_cx = 319.5f;
rgb_cy = 239.5f;
p.rgb_intr = Matx33f(rgb_fx, 0, rgb_cx,
0, rgb_fy, rgb_cy,
0, 0, 1);
// 5000 for the 16-bit PNG files
// 1 for the 32-bit float images in the ROS bag files
p.depthFactor = 5000;
// sigma_depth is scaled by depthFactor when calling bilateral filter
p.bilateral_sigma_depth = 0.04f; //meter
p.bilateral_sigma_spatial = 4.5; //pixels
p.bilateral_kernel_size = 7; //pixels
p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians
p.icpDistThresh = 0.1f; // meters
p.icpIterations = {10, 5, 4};
p.pyramidLevels = (int)p.icpIterations.size();
p.tsdf_min_camera_movement = 0.f; //meters, disabled
p.volumeDims = Vec3i::all(512); //number of voxels
float volSize = 3.f;
p.voxelSize = volSize/512.f; //meters
// default pose of volume cube
p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)).matrix;
p.tsdf_trunc_dist = 7 * p.voxelSize; // about 0.04f in meters
p.tsdf_max_weight = 64; //frames
p.raycast_step_factor = 0.25f; //in voxel sizes
// gradient delta factor is fixed at 1.0f and is not used
//p.gradient_delta_factor = 0.5f; //in voxel sizes
//p.lightPose = p.volume_pose.translation()/4; //meters
p.lightPose = Vec3f::all(0.f); //meters
// depth truncation is not used by default but can be useful in some scenes
p.truncateThreshold = 0.f; //meters
return makePtr<Params>(p);
}
Ptr<Params> Params::coarseParams()
{
Ptr<Params> p = defaultParams();
p->icpIterations = {5, 3, 2};
p->pyramidLevels = (int)p->icpIterations.size();
float volSize = 3.f;
p->volumeDims = Vec3i::all(128); //number of voxels
p->voxelSize = volSize/128.f;
p->tsdf_trunc_dist = 2 * p->voxelSize; // 0.04f in meters
p->raycast_step_factor = 0.75f; //in voxel sizes
return p;
}
Ptr<Params> Params::hashTSDFParams(bool isCoarse)
{
Ptr<Params> p;
if (isCoarse)
p = coarseParams();
else
p = defaultParams();
p->volumeKind = VolumeType::HashTSDF;
p->truncateThreshold = 4.f;
return p;
}
Ptr<Params> Params::coloredTSDFParams(bool isCoarse)
{
Ptr<Params> p;
if(isCoarse)
p = coarseParams();
else
p = defaultParams();
p->volumeKind = VolumeType::ColorTSDF;
return p;
}
// MatType should be Mat or UMat
template< typename MatType>
class ColoredKinFuImpl : public ColoredKinFu
{
public:
ColoredKinFuImpl(const Params& _params);
virtual ~ColoredKinFuImpl();
static VolumeSettings paramsToSettings(const Params& params);
const Params& getParams() const CV_OVERRIDE;
void render(OutputArray image) const CV_OVERRIDE;
void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
void getPoints(OutputArray points) const CV_OVERRIDE;
void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
void reset() CV_OVERRIDE;
Affine3f getPose() const CV_OVERRIDE;
bool update(InputArray depth, InputArray rgb) CV_OVERRIDE;
bool updateT(const MatType& depth, const MatType& rgb);
private:
Params params;
VolumeSettings settings;
Odometry icp;
Volume volume;
int frameCounter;
Matx44f pose;
// Mutable because it contains cache updated by icp.prepareFrame() during rendering
mutable OdometryFrame renderFrame;
OdometryFrame prevFrame;
};
template< typename MatType >
VolumeSettings ColoredKinFuImpl<MatType>::paramsToSettings(const Params& params)
{
VolumeSettings vs(VolumeType::TSDF);
vs.setVoxelSize(params.voxelSize);
vs.setVolumePose(params.volumePose);
vs.setRaycastStepFactor(params.raycast_step_factor);
vs.setTsdfTruncateDistance(params.tsdf_trunc_dist);
vs.setMaxWeight(params.tsdf_max_weight);
vs.setMaxDepth(params.truncateThreshold);
vs.setCameraIntegrateIntrinsics(params.intr);
vs.setDepthFactor(params.depthFactor);
vs.setVolumeResolution(params.volumeDims);
return vs;
}
template< typename MatType >
ColoredKinFuImpl<MatType>::ColoredKinFuImpl(const Params &_params) :
params(_params),
settings(paramsToSettings(params)),
volume(VolumeType::ColorTSDF, settings)
{
OdometrySettings ods;
ods.setCameraMatrix(Mat(params.intr));
ods.setMaxRotation(30.f);
ods.setMaxTranslation(params.voxelSize * (float)params.volumeDims[0] * 0.5f);
ods.setIterCounts(params.icpIterations);
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
reset();
}
template< typename MatType >
void ColoredKinFuImpl<MatType >::reset()
{
frameCounter = 0;
pose = Affine3f::Identity().matrix;
volume.reset();
}
template< typename MatType >
ColoredKinFuImpl<MatType>::~ColoredKinFuImpl()
{ }
template< typename MatType >
const Params& ColoredKinFuImpl<MatType>::getParams() const
{
return params;
}
template< typename MatType >
Affine3f ColoredKinFuImpl<MatType>::getPose() const
{
return pose;
}
template<>
bool ColoredKinFuImpl<Mat>::update(InputArray _depth, InputArray _rgb)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
Mat depth;
Mat rgb;
if(_depth.isUMat())
{
_depth.copyTo(depth);
_rgb.copyTo(rgb);
return updateT(depth, rgb);
}
else
{
return updateT(_depth.getMat(), _rgb.getMat());
}
}
template<>
bool ColoredKinFuImpl<UMat>::update(InputArray _depth, InputArray _rgb)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
UMat depth;
UMat rgb;
if(!_depth.isUMat())
{
_depth.copyTo(depth);
_rgb.copyTo(rgb);
return updateT(depth, rgb);
}
else
{
return updateT(_depth.getUMat(), _rgb.getUMat());
}
}
template< typename MatType >
bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _rgb)
{
CV_TRACE_FUNCTION();
MatType depth, rgb;
if(_depth.type() != DEPTH_TYPE)
_depth.convertTo(depth, DEPTH_TYPE);
else
depth = _depth;
if (_rgb.type() != COLOR_TYPE)
{
MatType rgb_tmp;
std::vector<MatType> channels;
_rgb.convertTo(rgb_tmp, COLOR_TYPE);
cv::split(rgb_tmp, channels);
// we use 4-channel RGB0 image
// for vectorization simplicity
channels.push_back(MatType::zeros(channels[0].size(), CV_32F));
merge(channels, rgb);
}
else
rgb = _rgb;
OdometryFrame newFrame(depth, rgb);
if(frameCounter == 0)
{
icp.prepareFrame(newFrame);
// use depth instead of distance
volume.integrate(depth, rgb, pose);
}
else
{
Affine3f affine;
Matx44d mrt;
Mat Rt;
icp.prepareFrames(newFrame, prevFrame);
bool success = icp.compute(newFrame, prevFrame, Rt);
if (!success)
return false;
affine.matrix = Matx44f(Rt);
pose = (Affine3f(pose) * affine).matrix;
float rnorm = (float)cv::norm(affine.rvec());
float tnorm = (float)cv::norm(affine.translation());
// We do not integrate volume if camera does not move
if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement)
{
// use depth instead of distance
volume.integrate(depth, rgb, pose);
}
MatType points, normals, colors;
volume.raycast(pose, points, normals, colors);
std::vector<MatType> pch(3);
split(points, pch);
newFrame = OdometryFrame(pch[2], colors, noArray(), normals);
}
renderFrame = newFrame;
prevFrame = newFrame;
frameCounter++;
return true;
}
template< typename MatType >
void ColoredKinFuImpl<MatType>::render(OutputArray image) const
{
CV_TRACE_FUNCTION();
MatType pts, nrm, rgb;
icp.prepareFrame(renderFrame);
renderFrame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
renderFrame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
renderFrame.getImage(rgb);
detail::renderPointsNormalsColors(pts, nrm, rgb, image);
}
template< typename MatType >
void ColoredKinFuImpl<MatType>::render(OutputArray image, const Matx44f& _cameraPose) const
{
CV_TRACE_FUNCTION();
Affine3f cameraPose(_cameraPose);
MatType points, normals, colors;
volume.raycast(_cameraPose, points, normals, colors);
detail::renderPointsNormalsColors(points, normals, colors, image);
}
template< typename MatType >
void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
{
volume.fetchPointsNormals(p, n);
}
template< typename MatType >
void ColoredKinFuImpl<MatType>::getPoints(OutputArray points) const
{
volume.fetchPointsNormals(points, noArray());
}
template< typename MatType >
void ColoredKinFuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
{
volume.fetchNormals(points, normals);
}
// importing class
#ifdef OPENCV_ENABLE_NONFREE
Ptr<ColoredKinFu> ColoredKinFu::create(const Ptr<Params>& params)
{
CV_Assert((int)params->icpIterations.size() == params->pyramidLevels);
CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1);
return makePtr< ColoredKinFuImpl<Mat> >(*params);
}
#else
Ptr<ColoredKinFu> ColoredKinFu::create(const Ptr<Params>& /* params */)
{
CV_Error(Error::StsNotImplemented,
"This algorithm is patented and is excluded in this configuration; "
"Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library");
}
#endif
ColoredKinFu::~ColoredKinFu() {}
} // namespace kinfu
} // namespace cv
modules/rgbd/src/dqb.cpp
+187
View File
@@ -0,0 +1,187 @@
#include "dqb.hpp"
namespace cv {
namespace dynafu {
Quaternion::Quaternion() : coeff(Vec4f(0.f, 0.f, 0.f, 0.f))
{}
Quaternion::Quaternion(float w, float i, float j, float k) : coeff(Vec4f(w, i, j, k))
{}
Quaternion::Quaternion(const Affine3f& r)
{
// Compute trace of matrix
float T = (float)trace(r.matrix);
float S, X, Y, Z, W;
if ( T > 0.00000001f ) // to avoid large distortions!
{
S = sqrt(T) * 2.f;
X = (r.matrix(1, 2) - r.matrix(2, 1)) / S;
Y = (r.matrix(2, 0) - r.matrix(0, 2)) / S;
Z = (r.matrix(0, 1) - r.matrix(1, 0)) / S;
W = 0.25f * S;
}
else
{
if (r.matrix(0, 0) > r.matrix(1, 1) && r.matrix(0, 0) > r.matrix(2, 2))
{
// Column 0 :
S = sqrt(1.0f + r.matrix(0,0) - r.matrix(1,1) - r.matrix(2,2)) * 2.f;
X = 0.25f * S;
Y = (r.matrix(1, 0) + r.matrix(0, 1)) / S;
Z = (r.matrix(0, 2) + r.matrix(2, 0)) / S;
W = (r.matrix(2, 1) - r.matrix(1, 2)) / S;
}
else if (r.matrix(1, 1) > r.matrix(2, 2))
{
// Column 1 :
S = sqrt(1.0f + r.matrix(1,1) - r.matrix(0,0) - r.matrix(2,2)) * 2.f;
X = (r.matrix(1, 0) + r.matrix(0, 1)) / S;
Y = 0.25f * S;
Z = (r.matrix(2, 1) + r.matrix(1, 2)) / S;
W = (r.matrix(0, 2) - r.matrix(2, 0)) / S;
}
else
{ // Column 2 :
S = sqrt( 1.0f + r.matrix(2, 2) - r.matrix(0, 0) - r.matrix(1, 1)) * 2.f;
X = (r.matrix(0, 2) + r.matrix(2, 0)) / S;
Y = (r.matrix(2, 1) + r.matrix(1, 2)) / S;
Z = 0.25f * S;
W = (r.matrix(1,0) - r.matrix(0, 1)) / S;
}
}
coeff = Vec4f(W, -X, -Y, -Z);
}
Affine3f Quaternion::getRotation() const
{
float W = coeff[0], X = -coeff[1], Y = -coeff[2], Z = -coeff[3];
float xx = X * X, xy = X * Y, xz = X * Z, xw = X * W;
float yy = Y * Y, yz = Y * Z, yw = Y * W, zz = Z * Z;
float zw = Z * W;
Matx33f rot(1.f - 2.f * (yy + zz), 2.f * (xy + zw), 2.f * (xz - yw),
2.f * (xy - zw), 1.f - 2.f * (xx + zz), 2.f * (yz + xw),
2.f * (xz + yw), 2.f * (yz - xw), 1.f - 2.f * (xx + yy));
Affine3f Rt = Affine3f(rot, Vec3f::all(0));
return Rt;
}
Quaternion operator*(float a, const Quaternion& q)
{
Vec4f newQ = a*q.coeff;
return Quaternion(newQ[0], newQ[1], newQ[2], newQ[3]);
}
Quaternion operator*(const Quaternion& q, float a)
{
return a*q;
}
Quaternion operator/(const Quaternion& q, float a)
{
Vec4f newQ = q.coeff/a;
return Quaternion(newQ[0], newQ[1], newQ[2], newQ[3]);
}
Quaternion operator+(const Quaternion& q1, const Quaternion& q2)
{
Vec4f newQ = q1.coeff + q2.coeff;
return Quaternion(newQ[0], newQ[1], newQ[2], newQ[3]);
}
Quaternion& operator+=(Quaternion& q1, const Quaternion& q2)
{
q1.coeff += q2.coeff;
return q1;
}
Quaternion& operator/=(Quaternion& q, float a)
{
q.coeff /= a;
return q;
}
DualQuaternion::DualQuaternion() : q0(), qe()
{}
DualQuaternion::DualQuaternion(const Affine3f& rt)
{
q0 = Quaternion(rt);
Vec3f t = rt.translation();
float w = -0.5f*( t[0] * q0.i() + t[1] * q0.j() + t[2] * q0.k());
float i = 0.5f*( t[0] * q0.w() + t[1] * q0.k() - t[2] * q0.j());
float j = 0.5f*(-t[0] * q0.k() + t[1] * q0.w() + t[2] * q0.i());
float k = 0.5f*( t[0] * q0.j() - t[1] * q0.i() + t[2] * q0.w());
qe = Quaternion(w, i, j, k);
}
DualQuaternion::DualQuaternion(Quaternion& _q0, Quaternion& _qe) : q0(_q0), qe(_qe)
{}
void DualQuaternion::normalize()
{
float n = q0.normalize();
qe /= n;
}
DualQuaternion& operator+=(DualQuaternion& q1, const DualQuaternion& q2)
{
q1.q0 += q2.q0;
q1.qe += q2.qe;
return q1;
}
DualQuaternion operator*(float a, const DualQuaternion& q)
{
Quaternion newQ0 = a*q.q0;
Quaternion newQe = a*q.qe;
return DualQuaternion(newQ0, newQe);
}
Affine3f DualQuaternion::getAffine() const
{
float norm = q0.norm();
Affine3f Rt = (q0/norm).getRotation();
Vec3f t(0.f, 0.f, 0.f);
t[0] = 2.f*(-qe.w()*q0.i() + qe.i()*q0.w() - qe.j()*q0.k() + qe.k()*q0.j()) / norm;
t[1] = 2.f*(-qe.w()*q0.j() + qe.i()*q0.k() + qe.j()*q0.w() - qe.k()*q0.i()) / norm;
t[2] = 2.f*(-qe.w()*q0.k() - qe.i()*q0.j() + qe.j()*q0.i() + qe.k()*q0.w()) / norm;
return Rt.translate(t);
}
DualQuaternion DQB(std::vector<float>& weights, std::vector<DualQuaternion>& quats)
{
size_t n = weights.size();
DualQuaternion blended;
for(size_t i = 0; i < n; i++)
blended += weights[i] * quats[i];
blended.normalize();
return blended;
}
Affine3f DQB(std::vector<float>& weights, std::vector<Affine3f>& transforms)
{
size_t n = transforms.size();
std::vector<DualQuaternion> quats(n);
std::transform(transforms.begin(), transforms.end(),
quats.begin(), [](const Affine3f& rt){return DualQuaternion(rt);});
DualQuaternion blended = DQB(weights, quats);
return blended.getAffine();
}
} // namespace dynafu
} // namespace cv
modules/rgbd/src/dqb.hpp
+99
View File
@@ -0,0 +1,99 @@
/*
The code for Dual Quaternion Blending provided here is a modified
version of the sample codes by Arkan.
Copyright (c) 2019 Arkan
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#ifndef __OPENCV_RGBD_DQB_HPP__
#define __OPENCV_RGBD_DQB_HPP__
#include "opencv2/core.hpp"
#include "opencv2/core/affine.hpp"
namespace cv {
namespace dynafu {
class Quaternion
{
public:
Quaternion();
Quaternion(float w, float i, float j, float k);
// Generate a quaternion from rotation of a Rt matrix.
Quaternion(const Affine3f& r);
float normalize()
{
float n = (float)cv::norm(coeff);
coeff /= n;
return n;
}
Affine3f getRotation() const;
float w() const {return coeff[0];}
float i() const {return coeff[1];}
float j() const {return coeff[2];}
float k() const {return coeff[3];}
float norm() const {return (float)cv::norm(coeff);}
friend Quaternion operator*(float a, const Quaternion& q);
friend Quaternion operator*(const Quaternion& q, float a);
friend Quaternion operator/(const Quaternion& q, float a);
friend Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
friend Quaternion& operator+=(Quaternion& q1, const Quaternion& q2);
friend Quaternion& operator/=(Quaternion& q, float a);
private:
// w, i, j, k coefficients
Vec4f coeff;
};
class DualQuaternion
{
public:
DualQuaternion();
DualQuaternion(const Affine3f& Rt);
DualQuaternion(Quaternion& q0, Quaternion& qe);
void normalize();
friend DualQuaternion& operator+=(DualQuaternion& q1, const DualQuaternion& q2);
friend DualQuaternion operator*(float a, const DualQuaternion& q);
Affine3f getAffine() const;
private:
Quaternion q0; // rotation quaternion
Quaternion qe; // translation quaternion
};
DualQuaternion DQB(std::vector<float>& weights, std::vector<DualQuaternion>& quats);
Affine3f DQB(std::vector<float>& weights, std::vector<Affine3f>& transforms);
} // namespace dynafu
} // namespace cv
#endif
modules/rgbd/src/dynafu.cpp
+537
View File
@@ -0,0 +1,537 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "precomp.hpp"
#include "dynafu_tsdf.hpp"
#include "warpfield.hpp"
#include "nonrigid_icp.hpp"
#include "opencv2/core/opengl.hpp"
#ifdef HAVE_OPENGL
#define GL_GLEXT_PROTOTYPES
#ifdef __APPLE__
# include <OpenGL/gl.h>
#else
#ifdef _WIN32
# define WIN32_LEAN_AND_MEAN
# include <windows.h>
#endif
# include <GL/gl.h>
#endif
// GL Extention definitions missing from standard Win32 gl.h
#if defined(_WIN32) && !defined(GL_RENDERBUFFER_EXT)
#define GL_COLOR_ATTACHMENT0_EXT 0x8CE0
#define GL_DEPTH_ATTACHMENT_EXT 0x8D00
#define GL_FRAMEBUFFER_EXT 0x8D40
#define GL_RENDERBUFFER_EXT 0x8D41
namespace {
PROC _wglGetProcAddress(const char *name)
{
auto proc = wglGetProcAddress(name);
if (!proc)
CV_Error(cv::Error::OpenGlApiCallError, cv::format("Can't load OpenGL extension [%s]", name) );
return proc;
}
void glGenFramebuffersEXT(GLsizei n, GLuint *framebuffers)
{
static auto proc = reinterpret_cast<void(*)(GLsizei, GLuint*)>(_wglGetProcAddress(__func__));
proc(n, framebuffers);
}
void glGenRenderbuffersEXT(GLsizei n, GLuint *renderbuffers)
{
static auto proc = reinterpret_cast<void(*)(GLsizei, GLuint*)>(_wglGetProcAddress(__func__));
proc(n, renderbuffers);
}
void glBindRenderbufferEXT(GLenum target, GLuint renderbuffer)
{
static auto proc = reinterpret_cast<void(*)(GLenum, GLuint)>(_wglGetProcAddress(__func__));
proc(target, renderbuffer);
}
void glBindFramebufferEXT(GLenum target, GLuint framebuffer)
{
static auto proc = reinterpret_cast<void(*)(GLenum, GLuint)>(_wglGetProcAddress(__func__));
proc(target, framebuffer);
}
void glFramebufferRenderbufferEXT(GLenum target, GLenum attachment, GLenum renderbuffertarget, GLuint renderbuffer)
{
static auto proc = reinterpret_cast<void(*)(GLenum, GLenum, GLenum, GLuint)>(_wglGetProcAddress(__func__));
proc(target, attachment, renderbuffertarget, renderbuffer);
}
void glRenderbufferStorageEXT(GLenum target, GLenum internalformat, GLsizei width, GLsizei height)
{
static auto proc = reinterpret_cast<void(*)(GLenum, GLenum, GLsizei, GLsizei)>(_wglGetProcAddress(__func__));
proc(target, internalformat, width, height);
}
} // anonymous namespace
#endif // defined(_WIN32) && !defined(GL_RENDERBUFFER_EXT)
#else
# define NO_OGL_ERR CV_Error(cv::Error::OpenGlNotSupported, \
"OpenGL support not enabled. Please rebuild the library with OpenGL support");
#endif
namespace cv {
namespace dynafu {
using namespace kinfu;
// T should be Mat or UMat
template< typename T >
class DynaFuImpl : public DynaFu
{
public:
DynaFuImpl(const Params& _params);
virtual ~DynaFuImpl();
const Params& getParams() const CV_OVERRIDE;
void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
void getPoints(OutputArray points) const CV_OVERRIDE;
void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
void reset() CV_OVERRIDE;
Affine3f getPose() const CV_OVERRIDE;
bool update(InputArray depth) CV_OVERRIDE;
bool updateT(const T& depth);
std::vector<Point3f> getNodesPos() const CV_OVERRIDE;
void marchCubes(OutputArray vertices, OutputArray edges) const CV_OVERRIDE;
void renderSurface(OutputArray depthImage, OutputArray vertImage, OutputArray normImage, bool warp=true) CV_OVERRIDE;
private:
Params params;
Odometry icp;
cv::Ptr<NonRigidICP> dynafuICP;
cv::Ptr<TSDFVolume> volume;
int frameCounter;
Affine3f pose;
OdometryFrame frame;
WarpField warpfield;
#ifdef HAVE_OPENGL
ogl::Arrays arr;
ogl::Buffer idx;
#endif
void drawScene(OutputArray depthImg, OutputArray shadedImg);
};
template< typename T>
std::vector<Point3f> DynaFuImpl<T>::getNodesPos() const
{
NodeVectorType nv = warpfield.getNodes();
std::vector<Point3f> nodesPos;
for(auto n: nv)
nodesPos.push_back(n->pos);
return nodesPos;
}
template< typename T >
DynaFuImpl<T>::DynaFuImpl(const Params &_params) :
params(_params),
dynafuICP(makeNonRigidICP(params.intr, volume, 2)),
volume(makeTSDFVolume(params.volumeDims, params.voxelSize, params.volumePose,
params.tsdf_trunc_dist, params.tsdf_max_weight,
params.raycast_step_factor)),
warpfield()
{
#ifdef HAVE_OPENGL
// Bind framebuffer for off-screen rendering
unsigned int fbo_depth;
glGenRenderbuffersEXT(1, &fbo_depth);
glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, fbo_depth);
glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_DEPTH_COMPONENT, params.frameSize.width, params.frameSize.height);
unsigned int fbo;
glGenFramebuffersEXT(1, &fbo);
glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fbo);
glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, fbo_depth);
// Make a color attachment to this framebuffer
unsigned int fbo_color;
glGenRenderbuffersEXT(1, &fbo_color);
glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, fbo_color);
glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, GL_RGB, params.frameSize.width, params.frameSize.height);
glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_RENDERBUFFER_EXT, fbo_color);
#endif
OdometrySettings ods;
ods.setCameraMatrix(Mat(params.intr));
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
//icp = FastICPOdometry::create(Mat(params.intr), params.icpDistThresh, params.icpAngleThresh,
// params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
// params.icpIterations, params.depthFactor, params.truncateThreshold);
reset();
}
template< typename T >
void DynaFuImpl<T>::drawScene(OutputArray depthImage, OutputArray shadedImage)
{
#ifdef HAVE_OPENGL
glViewport(0, 0, params.frameSize.width, params.frameSize.height);
glEnable(GL_DEPTH_TEST);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
float fovX = params.frameSize.width/params.intr(0, 0);
float fovY = params.frameSize.height/params.intr(1, 1);
Vec3f t;
t = Affine3f(params.volumePose).translation();
double nearZ = t[2];
double farZ = params.volumeDims[2] * params.voxelSize + nearZ;
// Define viewing volume
glFrustum(-nearZ*fovX/2, nearZ*fovX/2, -nearZ*fovY/2, nearZ*fovY/2, nearZ, farZ);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScalef(1.f, 1.f, -1.f); //Flip Z as camera points towards -ve Z axis
ogl::render(arr, idx, ogl::TRIANGLES);
Mat depthData(params.frameSize.height, params.frameSize.width, CV_32F);
Mat shadeData(params.frameSize.height, params.frameSize.width, CV_32FC3);
glReadPixels(0, 0, params.frameSize.width, params.frameSize.height, GL_DEPTH_COMPONENT, GL_FLOAT, depthData.ptr());
glReadPixels(0, 0, params.frameSize.width, params.frameSize.height, GL_RGB, GL_FLOAT, shadeData.ptr());
// linearise depth
for(auto it = depthData.begin<float>(); it != depthData.end<float>(); ++it)
{
*it = farZ * nearZ / ((*it)*(nearZ - farZ) + farZ);
if(*it >= farZ)
*it = std::numeric_limits<float>::quiet_NaN();
}
if(depthImage.needed()) {
depthData.copyTo(depthImage);
}
if(shadedImage.needed()) {
shadeData.copyTo(shadedImage);
}
#else
CV_UNUSED(depthImage);
CV_UNUSED(shadedImage);
NO_OGL_ERR;
#endif
}
template< typename T >
void DynaFuImpl<T>::reset()
{
frameCounter = 0;
pose = Affine3f::Identity();
warpfield.setAllRT(Affine3f::Identity());
volume->reset();
}
template< typename T >
DynaFuImpl<T>::~DynaFuImpl()
{ }
template< typename T >
const Params& DynaFuImpl<T>::getParams() const
{
return params;
}
template< typename T >
Affine3f DynaFuImpl<T>::getPose() const
{
return pose;
}
template<>
bool DynaFuImpl<Mat>::update(InputArray _depth)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
Mat depth;
if(_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getMat());
}
}
template<>
bool DynaFuImpl<UMat>::update(InputArray _depth)
{
CV_TRACE_FUNCTION();
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
UMat depth;
if(!_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getUMat());
}
}
template< typename T >
bool DynaFuImpl<T>::updateT(const T& _depth)
{
CV_TRACE_FUNCTION();
T depth;
if(_depth.type() != DEPTH_TYPE)
_depth.convertTo(depth, DEPTH_TYPE);
else
depth = _depth;
OdometryFrame newFrame(depth);
//icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);
if(frameCounter == 0)
{
// use depth instead of distance
volume->integrate(depth, params.depthFactor, pose, params.intr, makePtr<WarpField>(warpfield));
frame = newFrame;
warpfield.setAllRT(Affine3f::Identity());
}
else
{
UMat wfPoints;
volume->fetchPointsNormals(wfPoints, noArray(), true);
warpfield.updateNodesFromPoints(wfPoints);
Mat _depthRender, estdDepth, _vertRender, _normRender;
renderSurface(_depthRender, _vertRender, _normRender, false);
_depthRender.convertTo(estdDepth, DEPTH_TYPE);
OdometryFrame estdFrame(estdDepth);
//icp->setDepthFactor(1.f);
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
//icp->setDepthFactor(params.depthFactor);
frame = estdFrame;
Affine3f affine;
Matx44d mrt;
Mat Rt;
icp.prepareFrames(frame, newFrame);
bool success = icp.compute(newFrame, frame, Rt);
if (!success)
return false;
affine = Affine3f(Rt);
pose = pose * affine;
for(int iter = 0; iter < 1; iter++)
{
renderSurface(_depthRender, _vertRender, _normRender);
_depthRender.convertTo(estdDepth, DEPTH_TYPE);
estdFrame = OdometryFrame(estdDepth);
icp.prepareFrame(estdFrame);
//estdFrame = OdometryFrame::create(estdDepth, noArray(), noArray(), noArray(), -1);
//icp->setDepthFactor(1.f);
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
//icp->setDepthFactor(params.depthFactor);
T estdPts, estdNrm, newPts, newNrm;
estdFrame.getPyramidAt(estdPts, OdometryFramePyramidType::PYR_CLOUD, 0);
estdFrame.getPyramidAt(estdNrm, OdometryFramePyramidType::PYR_NORM, 0);
newFrame.getPyramidAt(newPts, OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.getPyramidAt(newNrm, OdometryFramePyramidType::PYR_NORM, 0);
success = dynafuICP->estimateWarpNodes(warpfield, pose, _vertRender, estdPts, estdNrm, newPts, newNrm);
if(!success)
return false;
}
float rnorm = (float)cv::norm(affine.rvec());
float tnorm = (float)cv::norm(affine.translation());
// We do not integrate volume if camera does not move
if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement)
{
// use depth instead of distance
volume->integrate(depth, params.depthFactor, pose, params.intr, makePtr<WarpField>(warpfield));
}
}
std::cout << "Frame# " << frameCounter++ << std::endl;
return true;
}
template< typename T >
void DynaFuImpl<T>::render(OutputArray image, const Matx44f& _cameraPose) const
{
CV_TRACE_FUNCTION();
Affine3f cameraPose(_cameraPose);
const Affine3f id = Affine3f::Identity();
if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) ||
(cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation()))
{
T pts, nrm;
frame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
frame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
detail::renderPointsNormals(pts, nrm, image, params.lightPose);
}
else
{
T points, normals;
volume->raycast(cameraPose, params.intr, params.frameSize, points, normals);
detail::renderPointsNormals(points, normals, image, params.lightPose);
}
}
template< typename T >
void DynaFuImpl<T>::getCloud(OutputArray p, OutputArray n) const
{
volume->fetchPointsNormals(p, n);
}
template< typename T >
void DynaFuImpl<T>::getPoints(OutputArray points) const
{
volume->fetchPointsNormals(points, noArray());
}
template< typename T >
void DynaFuImpl<T>::getNormals(InputArray points, OutputArray normals) const
{
volume->fetchNormals(points, normals);
}
template< typename T >
void DynaFuImpl<T>::marchCubes(OutputArray vertices, OutputArray edges) const
{
volume->marchCubes(vertices, edges);
}
template<typename T>
void DynaFuImpl<T>::renderSurface(OutputArray depthImage, OutputArray vertImage, OutputArray normImage, bool warp)
{
#ifdef HAVE_OPENGL
Mat _vertices, vertices, normals, meshIdx;
volume->marchCubes(_vertices, noArray());
if(_vertices.empty()) return;
_vertices.convertTo(vertices, POINT_TYPE);
getNormals(vertices, normals);
Mat warpedVerts(vertices.size(), vertices.type());
Affine3f invCamPose(pose.inv());
for(int i = 0; i < vertices.size().height; i++) {
ptype v = vertices.at<ptype>(i);
// transform vertex to RGB space
Point3f pVoxel = (Affine3f(params.volumePose).inv() * Point3f(v[0], v[1], v[2])) / params.voxelSize;
Point3f pGlobal = Point3f(pVoxel.x / params.volumeDims[0],
pVoxel.y / params.volumeDims[1],
pVoxel.z / params.volumeDims[2]);
vertices.at<ptype>(i) = ptype(pGlobal.x, pGlobal.y, pGlobal.z, 1.f);
// transform normals to RGB space
ptype n = normals.at<ptype>(i);
Point3f nGlobal = Affine3f(params.volumePose).rotation().inv() * Point3f(n[0], n[1], n[2]);
nGlobal.x = (nGlobal.x + 1)/2;
nGlobal.y = (nGlobal.y + 1)/2;
nGlobal.z = (nGlobal.z + 1)/2;
normals.at<ptype>(i) = ptype(nGlobal.x, nGlobal.y, nGlobal.z, 1.f);
//Point3f p = Point3f(v[0], v[1], v[2]);
if(!warp)
{
Point3f p(invCamPose * Affine3f(params.volumePose) * (pVoxel*params.voxelSize));
warpedVerts.at<ptype>(i) = ptype(p.x, p.y, p.z, 1.f);
}
else
{
int numNeighbours = 0;
const nodeNeighboursType neighbours = volume->getVoxelNeighbours(pVoxel, numNeighbours);
Point3f p = (invCamPose * Affine3f(params.volumePose)) * warpfield.applyWarp(pVoxel*params.voxelSize, neighbours, numNeighbours);
warpedVerts.at<ptype>(i) = ptype(p.x, p.y, p.z, 1.f);
}
}
for(int i = 0; i < vertices.size().height; i++)
meshIdx.push_back<int>(i);
arr.setVertexArray(warpedVerts);
arr.setColorArray(vertices);
idx.copyFrom(meshIdx);
drawScene(depthImage, vertImage);
arr.setVertexArray(warpedVerts);
arr.setColorArray(normals);
drawScene(noArray(), normImage);
#else
CV_UNUSED(depthImage);
CV_UNUSED(vertImage);
CV_UNUSED(normImage);
CV_UNUSED(warp);
NO_OGL_ERR;
#endif
}
// importing class
#ifdef OPENCV_ENABLE_NONFREE
Ptr<DynaFu> DynaFu::create(const Ptr<Params>& params)
{
return makePtr< DynaFuImpl<Mat> >(*params);
}
#else
Ptr<DynaFu> DynaFu::create(const Ptr<Params>& /*params*/)
{
CV_Error(Error::StsNotImplemented,
"This algorithm is patented and is excluded in this configuration; "
"Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library");
}
#endif
DynaFu::~DynaFu() {}
} // namespace dynafu
} // namespace cv
modules/rgbd/src/dynafu_tsdf.cpp
+950
View File
@@ -0,0 +1,950 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "precomp.hpp"
#include "dynafu_tsdf.hpp"
#include "marchingcubes.hpp"
namespace cv {
namespace dynafu {
using namespace kinfu;
// TODO: Optimization possible:
// * volumeType can be FP16
// * weight can be int16
typedef float volumeType;
struct Voxel
{
volumeType v;
float weight;
nodeNeighboursType neighbours;
float neighbourDists[DYNAFU_MAX_NEIGHBOURS];
int n;
};
typedef Vec<uchar, sizeof(Voxel)> VecT;
class TSDFVolumeCPU : public TSDFVolume
{
public:
// dimension in voxels, size in meters
TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor, bool zFirstMemOrder = true);
virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::Matx33f intrMatrix, Ptr<WarpField> wf) override;
virtual void raycast(cv::Affine3f cameraPose, cv::Matx33f intrMatrix, cv::Size frameSize,
cv::OutputArray points, cv::OutputArray normals) const override;
virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override;
virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals, bool fetchVoxels) const override;
virtual void marchCubes(OutputArray _vertices, OutputArray _edges) const override;
virtual void reset() override;
volumeType interpolateVoxel(cv::Point3f p) const;
Point3f getNormalVoxel(cv::Point3f p) const;
nodeNeighboursType const& getVoxelNeighbours(Point3i coords, int& n) const override;
// See zFirstMemOrder arg of parent class constructor
// for the array layout info
// Consist of Voxel elements
Mat volume;
private:
Point3f interpolate(Point3f p1, Point3f p2, float v1, float v2) const;
};
TSDFVolume::TSDFVolume(Point3i _res, float _voxelSize, Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor, bool zFirstMemOrder) :
voxelSize(_voxelSize),
voxelSizeInv(1.f/_voxelSize),
volResolution(_res),
maxWeight((float)_maxWeight),
pose(_pose),
raycastStepFactor(_raycastStepFactor)
{
// Unlike original code, this should work with any volume size
// Not only when (x,y,z % 32) == 0
volSize = Point3f(volResolution) * voxelSize;
truncDist = std::max(_truncDist, 2.1f * voxelSize);
// (xRes*yRes*zRes) array
// Depending on zFirstMemOrder arg:
// &elem(x, y, z) = data + x*zRes*yRes + y*zRes + z;
// &elem(x, y, z) = data + x + y*xRes + z*xRes*yRes;
int xdim, ydim, zdim;
if(zFirstMemOrder)
{
xdim = volResolution.z * volResolution.y;
ydim = volResolution.z;
zdim = 1;
}
else
{
xdim = 1;
ydim = volResolution.x;
zdim = volResolution.x * volResolution.y;
}
volDims = Vec4i(xdim, ydim, zdim);
neighbourCoords = Vec8i(
volDims.dot(Vec4i(0, 0, 0)),
volDims.dot(Vec4i(0, 0, 1)),
volDims.dot(Vec4i(0, 1, 0)),
volDims.dot(Vec4i(0, 1, 1)),
volDims.dot(Vec4i(1, 0, 0)),
volDims.dot(Vec4i(1, 0, 1)),
volDims.dot(Vec4i(1, 1, 0)),
volDims.dot(Vec4i(1, 1, 1))
);
}
// dimension in voxels, size in meters
TSDFVolumeCPU::TSDFVolumeCPU(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor, bool zFirstMemOrder) :
TSDFVolume(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor, zFirstMemOrder)
{
volume = Mat(1, volResolution.x * volResolution.y * volResolution.z, rawType<Voxel>());
reset();
}
// zero volume, leave rest params the same
void TSDFVolumeCPU::reset()
{
CV_TRACE_FUNCTION();
volume.forEach<VecT>([](VecT& vv, const int* /* position */)
{
Voxel& v = reinterpret_cast<Voxel&>(vv);
v.v = 0; v.weight = 0;
});
}
static const bool fixMissingData = false;
static inline depthType bilinearDepth(const Depth& m, cv::Point2f pt)
{
const depthType defaultValue = qnan;
if(pt.x < 0 || pt.x >= m.cols-1 ||
pt.y < 0 || pt.y >= m.rows-1)
return defaultValue;
int xi = cvFloor(pt.x), yi = cvFloor(pt.y);
const depthType* row0 = m[yi+0];
const depthType* row1 = m[yi+1];
depthType v00 = row0[xi+0];
depthType v01 = row0[xi+1];
depthType v10 = row1[xi+0];
depthType v11 = row1[xi+1];
// assume correct depth is positive
bool b00 = v00 > 0;
bool b01 = v01 > 0;
bool b10 = v10 > 0;
bool b11 = v11 > 0;
if(!fixMissingData)
{
if(!(b00 && b01 && b10 && b11))
return defaultValue;
else
{
float tx = pt.x - xi, ty = pt.y - yi;
depthType v0 = v00 + tx*(v01 - v00);
depthType v1 = v10 + tx*(v11 - v10);
return v0 + ty*(v1 - v0);
}
}
else
{
int nz = b00 + b01 + b10 + b11;
if(nz == 0)
{
return defaultValue;
}
if(nz == 1)
{
if(b00) return v00;
if(b01) return v01;
if(b10) return v10;
if(b11) return v11;
}
else if(nz == 2)
{
if(b00 && b10) v01 = v00, v11 = v10;
if(b01 && b11) v00 = v01, v10 = v11;
if(b00 && b01) v10 = v00, v11 = v01;
if(b10 && b11) v00 = v10, v01 = v11;
if(b00 && b11) v01 = v10 = (v00 + v11)*0.5f;
if(b01 && b10) v00 = v11 = (v01 + v10)*0.5f;
}
else if(nz == 3)
{
if(!b00) v00 = v10 + v01 - v11;
if(!b01) v01 = v00 + v11 - v10;
if(!b10) v10 = v00 + v11 - v01;
if(!b11) v11 = v01 + v10 - v00;
}
float tx = pt.x - xi, ty = pt.y - yi;
depthType v0 = v00 + tx*(v01 - v00);
depthType v1 = v10 + tx*(v11 - v10);
return v0 + ty*(v1 - v0);
}
}
struct IntegrateInvoker : ParallelLoopBody
{
IntegrateInvoker(TSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, cv::Affine3f cameraPose,
float depthFactor, Ptr<WarpField> wf) :
ParallelLoopBody(),
volume(_volume),
depth(_depth),
proj(intrinsics.makeProjector()),
vol2cam(cameraPose.inv() * _volume.pose),
truncDistInv(1.f/_volume.truncDist),
dfac(1.f/depthFactor),
warpfield(wf)
{
volDataStart = volume.volume.ptr<Voxel>();
}
virtual void operator() (const Range& range) const override
{
CV_TRACE_FUNCTION();
for(int x = range.start; x < range.end; x++)
{
Voxel* volDataX = volDataStart + x*volume.volDims[0];
for(int y = 0; y < volume.volResolution.y; y++)
{
Voxel* volDataY = volDataX+y*volume.volDims[1];
for(int z = 0; z < volume.volResolution.z; z++)
{
Voxel& voxel = volDataY[z*volume.volDims[2]];
Point3f volPt = Point3f((float)x, (float)y, (float)z)*volume.voxelSize;
if(warpfield->getNodeIndex())
{
std::vector<int> indices(warpfield->k);
std::vector<float> dists(warpfield->k);
warpfield->findNeighbours(volPt, indices, dists);
voxel.n = 0;
for(size_t i = 0; i < indices.size(); i++)
{
if(std::isnan(dists[i])) continue;
voxel.neighbourDists[voxel.n] = dists[i];
voxel.neighbours[voxel.n++] = indices[i];
}
}
Point3f camSpacePt =
vol2cam * warpfield->applyWarp(volPt, voxel.neighbours, voxel.n);
if(camSpacePt.z <= 0)
continue;
Point3f camPixVec;
Point2f projected = proj(camSpacePt, camPixVec);
depthType v = bilinearDepth(depth, projected);
if(v == 0)
continue;
// norm(camPixVec) produces double which is too slow
float pixNorm = sqrt(camPixVec.dot(camPixVec));
// difference between distances of point and of surface to camera
volumeType sdf = pixNorm*(v*dfac - camSpacePt.z);
// possible alternative is:
// kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1);
if(sdf >= -volume.truncDist)
{
volumeType tsdf = fmin(1.f, sdf * truncDistInv);
float& weight = voxel.weight;
volumeType& value = voxel.v;
// update TSDF
float newWeight = 0;
if(warpfield->getNodesLen() >= (size_t)warpfield->k)
{
for(int i = 0; i < voxel.n; i++)
newWeight += sqrt(voxel.neighbourDists[i]);
if(voxel.n > 0) newWeight /= voxel.n;
} else newWeight = 1.f;
if((weight + newWeight) != 0)
{
value = (value*weight+tsdf*newWeight) / (weight+newWeight);
weight = min(weight+newWeight, volume.maxWeight);
}
}
}
}
}
}
TSDFVolumeCPU& volume;
const Depth& depth;
const Intr::Projector proj;
const cv::Affine3f vol2cam;
const float truncDistInv;
const float dfac;
Voxel* volDataStart;
Ptr<WarpField> warpfield;
};
// use depth instead of distance (optimization)
void TSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose, cv::Matx33f intrMatrix, Ptr<WarpField> wf)
{
CV_TRACE_FUNCTION();
CV_Assert(_depth.type() == DEPTH_TYPE);
Depth depth = _depth.getMat();
Intr intrinsics(intrMatrix);
IntegrateInvoker ii(*this, depth, intrinsics, cameraPose, depthFactor, wf);
Range range(0, volResolution.x);
parallel_for_(range, ii);
}
inline volumeType TSDFVolumeCPU::interpolateVoxel(Point3f p) const
{
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
int ix = cvFloor(p.x);
int iy = cvFloor(p.y);
int iz = cvFloor(p.z);
float tx = p.x - ix;
float ty = p.y - iy;
float tz = p.z - iz;
int coordBase = ix*xdim + iy*ydim + iz*zdim;
const Voxel* volData = volume.ptr<Voxel>();
volumeType vx[8];
for(int i = 0; i < 8; i++)
vx[i] = volData[neighbourCoords[i] + coordBase].v;
volumeType v00 = vx[0] + tz*(vx[1] - vx[0]);
volumeType v01 = vx[2] + tz*(vx[3] - vx[2]);
volumeType v10 = vx[4] + tz*(vx[5] - vx[4]);
volumeType v11 = vx[6] + tz*(vx[7] - vx[6]);
volumeType v0 = v00 + ty*(v01 - v00);
volumeType v1 = v10 + ty*(v11 - v10);
return v0 + tx*(v1 - v0);
}
inline Point3f TSDFVolumeCPU::getNormalVoxel(Point3f p) const
{
const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
const Voxel* volData = volume.ptr<Voxel>();
if(p.x < 1 || p.x >= volResolution.x - 2 ||
p.y < 1 || p.y >= volResolution.y - 2 ||
p.z < 1 || p.z >= volResolution.z - 2)
return nan3;
int ix = cvFloor(p.x);
int iy = cvFloor(p.y);
int iz = cvFloor(p.z);
float tx = p.x - ix;
float ty = p.y - iy;
float tz = p.z - iz;
int coordBase = ix*xdim + iy*ydim + iz*zdim;
Vec3f an;
for(int c = 0; c < 3; c++)
{
const int dim = volDims[c];
float& nv = an[c];
volumeType vx[8];
for(int i = 0; i < 8; i++)
vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].v -
volData[neighbourCoords[i] + coordBase - 1*dim].v;
volumeType v00 = vx[0] + tz*(vx[1] - vx[0]);
volumeType v01 = vx[2] + tz*(vx[3] - vx[2]);
volumeType v10 = vx[4] + tz*(vx[5] - vx[4]);
volumeType v11 = vx[6] + tz*(vx[7] - vx[6]);
volumeType v0 = v00 + ty*(v01 - v00);
volumeType v1 = v10 + ty*(v11 - v10);
nv = v0 + tx*(v1 - v0);
}
return normalize(an);
}
struct RaycastInvoker : ParallelLoopBody
{
RaycastInvoker(Points& _points, Normals& _normals, Affine3f cameraPose,
Intr intrinsics, const TSDFVolumeCPU& _volume) :
ParallelLoopBody(),
points(_points),
normals(_normals),
volume(_volume),
tstep(volume.truncDist * volume.raycastStepFactor),
// We do subtract voxel size to minimize checks after
// Note: origin of volume coordinate is placed
// in the center of voxel (0,0,0), not in the corner of the voxel!
boxMax(volume.volSize - Point3f(volume.voxelSize,
volume.voxelSize,
volume.voxelSize)),
boxMin(),
cam2vol(volume.pose.inv() * cameraPose),
vol2cam(cameraPose.inv() * volume.pose),
reproj(intrinsics.makeReprojector())
{ }
virtual void operator() (const Range& range) const override
{
const Point3f camTrans = cam2vol.translation();
const Matx33f camRot = cam2vol.rotation();
const Matx33f volRot = vol2cam.rotation();
for(int y = range.start; y < range.end; y++)
{
ptype* ptsRow = points[y];
ptype* nrmRow = normals[y];
for(int x = 0; x < points.cols; x++)
{
Point3f point = nan3, normal = nan3;
Point3f orig = camTrans;
// direction through pixel in volume space
Point3f dir = normalize(Vec3f(camRot * reproj(Point3f((float)x, (float)y, 1.f))));
// compute intersection of ray with all six bbox planes
Vec3f rayinv(1.f/dir.x, 1.f/dir.y, 1.f/dir.z);
Point3f tbottom = rayinv.mul(boxMin - orig);
Point3f ttop = rayinv.mul(boxMax - orig);
// re-order intersections to find smallest and largest on each axis
Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), min(ttop.z, tbottom.z));
Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), max(ttop.z, tbottom.z));
// near clipping plane
const float clip = 0.f;
float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip);
float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z));
// precautions against getting coordinates out of bounds
tmin = tmin + tstep;
tmax = tmax - tstep;
if(tmin < tmax)
{
// interpolation optimized a little
orig = orig*volume.voxelSizeInv;
dir = dir*volume.voxelSizeInv;
Point3f rayStep = dir * tstep;
Point3f next = (orig + dir * tmin);
volumeType f = volume.interpolateVoxel(next), fnext = f;
//raymarch
int steps = 0;
int nSteps = (int)floor((tmax - tmin)/tstep);
for(; steps < nSteps; steps++)
{
next += rayStep;
int xdim = volume.volDims[0];
int ydim = volume.volDims[1];
int zdim = volume.volDims[2];
int ix = cvRound(next.x);
int iy = cvRound(next.y);
int iz = cvRound(next.z);
fnext = volume.volume.at<Voxel>(ix*xdim + iy*ydim + iz*zdim).v;
if(fnext != f)
{
fnext = volume.interpolateVoxel(next);
// when ray crosses a surface
if(std::signbit(f) != std::signbit(fnext))
break;
f = fnext;
}
}
// if ray penetrates a surface from outside
// linearly interpolate t between two f values
if(f > 0.f && fnext < 0.f)
{
Point3f tp = next - rayStep;
volumeType ft = volume.interpolateVoxel(tp);
volumeType ftdt = volume.interpolateVoxel(next);
// float t = tmin + steps*tstep;
// float ts = t - tstep*ft/(ftdt - ft);
float ts = tmin + tstep*(steps - ft/(ftdt - ft));
// avoid division by zero
if(!cvIsNaN(ts) && !cvIsInf(ts))
{
Point3f pv = (orig + dir*ts);
Point3f nv = volume.getNormalVoxel(pv);
if(!isNaN(nv))
{
//convert pv and nv to camera space
normal = volRot * nv;
// interpolation optimized a little
point = vol2cam * (pv*volume.voxelSize);
}
}
}
}
ptsRow[x] = toPtype(point);
nrmRow[x] = toPtype(normal);
}
}
}
Points& points;
Normals& normals;
const TSDFVolumeCPU& volume;
const float tstep;
const Point3f boxMax;
const Point3f boxMin;
const Affine3f cam2vol;
const Affine3f vol2cam;
const Intr::Reprojector reproj;
};
void TSDFVolumeCPU::raycast(cv::Affine3f cameraPose, cv::Matx33f intrMatrix, Size frameSize,
cv::OutputArray _points, cv::OutputArray _normals) const
{
CV_TRACE_FUNCTION();
CV_Assert(frameSize.area() > 0);
_points.create (frameSize, POINT_TYPE);
_normals.create(frameSize, POINT_TYPE);
Points points = _points.getMat();
Normals normals = _normals.getMat();
Intr intrinsics(intrMatrix);
RaycastInvoker ri(points, normals, cameraPose, intrinsics, *this);
const int nstripes = -1;
parallel_for_(Range(0, points.rows), ri, nstripes);
}
struct FetchPointsNormalsInvoker : ParallelLoopBody
{
FetchPointsNormalsInvoker(const TSDFVolumeCPU& _volume,
std::vector< std::vector<ptype> >& _pVecs,
std::vector< std::vector<ptype> >& _nVecs,
bool _needNormals, bool _fetchVoxels) :
ParallelLoopBody(),
vol(_volume),
pVecs(_pVecs),
nVecs(_nVecs),
needNormals(_needNormals),
fetchVoxels(_fetchVoxels)
{
volDataStart = vol.volume.ptr<Voxel>();
}
inline void coord(std::vector<ptype>& points, std::vector<ptype>& normals,
int x, int y, int z, Point3f V, float v0, int axis) const
{
// 0 for x, 1 for y, 2 for z
bool limits = false;
Point3i shift;
float Vc = 0.f;
if(axis == 0)
{
shift = Point3i(1, 0, 0);
limits = (x + 1 < vol.volResolution.x);
Vc = V.x;
}
if(axis == 1)
{
shift = Point3i(0, 1, 0);
limits = (y + 1 < vol.volResolution.y);
Vc = V.y;
}
if(axis == 2)
{
shift = Point3i(0, 0, 1);
limits = (z + 1 < vol.volResolution.z);
Vc = V.z;
}
if(limits)
{
const Voxel& voxeld = volDataStart[(x+shift.x)*vol.volDims[0] +
(y+shift.y)*vol.volDims[1] +
(z+shift.z)*vol.volDims[2]];
volumeType vd = voxeld.v;
if(voxeld.weight != 0 && vd != 1.f)
{
if((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0))
{
//linearly interpolate coordinate
float Vn = Vc + vol.voxelSize;
float dinv = 1.f/(abs(v0)+abs(vd));
float inter = (Vc*abs(vd) + Vn*abs(v0))*dinv;
Point3f p(shift.x ? inter : V.x,
shift.y ? inter : V.y,
shift.z ? inter : V.z);
{
if(fetchVoxels)
{
points.push_back(toPtype(p));
if(needNormals)
normals.push_back(toPtype(vol.getNormalVoxel(p*vol.voxelSizeInv)));
} else {
points.push_back(toPtype(vol.pose * p));
if(needNormals)
normals.push_back(toPtype(vol.pose.rotation() *
vol.getNormalVoxel(p*vol.voxelSizeInv)));
}
}
}
}
}
}
virtual void operator() (const Range& range) const override
{
std::vector<ptype> points, normals;
for(int x = range.start; x < range.end; x++)
{
const Voxel* volDataX = volDataStart + x*vol.volDims[0];
for(int y = 0; y < vol.volResolution.y; y++)
{
const Voxel* volDataY = volDataX + y*vol.volDims[1];
for(int z = 0; z < vol.volResolution.z; z++)
{
const Voxel& voxel0 = volDataY[z*vol.volDims[2]];
volumeType v0 = voxel0.v;
if(voxel0.weight != 0 && v0 != 1.f)
{
Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize);
coord(points, normals, x, y, z, V, v0, 0);
coord(points, normals, x, y, z, V, v0, 1);
coord(points, normals, x, y, z, V, v0, 2);
} // if voxel is not empty
}
}
}
AutoLock al(mutex);
pVecs.push_back(points);
nVecs.push_back(normals);
}
const TSDFVolumeCPU& vol;
std::vector< std::vector<ptype> >& pVecs;
std::vector< std::vector<ptype> >& nVecs;
const Voxel* volDataStart;
bool needNormals;
bool fetchVoxels;
mutable Mutex mutex;
};
void TSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals, bool fetchVoxels) const
{
CV_TRACE_FUNCTION();
if(_points.needed())
{
std::vector< std::vector<ptype> > pVecs, nVecs;
FetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed(), fetchVoxels);
Range range(0, volResolution.x);
const int nstripes = -1;
parallel_for_(range, fi, nstripes);
std::vector<ptype> points, normals;
for(size_t i = 0; i < pVecs.size(); i++)
{
points.insert(points.end(), pVecs[i].begin(), pVecs[i].end());
normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end());
}
_points.create((int)points.size(), 1, POINT_TYPE);
if(!points.empty())
Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat());
if(_normals.needed())
{
_normals.create((int)normals.size(), 1, POINT_TYPE);
if(!normals.empty())
Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat());
}
}
}
struct PushNormals
{
PushNormals(const TSDFVolumeCPU& _vol, Mat_<ptype>& _nrm) :
vol(_vol), normals(_nrm), invPose(vol.pose.inv())
{ }
void operator ()(const ptype &pp, const int * position) const
{
Point3f p = fromPtype(pp);
Point3f n = nan3;
if(!isNaN(p))
{
Point3f voxPt = (invPose * p);
voxPt = voxPt * vol.voxelSizeInv;
n = vol.pose.rotation() * vol.getNormalVoxel(voxPt);
}
normals(position[0], position[1]) = toPtype(n);
}
const TSDFVolumeCPU& vol;
Mat_<ptype>& normals;
Affine3f invPose;
};
void TSDFVolumeCPU::fetchNormals(InputArray _points, OutputArray _normals) const
{
CV_TRACE_FUNCTION();
if(_normals.needed())
{
Points points = _points.getMat();
CV_Assert(points.type() == POINT_TYPE);
_normals.createSameSize(_points, _points.type());
Mat_<ptype> normals = _normals.getMat();
points.forEach(PushNormals(*this, normals));
}
}
struct MarchCubesInvoker : ParallelLoopBody
{
MarchCubesInvoker(const TSDFVolumeCPU& _volume,
std::vector<Vec4f>& _meshPoints) :
volume(_volume),
meshPoints(_meshPoints),
mcNeighbourPts{
Point3f(0.f, 0.f, 0.f),
Point3f(0.f, 0.f, 1.f),
Point3f(0.f, 1.f, 1.f),
Point3f(0.f, 1.f, 0.f),
Point3f(1.f, 0.f, 0.f),
Point3f(1.f, 0.f, 1.f),
Point3f(1.f, 1.f, 1.f),
Point3f(1.f, 1.f, 0.f)},
mcNeighbourCoords(
Vec8i(
volume.volDims.dot(Vec4i(0, 0, 0)),
volume.volDims.dot(Vec4i(0, 0, 1)),
volume.volDims.dot(Vec4i(0, 1, 1)),
volume.volDims.dot(Vec4i(0, 1, 0)),
volume.volDims.dot(Vec4i(1, 0, 0)),
volume.volDims.dot(Vec4i(1, 0, 1)),
volume.volDims.dot(Vec4i(1, 1, 1)),
volume.volDims.dot(Vec4i(1, 1, 0))
))
{
volData = volume.volume.ptr<Voxel>();
}
Point3f interpolate(Point3f p1, Point3f p2, float v1, float v2) const
{
float dV = 0.5f;
if (abs(v1 - v2) > 0.0001f)
dV = v1 / (v1 - v2);
Point3f p = p1 + dV * (p2 - p1);
return p;
}
virtual void operator()(const Range &range) const override
{
std::vector<Vec4f> points;
for (int x = range.start; x < range.end; x++)
{
int coordBaseX = x * volume.volDims[0];
for (int y = 0; y < volume.volResolution.y - 1; y++)
{
int coordBaseY = coordBaseX + y * volume.volDims[1];
for (int z = 0; z < volume.volResolution.z - 1; z++)
{
int coordBase = coordBaseY + z * volume.volDims[2];
if (volData[coordBase].weight == 0)
continue;
uint8_t cubeIndex = 0;
float tsdfValues[8] = {0};
for (int i = 0; i < 8; i++)
{
if (volData[mcNeighbourCoords[i] + coordBase].weight == 0)
continue;
tsdfValues[i] = volData[mcNeighbourCoords[i] + coordBase].v;
if (tsdfValues[i] <= 0)
cubeIndex |= (1 << i);
}
if (edgeTable[cubeIndex] == 0)
continue;
Point3f vertices[12];
Point3f basePt((float)x, (float)y, (float)z);
if (edgeTable[cubeIndex] & 1)
vertices[0] = basePt + interpolate(mcNeighbourPts[0], mcNeighbourPts[1],
tsdfValues[0], tsdfValues[1]);
if (edgeTable[cubeIndex] & 2)
vertices[1] = basePt + interpolate(mcNeighbourPts[1], mcNeighbourPts[2],
tsdfValues[1], tsdfValues[2]);
if (edgeTable[cubeIndex] & 4)
vertices[2] = basePt + interpolate(mcNeighbourPts[2], mcNeighbourPts[3],
tsdfValues[2], tsdfValues[3]);
if (edgeTable[cubeIndex] & 8)
vertices[3] = basePt + interpolate(mcNeighbourPts[3], mcNeighbourPts[0],
tsdfValues[3], tsdfValues[0]);
if (edgeTable[cubeIndex] & 16)
vertices[4] = basePt + interpolate(mcNeighbourPts[4], mcNeighbourPts[5],
tsdfValues[4], tsdfValues[5]);
if (edgeTable[cubeIndex] & 32)
vertices[5] = basePt + interpolate(mcNeighbourPts[5], mcNeighbourPts[6],
tsdfValues[5], tsdfValues[6]);
if (edgeTable[cubeIndex] & 64)
vertices[6] = basePt + interpolate(mcNeighbourPts[6], mcNeighbourPts[7],
tsdfValues[6], tsdfValues[7]);
if (edgeTable[cubeIndex] & 128)
vertices[7] = basePt + interpolate(mcNeighbourPts[7], mcNeighbourPts[4],
tsdfValues[7], tsdfValues[4]);
if (edgeTable[cubeIndex] & 256)
vertices[8] = basePt + interpolate(mcNeighbourPts[0], mcNeighbourPts[4],
tsdfValues[0], tsdfValues[4]);
if (edgeTable[cubeIndex] & 512)
vertices[9] = basePt + interpolate(mcNeighbourPts[1], mcNeighbourPts[5],
tsdfValues[1], tsdfValues[5]);
if (edgeTable[cubeIndex] & 1024)
vertices[10] = basePt + interpolate(mcNeighbourPts[2], mcNeighbourPts[6],
tsdfValues[2], tsdfValues[6]);
if (edgeTable[cubeIndex] & 2048)
vertices[11] = basePt + interpolate(mcNeighbourPts[3], mcNeighbourPts[7],
tsdfValues[3], tsdfValues[7]);
for (int i = 0; triTable[cubeIndex][i] != -1; i += 3)
{
Point3f p = volume.pose * (vertices[triTable[cubeIndex][i]] * volume.voxelSize);
points.push_back(Vec4f(p.x, p.y, p.z, 1.f));
p = volume.pose * (vertices[triTable[cubeIndex][i + 1]] * volume.voxelSize);
points.push_back(Vec4f(p.x, p.y, p.z, 1.f));
p = volume.pose * (vertices[triTable[cubeIndex][i + 2]] * volume.voxelSize);
points.push_back(Vec4f(p.x, p.y, p.z, 1.f));
}
}
}
}
if(points.size() > 0)
{
AutoLock al(m);
meshPoints.insert(meshPoints.end(), points.begin(), points.end());
}
}
const TSDFVolumeCPU& volume;
std::vector<Vec4f>& meshPoints;
const Point3f mcNeighbourPts[8];
const Vec8i mcNeighbourCoords;
const Voxel* volData;
mutable Mutex m;
};
void TSDFVolumeCPU::marchCubes(OutputArray _vertices, OutputArray _edges) const
{
std::vector<Vec4f> meshPoints;
std::vector<int> meshEdges;
MarchCubesInvoker mci(*this, meshPoints);
Range range(0, volResolution.x - 1);
parallel_for_(range, mci);
for(int i = 0; i < (int)meshPoints.size(); i+= 3)
{
meshEdges.push_back(i);
meshEdges.push_back(i+1);
meshEdges.push_back(i+1);
meshEdges.push_back(i+2);
meshEdges.push_back(i+2);
meshEdges.push_back(i);
}
if (_vertices.needed())
Mat((int)meshPoints.size(), 1, CV_32FC4, &meshPoints[0]).copyTo(_vertices);
if (_edges.needed())
Mat((int)meshPoints.size(), 2, CV_32S, &meshEdges[0]).copyTo(_edges);
}
nodeNeighboursType const& TSDFVolumeCPU::getVoxelNeighbours(Point3i v, int& n) const
{
int baseX = v.x * volDims[0];
int baseY = baseX + v.y * volDims[1];
int base = baseY + v.z * volDims[2];
const Voxel *vox = volume.ptr<Voxel>()+base;
n = vox->n;
return vox->neighbours;
}
cv::Ptr<TSDFVolume> makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor)
{
return cv::makePtr<TSDFVolumeCPU>(_res, _voxelSize, _pose, _truncDist, _maxWeight, _raycastStepFactor);
}
} // namespace dynafu
} // namespace cv
modules/rgbd/src/dynafu_tsdf.hpp
+57
View File
@@ -0,0 +1,57 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef __OPENCV_DYNAFU_TSDF_H__
#define __OPENCV_DYNAFU_TSDF_H__
#include "warpfield.hpp"
namespace cv {
namespace dynafu {
class TSDFVolume
{
public:
// dimension in voxels, size in meters
TSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor, bool zFirstMemOrder = true);
virtual void integrate(InputArray _depth, float depthFactor, cv::Affine3f cameraPose,
cv::Matx33f intrMatrix, Ptr<WarpField> wf) = 0;
virtual void raycast(cv::Affine3f cameraPose, cv::Matx33f intrMatrix, cv::Size frameSize,
cv::OutputArray points, cv::OutputArray normals) const = 0;
virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals,
bool fetchVoxels=false) const = 0;
virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0;
virtual void marchCubes(OutputArray _vertices, OutputArray _edges) const = 0;
virtual void reset() = 0;
virtual nodeNeighboursType const& getVoxelNeighbours(Point3i v, int& n) const = 0;
virtual ~TSDFVolume() { }
float voxelSize;
float voxelSizeInv;
Point3i volResolution;
float maxWeight;
cv::Affine3f pose;
float raycastStepFactor;
Point3f volSize;
float truncDist;
Vec4i volDims;
Vec8i neighbourCoords;
};
cv::Ptr<TSDFVolume> makeTSDFVolume(Point3i _res, float _voxelSize, cv::Affine3f _pose, float _truncDist, int _maxWeight,
float _raycastStepFactor);
} // namespace dynafu
} // namespace cv
#endif
modules/rgbd/src/kinfu.cpp
+376
View File
@@ -0,0 +1,376 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "precomp.hpp"
namespace cv {
namespace kinfu {
void Params::setInitialVolumePose(Matx33f R, Vec3f t)
{
setInitialVolumePose(Affine3f(R,t).matrix);
}
void Params::setInitialVolumePose(Matx44f homogen_tf)
{
Params::volumePose = homogen_tf;
}
Ptr<Params> Params::defaultParams()
{
Params p;
p.frameSize = Size(640, 480);
p.volumeKind = VolumeType::TSDF;
float fx, fy, cx, cy;
fx = fy = 525.f;
cx = p.frameSize.width/2 - 0.5f;
cy = p.frameSize.height/2 - 0.5f;
p.intr = Matx33f(fx, 0, cx,
0, fy, cy,
0, 0, 1);
// 5000 for the 16-bit PNG files
// 1 for the 32-bit float images in the ROS bag files
p.depthFactor = 5000;
// sigma_depth is scaled by depthFactor when calling bilateral filter
p.bilateral_sigma_depth = 0.04f; //meter
p.bilateral_sigma_spatial = 4.5; //pixels
p.bilateral_kernel_size = 7; //pixels
p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians
p.icpDistThresh = 0.1f; // meters
p.icpIterations = {10, 5, 4};
p.pyramidLevels = (int)p.icpIterations.size();
p.tsdf_min_camera_movement = 0.f; //meters, disabled
p.volumeDims = Vec3i::all(512); //number of voxels
float volSize = 3.f;
p.voxelSize = volSize/512.f; //meters
// default pose of volume cube
p.volumePose = Affine3f().translate(Vec3f(-volSize/2.f, -volSize/2.f, 0.5f)).matrix;
p.tsdf_trunc_dist = 7 * p.voxelSize; // about 0.04f in meters
p.tsdf_max_weight = 64; //frames
p.raycast_step_factor = 0.25f; //in voxel sizes
// gradient delta factor is fixed at 1.0f and is not used
//p.gradient_delta_factor = 0.5f; //in voxel sizes
//p.lightPose = p.volume_pose.translation()/4; //meters
p.lightPose = Vec3f::all(0.f); //meters
// depth truncation is not used by default but can be useful in some scenes
p.truncateThreshold = 0.f; //meters
return makePtr<Params>(p);
}
Ptr<Params> Params::coarseParams()
{
Ptr<Params> p = defaultParams();
p->icpIterations = {5, 3, 2};
p->pyramidLevels = (int)p->icpIterations.size();
float volSize = 3.f;
p->volumeDims = Vec3i::all(128); //number of voxels
p->voxelSize = volSize/128.f;
p->tsdf_trunc_dist = 2 * p->voxelSize; // 0.04f in meters
p->raycast_step_factor = 0.75f; //in voxel sizes
return p;
}
Ptr<Params> Params::hashTSDFParams(bool isCoarse)
{
Ptr<Params> p;
if(isCoarse)
p = coarseParams();
else
p = defaultParams();
p->volumeKind = VolumeType::HashTSDF;
p->truncateThreshold = 4.f;
return p;
}
Ptr<Params> Params::coloredTSDFParams(bool isCoarse)
{
Ptr<Params> p;
if (isCoarse)
p = coarseParams();
else
p = defaultParams();
p->volumeKind = VolumeType::ColorTSDF;
return p;
}
// MatType should be Mat or UMat
template< typename MatType>
class KinFuImpl : public KinFu
{
public:
KinFuImpl(const Params& _params);
virtual ~KinFuImpl();
static VolumeSettings paramsToSettings(const Params& params);
const Params& getParams() const CV_OVERRIDE;
void render(OutputArray image) const CV_OVERRIDE;
void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
void getPoints(OutputArray points) const CV_OVERRIDE;
void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
void reset() CV_OVERRIDE;
Affine3f getPose() const CV_OVERRIDE;
bool update(InputArray depth) CV_OVERRIDE;
bool updateT(const MatType& depth);
private:
Params params;
VolumeSettings settings;
Odometry icp;
Volume volume;
int frameCounter;
Matx44f pose;
OdometryFrame prevFrame;
OdometryFrame renderFrame;
};
template< typename MatType >
VolumeSettings KinFuImpl<MatType>::paramsToSettings(const Params& params)
{
VolumeSettings vs(VolumeType::TSDF);
vs.setVoxelSize(params.voxelSize);
vs.setVolumePose(params.volumePose);
vs.setRaycastStepFactor(params.raycast_step_factor);
vs.setTsdfTruncateDistance(params.tsdf_trunc_dist);
vs.setMaxWeight(params.tsdf_max_weight);
vs.setMaxDepth(params.truncateThreshold);
vs.setCameraIntegrateIntrinsics(params.intr);
vs.setDepthFactor(params.depthFactor);
vs.setVolumeResolution(params.volumeDims);
return vs;
}
template< typename MatType >
KinFuImpl<MatType>::KinFuImpl(const Params &_params) :
params(_params),
settings(paramsToSettings(params)),
volume(VolumeType::TSDF, settings)
{
OdometrySettings ods;
ods.setMaxRotation(30.f);
ods.setMaxTranslation(params.voxelSize * params.volumeDims[0] * 0.5f);
ods.setCameraMatrix(Mat(params.intr));
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
reset();
}
template< typename MatType >
void KinFuImpl<MatType >::reset()
{
frameCounter = 0;
pose = Affine3f::Identity().matrix;
volume.reset();
}
template< typename MatType >
KinFuImpl<MatType>::~KinFuImpl()
{ }
template< typename MatType >
const Params& KinFuImpl<MatType>::getParams() const
{
return params;
}
template< typename MatType >
Affine3f KinFuImpl<MatType>::getPose() const
{
return pose;
}
template<>
bool KinFuImpl<Mat>::update(InputArray _depth)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
Mat depth;
if(_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getMat());
}
}
template<>
bool KinFuImpl<UMat>::update(InputArray _depth)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
UMat depth;
if(!_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getUMat());
}
}
template< typename MatType >
bool KinFuImpl<MatType>::updateT(const MatType& _depth)
{
CV_TRACE_FUNCTION();
MatType depth;
if(_depth.type() != DEPTH_TYPE)
_depth.convertTo(depth, DEPTH_TYPE);
else
depth = _depth;
OdometryFrame newFrame(depth);
if(frameCounter == 0)
{
icp.prepareFrame(newFrame);
// use depth instead of distance
volume.integrate(depth, pose);
}
else
{
Affine3f affine;
Matx44d mrt;
Mat Rt;
icp.prepareFrames(newFrame, prevFrame);
bool success = icp.compute(newFrame, prevFrame, Rt);
if(!success)
return false;
affine.matrix = Matx44f(Rt);
pose = (Affine3f(pose) * affine).matrix;
float rnorm = (float)cv::norm(affine.rvec());
float tnorm = (float)cv::norm(affine.translation());
// We do not integrate volume if camera does not move
if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement)
{
// use depth instead of distance
volume.integrate(depth, pose);
}
MatType points, normals;
volume.raycast(pose, points, normals);
std::vector<MatType> pch(3);
split(points, pch);
newFrame = OdometryFrame(pch[2], noArray(), noArray(), normals);
}
renderFrame = newFrame;
prevFrame = newFrame;
frameCounter++;
return true;
}
template< typename MatType >
void KinFuImpl<MatType>::render(OutputArray image) const
{
CV_TRACE_FUNCTION();
MatType pts, nrm;
renderFrame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
renderFrame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
detail::renderPointsNormals(pts, nrm, image, params.lightPose);
}
template< typename MatType >
void KinFuImpl<MatType>::render(OutputArray image, const Matx44f& _cameraPose) const
{
CV_TRACE_FUNCTION();
Affine3f cameraPose(_cameraPose);
MatType points, normals;
volume.raycast(_cameraPose, points, normals);
detail::renderPointsNormals(points, normals, image, params.lightPose);
}
template< typename MatType >
void KinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
{
volume.fetchPointsNormals(p, n);
}
template< typename MatType >
void KinFuImpl<MatType>::getPoints(OutputArray points) const
{
volume.fetchPointsNormals(points, noArray());
}
template< typename MatType >
void KinFuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
{
volume.fetchNormals(points, normals);
}
// importing class
#ifdef OPENCV_ENABLE_NONFREE
Ptr<KinFu> KinFu::create(const Ptr<Params>& params)
{
CV_Assert((int)params->icpIterations.size() == params->pyramidLevels);
CV_Assert(params->intr(0,1) == 0 && params->intr(1,0) == 0 && params->intr(2,0) == 0 && params->intr(2,1) == 0 && params->intr(2,2) == 1);
#ifdef HAVE_OPENCL
if(cv::ocl::useOpenCL())
return makePtr< KinFuImpl<UMat> >(*params);
#endif
return makePtr< KinFuImpl<Mat> >(*params);
}
#else
Ptr<KinFu> KinFu::create(const Ptr<Params>& /* params */)
{
CV_Error(Error::StsNotImplemented,
"This algorithm is patented and is excluded in this configuration; "
"Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library");
}
#endif
KinFu::~KinFu() {}
} // namespace kinfu
} // namespace cv
modules/rgbd/src/large_kinfu.cpp
+475
View File
@@ -0,0 +1,475 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
// module's directory
#include "precomp.hpp"
namespace cv
{
namespace large_kinfu
{
using namespace kinfu;
Ptr<VolumeParams> VolumeParams::defaultParams(VolumeType _volumeKind)
{
VolumeParams params;
params.kind = _volumeKind;
params.maxWeight = 64;
params.raycastStepFactor = 0.25f;
params.unitResolution = 0; // unitResolution not used for TSDF
float volumeSize = 3.0f;
Matx44f pose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f)).matrix;
params.pose = Mat(pose);
if (params.kind == VolumeType::TSDF)
{
params.resolutionX = 512;
params.resolutionY = 512;
params.resolutionZ = 512;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
params.tsdfTruncDist = 7 * params.voxelSize; // About 0.04f in meters
return makePtr<VolumeParams>(params);
}
else if (params.kind == VolumeType::HashTSDF)
{
params.unitResolution = 16;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 4.f;
params.tsdfTruncDist = 7 * params.voxelSize; // About 0.04f in meters
return makePtr<VolumeParams>(params);
}
else if (params.kind == VolumeType::ColorTSDF)
{
params.resolutionX = 512;
params.resolutionY = 512;
params.resolutionZ = 512;
params.voxelSize = volumeSize / 512.f;
params.depthTruncThreshold = 0.f; // depthTruncThreshold not required for TSDF
params.tsdfTruncDist = 7 * params.voxelSize; // About 0.04f in meters
return makePtr<VolumeParams>(params);
}
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
Ptr<VolumeParams> VolumeParams::coarseParams(VolumeType _volumeKind)
{
Ptr<VolumeParams> params = defaultParams(_volumeKind);
params->raycastStepFactor = 0.75f;
float volumeSize = 3.0f;
if (params->kind == VolumeType::TSDF)
{
params->resolutionX = 128;
params->resolutionY = 128;
params->resolutionZ = 128;
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; // About 0.04f in meters
return params;
}
else if (params->kind == VolumeType::HashTSDF)
{
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; // About 0.04f in meters
return params;
}
else if (params->kind == VolumeType::ColorTSDF)
{
params->resolutionX = 128;
params->resolutionY = 128;
params->resolutionZ = 128;
params->voxelSize = volumeSize / 128.f;
params->tsdfTruncDist = 2 * params->voxelSize; // About 0.04f in meters
return params;
}
CV_Error(Error::StsBadArg, "Invalid VolumeType does not have parameters");
}
Ptr<Params> Params::defaultParams()
{
Params p;
// Frame parameters
{
p.frameSize = Size(640, 480);
float fx, fy, cx, cy;
fx = fy = 525.f;
cx = p.frameSize.width / 2.0f - 0.5f;
cy = p.frameSize.height / 2.0f - 0.5f;
p.intr = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1);
// 5000 for the 16-bit PNG files
// 1 for the 32-bit float images in the ROS bag files
p.depthFactor = 5000;
// sigma_depth is scaled by depthFactor when calling bilateral filter
p.bilateral_sigma_depth = 0.04f; // meter
p.bilateral_sigma_spatial = 4.5; // pixels
p.bilateral_kernel_size = 7; // pixels
p.truncateThreshold = 0.f; // meters
}
// ICP parameters
{
p.icpAngleThresh = (float)(30. * CV_PI / 180.); // radians
p.icpDistThresh = 0.1f; // meters
p.icpIterations = { 10, 5, 4 };
p.pyramidLevels = (int)p.icpIterations.size();
}
// Volume parameters
{
float volumeSize = 3.0f;
p.volumeParams.kind = VolumeType::TSDF;
p.volumeParams.resolutionX = 512;
p.volumeParams.resolutionY = 512;
p.volumeParams.resolutionZ = 512;
Affine3f newPose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.5f));
p.volumeParams.pose = Mat(newPose.matrix);
p.volumeParams.voxelSize = volumeSize / 512.f; // meters
p.volumeParams.tsdfTruncDist = 7 * p.volumeParams.voxelSize; // about 0.04f in meters
p.volumeParams.maxWeight = 64; // frames
p.volumeParams.raycastStepFactor = 0.25f; // in voxel sizes
p.volumeParams.depthTruncThreshold = p.truncateThreshold;
}
// Unused parameters
p.tsdf_min_camera_movement = 0.f; // meters, disabled
p.lightPose = Vec3f::all(0.f); // meters
return makePtr<Params>(p);
}
Ptr<Params> Params::coarseParams()
{
Ptr<Params> p = defaultParams();
// Reduce ICP iterations and pyramid levels
{
p->icpIterations = { 5, 3, 2 };
p->pyramidLevels = (int)p->icpIterations.size();
}
// Make the volume coarse
{
float volumeSize = 3.f;
p->volumeParams.resolutionX = 128; // number of voxels
p->volumeParams.resolutionY = 128;
p->volumeParams.resolutionZ = 128;
p->volumeParams.voxelSize = volumeSize / 128.f;
p->volumeParams.tsdfTruncDist = 2 * p->volumeParams.voxelSize; // 0.04f in meters
p->volumeParams.raycastStepFactor = 0.75f; // in voxel sizes
}
return p;
}
Ptr<Params> Params::hashTSDFParams(bool isCoarse)
{
Ptr<Params> p;
if (isCoarse)
p = coarseParams();
else
p = defaultParams();
p->volumeParams.kind = VolumeType::HashTSDF;
p->volumeParams.depthTruncThreshold = 4.f;
p->volumeParams.unitResolution = 16;
return p;
}
// MatType should be Mat or UMat
template<typename MatType>
class LargeKinfuImpl : public LargeKinfu
{
public:
LargeKinfuImpl(const Params& _params);
virtual ~LargeKinfuImpl();
static VolumeSettings paramsToSettings(const Params& params);
const Params& getParams() const CV_OVERRIDE;
void render(OutputArray image) const CV_OVERRIDE;
void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
void getPoints(OutputArray points) const CV_OVERRIDE;
void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
void reset() CV_OVERRIDE;
Affine3f getPose() const CV_OVERRIDE;
bool update(InputArray depth) CV_OVERRIDE;
bool updateT(const MatType& depth);
private:
Params params;
VolumeSettings settings;
Odometry icp;
// TODO: Submap manager and Pose graph optimizer
cv::Ptr<detail::SubmapManager<MatType>> submapMgr;
int frameCounter;
Affine3f pose;
};
template<typename MatType>
VolumeSettings LargeKinfuImpl<MatType>::paramsToSettings(const Params& params)
{
VolumeSettings vs(VolumeType::HashTSDF);
vs.setMaxDepth(params.truncateThreshold);
vs.setCameraIntegrateIntrinsics(params.intr);
vs.setCameraRaycastIntrinsics(params.intr);
vs.setDepthFactor(params.depthFactor);
vs.setVoxelSize(params.volumeParams.voxelSize);
vs.setVolumePose(params.volumeParams.pose);
vs.setRaycastStepFactor(params.volumeParams.raycastStepFactor);
vs.setTsdfTruncateDistance(params.volumeParams.tsdfTruncDist);
vs.setMaxWeight(params.volumeParams.maxWeight);
vs.setVolumeResolution(Vec3i(params.volumeParams.unitResolution, params.volumeParams.unitResolution, params.volumeParams.unitResolution));
return vs;
}
template<typename MatType>
LargeKinfuImpl<MatType>::LargeKinfuImpl(const Params& _params)
: params(_params),
settings(paramsToSettings(params))
{
OdometrySettings ods;
ods.setCameraMatrix(Mat(params.intr));
ods.setMaxRotation(30.f);
ods.setMaxTranslation(params.volumeParams.voxelSize * params.volumeParams.resolutionX * 0.5f);
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
submapMgr = cv::makePtr<detail::SubmapManager<MatType>>(settings);
reset();
submapMgr->createNewSubmap(true);
}
template<typename MatType>
void LargeKinfuImpl<MatType>::reset()
{
frameCounter = 0;
pose = Affine3f::Identity();
submapMgr->reset();
}
template<typename MatType>
LargeKinfuImpl<MatType>::~LargeKinfuImpl()
{
}
template<typename MatType>
const Params& LargeKinfuImpl<MatType>::getParams() const
{
return params;
}
template<typename MatType>
Affine3f LargeKinfuImpl<MatType>::getPose() const
{
return pose;
}
template<>
bool LargeKinfuImpl<Mat>::update(InputArray _depth)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
Mat depth;
if (_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getMat());
}
}
template<>
bool LargeKinfuImpl<UMat>::update(InputArray _depth)
{
CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
UMat depth;
if (!_depth.isUMat())
{
_depth.copyTo(depth);
return updateT(depth);
}
else
{
return updateT(_depth.getUMat());
}
}
template<typename MatType>
bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
{
CV_TRACE_FUNCTION();
MatType depth;
if (_depth.type() != DEPTH_TYPE)
_depth.convertTo(depth, DEPTH_TYPE);
else
depth = _depth;
OdometryFrame newFrame(depth);
CV_LOG_INFO(NULL, "Current frameID: " << frameCounter);
for (const auto& it : submapMgr->activeSubmaps)
{
int currTrackingId = it.first;
auto submapData = it.second;
Ptr<detail::Submap<MatType>> currTrackingSubmap = submapMgr->getSubmap(currTrackingId);
Affine3f affine;
CV_LOG_INFO(NULL, "Current tracking ID: " << currTrackingId);
if(frameCounter == 0) // Only one current tracking map
{
icp.prepareFrame(newFrame);
currTrackingSubmap->integrate(depth, frameCounter);
currTrackingSubmap->frame = newFrame;
currTrackingSubmap->renderFrame = newFrame;
continue;
}
//1. Track
Matx44d mrt;
Mat Rt;
icp.prepareFrames(newFrame, currTrackingSubmap->frame);
bool trackingSuccess = icp.compute(newFrame, currTrackingSubmap->frame, Rt);
if (trackingSuccess)
{
affine.matrix = Matx44f(Rt);
currTrackingSubmap->composeCameraPose(affine);
}
else
{
CV_LOG_INFO(NULL, "Tracking failed");
continue;
}
//2. Integrate
if(submapData.type == detail::SubmapManager<MatType>::Type::NEW || submapData.type == detail::SubmapManager<MatType>::Type::CURRENT)
{
float rnorm = (float)cv::norm(affine.rvec());
float tnorm = (float)cv::norm(affine.translation());
// We do not integrate volume if camera does not move
if ((rnorm + tnorm) / 2 >= params.tsdf_min_camera_movement)
currTrackingSubmap->integrate(depth, frameCounter);
}
//3. Raycast
currTrackingSubmap->raycast(currTrackingSubmap->cameraPose, params.frameSize, params.intr);
CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks());
CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter));
}
//4. Update map
bool isMapUpdated = submapMgr->updateMap(frameCounter, newFrame);
if(isMapUpdated)
{
// TODO: Convert constraints to posegraph
Ptr<detail::PoseGraph> poseGraph = submapMgr->MapToPoseGraph();
CV_LOG_INFO(NULL, "Created posegraph");
LevMarq::Report r = poseGraph->optimize();
if (!r.found)
{
CV_LOG_INFO(NULL, "Failed to perform pose graph optimization");
return false;
}
submapMgr->PoseGraphToMap(poseGraph);
}
CV_LOG_INFO(NULL, "Number of submaps: " << submapMgr->submapList.size());
frameCounter++;
return true;
}
template<typename MatType>
void LargeKinfuImpl<MatType>::render(OutputArray image) const
{
CV_TRACE_FUNCTION();
auto currSubmap = submapMgr->getCurrentSubmap();
// TODO: Can render be dependent on current submap
MatType pts, nrm;
currSubmap->renderFrame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
currSubmap->renderFrame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
detail::renderPointsNormals(pts, nrm, image, params.lightPose);
}
template<typename MatType>
void LargeKinfuImpl<MatType>::render(OutputArray image, const Matx44f& _cameraPose) const
{
CV_TRACE_FUNCTION();
Affine3f cameraPose(_cameraPose);
auto currSubmap = submapMgr->getCurrentSubmap();
MatType points, normals;
currSubmap->raycast(cameraPose, params.frameSize, params.intr, points, normals);
detail::renderPointsNormals(points, normals, image, params.lightPose);
}
template<typename MatType>
void LargeKinfuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
{
auto currSubmap = submapMgr->getCurrentSubmap();
currSubmap->volume.fetchPointsNormals(p, n);
}
template<typename MatType>
void LargeKinfuImpl<MatType>::getPoints(OutputArray points) const
{
auto currSubmap = submapMgr->getCurrentSubmap();
currSubmap->volume.fetchPointsNormals(points, noArray());
}
template<typename MatType>
void LargeKinfuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
{
auto currSubmap = submapMgr->getCurrentSubmap();
currSubmap->volume.fetchNormals(points, normals);
}
// importing class
#ifdef OPENCV_ENABLE_NONFREE
Ptr<LargeKinfu> LargeKinfu::create(const Ptr<Params>& params)
{
CV_Assert((int)params->icpIterations.size() == params->pyramidLevels);
CV_Assert(params->intr(0, 1) == 0 && params->intr(1, 0) == 0 && params->intr(2, 0) == 0 && params->intr(2, 1) == 0 &&
params->intr(2, 2) == 1);
#ifdef HAVE_OPENCL
if (cv::ocl::useOpenCL())
return makePtr<LargeKinfuImpl<UMat>>(*params);
#endif
return makePtr<LargeKinfuImpl<Mat>>(*params);
}
#else
Ptr<LargeKinfu> LargeKinfu::create(const Ptr<Params>& /* params */)
{
CV_Error(Error::StsNotImplemented,
"This algorithm is patented and is excluded in this configuration; "
"Set OPENCV_ENABLE_NONFREE CMake option and rebuild the library");
}
#endif
} // namespace large_kinfu
} // namespace cv
modules/rgbd/src/linemod.cpp
+1840
View File
File diff suppressed because it is too large Load Diff
modules/rgbd/src/marchingcubes.hpp
+304
View File
@@ -0,0 +1,304 @@
#ifndef __OPENCV_DYNAFU_MARCHINGCUBES_H__
#define __OPENCV_DYNAFU_MARCHINGCUBES_H__
/*
These tables are originally generated by Cory Gene Bloyd
(http://paulbourke.net/geometry/polygonise) and are released
in the public domain
*/
namespace cv {
namespace dynafu {
// For any edge, if one vertex is inside of the surface and the other is outside of the surface
// then the edge intersects the surface
// For each of the 8 vertices of the cube can be two possible states : either inside or outside of the surface
// For any cube the are 2^8=256 possible sets of vertex states
// This table lists the edges intersected by the surface for all 256 possible vertex states
// There are 12 edges. For each entry in the table, if edge #n is intersected, then bit #n is set to 1
int edgeTable[256] =
{
0x000, 0x109, 0x203, 0x30a, 0x406, 0x50f, 0x605, 0x70c, 0x80c, 0x905, 0xa0f, 0xb06, 0xc0a, 0xd03, 0xe09, 0xf00,
0x190, 0x099, 0x393, 0x29a, 0x596, 0x49f, 0x795, 0x69c, 0x99c, 0x895, 0xb9f, 0xa96, 0xd9a, 0xc93, 0xf99, 0xe90,
0x230, 0x339, 0x033, 0x13a, 0x636, 0x73f, 0x435, 0x53c, 0xa3c, 0xb35, 0x83f, 0x936, 0xe3a, 0xf33, 0xc39, 0xd30,
0x3a0, 0x2a9, 0x1a3, 0x0aa, 0x7a6, 0x6af, 0x5a5, 0x4ac, 0xbac, 0xaa5, 0x9af, 0x8a6, 0xfaa, 0xea3, 0xda9, 0xca0,
0x460, 0x569, 0x663, 0x76a, 0x066, 0x16f, 0x265, 0x36c, 0xc6c, 0xd65, 0xe6f, 0xf66, 0x86a, 0x963, 0xa69, 0xb60,
0x5f0, 0x4f9, 0x7f3, 0x6fa, 0x1f6, 0x0ff, 0x3f5, 0x2fc, 0xdfc, 0xcf5, 0xfff, 0xef6, 0x9fa, 0x8f3, 0xbf9, 0xaf0,
0x650, 0x759, 0x453, 0x55a, 0x256, 0x35f, 0x055, 0x15c, 0xe5c, 0xf55, 0xc5f, 0xd56, 0xa5a, 0xb53, 0x859, 0x950,
0x7c0, 0x6c9, 0x5c3, 0x4ca, 0x3c6, 0x2cf, 0x1c5, 0x0cc, 0xfcc, 0xec5, 0xdcf, 0xcc6, 0xbca, 0xac3, 0x9c9, 0x8c0,
0x8c0, 0x9c9, 0xac3, 0xbca, 0xcc6, 0xdcf, 0xec5, 0xfcc, 0x0cc, 0x1c5, 0x2cf, 0x3c6, 0x4ca, 0x5c3, 0x6c9, 0x7c0,
0x950, 0x859, 0xb53, 0xa5a, 0xd56, 0xc5f, 0xf55, 0xe5c, 0x15c, 0x055, 0x35f, 0x256, 0x55a, 0x453, 0x759, 0x650,
0xaf0, 0xbf9, 0x8f3, 0x9fa, 0xef6, 0xfff, 0xcf5, 0xdfc, 0x2fc, 0x3f5, 0x0ff, 0x1f6, 0x6fa, 0x7f3, 0x4f9, 0x5f0,
0xb60, 0xa69, 0x963, 0x86a, 0xf66, 0xe6f, 0xd65, 0xc6c, 0x36c, 0x265, 0x16f, 0x066, 0x76a, 0x663, 0x569, 0x460,
0xca0, 0xda9, 0xea3, 0xfaa, 0x8a6, 0x9af, 0xaa5, 0xbac, 0x4ac, 0x5a5, 0x6af, 0x7a6, 0x0aa, 0x1a3, 0x2a9, 0x3a0,
0xd30, 0xc39, 0xf33, 0xe3a, 0x936, 0x83f, 0xb35, 0xa3c, 0x53c, 0x435, 0x73f, 0x636, 0x13a, 0x033, 0x339, 0x230,
0xe90, 0xf99, 0xc93, 0xd9a, 0xa96, 0xb9f, 0x895, 0x99c, 0x69c, 0x795, 0x49f, 0x596, 0x29a, 0x393, 0x099, 0x190,
0xf00, 0xe09, 0xd03, 0xc0a, 0xb06, 0xa0f, 0x905, 0x80c, 0x70c, 0x605, 0x50f, 0x406, 0x30a, 0x203, 0x109, 0x000};
// For each of the possible vertex states listed in aiCubeEdgeFlags there is a specific triangulation
// of the edge intersection points. a2iTriangleConnectionTable lists all of them in the form of
// 0-5 edge triples with the list terminated by the invalid value -1.
// For example: a2iTriangleConnectionTable[3] list the 2 triangles formed when corner[0]
// and corner[1] are inside of the surface, but the rest of the cube is not.
int triTable[256][16] =
{
{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 8, 3, 9, 8, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 2, 10, 0, 2, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{2, 8, 3, 2, 10, 8, 10, 9, 8, -1, -1, -1, -1, -1, -1, -1},
{3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 11, 2, 8, 11, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 9, 0, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 11, 2, 1, 9, 11, 9, 8, 11, -1, -1, -1, -1, -1, -1, -1},
{3, 10, 1, 11, 10, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 10, 1, 0, 8, 10, 8, 11, 10, -1, -1, -1, -1, -1, -1, -1},
{3, 9, 0, 3, 11, 9, 11, 10, 9, -1, -1, -1, -1, -1, -1, -1},
{9, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 3, 0, 7, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 9, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 1, 9, 4, 7, 1, 7, 3, 1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 4, 7, 3, 0, 4, 1, 2, 10, -1, -1, -1, -1, -1, -1, -1},
{9, 2, 10, 9, 0, 2, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
{2, 10, 9, 2, 9, 7, 2, 7, 3, 7, 9, 4, -1, -1, -1, -1},
{8, 4, 7, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{11, 4, 7, 11, 2, 4, 2, 0, 4, -1, -1, -1, -1, -1, -1, -1},
{9, 0, 1, 8, 4, 7, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
{4, 7, 11, 9, 4, 11, 9, 11, 2, 9, 2, 1, -1, -1, -1, -1},
{3, 10, 1, 3, 11, 10, 7, 8, 4, -1, -1, -1, -1, -1, -1, -1},
{1, 11, 10, 1, 4, 11, 1, 0, 4, 7, 11, 4, -1, -1, -1, -1},
{4, 7, 8, 9, 0, 11, 9, 11, 10, 11, 0, 3, -1, -1, -1, -1},
{4, 7, 11, 4, 11, 9, 9, 11, 10, -1, -1, -1, -1, -1, -1, -1},
{9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 5, 4, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 5, 4, 1, 5, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{8, 5, 4, 8, 3, 5, 3, 1, 5, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 0, 8, 1, 2, 10, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
{5, 2, 10, 5, 4, 2, 4, 0, 2, -1, -1, -1, -1, -1, -1, -1},
{2, 10, 5, 3, 2, 5, 3, 5, 4, 3, 4, 8, -1, -1, -1, -1},
{9, 5, 4, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 11, 2, 0, 8, 11, 4, 9, 5, -1, -1, -1, -1, -1, -1, -1},
{0, 5, 4, 0, 1, 5, 2, 3, 11, -1, -1, -1, -1, -1, -1, -1},
{2, 1, 5, 2, 5, 8, 2, 8, 11, 4, 8, 5, -1, -1, -1, -1},
{10, 3, 11, 10, 1, 3, 9, 5, 4, -1, -1, -1, -1, -1, -1, -1},
{4, 9, 5, 0, 8, 1, 8, 10, 1, 8, 11, 10, -1, -1, -1, -1},
{5, 4, 0, 5, 0, 11, 5, 11, 10, 11, 0, 3, -1, -1, -1, -1},
{5, 4, 8, 5, 8, 10, 10, 8, 11, -1, -1, -1, -1, -1, -1, -1},
{9, 7, 8, 5, 7, 9, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 3, 0, 9, 5, 3, 5, 7, 3, -1, -1, -1, -1, -1, -1, -1},
{0, 7, 8, 0, 1, 7, 1, 5, 7, -1, -1, -1, -1, -1, -1, -1},
{1, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 7, 8, 9, 5, 7, 10, 1, 2, -1, -1, -1, -1, -1, -1, -1},
{10, 1, 2, 9, 5, 0, 5, 3, 0, 5, 7, 3, -1, -1, -1, -1},
{8, 0, 2, 8, 2, 5, 8, 5, 7, 10, 5, 2, -1, -1, -1, -1},
{2, 10, 5, 2, 5, 3, 3, 5, 7, -1, -1, -1, -1, -1, -1, -1},
{7, 9, 5, 7, 8, 9, 3, 11, 2, -1, -1, -1, -1, -1, -1, -1},
{9, 5, 7, 9, 7, 2, 9, 2, 0, 2, 7, 11, -1, -1, -1, -1},
{2, 3, 11, 0, 1, 8, 1, 7, 8, 1, 5, 7, -1, -1, -1, -1},
{11, 2, 1, 11, 1, 7, 7, 1, 5, -1, -1, -1, -1, -1, -1, -1},
{9, 5, 8, 8, 5, 7, 10, 1, 3, 10, 3, 11, -1, -1, -1, -1},
{5, 7, 0, 5, 0, 9, 7, 11, 0, 1, 0, 10, 11, 10, 0, -1},
{11, 10, 0, 11, 0, 3, 10, 5, 0, 8, 0, 7, 5, 7, 0, -1},
{11, 10, 5, 7, 11, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 0, 1, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 8, 3, 1, 9, 8, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
{1, 6, 5, 2, 6, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 6, 5, 1, 2, 6, 3, 0, 8, -1, -1, -1, -1, -1, -1, -1},
{9, 6, 5, 9, 0, 6, 0, 2, 6, -1, -1, -1, -1, -1, -1, -1},
{5, 9, 8, 5, 8, 2, 5, 2, 6, 3, 2, 8, -1, -1, -1, -1},
{2, 3, 11, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{11, 0, 8, 11, 2, 0, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 9, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1, -1, -1, -1},
{5, 10, 6, 1, 9, 2, 9, 11, 2, 9, 8, 11, -1, -1, -1, -1},
{6, 3, 11, 6, 5, 3, 5, 1, 3, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 11, 0, 11, 5, 0, 5, 1, 5, 11, 6, -1, -1, -1, -1},
{3, 11, 6, 0, 3, 6, 0, 6, 5, 0, 5, 9, -1, -1, -1, -1},
{6, 5, 9, 6, 9, 11, 11, 9, 8, -1, -1, -1, -1, -1, -1, -1},
{5, 10, 6, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 3, 0, 4, 7, 3, 6, 5, 10, -1, -1, -1, -1, -1, -1, -1},
{1, 9, 0, 5, 10, 6, 8, 4, 7, -1, -1, -1, -1, -1, -1, -1},
{10, 6, 5, 1, 9, 7, 1, 7, 3, 7, 9, 4, -1, -1, -1, -1},
{6, 1, 2, 6, 5, 1, 4, 7, 8, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 5, 5, 2, 6, 3, 0, 4, 3, 4, 7, -1, -1, -1, -1},
{8, 4, 7, 9, 0, 5, 0, 6, 5, 0, 2, 6, -1, -1, -1, -1},
{7, 3, 9, 7, 9, 4, 3, 2, 9, 5, 9, 6, 2, 6, 9, -1},
{3, 11, 2, 7, 8, 4, 10, 6, 5, -1, -1, -1, -1, -1, -1, -1},
{5, 10, 6, 4, 7, 2, 4, 2, 0, 2, 7, 11, -1, -1, -1, -1},
{0, 1, 9, 4, 7, 8, 2, 3, 11, 5, 10, 6, -1, -1, -1, -1},
{9, 2, 1, 9, 11, 2, 9, 4, 11, 7, 11, 4, 5, 10, 6, -1},
{8, 4, 7, 3, 11, 5, 3, 5, 1, 5, 11, 6, -1, -1, -1, -1},
{5, 1, 11, 5, 11, 6, 1, 0, 11, 7, 11, 4, 0, 4, 11, -1},
{0, 5, 9, 0, 6, 5, 0, 3, 6, 11, 6, 3, 8, 4, 7, -1},
{6, 5, 9, 6, 9, 11, 4, 7, 9, 7, 11, 9, -1, -1, -1, -1},
{10, 4, 9, 6, 4, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 10, 6, 4, 9, 10, 0, 8, 3, -1, -1, -1, -1, -1, -1, -1},
{10, 0, 1, 10, 6, 0, 6, 4, 0, -1, -1, -1, -1, -1, -1, -1},
{8, 3, 1, 8, 1, 6, 8, 6, 4, 6, 1, 10, -1, -1, -1, -1},
{1, 4, 9, 1, 2, 4, 2, 6, 4, -1, -1, -1, -1, -1, -1, -1},
{3, 0, 8, 1, 2, 9, 2, 4, 9, 2, 6, 4, -1, -1, -1, -1},
{0, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{8, 3, 2, 8, 2, 4, 4, 2, 6, -1, -1, -1, -1, -1, -1, -1},
{10, 4, 9, 10, 6, 4, 11, 2, 3, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 2, 2, 8, 11, 4, 9, 10, 4, 10, 6, -1, -1, -1, -1},
{3, 11, 2, 0, 1, 6, 0, 6, 4, 6, 1, 10, -1, -1, -1, -1},
{6, 4, 1, 6, 1, 10, 4, 8, 1, 2, 1, 11, 8, 11, 1, -1},
{9, 6, 4, 9, 3, 6, 9, 1, 3, 11, 6, 3, -1, -1, -1, -1},
{8, 11, 1, 8, 1, 0, 11, 6, 1, 9, 1, 4, 6, 4, 1, -1},
{3, 11, 6, 3, 6, 0, 0, 6, 4, -1, -1, -1, -1, -1, -1, -1},
{6, 4, 8, 11, 6, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{7, 10, 6, 7, 8, 10, 8, 9, 10, -1, -1, -1, -1, -1, -1, -1},
{0, 7, 3, 0, 10, 7, 0, 9, 10, 6, 7, 10, -1, -1, -1, -1},
{10, 6, 7, 1, 10, 7, 1, 7, 8, 1, 8, 0, -1, -1, -1, -1},
{10, 6, 7, 10, 7, 1, 1, 7, 3, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 6, 1, 6, 8, 1, 8, 9, 8, 6, 7, -1, -1, -1, -1},
{2, 6, 9, 2, 9, 1, 6, 7, 9, 0, 9, 3, 7, 3, 9, -1},
{7, 8, 0, 7, 0, 6, 6, 0, 2, -1, -1, -1, -1, -1, -1, -1},
{7, 3, 2, 6, 7, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{2, 3, 11, 10, 6, 8, 10, 8, 9, 8, 6, 7, -1, -1, -1, -1},
{2, 0, 7, 2, 7, 11, 0, 9, 7, 6, 7, 10, 9, 10, 7, -1},
{1, 8, 0, 1, 7, 8, 1, 10, 7, 6, 7, 10, 2, 3, 11, -1},
{11, 2, 1, 11, 1, 7, 10, 6, 1, 6, 7, 1, -1, -1, -1, -1},
{8, 9, 6, 8, 6, 7, 9, 1, 6, 11, 6, 3, 1, 3, 6, -1},
{0, 9, 1, 11, 6, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{7, 8, 0, 7, 0, 6, 3, 11, 0, 11, 6, 0, -1, -1, -1, -1},
{7, 11, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 0, 8, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 9, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{8, 1, 9, 8, 3, 1, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
{10, 1, 2, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, 3, 0, 8, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
{2, 9, 0, 2, 10, 9, 6, 11, 7, -1, -1, -1, -1, -1, -1, -1},
{6, 11, 7, 2, 10, 3, 10, 8, 3, 10, 9, 8, -1, -1, -1, -1},
{7, 2, 3, 6, 2, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{7, 0, 8, 7, 6, 0, 6, 2, 0, -1, -1, -1, -1, -1, -1, -1},
{2, 7, 6, 2, 3, 7, 0, 1, 9, -1, -1, -1, -1, -1, -1, -1},
{1, 6, 2, 1, 8, 6, 1, 9, 8, 8, 7, 6, -1, -1, -1, -1},
{10, 7, 6, 10, 1, 7, 1, 3, 7, -1, -1, -1, -1, -1, -1, -1},
{10, 7, 6, 1, 7, 10, 1, 8, 7, 1, 0, 8, -1, -1, -1, -1},
{0, 3, 7, 0, 7, 10, 0, 10, 9, 6, 10, 7, -1, -1, -1, -1},
{7, 6, 10, 7, 10, 8, 8, 10, 9, -1, -1, -1, -1, -1, -1, -1},
{6, 8, 4, 11, 8, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 6, 11, 3, 0, 6, 0, 4, 6, -1, -1, -1, -1, -1, -1, -1},
{8, 6, 11, 8, 4, 6, 9, 0, 1, -1, -1, -1, -1, -1, -1, -1},
{9, 4, 6, 9, 6, 3, 9, 3, 1, 11, 3, 6, -1, -1, -1, -1},
{6, 8, 4, 6, 11, 8, 2, 10, 1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, 3, 0, 11, 0, 6, 11, 0, 4, 6, -1, -1, -1, -1},
{4, 11, 8, 4, 6, 11, 0, 2, 9, 2, 10, 9, -1, -1, -1, -1},
{10, 9, 3, 10, 3, 2, 9, 4, 3, 11, 3, 6, 4, 6, 3, -1},
{8, 2, 3, 8, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1},
{0, 4, 2, 4, 6, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 9, 0, 2, 3, 4, 2, 4, 6, 4, 3, 8, -1, -1, -1, -1},
{1, 9, 4, 1, 4, 2, 2, 4, 6, -1, -1, -1, -1, -1, -1, -1},
{8, 1, 3, 8, 6, 1, 8, 4, 6, 6, 10, 1, -1, -1, -1, -1},
{10, 1, 0, 10, 0, 6, 6, 0, 4, -1, -1, -1, -1, -1, -1, -1},
{4, 6, 3, 4, 3, 8, 6, 10, 3, 0, 3, 9, 10, 9, 3, -1},
{10, 9, 4, 6, 10, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 9, 5, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, 4, 9, 5, 11, 7, 6, -1, -1, -1, -1, -1, -1, -1},
{5, 0, 1, 5, 4, 0, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
{11, 7, 6, 8, 3, 4, 3, 5, 4, 3, 1, 5, -1, -1, -1, -1},
{9, 5, 4, 10, 1, 2, 7, 6, 11, -1, -1, -1, -1, -1, -1, -1},
{6, 11, 7, 1, 2, 10, 0, 8, 3, 4, 9, 5, -1, -1, -1, -1},
{7, 6, 11, 5, 4, 10, 4, 2, 10, 4, 0, 2, -1, -1, -1, -1},
{3, 4, 8, 3, 5, 4, 3, 2, 5, 10, 5, 2, 11, 7, 6, -1},
{7, 2, 3, 7, 6, 2, 5, 4, 9, -1, -1, -1, -1, -1, -1, -1},
{9, 5, 4, 0, 8, 6, 0, 6, 2, 6, 8, 7, -1, -1, -1, -1},
{3, 6, 2, 3, 7, 6, 1, 5, 0, 5, 4, 0, -1, -1, -1, -1},
{6, 2, 8, 6, 8, 7, 2, 1, 8, 4, 8, 5, 1, 5, 8, -1},
{9, 5, 4, 10, 1, 6, 1, 7, 6, 1, 3, 7, -1, -1, -1, -1},
{1, 6, 10, 1, 7, 6, 1, 0, 7, 8, 7, 0, 9, 5, 4, -1},
{4, 0, 10, 4, 10, 5, 0, 3, 10, 6, 10, 7, 3, 7, 10, -1},
{7, 6, 10, 7, 10, 8, 5, 4, 10, 4, 8, 10, -1, -1, -1, -1},
{6, 9, 5, 6, 11, 9, 11, 8, 9, -1, -1, -1, -1, -1, -1, -1},
{3, 6, 11, 0, 6, 3, 0, 5, 6, 0, 9, 5, -1, -1, -1, -1},
{0, 11, 8, 0, 5, 11, 0, 1, 5, 5, 6, 11, -1, -1, -1, -1},
{6, 11, 3, 6, 3, 5, 5, 3, 1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 10, 9, 5, 11, 9, 11, 8, 11, 5, 6, -1, -1, -1, -1},
{0, 11, 3, 0, 6, 11, 0, 9, 6, 5, 6, 9, 1, 2, 10, -1},
{11, 8, 5, 11, 5, 6, 8, 0, 5, 10, 5, 2, 0, 2, 5, -1},
{6, 11, 3, 6, 3, 5, 2, 10, 3, 10, 5, 3, -1, -1, -1, -1},
{5, 8, 9, 5, 2, 8, 5, 6, 2, 3, 8, 2, -1, -1, -1, -1},
{9, 5, 6, 9, 6, 0, 0, 6, 2, -1, -1, -1, -1, -1, -1, -1},
{1, 5, 8, 1, 8, 0, 5, 6, 8, 3, 8, 2, 6, 2, 8, -1},
{1, 5, 6, 2, 1, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 3, 6, 1, 6, 10, 3, 8, 6, 5, 6, 9, 8, 9, 6, -1},
{10, 1, 0, 10, 0, 6, 9, 5, 0, 5, 6, 0, -1, -1, -1, -1},
{0, 3, 8, 5, 6, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{10, 5, 6, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{11, 5, 10, 7, 5, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{11, 5, 10, 11, 7, 5, 8, 3, 0, -1, -1, -1, -1, -1, -1, -1},
{5, 11, 7, 5, 10, 11, 1, 9, 0, -1, -1, -1, -1, -1, -1, -1},
{10, 7, 5, 10, 11, 7, 9, 8, 1, 8, 3, 1, -1, -1, -1, -1},
{11, 1, 2, 11, 7, 1, 7, 5, 1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, 1, 2, 7, 1, 7, 5, 7, 2, 11, -1, -1, -1, -1},
{9, 7, 5, 9, 2, 7, 9, 0, 2, 2, 11, 7, -1, -1, -1, -1},
{7, 5, 2, 7, 2, 11, 5, 9, 2, 3, 2, 8, 9, 8, 2, -1},
{2, 5, 10, 2, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1},
{8, 2, 0, 8, 5, 2, 8, 7, 5, 10, 2, 5, -1, -1, -1, -1},
{9, 0, 1, 5, 10, 3, 5, 3, 7, 3, 10, 2, -1, -1, -1, -1},
{9, 8, 2, 9, 2, 1, 8, 7, 2, 10, 2, 5, 7, 5, 2, -1},
{1, 3, 5, 3, 7, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 7, 0, 7, 1, 1, 7, 5, -1, -1, -1, -1, -1, -1, -1},
{9, 0, 3, 9, 3, 5, 5, 3, 7, -1, -1, -1, -1, -1, -1, -1},
{9, 8, 7, 5, 9, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{5, 8, 4, 5, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1},
{5, 0, 4, 5, 11, 0, 5, 10, 11, 11, 3, 0, -1, -1, -1, -1},
{0, 1, 9, 8, 4, 10, 8, 10, 11, 10, 4, 5, -1, -1, -1, -1},
{10, 11, 4, 10, 4, 5, 11, 3, 4, 9, 4, 1, 3, 1, 4, -1},
{2, 5, 1, 2, 8, 5, 2, 11, 8, 4, 5, 8, -1, -1, -1, -1},
{0, 4, 11, 0, 11, 3, 4, 5, 11, 2, 11, 1, 5, 1, 11, -1},
{0, 2, 5, 0, 5, 9, 2, 11, 5, 4, 5, 8, 11, 8, 5, -1},
{9, 4, 5, 2, 11, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{2, 5, 10, 3, 5, 2, 3, 4, 5, 3, 8, 4, -1, -1, -1, -1},
{5, 10, 2, 5, 2, 4, 4, 2, 0, -1, -1, -1, -1, -1, -1, -1},
{3, 10, 2, 3, 5, 10, 3, 8, 5, 4, 5, 8, 0, 1, 9, -1},
{5, 10, 2, 5, 2, 4, 1, 9, 2, 9, 4, 2, -1, -1, -1, -1},
{8, 4, 5, 8, 5, 3, 3, 5, 1, -1, -1, -1, -1, -1, -1, -1},
{0, 4, 5, 1, 0, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{8, 4, 5, 8, 5, 3, 9, 0, 5, 0, 3, 5, -1, -1, -1, -1},
{9, 4, 5, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 11, 7, 4, 9, 11, 9, 10, 11, -1, -1, -1, -1, -1, -1, -1},
{0, 8, 3, 4, 9, 7, 9, 11, 7, 9, 10, 11, -1, -1, -1, -1},
{1, 10, 11, 1, 11, 4, 1, 4, 0, 7, 4, 11, -1, -1, -1, -1},
{3, 1, 4, 3, 4, 8, 1, 10, 4, 7, 4, 11, 10, 11, 4, -1},
{4, 11, 7, 9, 11, 4, 9, 2, 11, 9, 1, 2, -1, -1, -1, -1},
{9, 7, 4, 9, 11, 7, 9, 1, 11, 2, 11, 1, 0, 8, 3, -1},
{11, 7, 4, 11, 4, 2, 2, 4, 0, -1, -1, -1, -1, -1, -1, -1},
{11, 7, 4, 11, 4, 2, 8, 3, 4, 3, 2, 4, -1, -1, -1, -1},
{2, 9, 10, 2, 7, 9, 2, 3, 7, 7, 4, 9, -1, -1, -1, -1},
{9, 10, 7, 9, 7, 4, 10, 2, 7, 8, 7, 0, 2, 0, 7, -1},
{3, 7, 10, 3, 10, 2, 7, 4, 10, 1, 10, 0, 4, 0, 10, -1},
{1, 10, 2, 8, 7, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 9, 1, 4, 1, 7, 7, 1, 3, -1, -1, -1, -1, -1, -1, -1},
{4, 9, 1, 4, 1, 7, 0, 8, 1, 8, 7, 1, -1, -1, -1, -1},
{4, 0, 3, 7, 4, 3, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{4, 8, 7, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{9, 10, 8, 10, 11, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 0, 9, 3, 9, 11, 11, 9, 10, -1, -1, -1, -1, -1, -1, -1},
{0, 1, 10, 0, 10, 8, 8, 10, 11, -1, -1, -1, -1, -1, -1, -1},
{3, 1, 10, 11, 3, 10, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 2, 11, 1, 11, 9, 9, 11, 8, -1, -1, -1, -1, -1, -1, -1},
{3, 0, 9, 3, 9, 11, 1, 2, 9, 2, 11, 9, -1, -1, -1, -1},
{0, 2, 11, 8, 0, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{3, 2, 11, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{2, 3, 8, 2, 8, 10, 10, 8, 9, -1, -1, -1, -1, -1, -1, -1},
{9, 10, 2, 0, 9, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{2, 3, 8, 2, 8, 10, 0, 1, 8, 1, 10, 8, -1, -1, -1, -1},
{1, 10, 2, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{1, 3, 8, 9, 1, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 9, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{0, 3, 8, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1},
{-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}};
} // namespace dynafu
} // namespace cv
#endif
modules/rgbd/src/nonrigid_icp.cpp
+473
View File
@@ -0,0 +1,473 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include <algorithm>
#include "precomp.hpp"
#include "nonrigid_icp.hpp"
#define MAD_SCALE 1.4826f
#define TUKEY_B 4.6851f
#define HUBER_K 1.345f
namespace cv
{
namespace dynafu
{
using namespace kinfu;
NonRigidICP::NonRigidICP(const Matx33f _intrinsics, const cv::Ptr<dynafu::TSDFVolume>& _volume, int _iterations) :
iterations(_iterations), volume(_volume), intrinsics(_intrinsics)
{}
class ICPImpl : public NonRigidICP
{
public:
ICPImpl(const cv::Matx33f _intrinsics, const cv::Ptr<dynafu::TSDFVolume>& _volume, int _iterations);
virtual bool estimateWarpNodes(WarpField& currentWarp, const Affine3f &pose,
InputArray vertImage, InputArray oldPoints,
InputArray oldNormals, InputArray newPoints,
InputArray newNormals) const override;
virtual ~ICPImpl() {}
private:
float median(std::vector<float> v) const;
float tukeyWeight(float x, float sigma) const;
float huberWeight(Vec3f v, float sigma) const;
};
ICPImpl::ICPImpl(const Matx33f _intrinsics, const cv::Ptr<dynafu::TSDFVolume>& _volume, int _iterations) :
NonRigidICP(_intrinsics, _volume, _iterations)
{}
static inline bool fastCheck(const Point3f& p)
{
return !cvIsNaN(p.x);
}
float ICPImpl::median(std::vector<float> v) const
{
size_t n = v.size()/2;
if(n == 0) return 0;
std::nth_element(v.begin(), v.begin()+n, v.end());
float vn = v[n];
if(n%2 == 0)
{
std::nth_element(v.begin(), v.begin()+n-1, v.end());
return (vn+v[n-1])/2.f;
}
else
{
return vn;
}
}
float ICPImpl::tukeyWeight(float r, float sigma) const
{
float x = r/sigma;
if(std::abs(x) <= TUKEY_B)
{
float y = 1 - (x*x)/(TUKEY_B * TUKEY_B);
return y*y;
} else return 0;
}
float ICPImpl::huberWeight(Vec3f v, float sigma) const
{
if(sigma == 0) return 0.f;
float x = (float)std::abs(norm(v)/sigma);
return (x > HUBER_K)? HUBER_K/x : 1.f;
}
bool ICPImpl::estimateWarpNodes(WarpField& currentWarp, const Affine3f &pose,
InputArray _vertImage, InputArray _oldPoints,
InputArray _oldNormals, InputArray _newPoints,
InputArray _newNormals) const
{
CV_Assert(_vertImage.isMat());
CV_Assert(_oldPoints.isMat());
CV_Assert(_newPoints.isMat());
CV_Assert(_newNormals.isMat());
Mat vertImage = _vertImage.getMat();
Mat oldPoints = _oldPoints.getMat();
Mat newPoints = _newPoints.getMat();
Mat newNormals = _newNormals.getMat();
Mat oldNormals = _oldNormals.getMat();
CV_Assert(!vertImage.empty());
CV_Assert(!oldPoints.empty());
CV_Assert(!newPoints.empty());
CV_Assert(!newNormals.empty());
const NodeVectorType& warpNodes = currentWarp.getNodes();
Affine3f T_lw = pose.inv() * volume->pose;
// Accumulate regularisation term for each node in the heiarchy
const std::vector<NodeVectorType>& regNodes = currentWarp.getGraphNodes();
const heirarchyType& regGraph = currentWarp.getRegGraph();
int totalNodes = (int)warpNodes.size();
for(const auto& nodes: regNodes) totalNodes += (int)nodes.size();
// level-wise regularisation components of A and b (from Ax = b) for each node in hierarchy
Mat_<float> b_reg(6*totalNodes, 1, 0.f);
Mat_<float> A_reg(6*totalNodes, 6*totalNodes, 0.f);
// indices for each node block to A,b matrices. It determines the order
// in which paramters are laid out
std::vector<int> baseIndices(currentWarp.n_levels, 0);
for(int l = currentWarp.n_levels-2; l >= 0; l--)
{
baseIndices[l] = baseIndices[l+1]+6*((int)regNodes[l].size());
}
for(const int& i: baseIndices) std::cout << i << ", ";
std::cout << std::endl;
// populate residuals for each edge in the graph to calculate sigma
std::vector<float> reg_residuals;
float RegEnergy = 0;
int numEdges = 0;
for(int l = 0; l < (currentWarp.n_levels-1); l++)
{
const std::vector<nodeNeighboursType>& level = regGraph[l];
const NodeVectorType& currentLevelNodes = (l == 0)? warpNodes : regNodes[l-1];
const NodeVectorType& nextLevelNodes = regNodes[l];
std::cout << currentLevelNodes.size() << " " << nextLevelNodes.size() << std::endl;
for(size_t node = 0; node < level.size(); node++)
{
const nodeNeighboursType& children = level[node];
Vec3f nodePos = currentLevelNodes[node]->pos;
Affine3f nodeTransform = currentLevelNodes[node]->transform;
for(int c = 0; c < currentWarp.k; c++)
{
const int child = children[c];
Vec3f childPos = nextLevelNodes[child]->pos;
Vec3f childTranslation = nextLevelNodes[child]->transform.translation();
Vec3f re = nodeTransform * (childPos - nodePos) + nodePos
- (childTranslation + childPos);
numEdges++;
reg_residuals.push_back((float)norm(re));
RegEnergy += (float)norm(re);
}
}
}
Mat_<float> J_reg(6*numEdges, 6*totalNodes, 0.f);
std::cout << "Total reg energy: " << RegEnergy << ", Average: " << RegEnergy/numEdges << std::endl;
float reg_med = median(reg_residuals);
std::for_each(reg_residuals.begin(), reg_residuals.end(),
[reg_med](float& x)
{
x = std::abs(x-reg_med);
});
float reg_sigma = MAD_SCALE * median(reg_residuals);
std::cout << "[Reg] Sigma: " << reg_sigma << " from " << reg_residuals.size() << " residuals " << std::endl;
for(int l = 0; l < (currentWarp.n_levels-1); l++)
{
const std::vector<nodeNeighboursType>& level = regGraph[l];
const NodeVectorType& currentLevelNodes = (l == 0)? warpNodes : regNodes[l-1];
const NodeVectorType& nextLevelNodes = regNodes[l];
for(size_t node = 0; node < level.size(); node++)
{
const nodeNeighboursType& children = level[node];
Vec3f nodePos = currentLevelNodes[node]->pos;
Affine3f nodeTransform = currentLevelNodes[node]->transform;
int parentIndex = baseIndices[l]+6*(int)node;
for(int edge = 0; edge < currentWarp.k; edge++)
{
const int child = children[edge];
const Ptr<WarpNode> childNode = nextLevelNodes[child];
Vec3f childTranslation = childNode->transform.translation();
Vec3f childPos = childNode->pos;
Vec3f transformedChild = nodeTransform * (childPos - nodePos);
Vec3f r_edge = transformedChild + nodePos - (childTranslation + childPos);
if(norm(r_edge) > 0.01) continue;
float robustWeight = huberWeight(r_edge, reg_sigma);
// take sqrt since radius is stored as squared distance
float edgeWeight = sqrt(min(childNode->radius, currentLevelNodes[node]->radius));
Vec3f v1 = transformedChild.cross(r_edge);
float w = 1 * robustWeight * edgeWeight;
b_reg(parentIndex+0) += -w * v1[0];
b_reg(parentIndex+1) += -w * v1[1];
b_reg(parentIndex+2) += -w * v1[2];
b_reg(parentIndex+3) += -w * r_edge[0];
b_reg(parentIndex+4) += -w * r_edge[1];
b_reg(parentIndex+5) += -w * r_edge[2];
int childIndex = baseIndices[l+1]+6*child;
Vec3f v2 = childTranslation.cross(r_edge);
b_reg(childIndex+0) += w * v2[0];
b_reg(childIndex+1) += w * v2[1];
b_reg(childIndex+2) += w * v2[2];
b_reg(childIndex+3) += w * r_edge[0];
b_reg(childIndex+4) += w * r_edge[1];
b_reg(childIndex+5) += w * r_edge[2];
Matx33f Tj_Vj_Vi_cross(0, -transformedChild[2], transformedChild[1],
transformedChild[2], 0, -transformedChild[0],
-transformedChild[1], transformedChild[0], 0);
Matx33f tj_cross(0, -childTranslation[2], childTranslation[1],
childTranslation[2], 0, -childTranslation[0],
-childTranslation[1], childTranslation[0], 0);
// place top left elements
Matx33f top_left = Tj_Vj_Vi_cross * tj_cross;
for(int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
{
A_reg(parentIndex+i, childIndex+j) += w * top_left(i, j);
A_reg(childIndex+i, parentIndex+j) += w * top_left(i, j);
}
// place top right elements
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
{
A_reg(parentIndex+i, childIndex+j+3) += - w * Tj_Vj_Vi_cross(i, j);
A_reg(childIndex+i, parentIndex+j+3) += - w * Tj_Vj_Vi_cross(i, j);
}
// place bottom left elements
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
{
A_reg(parentIndex+i+3, childIndex+j) += w * tj_cross(i, j);
A_reg(childIndex+i+3, parentIndex+j) += w * tj_cross(i, j);
}
// place bottom right elements (which is -I_3)
for(int i = 0; i < 3; i++)
{
A_reg(parentIndex+i+3, childIndex+i+3) += -w;
A_reg(childIndex+i+3, parentIndex+i+3) += -w;
}
}
}
}
std::vector<float> residuals;
Mat Vg(oldPoints.size(), CV_32FC3, nan3);
Mat Vc(oldPoints.size(), CV_32FC3, nan3);
Mat Nc(oldPoints.size(), CV_32FC3, nan3);
cv::Intr::Projector proj = Intr(intrinsics).makeProjector();
for (int y = 0; y < oldPoints.size().height; y++)
{
for (int x = 0; x < oldPoints.size().width; x++)
{
// Obtain correspondence by projecting Tu_Vg
Vec3f curV = oldPoints.at<Vec3f>(y, x);
if (curV == Vec3f::all(0) || cvIsNaN(curV[0]) || cvIsNaN(curV[1]) || cvIsNaN(curV[2]))
continue;
Point2f newCoords = proj(oldPoints.at<Point3f>(y, x));
if(!(newCoords.x >= 0 && newCoords.x < newPoints.cols - 1 &&
newCoords.y >= 0 && newCoords.y < newPoints.rows - 1))
continue;
// TODO: interpolate Vg instead of simply converting projected coords to int
Vg.at<Vec3f>(y, x) = vertImage.at<Vec3f>((int)newCoords.y, (int)newCoords.x);
// bilinearly interpolate newPoints under newCoords point
int xi = cvFloor(newCoords.x), yi = cvFloor(newCoords.y);
float tx = newCoords.x - xi, ty = newCoords.y - yi;
const ptype* prow0 = newPoints.ptr<ptype>(yi+0);
const ptype* prow1 = newPoints.ptr<ptype>(yi+1);
Point3f p00 = fromPtype(prow0[xi+0]);
Point3f p01 = fromPtype(prow0[xi+1]);
Point3f p10 = fromPtype(prow1[xi+0]);
Point3f p11 = fromPtype(prow1[xi+1]);
//do not fix missing data
if(!(fastCheck(p00) && fastCheck(p01) &&
fastCheck(p10) && fastCheck(p11)))
continue;
Point3f p0 = p00 + tx*(p01 - p00);
Point3f p1 = p10 + tx*(p11 - p10);
Point3f newP = (p0 + ty*(p1 - p0));
const ptype* nrow0 = newNormals.ptr<ptype>(yi+0);
const ptype* nrow1 = newNormals.ptr<ptype>(yi+1);
Point3f n00 = fromPtype(nrow0[xi+0]);
Point3f n01 = fromPtype(nrow0[xi+1]);
Point3f n10 = fromPtype(nrow1[xi+0]);
Point3f n11 = fromPtype(nrow1[xi+1]);
if(!(fastCheck(n00) && fastCheck(n01) &&
fastCheck(n10) && fastCheck(n11)))
continue;
Point3f n0 = n00 + tx*(n01 - n00);
Point3f n1 = n10 + tx*(n11 - n10);
Point3f newN = n0 + ty*(n1 - n0);
Vc.at<Point3f>(y, x) = newP;
Nc.at<Point3f>(y, x) = newN;
Vec3f diff = oldPoints.at<Vec3f>(y, x) - Vec3f(newP);
if(diff.dot(diff) > 0.0004f) continue;
if(abs(newN.dot(oldNormals.at<Point3f>(y, x))) < std::cos((float)CV_PI / 2)) continue;
float rd = newN.dot(diff);
residuals.push_back(rd);
}
}
float med = median(residuals);
std::for_each(residuals.begin(), residuals.end(), [med](float& x){x = std::abs(x-med);});
std::cout << "median: " << med << " from " << residuals.size() << " residuals " << std::endl;
float sigma = MAD_SCALE * median(residuals);
for(int y = 0; y < oldPoints.size().height; y++)
{
for(int x = 0; x < oldPoints.size().width; x++)
{
Vec3f curV = oldPoints.at<Vec3f>(y, x);
if (curV == Vec3f::all(0) || cvIsNaN(curV[0]))
continue;
Vec3f V = Vg.at<Vec3f>(y, x);
if (V == Vec3f::all(0) || cvIsNaN(V[0]))
continue;
V[0] *= volume->volResolution.x;
V[1] *= volume->volResolution.y;
V[2] *= volume->volResolution.z;
if(!fastCheck(Vc.at<Point3f>(y, x)))
continue;
if(!fastCheck(Nc.at<Point3f>(y, x)))
continue;
Point3i p((int)V[0], (int)V[1], (int)V[2]);
Vec3f diff = oldPoints.at<Vec3f>(y, x) - Vc.at<Vec3f>(y, x);
float rd = Nc.at<Vec3f>(y, x).dot(diff);
int n;
nodeNeighboursType neighbours = volume->getVoxelNeighbours(p, n);
float totalNeighbourWeight = 0.f;
float neighWeights[DYNAFU_MAX_NEIGHBOURS];
for (int i = 0; i < n; i++)
{
int neigh = neighbours[i];
neighWeights[i] = warpNodes[neigh]->weight(Point3f(V)*volume->voxelSize);
totalNeighbourWeight += neighWeights[i];
}
if(totalNeighbourWeight < 1e-5) continue;
for (int i = 0; i < n; i++)
{
if(neighWeights[i] < 0.01) continue;
int neigh = neighbours[i];
Vec3f Tj_Vg_Vj = (warpNodes[neigh]->transform *
(Point3f(V)*volume->voxelSize - warpNodes[neigh]->pos));
Matx33f Tj_Vg_Vj_x(0, -Tj_Vg_Vj[2], Tj_Vg_Vj[1],
Tj_Vg_Vj[2], 0, -Tj_Vg_Vj[0],
-Tj_Vg_Vj[1], Tj_Vg_Vj[0], 0);
Vec3f v1 = (Tj_Vg_Vj_x * T_lw.rotation().t()) * Nc.at<Vec3f>(y, x);
Vec3f v2 = T_lw.rotation().t() * Nc.at<Vec3f>(y, x);
Matx61f J_dataT(v1[0], v1[1], v1[2], v2[0], v2[1], v2[2]);
Matx16f J_data = J_dataT.t();
Matx66f H_data = J_dataT * J_data;
float w = (neighWeights[i] / totalNeighbourWeight);
float robustWeight = tukeyWeight(rd, sigma);
int blockIndex = baseIndices[0]+6*neigh;
for(int row = 0; row < 6; row++)
for(int col = 0; col < 6; col++)
A_reg(blockIndex+row, blockIndex+col) += robustWeight * w * w * H_data(row, col);
for(int row = 0; row < 6; row++)
b_reg(blockIndex+row) += -robustWeight * rd * w * J_dataT(row);
}
}
}
double det = determinant(A_reg);
std::cout << "A_reg det:" << det << std::endl;
std::cout << "Solving " << 6*totalNodes << std::endl;
Mat_<float> nodeTwists(6*totalNodes, 1, 0.f);
bool result = solve(A_reg, b_reg, nodeTwists, DECOMP_SVD);
std::cout << "Done " << result << std::endl;
for(int i = 0; i < (int)warpNodes.size(); i++)
{
int idx = baseIndices[0]+6*i;
Vec3f r(nodeTwists(idx), nodeTwists(idx+1), nodeTwists(idx+2));
Vec3f t(nodeTwists(idx+3), nodeTwists(idx+4), nodeTwists(idx+5));
Affine3f tinc(r, t);
warpNodes[i]->transform = warpNodes[i]->transform * tinc;
}
std::cout << "Nan count: " << "/" << warpNodes.size() << "\n" << std::endl;
return true;
}
cv::Ptr<NonRigidICP> makeNonRigidICP(const cv::Matx33f& _intrinsics, const cv::Ptr<TSDFVolume>& _volume,
int _iterations)
{
return makePtr<ICPImpl>(_intrinsics, _volume, _iterations);
}
} // namespace dynafu
} // namespace cv
modules/rgbd/src/nonrigid_icp.hpp
+41
View File
@@ -0,0 +1,41 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#ifndef __OPENCV_DYNAFU_NONRIGID_ICP_H__
#define __OPENCV_DYNAFU_NONRIGID_ICP_H__
#include "precomp.hpp"
#include "warpfield.hpp"
#include "dynafu_tsdf.hpp"
namespace cv {
namespace dynafu {
class NonRigidICP
{
public:
NonRigidICP(const cv::Matx33f _intrinsics, const cv::Ptr<TSDFVolume>& _volume, int _iterations);
virtual bool estimateWarpNodes(WarpField& currentWarp, const Affine3f& pose,
InputArray vertImage, InputArray oldPoints,
InputArray oldNormals,
InputArray newPoints, InputArray newNormals) const = 0;
virtual ~NonRigidICP() { }
protected:
int iterations;
const cv::Ptr<dynafu::TSDFVolume>& volume;
cv::Matx33f intrinsics;
};
cv::Ptr<NonRigidICP> makeNonRigidICP(const cv::Matx33f& _intrinsics, const cv::Ptr<TSDFVolume>& _volume,
int _iterations);
} // namespace dynafu
} // namespace cv
#endif
modules/rgbd/src/normal_lut.hpp
+404
View File
@@ -0,0 +1,404 @@
// Auto-generated by scripts/create_depth_normal_lut.py 20
static const int GRANULARITY = 20;
static const unsigned char NORMAL_LUT[20][20][20] =
{{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}},
{{32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128},
{16, 32, 32, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 128, 128},
{16, 16, 16, 32, 32, 32, 32, 32, 32, 64, 64, 64, 128, 128, 128, 128, 128, 128, 1, 1},
{16, 16, 16, 16, 16, 16, 32, 32, 32, 32, 64, 128, 128, 128, 128, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 32, 32, 64, 128, 128, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 16, 16, 8, 8, 4, 2, 2, 1, 1, 1, 1, 1, 1, 1},
{16, 16, 16, 16, 16, 16, 8, 8, 8, 8, 4, 2, 2, 2, 2, 1, 1, 1, 1, 1},
{16, 16, 16, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 1, 1},
{16, 8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2},
{8, 8, 8, 8, 8, 8, 8, 4, 4, 4, 4, 4, 4, 4, 2, 2, 2, 2, 2, 2}}};
modules/rgbd/src/precomp.hpp
+37
View File
@@ -0,0 +1,37 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
#ifndef __OPENCV_PRECOMP_H__
#define __OPENCV_PRECOMP_H__
#include <iostream>
#include <limits>
#include <vector>
#include <list>
#include <set>
#include <unordered_set>
#include "opencv2/core/utility.hpp"
#include "opencv2/core/private.hpp"
#include "opencv2/core/hal/intrin.hpp"
#include "opencv2/core/ocl.hpp"
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/geometry.hpp"
#include "opencv2/geometry/detail/optimizer.hpp"
#include "opencv2/ptcloud.hpp"
#include "opencv2/ptcloud/volume_settings.hpp"
#include "opencv2/ptcloud/detail/pose_graph.hpp"
#include "opencv2/ptcloud/detail/kinfu_frame.hpp"
#include "opencv2/ptcloud/detail/submap.hpp"
#include "opencv2/rgbd.hpp"
#include "utils.hpp"
#endif
modules/rgbd/src/utils.hpp
+181
View File
@@ -0,0 +1,181 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
#ifndef __OPENCV_RGBD_UTILS_HPP__
#define __OPENCV_RGBD_UTILS_HPP__
#include "opencv2/core/matx.hpp"
namespace cv
{
// One place to turn intrinsics on and off
#define USE_INTRINSICS CV_SIMD128
typedef float depthType;
const float qnan = std::numeric_limits<float>::quiet_NaN();
const cv::Vec3f nan3(qnan, qnan, qnan);
#if USE_INTRINSICS
const cv::v_float32x4 nanv(qnan, qnan, qnan, qnan);
#endif
inline bool isNaN(cv::Point3f p)
{
return (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z));
}
#if USE_INTRINSICS
static inline bool isNaN(const cv::v_float32x4& p)
{
return cv::v_check_any(v_ne(p, p));
}
#endif
inline size_t roundDownPow2(size_t x)
{
size_t shift = 0;
while(x != 0)
{
shift++; x >>= 1;
}
return (size_t)(1ULL << (shift-1));
}
template<> class DataType<cv::Point3f>
{
public:
typedef float value_type;
typedef value_type work_type;
typedef value_type channel_type;
typedef value_type vec_type;
enum { generic_type = 0,
depth = CV_32F,
channels = 3,
fmt = (int)'f',
type = CV_MAKETYPE(depth, channels)
};
};
template<> class DataType<cv::Vec3f>
{
public:
typedef float value_type;
typedef value_type work_type;
typedef value_type channel_type;
typedef value_type vec_type;
enum { generic_type = 0,
depth = CV_32F,
channels = 3,
fmt = (int)'f',
type = CV_MAKETYPE(depth, channels)
};
};
template<> class DataType<cv::Vec4f>
{
public:
typedef float value_type;
typedef value_type work_type;
typedef value_type channel_type;
typedef value_type vec_type;
enum { generic_type = 0,
depth = CV_32F,
channels = 4,
fmt = (int)'f',
type = CV_MAKETYPE(depth, channels)
};
};
typedef cv::Vec4f ptype;
inline cv::Vec3f fromPtype(const ptype& x)
{
return cv::Vec3f(x[0], x[1], x[2]);
}
inline ptype toPtype(const cv::Vec3f& x)
{
return ptype(x[0], x[1], x[2], 0);
}
enum
{
DEPTH_TYPE = DataType<depthType>::type,
POINT_TYPE = DataType<ptype >::type,
COLOR_TYPE = DataType<ptype >::type
};
typedef cv::Mat_< ptype > Points;
typedef Points Normals;
typedef Points Colors;
typedef cv::Mat_< depthType > Depth;
struct Intr
{
/** @brief Camera intrinsics */
/** Reprojects screen point to camera space given z coord. */
struct Reprojector
{
Reprojector() {}
inline Reprojector(Intr intr)
{
fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy;
cx = intr.cx, cy = intr.cy;
}
template<typename T>
inline cv::Point3_<T> operator()(cv::Point3_<T> p) const
{
T x = p.z * (p.x - cx) * fxinv;
T y = p.z * (p.y - cy) * fyinv;
return cv::Point3_<T>(x, y, p.z);
}
float fxinv, fyinv, cx, cy;
};
/** Projects camera space vector onto screen */
struct Projector
{
inline Projector(Intr intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { }
template<typename T>
inline cv::Point_<T> operator()(cv::Point3_<T> p) const
{
T invz = T(1)/p.z;
T x = fx*(p.x*invz) + cx;
T y = fy*(p.y*invz) + cy;
return cv::Point_<T>(x, y);
}
template<typename T>
inline cv::Point_<T> operator()(cv::Point3_<T> p, cv::Point3_<T>& pixVec) const
{
T invz = T(1)/p.z;
pixVec = cv::Point3_<T>(p.x*invz, p.y*invz, 1);
T x = fx*pixVec.x + cx;
T y = fy*pixVec.y + cy;
return cv::Point_<T>(x, y);
}
float fx, fy, cx, cy;
};
Intr() : fx(), fy(), cx(), cy() { }
Intr(float _fx, float _fy, float _cx, float _cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { }
Intr(cv::Matx33f m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { }
// scale intrinsics to pyramid level
inline Intr scale(int pyr) const
{
float factor = (1.f /(1 << pyr));
return Intr(fx*factor, fy*factor, cx*factor, cy*factor);
}
inline Reprojector makeReprojector() const { return Reprojector(*this); }
inline Projector makeProjector() const { return Projector(*this); }
inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); }
float fx, fy, cx, cy;
};
} // namespace cv
#endif
modules/rgbd/src/warpfield.cpp
+353
View File
@@ -0,0 +1,353 @@
#include "precomp.hpp"
#include "warpfield.hpp"
namespace cv {
namespace dynafu {
WarpField::WarpField(int _maxNeighbours, int K, int levels, float baseResolution, float resolutionGrowth):
k(K), n_levels(levels),
nodes(), maxNeighbours(_maxNeighbours), // good amount for dense kinfu pointclouds
baseRes(baseResolution),
resGrowthRate(resolutionGrowth),
regGraphNodes(n_levels-1),
heirarchy(n_levels-1),
nodeIndex(nullptr)
{
CV_Assert(k <= DYNAFU_MAX_NEIGHBOURS);
}
NodeVectorType const& WarpField::getNodes() const
{
return nodes;
}
std::vector<NodeVectorType> const& WarpField::getGraphNodes() const
{
return regGraphNodes;
}
heirarchyType const& WarpField::getRegGraph() const
{
return heirarchy;
}
bool PtCmp(cv::Point3f a, cv::Point3f b)
{
return (a.x < b.x) ||
((a.x >= b.x) && (a.y < b.y)) ||
((a.x >= b.x) && (a.y >= b.y) && (a.z < b.z));
}
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > WarpField::getNodeIndex() const
{
return nodeIndex;
}
Mat getNodesPos(NodeVectorType nv)
{
Mat nodePos((int)nv.size(), 3, CV_32F);
for(int i = 0; i < (int)nv.size(); i++)
{
nodePos.at<float>(i, 0) = nv[i]->pos.x;
nodePos.at<float>(i, 1) = nv[i]->pos.y;
nodePos.at<float>(i, 2) = nv[i]->pos.z;
}
return nodePos;
}
void WarpField::updateNodesFromPoints(InputArray inputPoints)
{
Mat points_matrix(inputPoints.size().height, 3, CV_32F);
if(inputPoints.channels() == 1)
{
points_matrix = inputPoints.getMat().colRange(0, 3);
}
else
{
points_matrix = inputPoints.getMat().reshape(1).colRange(0, 3).clone();
}
cvflann::LinearIndexParams params;
flann::GenericIndex<flann::L2_Simple <float> > searchIndex(points_matrix, params);
AutoBuffer<bool> validIndex;
removeSupported(searchIndex, validIndex);
NodeVectorType newNodes;
if((int)nodes.size() > k)
{
newNodes = subsampleIndex(points_matrix, searchIndex, validIndex, baseRes, nodeIndex);
}
else
{
newNodes = subsampleIndex(points_matrix, searchIndex, validIndex, baseRes);
}
initTransforms(newNodes);
nodes.insert(nodes.end(), newNodes.begin(), newNodes.end());
nodesPos = getNodesPos(nodes);
//re-build index
nodeIndex = new flann::GenericIndex<flann::L2_Simple<float> >(nodesPos,
cvflann::LinearIndexParams());
constructRegGraph();
}
void WarpField::removeSupported(flann::GenericIndex<flann::L2_Simple<float> >& ind,
AutoBuffer<bool>& validInd)
{
validInd.allocate(ind.size());
std::fill_n(validInd.data(), ind.size(), true);
for(WarpNode* n: nodes)
{
std::vector<float> query = {n->pos.x, n->pos.y, n->pos.z};
std::vector<int> indices_vec(maxNeighbours);
std::vector<float> dists_vec(maxNeighbours);
ind.radiusSearch(query, indices_vec, dists_vec, n->radius, cvflann::SearchParams());
for(auto i: indices_vec)
{
validInd[i] = false;
}
}
}
NodeVectorType WarpField::subsampleIndex(Mat& pmat,
flann::GenericIndex<flann::L2_Simple<float> >& ind,
AutoBuffer<bool>& validIndex, float res,
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > knnIndex)
{
CV_TRACE_FUNCTION();
NodeVectorType temp_nodes;
for(int i = 0; i < (int)validIndex.size(); i++)
{
if(!validIndex[i])
{
continue;
}
std::vector<int> indices_vec(maxNeighbours);
std::vector<float> dist_vec(maxNeighbours);
ind.radiusSearch(pmat.row(i), indices_vec, dist_vec, res, cvflann::SearchParams());
Ptr<WarpNode> wn = new WarpNode;
Point3f centre(0, 0, 0);
float len = 0;
for(int index: indices_vec)
{
if(validIndex[index])
{
centre += Point3f(pmat.at<float>(index, 0),
pmat.at<float>(index, 1),
pmat.at<float>(index, 2));
len++;
}
}
centre /= len;
wn->pos = centre;
for(auto index: indices_vec)
{
validIndex[index] = false;
}
std::vector<int> knn_indices(k+1, 0);
std::vector<float> knn_dists(k+1, 0);
std::vector<float> query = {wn->pos.x, wn->pos.y, wn->pos.z};
if(knnIndex != nullptr)
{
knnIndex->knnSearch(query, knn_indices, knn_dists, (k+1), cvflann::SearchParams());
wn->radius = *std::max_element(knn_dists.begin(), knn_dists.end());
}
else
{
wn->radius = res;
}
wn->transform = Affine3f::Identity();
temp_nodes.push_back(wn);
}
return temp_nodes;
}
void WarpField::initTransforms(NodeVectorType nv)
{
if(nodesPos.size().height == 0)
{
return;
}
for(auto nodePtr: nv)
{
std::vector<int> knnIndices(k);
std::vector<float> knnDists(k);
std::vector<float> query = {nodePtr->pos.x, nodePtr->pos.y, nodePtr->pos.z};
nodeIndex->knnSearch(query ,knnIndices, knnDists, k, cvflann::SearchParams());
std::vector<float> weights(knnIndices.size());
std::vector<Affine3f> transforms(knnIndices.size());
size_t i = 0;
for(int idx: knnIndices)
{
weights[i] = nodes[idx]->weight(nodePtr->pos);
transforms[i++] = nodes[idx]->transform;
}
Affine3f pose = DQB(weights, transforms);
// linearly interpolate translations
Vec3f translation(0,0,0);
float totalWeight = 0;
for(i = 0; i < transforms.size(); i++)
{
translation += weights[i]*transforms[i].translation();
totalWeight += weights[i];
}
if(totalWeight < 1e-5) translation = Vec3f(0, 0, 0);
else translation /= totalWeight;
nodePtr->transform = Affine3f(pose.rotation(), translation);
}
}
void WarpField::constructRegGraph()
{
CV_TRACE_FUNCTION();
regGraphNodes.clear();
if(n_levels == 1) // First level (Nwarp) is already stored in nodes
{
return;
}
float effResolution = baseRes*resGrowthRate;
NodeVectorType curNodes = nodes;
Mat curNodeMatrix = getNodesPos(curNodes);
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > curNodeIndex(
new flann::GenericIndex<flann::L2_Simple<float> >(curNodeMatrix,
cvflann::LinearIndexParams()));
for(int l = 0; l < (n_levels-1); l++)
{
AutoBuffer<bool> nodeValidity;
nodeValidity.allocate(curNodeIndex->size());
std::fill_n(nodeValidity.data(), curNodeIndex->size(), true);
NodeVectorType coarseNodes = subsampleIndex(curNodeMatrix, *curNodeIndex, nodeValidity,
effResolution);
initTransforms(coarseNodes);
Mat coarseNodeMatrix = getNodesPos(coarseNodes);
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > coarseNodeIndex(
new flann::GenericIndex<flann::L2_Simple<float> >(coarseNodeMatrix,
cvflann::LinearIndexParams()));
heirarchy[l] = std::vector<nodeNeighboursType>(curNodes.size());
for(int i = 0; i < (int)curNodes.size(); i++)
{
std::vector<int> children_indices(k);
std::vector<float> children_dists(k);
std::vector<float> query = {curNodeMatrix.at<float>(i, 0),
curNodeMatrix.at<float>(i, 1),
curNodeMatrix.at<float>(i, 2)};
coarseNodeIndex->knnSearch(query, children_indices, children_dists, k,
cvflann::SearchParams());
heirarchy[l][i].fill(-1);
std::copy(children_indices.begin(), children_indices.end(), heirarchy[l][i].begin());
}
regGraphNodes.push_back(coarseNodes);
curNodes = coarseNodes;
curNodeMatrix = coarseNodeMatrix;
curNodeIndex = coarseNodeIndex;
effResolution *= resGrowthRate;
}
}
Point3f WarpField::applyWarp(Point3f p, const nodeNeighboursType neighbours, int n, bool normal) const
{
CV_TRACE_FUNCTION();
if(n == 0)
{
return p;
}
float totalWeight = 0;
Point3f WarpedPt(0,0,0);
for(int i = 0; i < n; i++)
{
Ptr<WarpNode> neigh = nodes[neighbours[i]];
float w = neigh->weight(p);
if(w < 0.01)
{
continue;
}
Matx33f R = neigh->transform.rotation();
Point3f newPt(0, 0, 0);
if(normal)
{
newPt = R * p;
}
else
{
newPt = R * (p - neigh->pos) + neigh->pos;
Vec3f T = neigh->transform.translation();
newPt.x += T[0];
newPt.y += T[1];
newPt.z += T[2];
}
WarpedPt += newPt * w;
totalWeight += w;
}
WarpedPt /= totalWeight;
if(totalWeight == 0)
{
return p;
}
else
{
return WarpedPt;
}
}
void WarpField::setAllRT(Affine3f warpRT)
{
for(auto n: nodes)
{
n->transform = warpRT;
}
}
} // namepsace dynafu
} // namespace cv
modules/rgbd/src/warpfield.hpp
+94
View File
@@ -0,0 +1,94 @@
#ifndef __OPENCV_RGBD_WARPFIELD_HPP__
#define __OPENCV_RGBD_WARPFIELD_HPP__
#include "opencv2/core.hpp"
#include "opencv2/flann.hpp"
#include "dqb.hpp"
#define DYNAFU_MAX_NEIGHBOURS 10
typedef std::array<int, DYNAFU_MAX_NEIGHBOURS> nodeNeighboursType;
typedef std::vector<std::vector<nodeNeighboursType> > heirarchyType;
namespace cv {
namespace dynafu {
struct WarpNode
{
Point3f pos;
float radius;
Affine3f transform;
float weight(Point3f x)
{
Point3f diff = pos - x;
float L2 = diff.x*diff.x + diff.y*diff.y + diff.z*diff.z;
return expf(-L2/(2.f*radius));
}
};
typedef std::vector<Ptr<WarpNode> > NodeVectorType;
class WarpField
{
public:
WarpField(int _maxNeighbours=1000000, int K=4, int levels=4, float baseResolution=.10f,
float resolutionGrowth=4);
void updateNodesFromPoints(InputArray _points);
const NodeVectorType& getNodes() const;
const heirarchyType& getRegGraph() const;
const std::vector<NodeVectorType>& getGraphNodes() const;
size_t getNodesLen() const
{
return nodes.size();
}
Point3f applyWarp(Point3f p, const nodeNeighboursType neighbours, int n, bool normal=false) const;
void setAllRT(Affine3f warpRT);
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > getNodeIndex() const;
inline void findNeighbours(Point3f queryPt, std::vector<int>& indices, std::vector<float>& dists)
{
std::vector<float> query = {queryPt.x, queryPt.y, queryPt.z};
nodeIndex->knnSearch(query, indices, dists, k, cvflann::SearchParams());
}
int k; //k-nearest neighbours will be used
int n_levels; // number of levels in the heirarchy
private:
void removeSupported(flann::GenericIndex<flann::L2_Simple<float> >& ind, AutoBuffer<bool>& supInd);
NodeVectorType subsampleIndex(Mat& pmat, flann::GenericIndex<flann::L2_Simple<float> >& ind,
AutoBuffer<bool>& supInd, float res,
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > knnIndex = nullptr);
void constructRegGraph();
void initTransforms(NodeVectorType nv);
NodeVectorType nodes; //heirarchy level 0
int maxNeighbours;
float baseRes;
float resGrowthRate;
std::vector<NodeVectorType> regGraphNodes; // heirarchy levels 1 to L
heirarchyType heirarchy;
Ptr<flann::GenericIndex<flann::L2_Simple<float> > > nodeIndex;
Mat nodesPos;
};
bool PtCmp(cv::Point3f a, cv::Point3f b);
Mat getNodesPos(NodeVectorType nv);
} // namepsace dynafu
} // namespace cv
#endif
modules/rgbd/test/test_colored_kinfu.cpp
+481
View File
@@ -0,0 +1,481 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "test_precomp.hpp"
// Inspired by Inigo Quilez' raymarching guide:
// http://iquilezles.org/www/articles/distfunctions/distfunctions.htm
namespace opencv_test {
namespace {
using namespace cv;
/** Reprojects screen point to camera space given z coord. */
struct Reprojector
{
Reprojector() {}
inline Reprojector(Matx33f intr)
{
fxinv = 1.f / intr(0, 0), fyinv = 1.f / intr(1, 1);
cx = intr(0, 2), cy = intr(1, 2);
}
template<typename T>
inline cv::Point3_<T> operator()(cv::Point3_<T> p) const
{
T x = p.z * (p.x - cx) * fxinv;
T y = p.z * (p.y - cy) * fyinv;
return cv::Point3_<T>(x, y, p.z);
}
float fxinv, fyinv, cx, cy;
};
template<class Scene>
struct RenderInvoker : ParallelLoopBody
{
RenderInvoker(Mat_<float>& _frame, Affine3f _pose,
Reprojector _reproj,
float _depthFactor) : ParallelLoopBody(),
frame(_frame),
pose(_pose),
reproj(_reproj),
depthFactor(_depthFactor)
{ }
virtual void operator ()(const cv::Range& r) const
{
for (int y = r.start; y < r.end; y++)
{
float* frameRow = frame[y];
for (int x = 0; x < frame.cols; x++)
{
float pix = 0;
Point3f orig = pose.translation();
// direction through pixel
Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f));
float xyt = 1.f / (screenVec.x * screenVec.x +
screenVec.y * screenVec.y + 1.f);
Point3f dir = normalize(Vec3f(pose.rotation() * screenVec));
// screen space axis
dir.y = -dir.y;
const float maxDepth = 20.f;
const float maxSteps = 256;
float t = 0.f;
for (int step = 0; step < maxSteps && t < maxDepth; step++)
{
Point3f p = orig + dir * t;
float d = Scene::map(p);
if (d < 0.000001f)
{
float depth = std::sqrt(t * t * xyt);
pix = depth * depthFactor;
break;
}
t += d;
}
frameRow[x] = pix;
}
}
}
Mat_<float>& frame;
Affine3f pose;
Reprojector reproj;
float depthFactor;
};
template<class Scene>
struct RenderColorInvoker : ParallelLoopBody
{
RenderColorInvoker(Mat_<Vec3f>& _frame, Affine3f _pose,
Reprojector _reproj,
float _depthFactor) : ParallelLoopBody(),
frame(_frame),
pose(_pose),
reproj(_reproj),
depthFactor(_depthFactor)
{ }
virtual void operator ()(const cv::Range& r) const
{
for (int y = r.start; y < r.end; y++)
{
Vec3f* frameRow = frame[y];
for (int x = 0; x < frame.cols; x++)
{
Vec3f pix = 0;
Point3f orig = pose.translation();
// direction through pixel
Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f));
Point3f dir = normalize(Vec3f(pose.rotation() * screenVec));
// screen space axis
dir.y = -dir.y;
const float maxDepth = 20.f;
const float maxSteps = 256;
float t = 0.f;
for (int step = 0; step < maxSteps && t < maxDepth; step++)
{
Point3f p = orig + dir * t;
float d = Scene::map(p);
if (d < 0.000001f)
{
float m = 0.25f;
float p0 = float(abs(fmod(p.x, m)) > m / 2.f);
float p1 = float(abs(fmod(p.y, m)) > m / 2.f);
float p2 = float(abs(fmod(p.z, m)) > m / 2.f);
pix[0] = p0 + p1;
pix[1] = p1 + p2;
pix[2] = p0 + p2;
pix *= 128.f;
break;
}
t += d;
}
frameRow[x] = pix;
}
}
}
Mat_<Vec3f>& frame;
Affine3f pose;
Reprojector reproj;
float depthFactor;
};
struct Scene
{
virtual ~Scene() {}
static Ptr<Scene> create(int nScene, Size sz, Matx33f _intr, float _depthFactor);
virtual Mat_<float> depth(const Affine3f& pose) = 0;
virtual Mat_<Vec3f> rgb(const Affine3f& pose) = 0;
virtual std::vector<Affine3f> getPoses() = 0;
};
struct CubeSpheresScene : Scene
{
const int framesPerCycle = 32;
const float nCycles = 0.25f;
const Affine3f startPose = Affine3f(Vec3f(-0.5f, 0.f, 0.f), Vec3f(2.1f, 1.4f, -2.1f));
CubeSpheresScene(Size sz, Matx33f _intr, float _depthFactor) :
frameSize(sz), intr(_intr), depthFactor(_depthFactor)
{ }
static float map(Point3f p)
{
float plane = p.y + 0.5f;
Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.0f);
float boxSize = 0.5f;
float roundness = 0.08f;
Point3f boxTmp;
boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f);
boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f);
boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f);
float roundBox = (float)cv::norm(boxTmp) - roundness;
float sphereRadius = 0.7f;
float sphere = (float)cv::norm(boxPose) - sphereRadius;
float boxMinusSphere = max(roundBox, -sphere);
float sphere2 = (float)cv::norm(p - Point3f(0.3f, 1.f, 0.f)) - 0.1f;
float sphere3 = (float)cv::norm(p - Point3f(0.0f, 1.f, 0.f)) - 0.2f;
float res = min(min(plane, boxMinusSphere), min(sphere2, sphere3));
return res;
}
Mat_<float> depth(const Affine3f& pose) override
{
Mat_<float> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderInvoker<CubeSpheresScene>(frame, pose, reproj, depthFactor));
return frame;
}
Mat_<Vec3f> rgb(const Affine3f& pose) override
{
Mat_<Vec3f> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderColorInvoker<CubeSpheresScene>(frame, pose, reproj, depthFactor));
return frame;
}
std::vector<Affine3f> getPoses() override
{
std::vector<Affine3f> poses;
for (int i = 0; i < (int)(framesPerCycle * nCycles); i++)
{
float angle = (float)(CV_2PI * i / framesPerCycle);
Affine3f pose;
pose = pose.rotate(startPose.rotation());
pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle);
pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle),
startPose.translation()[1],
startPose.translation()[2] * cos(angle)));
poses.push_back(pose);
}
return poses;
}
Size frameSize;
Matx33f intr;
float depthFactor;
};
struct RotatingScene : Scene
{
const int framesPerCycle = 32;
const float nCycles = 0.5f;
const Affine3f startPose = Affine3f(Vec3f(-1.f, 0.f, 0.f), Vec3f(1.5f, 2.f, -1.5f));
RotatingScene(Size sz, Matx33f _intr, float _depthFactor) :
frameSize(sz), intr(_intr), depthFactor(_depthFactor)
{
cv::RNG rng(0);
rng.fill(randTexture, cv::RNG::UNIFORM, 0.f, 1.f);
}
static float noise(Point2f pt)
{
pt.x = abs(pt.x - (int)pt.x);
pt.y = abs(pt.y - (int)pt.y);
pt *= 256.f;
int xi = cvFloor(pt.x), yi = cvFloor(pt.y);
const float* row0 = randTexture[(yi + 0) % 256];
const float* row1 = randTexture[(yi + 1) % 256];
float v00 = row0[(xi + 0) % 256];
float v01 = row0[(xi + 1) % 256];
float v10 = row1[(xi + 0) % 256];
float v11 = row1[(xi + 1) % 256];
float tx = pt.x - xi, ty = pt.y - yi;
float v0 = v00 + tx * (v01 - v00);
float v1 = v10 + tx * (v11 - v10);
return v0 + ty * (v1 - v0);
}
static float map(Point3f p)
{
const Point3f torPlace(0.f, 0.f, 0.f);
Point3f torPos(p - torPlace);
const Point2f torusParams(1.f, 0.2f);
Point2f torq(std::sqrt(torPos.x * torPos.x + torPos.z * torPos.z) - torusParams.x, torPos.y);
float torus = (float)cv::norm(torq) - torusParams.y;
const Point3f cylShift(0.25f, 0.25f, 0.25f);
Point3f cylPos = Point3f(abs(std::fmod(p.x - 0.1f, cylShift.x)),
p.y,
abs(std::fmod(p.z - 0.2f, cylShift.z))) - cylShift * 0.5f;
const Point2f cylParams(0.1f,
0.1f + 0.1f * sin(p.x * p.y * 5.f /* +std::log(1.f+abs(p.x*0.1f)) */));
Point2f cyld = Point2f(abs(std::sqrt(cylPos.x * cylPos.x + cylPos.z * cylPos.z)), abs(cylPos.y)) - cylParams;
float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f)));
float terrain = p.y + 0.25f * noise(Point2f(p.x, p.z) * 0.01f);
float res = min(terrain, max(-pins, torus));
return res;
}
Mat_<float> depth(const Affine3f& pose) override
{
Mat_<float> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderInvoker<RotatingScene>(frame, pose, reproj, depthFactor));
return frame;
}
Mat_<Vec3f> rgb(const Affine3f& pose) override
{
Mat_<Vec3f> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderColorInvoker<RotatingScene>(frame, pose, reproj, depthFactor));
return frame;
}
std::vector<Affine3f> getPoses() override
{
std::vector<Affine3f> poses;
for (int i = 0; i < framesPerCycle * nCycles; i++)
{
float angle = (float)(CV_2PI * i / framesPerCycle);
Affine3f pose;
pose = pose.rotate(startPose.rotation());
pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle);
pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle),
startPose.translation()[1],
startPose.translation()[2] * cos(angle)));
poses.push_back(pose);
}
return poses;
}
Size frameSize;
Matx33f intr;
float depthFactor;
static cv::Mat_<float> randTexture;
};
Mat_<float> RotatingScene::randTexture(256, 256);
Ptr<Scene> Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor)
{
if (nScene == 0)
return makePtr<RotatingScene>(sz, _intr, _depthFactor);
else
return makePtr<CubeSpheresScene>(sz, _intr, _depthFactor);
}
static inline void checkRenderedColors(Mat image)
{
float area = (float)image.total();
int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0;
Vec3b c1 = Vec3b(200, 0, 0), c2 = Vec3b(0, 200, 0);
Vec3b c3 = Vec3b(0, 0, 200), c4 = Vec3b(100, 100, 100);
for (int i = 0; i < image.size().height; i++)
{
for (int j = 0; j < image.size().width; j++)
{
Vec3b color = image.at<Vec3b>(i, j);
if (color == c1) cc1++;
if (color == c2) cc2++;
if (color == c3) cc3++;
if (color == c4) cc4++;
}
}
ASSERT_LT(float(cc1) / area, 0.2);
ASSERT_LT(float(cc2) / area, 0.2);
ASSERT_LT(float(cc3) / area, 0.2);
ASSERT_LT(float(cc4) / area, 0.2);
}
static const bool display = false;
void flyTest(bool hiDense, bool test_colors)
{
Ptr<colored_kinfu::Params> params;
params = colored_kinfu::Params::coloredTSDFParams(!hiDense);
Ptr<Scene> scene = Scene::create(false, params->frameSize, params->intr, params->depthFactor);
Ptr<colored_kinfu::ColoredKinFu> kf = colored_kinfu::ColoredKinFu::create(params);
std::vector<Affine3f> poses = scene->getPoses();
Affine3f startPoseGT = poses[0], startPoseKF;
Affine3f pose, kfPose;
for (size_t i = 0; i < poses.size(); i++)
{
pose = poses[i];
Mat depth = scene->depth(pose);
//DEBUG
Mat rgb = scene->rgb(pose);// creareRGBframe(depth.size());
ASSERT_TRUE(kf->update(depth, rgb));
kfPose = kf->getPose();
if (i == 0)
startPoseKF = kfPose;
pose = (startPoseGT.inv() * pose) * startPoseKF;
if (display)
{
imshow("depth", depth * (1.f / params->depthFactor / 4.f));
imshow("rgb", rgb * (1.f / 255.f));
Mat rendered;
kf->render(rendered);
imshow("render", rendered);
waitKey(10);
}
if (test_colors)
{
Mat rendered;
kf->render(rendered);
checkRenderedColors(rendered);
return;
}
}
double rvecThreshold = hiDense ? 0.01 : 0.02;
ASSERT_LT(cv::norm(kfPose.rvec() - pose.rvec()), rvecThreshold);
double poseThreshold = hiDense ? 0.1 : 0.2;
ASSERT_LT(cv::norm(kfPose.translation() - pose.translation()), poseThreshold);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST(ColoredKinectFusion, lowDense)
#else
TEST(ColoredKinectFusion, DISABLED_lowDense)
#endif
{
flyTest(false, false);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST(ColoredKinectFusion, highDense)
#else
TEST(ColoredKinectFusion, DISABLED_highDense)
#endif
{
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
flyTest(true, false);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST(ColoredKinectFusion, color_lowDense)
#else
TEST(ColoredKinectFusion, DISABLED_color_lowDense)
#endif
{
flyTest(false, true);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST(ColoredKinectFusion, color_highDense)
#else
TEST(ColoredKinectFusion, DISABLED_color_highDense)
#endif
{
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
flyTest(true, true);
}
}
} // namespace
modules/rgbd/test/test_dynafu.cpp
+129
View File
@@ -0,0 +1,129 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "test_precomp.hpp"
#ifdef HAVE_OPENGL
namespace opencv_test { namespace {
static std::vector<std::string> readDepth(std::string fileList)
{
std::vector<std::string> v;
std::fstream file(fileList);
if(!file.is_open())
throw std::runtime_error("Failed to read depth list");
std::string dir;
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
while(!file.eof())
{
std::string s, imgPath;
std::getline(file, s);
if(s.empty() || s[0] == '#') continue;
std::stringstream ss;
ss << s;
double thumb;
ss >> thumb >> imgPath;
v.push_back(dir+'/'+imgPath);
}
return v;
}
static const bool display = false;
void flyTest(bool hiDense, bool inequal)
{
Ptr<kinfu::Params> params;
if(hiDense)
params = kinfu::Params::defaultParams();
else
params = kinfu::Params::coarseParams();
if(inequal)
{
params->volumeDims[0] += 32;
params->volumeDims[1] -= 32;
}
std::vector<String> depths = readDepth(cvtest::TS::ptr()->get_data_path() + "dynafu/depth.txt");
CV_Assert(!depths.empty());
Ptr<dynafu::DynaFu> df = dynafu::DynaFu::create(params);
// Check for first 10 frames
CV_Assert(depths.size() >= 10);
Mat currentDepth, prevDepth;
for(size_t i = 0; i < 10; i++)
{
currentDepth = cv::imread(depths[i], IMREAD_ANYDEPTH);
ASSERT_TRUE(df->update(currentDepth));
Mat renderedDepth;
df->renderSurface(renderedDepth, noArray(), noArray());
if(i > 0)
{
// Check if estimated depth aligns with actual depth in the previous frame
Mat depthCvt8, renderCvt8;
convertScaleAbs(prevDepth, depthCvt8, 0.25*256. / params->depthFactor);
convertScaleAbs(renderedDepth, renderCvt8, 0.33*255, -0.5*0.33*255);
Mat diff;
absdiff(depthCvt8, renderCvt8, diff);
Scalar_<float> mu, sigma;
meanStdDev(diff, mu, sigma);
std::cout << "Mean: " << mu[0] << ", Std dev: " << sigma[0] << std::endl;
}
if(display)
{
imshow("depth", currentDepth*(1.f/params->depthFactor/4.f));
Mat rendered;
df->render(rendered);
imshow("render", rendered);
waitKey(10);
}
currentDepth.copyTo(prevDepth);
}
}
/*
#ifdef OPENCV_ENABLE_NONFREE
TEST( DynamicFusion, lowDense )
#else
TEST(DynamicFusion, DISABLED_lowDense)
#endif
{
flyTest(false, false);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST( DynamicFusion, inequal )
#else
TEST(DynamicFusion, DISABLED_inequal)
#endif
{
flyTest(false, true);
}
*/
// To enable DynamicFusion tests, uncomment the above lines and delete the following lines
TEST(DynamicFusion, DISABLED)
{
CV_UNUSED(flyTest);
}
}} // namespace
#endif
modules/rgbd/test/test_kinfu.cpp
+382
View File
@@ -0,0 +1,382 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
#include "test_precomp.hpp"
// Inspired by Inigo Quilez' raymarching guide:
// http://iquilezles.org/www/articles/distfunctions/distfunctions.htm
namespace opencv_test { namespace {
using namespace cv;
/** Reprojects screen point to camera space given z coord. */
struct Reprojector
{
Reprojector() {}
inline Reprojector(Matx33f intr)
{
fxinv = 1.f/intr(0, 0), fyinv = 1.f/intr(1, 1);
cx = intr(0, 2), cy = intr(1, 2);
}
template<typename T>
inline cv::Point3_<T> operator()(cv::Point3_<T> p) const
{
T x = p.z * (p.x - cx) * fxinv;
T y = p.z * (p.y - cy) * fyinv;
return cv::Point3_<T>(x, y, p.z);
}
float fxinv, fyinv, cx, cy;
};
template<class Scene>
struct RenderInvoker : ParallelLoopBody
{
RenderInvoker(Mat_<float>& _frame, Affine3f _pose,
Reprojector _reproj,
float _depthFactor) : ParallelLoopBody(),
frame(_frame),
pose(_pose),
reproj(_reproj),
depthFactor(_depthFactor)
{ }
virtual void operator ()(const cv::Range& r) const
{
for(int y = r.start; y < r.end; y++)
{
float* frameRow = frame[y];
for(int x = 0; x < frame.cols; x++)
{
float pix = 0;
Point3f orig = pose.translation();
// direction through pixel
Point3f screenVec = reproj(Point3f((float)x, (float)y, 1.f));
float xyt = 1.f/(screenVec.x*screenVec.x +
screenVec.y*screenVec.y + 1.f);
Point3f dir = normalize(Vec3f(pose.rotation() * screenVec));
// screen space axis
dir.y = - dir.y;
const float maxDepth = 20.f;
const float maxSteps = 256;
float t = 0.f;
for(int step = 0; step < maxSteps && t < maxDepth; step++)
{
Point3f p = orig + dir*t;
float d = Scene::map(p);
if(d < 0.000001f)
{
float depth = std::sqrt(t*t*xyt);
pix = depth*depthFactor;
break;
}
t += d;
}
frameRow[x] = pix;
}
}
}
Mat_<float>& frame;
Affine3f pose;
Reprojector reproj;
float depthFactor;
};
struct Scene
{
virtual ~Scene() {}
static Ptr<Scene> create(int nScene, Size sz, Matx33f _intr, float _depthFactor);
virtual Mat depth(Affine3f pose) = 0;
virtual std::vector<Affine3f> getPoses() = 0;
};
struct CubeSpheresScene : Scene
{
const int framesPerCycle = 32;
const float nCycles = 0.25f;
const Affine3f startPose = Affine3f(Vec3f(-0.5f, 0.f, 0.f), Vec3f(2.1f, 1.4f, -2.1f));
CubeSpheresScene(Size sz, Matx33f _intr, float _depthFactor) :
frameSize(sz), intr(_intr), depthFactor(_depthFactor)
{ }
static float map(Point3f p)
{
float plane = p.y + 0.5f;
Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.0f);
float boxSize = 0.5f;
float roundness = 0.08f;
Point3f boxTmp;
boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f);
boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f);
boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f);
float roundBox = (float)cv::norm(boxTmp) - roundness;
float sphereRadius = 0.7f;
float sphere = (float)cv::norm(boxPose) - sphereRadius;
float boxMinusSphere = max(roundBox, -sphere);
float sphere2 = (float)cv::norm(p - Point3f(0.3f, 1.f, 0.f)) - 0.1f;
float sphere3 = (float)cv::norm(p - Point3f(0.0f, 1.f, 0.f)) - 0.2f;
float res = min(min(plane, boxMinusSphere), min(sphere2, sphere3));
return res;
}
Mat depth(Affine3f pose) override
{
Mat_<float> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderInvoker<CubeSpheresScene>(frame, pose, reproj, depthFactor));
return static_cast<Mat>(frame);
}
std::vector<Affine3f> getPoses() override
{
std::vector<Affine3f> poses;
for(int i = 0; i < (int)(framesPerCycle*nCycles); i++)
{
float angle = (float)(CV_2PI*i/framesPerCycle);
Affine3f pose;
pose = pose.rotate(startPose.rotation());
pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle);
pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle),
startPose.translation()[1],
startPose.translation()[2]*cos(angle)));
poses.push_back(pose);
}
return poses;
}
Size frameSize;
Matx33f intr;
float depthFactor;
};
struct RotatingScene : Scene
{
const int framesPerCycle = 32;
const float nCycles = 0.5f;
const Affine3f startPose = Affine3f(Vec3f(-1.f, 0.f, 0.f), Vec3f(1.5f, 2.f, -1.5f));
RotatingScene(Size sz, Matx33f _intr, float _depthFactor) :
frameSize(sz), intr(_intr), depthFactor(_depthFactor)
{
cv::RNG rng(0);
rng.fill(randTexture, cv::RNG::UNIFORM, 0.f, 1.f);
}
static float noise(Point2f pt)
{
pt.x = abs(pt.x - (int)pt.x);
pt.y = abs(pt.y - (int)pt.y);
pt *= 256.f;
int xi = cvFloor(pt.x), yi = cvFloor(pt.y);
const float* row0 = randTexture[(yi+0)%256];
const float* row1 = randTexture[(yi+1)%256];
float v00 = row0[(xi+0)%256];
float v01 = row0[(xi+1)%256];
float v10 = row1[(xi+0)%256];
float v11 = row1[(xi+1)%256];
float tx = pt.x - xi, ty = pt.y - yi;
float v0 = v00 + tx*(v01 - v00);
float v1 = v10 + tx*(v11 - v10);
return v0 + ty*(v1 - v0);
}
static float map(Point3f p)
{
const Point3f torPlace(0.f, 0.f, 0.f);
Point3f torPos(p - torPlace);
const Point2f torusParams(1.f, 0.2f);
Point2f torq(std::sqrt(torPos.x*torPos.x + torPos.z*torPos.z) - torusParams.x, torPos.y);
float torus = (float)cv::norm(torq) - torusParams.y;
const Point3f cylShift(0.25f, 0.25f, 0.25f);
Point3f cylPos = Point3f(abs(std::fmod(p.x-0.1f, cylShift.x)),
p.y,
abs(std::fmod(p.z-0.2f, cylShift.z))) - cylShift*0.5f;
const Point2f cylParams(0.1f,
0.1f+0.1f*sin(p.x*p.y*5.f /* +std::log(1.f+abs(p.x*0.1f)) */));
Point2f cyld = Point2f(abs(std::sqrt(cylPos.x*cylPos.x + cylPos.z*cylPos.z)), abs(cylPos.y)) - cylParams;
float pins = min(max(cyld.x, cyld.y), 0.0f) + (float)cv::norm(Point2f(max(cyld.x, 0.f), max(cyld.y, 0.f)));
float terrain = p.y + 0.25f*noise(Point2f(p.x, p.z)*0.01f);
float res = min(terrain, max(-pins, torus));
return res;
}
Mat depth(Affine3f pose) override
{
Mat_<float> frame(frameSize);
Reprojector reproj(intr);
Range range(0, frame.rows);
parallel_for_(range, RenderInvoker<RotatingScene>(frame, pose, reproj, depthFactor));
return static_cast<Mat>(frame);
}
std::vector<Affine3f> getPoses() override
{
std::vector<Affine3f> poses;
for(int i = 0; i < framesPerCycle*nCycles; i++)
{
float angle = (float)(CV_2PI*i/framesPerCycle);
Affine3f pose;
pose = pose.rotate(startPose.rotation());
pose = pose.rotate(Vec3f(0.f, -1.f, 0.f)*angle);
pose = pose.translate(Vec3f(startPose.translation()[0]*sin(angle),
startPose.translation()[1],
startPose.translation()[2]*cos(angle)));
poses.push_back(pose);
}
return poses;
}
Size frameSize;
Matx33f intr;
float depthFactor;
static cv::Mat_<float> randTexture;
};
Mat_<float> RotatingScene::randTexture(256, 256);
Ptr<Scene> Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor)
{
if(nScene == 0)
return makePtr<RotatingScene>(sz, _intr, _depthFactor);
else
return makePtr<CubeSpheresScene>(sz, _intr, _depthFactor);
}
static const bool display = false;
void flyTest(bool hiDense, bool inequal, bool hashTsdf = false)
{
Ptr<kinfu::Params> params;
if(hiDense)
params = kinfu::Params::defaultParams();
else
params = kinfu::Params::coarseParams();
if(hashTsdf)
params = kinfu::Params::hashTSDFParams(!hiDense);
if(inequal)
{
params->volumeDims[0] += 32;
params->volumeDims[1] -= 32;
}
Ptr<Scene> scene = Scene::create(hiDense, params->frameSize, params->intr, params->depthFactor);
Ptr<kinfu::KinFu> kf = kinfu::KinFu::create(params);
std::vector<Affine3f> poses = scene->getPoses();
Affine3f startPoseGT = poses[0], startPoseKF;
Affine3f pose, kfPose;
for(size_t i = 0; i < poses.size(); i++)
{
pose = poses[i];
Mat depth = scene->depth(pose);
ASSERT_TRUE(kf->update(depth));
kfPose = kf->getPose();
if(i == 0)
startPoseKF = kfPose;
pose = ( startPoseGT.inv() * pose )*startPoseKF;
if(display)
{
imshow("depth", depth*(1.f/params->depthFactor/4.f));
Mat rendered;
kf->render(rendered);
imshow("render", rendered);
waitKey(10);
}
}
double rvecThreshold = hiDense ? 0.01 : 0.02;
ASSERT_LT(cv::norm(kfPose.rvec() - pose.rvec()), rvecThreshold);
double poseThreshold = hiDense ? 0.11 : 0.2;
ASSERT_LT(cv::norm(kfPose.translation() - pose.translation()), poseThreshold);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST( KinectFusion, lowDense )
#else
TEST(KinectFusion, DISABLED_lowDense)
#endif
{
flyTest(false, false);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST( KinectFusion, highDense )
#else
TEST(KinectFusion, DISABLED_highDense)
#endif
{
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
flyTest(true, false);
}
#ifdef OPENCV_ENABLE_NONFREE
TEST( KinectFusion, inequal )
#else
TEST(KinectFusion, DISABLED_inequal)
#endif
{
flyTest(false, true);
}
#ifdef HAVE_OPENCL
#ifdef OPENCV_ENABLE_NONFREE
TEST( KinectFusion, OCL )
#else
TEST(KinectFusion, DISABLED_OCL)
#endif
{
cv::ocl::setUseOpenCL(false);
flyTest(false, false);
cv::ocl::setUseOpenCL(true);
flyTest(false, false);
}
#endif
TEST( KinectFusion, DISABLED_hashTsdf )
{
flyTest(false, false, true);
//! hashTSDF does not support non-equal volumeDims
flyTest(true, false, true);
}
}} // namespace
modules/rgbd/test/test_main.cpp
+11
View File
@@ -0,0 +1,11 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
#include "test_precomp.hpp"
CV_TEST_MAIN("cv")
modules/rgbd/test/test_precomp.hpp
+25
View File
@@ -0,0 +1,25 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this module's directory
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
#ifndef __OPENCV_TEST_PRECOMP_HPP__
#define __OPENCV_TEST_PRECOMP_HPP__
#include <opencv2/ts.hpp>
#include <opencv2/rgbd.hpp>
#include <opencv2/geometry.hpp>
#include <opencv2/ptcloud.hpp>
#ifdef HAVE_OPENCL
#include <opencv2/core/ocl.hpp>
#endif
namespace opencv_test {
}
#endif