Merge pull request #2161 from UnderscoreAsterisk:dynafu

DynamicFusion Implementation

* Add new nodes from points

* Addition of new node in the field

* Warp nodes demo

* Add newline before {

* Remove 3rd party flann dependency

* Regularisation Heirarchy

* Correct node radius

* Change default growth rate

* New node position = centroid of the neighbourhood

* Enlarge nodes while paused

* Dynafu TSDF

* TSDF demo

* Avoid double calc and adjust initial tsdf weight

* Fix bug that caused some voxels to disappear

* getNodePos helper

* Remove USE_INTRINSIC check

* Correct RT avg calculation and remove redundant knn calc

* Slight perf improvements

* Use LinearIndex

* Debug calculations

* set all nodes' RT

* Various bug fixes

* Separate camera and warpfield

* Add dynafu documentation

* Adhere to coding style

* Add dynafu unit test

* update demo

* Merge pull request #2133 from savuor:kinfu_truncate

KinectFusion: several updates (#2133)

* truncateThreshold added

* demo updated for RealSense

* first Kinect2, then RealSense

* more distance-based parameters

* Remove trailing whitespaces

* Replace bool vector with array

* create findNeighbours in WarpField

* Maintain nodesPos matrix in WarpField

* Fix warnings on Windows build

* Remove cameraPose from WarpField

* Use AutoBuffer

* Marching Cubes

* Fix MC

* Split mesh vertices & edges

* Change Mat types in MC

* OpenGL rendering

* Check for HAVE_OPENGL

* Error handling in case HAVE_OPENGL is not defined

* Replace Mat_ with std::vector inside marchCubes

* Parallelise marching cubes

* Fix warpfield and estimate depth inside DynaFuImpl::updateT()

* Linearise depth and use points/normals from MC

* Don't test dynafu without OpenGL support

* Analytical calculation of Jacobian matrices

* Add details about rotation and differentiate graph terms in J_r

* Use derivative of T^-1 from the tutorial

* Remove L2 norm from reg term

* Use chain rule to differentiate data term

* Markdown

* Fix markdown

* Replace MD file by HTML

* Change the data term expression

* Calculate J_d using vector identities

* Rasterize vertex and normals

* Apply warpfield before rendering

* Add CV_UNUSED for normImage to fix warning

* Render float image instead of uint8

* Implement ICP data term and other changes:
1. add option to applyWarp to normals
2. add option to `fetchPointNormals` to return points in voxel coordinates
3. Fix: Use voxel coordinates to update WarpField

* Fix non-OpenGL build

* Intialise newly discovered node transforms with DQB

* Fix data term

* Change data term normal and add kinfu-like distance/angle checks

* Implement regularisation

* Fix warnings

* Credit authors of DQB and table for MC

* cast size_t to int to supress warning

* Correct regularisation and add normal equation set up

* Delete html

* Dynafu unit test

modules/rgbd/doc/rgbd.bib

@@ -16,3 +16,10 @@ volume = {},
 chapter = {},
 isbn = {978-1-4503-0716-1},
 }
+@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

@@ -12,6 +12,7 @@
 #include "opencv2/rgbd/linemod.hpp"
 #include "opencv2/rgbd/depth.hpp"
 #include "opencv2/rgbd/kinfu.hpp"
+#include "opencv2/rgbd/dynafu.hpp"
 
 
 /** @defgroup rgbd RGB-Depth Processing

modules/rgbd/include/opencv2/rgbd/dynafu.hpp

+// 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"
+
+namespace cv {
+namespace dynafu {
+
+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 frame size in pixels */
+    CV_PROP_RW Size frameSize;
+
+    /** @brief camera intrinsics */
+    CV_PROP Matx33f 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 */
+    Affine3f 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 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 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 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.
+*/
+class CV_EXPORTS_W DynaFu
+{
+public:
+    CV_WRAP static Ptr<DynaFu> create(const Ptr<Params>& _params);
+    virtual ~DynaFu();
+
+    /** @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 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 const 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 succeded 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;
+};
+
+//! @}
+}
+}
+#endif

modules/rgbd/src/dqb.cpp

+#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
\ No newline at end of file

modules/rgbd/src/dqb.hpp

+/*
+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
\ No newline at end of file

modules/rgbd/src/dynafu_tsdf.hpp

+// 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 "kinfu_frame.hpp"
+#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::kinfu::Intr intrinsics, Ptr<WarpField> wf) = 0;
+
+    virtual void raycast(cv::Affine3f cameraPose, cv::kinfu::Intr intrinsics, 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/nonrigid_icp.hpp

+// 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 "kinfu_frame.hpp"
+#include "warpfield.hpp"
+#include "dynafu_tsdf.hpp"
+
+namespace cv {
+namespace dynafu {
+
+class NonRigidICP
+{
+public:
+    NonRigidICP(const cv::kinfu::Intr _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<TSDFVolume>& volume;
+    cv::kinfu::Intr intrinsics;
+};
+
+cv::Ptr<NonRigidICP> makeNonRigidICP(const cv::kinfu::Intr _intrinsics, const cv::Ptr<TSDFVolume>& _volume,
+                                     int _iterations);
+
+} // namespace dynafu
+} // namespace cv
+#endif

modules/rgbd/src/warpfield.cpp

+#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

+#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_dynafu.cpp

+// 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<dynafu::Params> params;
+    if(hiDense)
+        params = dynafu::Params::defaultParams();
+    else
+        params = dynafu::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