KinectFusion implemented (#1627)

* empty kinfu module created

* KinFu: skeleton is done

* some intermediate state fixed

* fixed normal calculation

* bilinear depth interp: fixing missing data

* TSDF integration optimized

* TSDF: adding constness

* utils: isNaN; Intr::Projector const reference fixed

* TSDF raycast: quality improvements

* TSDF fetchCloud is done

* render() added

* ICP implemented

* debug code moved to demo.cpp

* less TODOs

* partial refactoring

* TSDF: fetchPoints() and fetchNormals() rewritten in parallel manner

* platform choose added

* reordered

* data types isolated off the platform

* minor fixes

* ScopeTime added

* fixed iterations gathering

* volume::integrate() parallelized but works slow (big overhead for

* raycast is done in parallel

* got rid of kftype and p3type

* fetchNormals() fixed

* less code duplication

* nan check reduced, interpolate() refactored to fetchVoxel()

* ICP: optimizations

* TSDF: bilinear specialized

* TSDF: voxelSizeInv pushed away

* TSDF: interpolation optimized

* TSDF::integrate: parallel_for now works fast

* Frame::render: pow -> float ipow<int p>(x)

* ICP::getAb: parallel_for

* ICP::getAb: time print disabled

* ICP::getAb::bilinear: 2 calls joined

* refactored, extra functions removed

* optimized to use only 27 elems

* ICP::getAb: better optimized

* Points and Normals data type expanded to 4 channels

* ICP optimized (doesn't work)

* ICP::getAb is on intrinsics and it works

* NaN check is faster

* ICP::getAB: minors

* added non-SIMD code as fallback

* TSDF::fetchVoxel and interpolation: got rid of coord check

* TSDF::fetchVoxel: refactor

* TSDF::raycast: local copies of members

* TSDF::interpolate: refactored for simplier vectorization

* TSDF::getNormal: refactored for simplier vectorization

* minor

* include "intrin.hpp" moved to precomp.hpp

* TSDF::coords shifts moved to class body

* TSDF::getNormal vectorized

* TSDF::getNormal: little improvements

* TSDF::interpolate: little improvements

* TSDF::raycast vectorized

* more to precomp.hpp

* TSDF: minor optimizations

* TSDF::raycast cycles optimized

* TSDF::fetchPointsNormals instead of separate p and n

* TSDF::bilinearInterpolate: little speedup

* TSDF::interpolate: speed up

* TSDF::interpolate: more compact code

* TSDF::getNormal and raycast main cycle made faster

* ICP: few improvements

* Frame: a lot of parts parallelized

* TSDF::fetchPointsNormals minor improvements

* TSDF::integrate and bilinear vectorized

* TSDF::interpolate and getNormal: interpolation vectorized

* ICP: minor changes

* gradientDeltaFactor removed, coarseParams() added

* TSDF::raycast: fixed bug with tmin/tmax

* minors

* baseZ fixed

* ICP: interpolation fixed, non-parallelized code fixed

* TSDF::interpolate: bilinear fixed, less artifacts

* TSDF: minor refactoring

* TSDF: some members moved to parent class

* added tests for KinFu

* KinFu documented

* docs fixed

* warnings fixed

* license added, overrides added

* minors

* ScopeTime moved to separate file

* less memory allocations

* demo improved, java binding disabled

* viz module made optional

* fix to demo

* frameGenerator interface: got rid of refs to cv::Ptr

* demo made interactive

* trying to fix build

* trying to fix warnings

* warning fixed

* demo fixed

* raycast_step_factor tuned

* legal info added

* don't reset if ICP failed

* refactoring: KinFu::operator() => update()

* KinFu::KinFuParams => ::Params

* get/setParams

* fetch => get

* all src moved to cv::kinfu namespace

* struct Intr made internal

* kinfu_module merged into rgbd module

* License preambule updated

* minors

* frame.* renamed to kinfu_frame.*

* warnings fixed

* more warnings fixed

* RGBD normals: a fix against Inf/Nan values

* FastICP: fixed transformation direction

* RGBD Odometry tests: added epsilon for id transform; minors

* RGBD Odometry tests enabled

* modules list fixed

modules/README.md

@@ -48,7 +48,7 @@ $ cmake -D OPENCV_EXTRA_MODULES_PATH=<opencv_contrib>/modules -D BUILD_opencv_<r
 
 - **reg**: Image Registration -- Pixels based image registration for precise alignment. Follows the paper "Image Alignment and Stitching: A Tutorial", by Richard Szeliski.
 
-- **rgbd**: RGB-Depth Processing module -- Linemod 3D object recognition; Fast surface normals and 3D plane finding. 3D visual odometry
+- **rgbd**: RGB-Depth Processing module -- Linemod 3D object recognition; Fast surface normals and 3D plane finding. 3D visual odometry. 3d reconstruction using KinectFusion.
 
 - **saliency**: Saliency API -- Where humans would look in a scene. Has routines for static, motion and "objectness" saliency.
 

modules/rgbd/CMakeLists.txt

 set(the_description "RGBD algorithms")
-ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc WRAP python)
+ocv_define_module(rgbd opencv_core opencv_calib3d opencv_imgproc OPTIONAL opencv_viz WRAP python)

modules/rgbd/LICENSE_KinectFusion.md

+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/LICENSE_WillowGarage.md

+Software License Agreement (BSD License)
+
+Copyright (c) 2009, Willow Garage, Inc.
+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 Willow Garage, Inc. 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 OWNER 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

  RGB-Depth Processing module
 ============================
 
-RGB-Depth Processing module -- Linemod 3D object recognition; Fast surface normals and 3D plane finding. 3D visual odometry
+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.

modules/rgbd/doc/rgbd.bib

+@inproceedings{kinectfusion,
+author = {Izadi, Shahram and Kim, David and Hilliges, Otmar and Molyneaux, David and Newcombe, Richard and Kohli, Pushmeet and Shotton, Jamie and Hodges, Steve and Freeman, Dustin and Davison, Andrew and Fitzgibbon, Andrew},
+title = {KinectFusion: Real-time 3D Reconstruction and Interaction Using a Moving Depth Camera},
+booktitle = {},
+year = {2011},
+month = {October},
+abstract = {
+KinectFusion enables a user holding and moving a standard Kinect camera to rapidly create detailed 3D reconstructions of an indoor scene. Only the depth data from Kinect is used to track the 3D pose of the sensor and reconstruct, geometrically precise, 3D models of the physical scene in real-time. The capabilities of KinectFusion, as well as the novel GPU-based pipeline are described in full. We show uses of the core system for low-cost handheld scanning, and geometry-aware augmented reality and physics-based interactions. Novel extensions to the core GPU pipeline demonstrate object segmentation and user interaction directly in front of the sensor, without degrading camera tracking or reconstruction. These extensions are used to enable real-time multi-touch interactions anywhere, allowing any planar or non-planar reconstructed physical surface to be appropriated for touch.
+},
+publisher = {ACM},
+url = {https://www.microsoft.com/en-us/research/publication/kinectfusion-real-time-3d-reconstruction-and-interaction-using-a-moving-depth-camera/},
+address = {},
+pages = {559-568},
+journal = {},
+volume = {},
+chapter = {},
+isbn = {978-1-4503-0716-1},
+}

modules/rgbd/include/opencv2/rgbd/kinfu.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_KINFU_HPP__
+#define __OPENCV_RGBD_KINFU_HPP__
+
+#include "opencv2/core.hpp"
+#include "opencv2/core/affine.hpp"
+
+namespace cv {
+namespace kinfu {
+//! @addtogroup kinect_fusion
+//! @{
+
+/** @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 cube 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.
+
+  This implementation is based on (kinfu-remake)[https://github.com/Nerei/kinfu_remake].
+*/
+class CV_EXPORTS KinFu
+{
+public:
+    struct CV_EXPORTS Params
+    {
+        /** @brief Default parameters
+        A set of parameters which provides better model quality, can be very slow.
+        */
+        static Params defaultParams();
+
+        /** @brief Coarse parameters
+        A set of parameters which provides better speed, can fail to match frames
+        in case of rapid sensor motion.
+        */
+        static Params coarseParams();
+
+        enum PlatformType
+        {
+
+            PLATFORM_CPU, PLATFORM_GPU
+        };
+
+        /** @brief A platform on which to run the algorithm.
+         *
+        Currently KinFu supports only one platform which is a CPU.
+        GPU platform is to be implemented in the future.
+        */
+        PlatformType platform;
+
+        /** @brief frame size in pixels */
+        Size frameSize;
+
+        /** @brief camera intrinsics */
+        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
+        * 1 per 1 meter for the 32-bit float images in the ROS bag files
+        */
+        float depthFactor;
+
+        /** @brief Depth sigma in meters for bilateral smooth */
+        float bilateral_sigma_depth;
+        /** @brief Spatial sigma in pixels for bilateral smooth */
+        float bilateral_sigma_spatial;
+        /** @brief Kernel size in pixels for bilateral smooth */
+        int   bilateral_kernel_size;
+
+        /** @brief Number of pyramid levels for ICP */
+        int pyramidLevels;
+
+        /** @brief Resolution of voxel cube
+
+        Number of voxels in each cube edge.
+        */
+        int volumeDims;
+        /** @brief Size of voxel cube side in meters */
+        float volumeSize;
+
+        /** @brief Minimal camera movement in meters
+
+        Integrate new depth frame only if camera movement exceeds this value.
+        */
+        float tsdf_min_camera_movement;
+
+        /** @brief initial volume pose in meters */
+        Affine3f volumePose;
+
+        /** @brief distance to truncate in meters
+
+        Distances that exceed this value will be truncated in voxel cube values.
+        */
+        float tsdf_trunc_dist;
+
+        /** @brief max number of frames per voxel
+
+        Each voxel keeps running average of distances no longer than this value.
+        */
+        int tsdf_max_weight;
+
+        /** @brief A length of one raycast step
+
+        How much voxel sizes we skip each raycast step
+        */
+        float raycast_step_factor;
+
+        // gradient delta in voxel sizes
+        // fixed at 1.0f
+        // float gradient_delta_factor;
+
+        /** @brief light pose for rendering in meters */
+        Vec3f lightPose;
+
+        /** @brief distance theshold for ICP in meters */
+        float icpDistThresh;
+        /** angle threshold for ICP in radians */
+        float icpAngleThresh;
+        /** number of ICP iterations for each pyramid level */
+        std::vector<int> icpIterations;
+
+        // depth truncation is not used by default
+        // float icp_truncate_depth_dist; //meters
+    };
+
+    KinFu(const Params& _params);
+    virtual ~KinFu();
+
+    /** @brief Get current parameters */
+    const Params& getParams() const;
+    void setParams(const Params&);
+
+    /** @brief Renders a volume into an image
+
+      Renders a 0-surface of TSDF using Phong shading into a CV_8UC3 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.
+    */
+
+    void render(OutputArray image, const Affine3f cameraPose = Affine3f::Identity()) const;
+
+    /** @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
+     */
+    void getCloud(OutputArray points, OutputArray normals) const;
+
+    /** @brief Gets points of current 3d mesh
+
+     The order of points is undefined.
+
+        @param points vector of points which are 4-float vectors
+     */
+    void getPoints(OutputArray points) const;
+
+    /** @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
+     */
+    void getNormals(InputArray points, OutputArray normals) const;
+
+    /** @brief Resets the algorithm
+
+    Clears current model and resets a pose.
+    */
+    void reset();
+
+    /** @brief Get current pose in voxel cube space */
+    const Affine3f getPose() const;
+
+    /** @brief Process next depth frame
+
+      Integrates depth into voxel cube 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
+    */
+    bool update(InputArray depth);
+
+private:
+
+    class KinFuImpl;
+    cv::Ptr<KinFuImpl> impl;
+};
+
+//! @}
+}
+}
+#endif

modules/rgbd/include/opencv2/rgbd/linemod.hpp

-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                          License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
-// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's 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.
-//
-//   * The name of the copyright holders may not 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 Intel Corporation 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.
-//
-//M*/
-
-#ifndef __OPENCV_OBJDETECT_LINEMOD_HPP__
-#define __OPENCV_OBJDETECT_LINEMOD_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_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>

modules/rgbd/samples/CMakeLists.txt

-cmake_minimum_required(VERSION 2.8)
-project(map_test)
-find_package(OpenCV REQUIRED)
-
-set(SOURCES odometry_evaluation.cpp)
-
-include_directories(${OpenCV_INCLUDE_DIRS})
-add_executable(odometry_evaluation ${SOURCES} ${HEADERS})
-target_link_libraries(odometry_evaluation ${OpenCV_LIBS})

modules/rgbd/samples/kinfu_demo.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 <iostream>
+#include <fstream>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/rgbd/kinfu.hpp>
+
+#ifdef HAVE_OPENCV_VIZ
+
+#include <opencv2/viz.hpp>
+
+using namespace cv;
+using namespace cv::kinfu;
+using namespace std;
+
+static vector<string> readDepth(std::string fileList);
+
+static vector<string> readDepth(std::string fileList)
+{
+    vector<string> v;
+
+    fstream file(fileList);
+    if(!file.is_open())
+        throw std::runtime_error("Failed to open file");
+
+    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;
+}
+
+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);
+        Mat rendered;
+        pca.kf.render(rendered, window.getViewerPose());
+        imshow("render", rendered);
+        waitKey(1);
+    }
+}
+
+static const char* keys =
+{
+    "{help h usage ? | | print this message   }"
+    "{@depth |<none>| Path to depth.txt file listing a set of depth images }"
+    "{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
+        " in coarse mode points and normals are displayed }"
+};
+
+static const std::string message =
+ "\nThis demo uses 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;
+
+    CommandLineParser parser(argc, argv, keys);
+    parser.about(message);
+    if(parser.has("help"))
+    {
+        parser.printMessage();
+        return 0;
+    }
+    if(parser.has("coarse"))
+    {
+        coarse = true;
+    }
+
+    String depthPath = parser.get<String>(0);
+
+    if(!parser.check())
+    {
+        parser.printMessage();
+        parser.printErrors();
+        return -1;
+    }
+
+    vector<string> depthFileList = readDepth(depthPath);
+
+    KinFu::Params params;
+    if(coarse)
+        params = KinFu::Params::coarseParams();
+    else
+        params = KinFu::Params::defaultParams();
+
+    // 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;
+
+    KinFu kf(params);
+
+    cv::viz::Viz3d window(vizWindowName);
+    window.setViewerPose(Affine3f::Identity());
+
+    // TODO: can we use UMats for that?
+    Mat rendered;
+    Mat points;
+    Mat normals;
+
+    double prevTime = getTickCount();
+
+    bool pause = false;
+
+    for(size_t nFrame = 0; nFrame < depthFileList.size(); nFrame++)
+    {
+        if(pause)
+        {
+            kf.getCloud(points, normals);
+            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("cube", viz::WCube(Vec3d::all(0),
+                                                 Vec3d::all(kf.getParams().volumeSize)),
+                              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.registerMouseCallback(0);
+
+            pause = false;
+        }
+        else
+        {
+            Mat frame = cv::imread(depthFileList[nFrame], IMREAD_ANYDEPTH);
+            if(frame.empty())
+                throw std::runtime_error("Matrix is empty");
+
+            // 5000 for typical per-meter coefficient of PNG files
+            Mat cvt8;
+            convertScaleAbs(frame, cvt8, 0.25/5000.*256.);
+            imshow("depth", cvt8);
+
+            if(!kf.update(frame))
+            {
+                kf.reset();
+                std::cout << "reset" << std::endl;
+            }
+            else
+            {
+                if(coarse)
+                {
+                    kf.getCloud(points, normals);
+                    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());
+                window.showWidget("cube", viz::WCube(Vec3d::all(0),
+                                                     Vec3d::all(kf.getParams().volumeSize)),
+                                  kf.getParams().volumePose);
+                window.setViewerPose(kf.getPose());
+                window.spinOnce(1, true);
+            }
+
+            kf.render(rendered);
+        }
+
+        double 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':
+            kf.reset();
+            break;
+        case 'q':
+            return 0;
+        case 'p':
+            pause = true;
+        default:
+            break;
+        }
+    }
+
+    return 0;
+}
+
+#else
+
+int main(int /* argc */, char ** /* argv */)
+{
+    std::cout << "This demo requires viz module" << std::endl;
+    return 0;
+}
+#endif

modules/rgbd/samples/linemod.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_WillowGarage.md file found in this module's directory
+
 #include <opencv2/core.hpp>
 #include <opencv2/core/utility.hpp>
 #include <opencv2/imgproc/imgproc_c.h> // cvFindContours

modules/rgbd/samples/odometry_evaluation.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2009, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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
 
 #include <opencv2/rgbd.hpp>
 
@@ -135,7 +106,7 @@ int main(int argc, char** argv)
 {
     if(argc != 4)
     {
-        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP]" << endl;
+        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
         return -1;
     }
     

modules/rgbd/src/depth_cleaner.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2012, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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
 
 #include "precomp.hpp"
 

modules/rgbd/src/depth_registration.cpp

-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                          License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
-// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's 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.
-//
-//   * The name of the copyright holders may not 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 Intel Corporation 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.
-//
-//M*/
+// 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
 
 #include "precomp.hpp"
 
-
 namespace cv
 {
 namespace rgbd

modules/rgbd/src/depth_to_3d.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2009, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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
+
 
-#include <opencv2/rgbd.hpp>
+#include "precomp.hpp"
 
-#include "depth_to_3d.h"
-#include "utils.h"
+#include "depth_to_3d.hpp"
+#include "utils.hpp"
 
 namespace cv
 {

modules/rgbd/src/depth_to_3d.h → modules/rgbd/src/depth_to_3d.hpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2009, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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_DEPTH_TO_3D_HPP__
 #define __OPENCV_RGBD_DEPTH_TO_3D_HPP__
 
-#ifdef __cplusplus
-
-#include <opencv2/core.hpp>
-#include <limits.h>
+#include "precomp.hpp"
 
 namespace cv
 {
@@ -122,8 +90,6 @@ convertDepthToFloat(const cv::Mat& depth, float scale, const cv::Mat &uv_mat, cv
 }
 }
 
-#endif /* __cplusplus */
-
 #endif
 
 /* End of file. */

modules/rgbd/src/fast_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_KINFU_FAST_ICP_H__
+#define __OPENCV_KINFU_FAST_ICP_H__
+
+#include "precomp.hpp"
+#include "kinfu_frame.hpp"
+
+namespace cv {
+namespace kinfu {
+
+class ICP
+{
+public:
+    ICP(const cv::kinfu::Intr _intrinsics, const std::vector<int> &_iterations, float _angleThreshold, float _distanceThreshold);
+
+    virtual bool estimateTransform(cv::Affine3f& transform, cv::Ptr<Frame> oldFrame, cv::Ptr<Frame> newFrame) const = 0;
+
+    virtual ~ICP() { }
+
+protected:
+
+    std::vector<int> iterations;
+    float angleThreshold;
+    float distanceThreshold;
+    cv::kinfu::Intr intrinsics;
+};
+
+cv::Ptr<ICP> makeICP(cv::kinfu::KinFu::Params::PlatformType t,
+                     const cv::kinfu::Intr _intrinsics, const std::vector<int> &_iterations,
+                     float _angleThreshold, float _distanceThreshold);
+
+} // namespace kinfu
+} // namespace cv
+#endif

modules/rgbd/src/kinfu.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 "precomp.hpp"
+#include "fast_icp.hpp"
+#include "tsdf.hpp"
+#include "kinfu_frame.hpp"
+
+namespace cv {
+namespace kinfu {
+
+class KinFu::KinFuImpl
+{
+public:
+    KinFuImpl(const KinFu::Params& _params);
+    virtual ~KinFuImpl();
+
+    const KinFu::Params& getParams() const;
+    void setParams(const KinFu::Params&);
+
+    void render(OutputArray image, const Affine3f cameraPose = Affine3f::Identity()) const;
+
+    void getCloud(OutputArray points, OutputArray normals) const;
+    void getPoints(OutputArray points) const;
+    void getNormals(InputArray points, OutputArray normals) const;
+
+    void reset();
+
+    const Affine3f getPose() const;
+
+    bool update(InputArray depth);
+
+private:
+    KinFu::Params params;
+
+    cv::Ptr<FrameGenerator> frameGenerator;
+    cv::Ptr<ICP> icp;
+    cv::Ptr<TSDFVolume> volume;
+
+    int frameCounter;
+    Affine3f pose;
+    cv::Ptr<Frame> frame;
+};
+
+KinFu::Params KinFu::Params::defaultParams()
+{
+    Params p;
+
+    p.platform = PLATFORM_CPU;
+
+    p.frameSize = Size(640, 480);
+
+    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
+    // first non-zero numbers are accepted
+    const int iters[] = {10, 5, 4, 0};
+
+    for(size_t i = 0; i < sizeof(iters)/sizeof(int); i++)
+    {
+        if(iters[i])
+        {
+            p.icpIterations.push_back(iters[i]);
+        }
+        else
+            break;
+    }
+    p.pyramidLevels = (int)p.icpIterations.size();
+
+    p.tsdf_min_camera_movement = 0.f; //meters, disabled
+
+    p.volumeDims = 512;  //number of voxels
+
+    p.volumeSize = 3.f;  //meters
+
+    // default pose of volume cube
+    p.volumePose = Affine3f().translate(Vec3f(-p.volumeSize/2, -p.volumeSize/2, 0.5f));
+    p.tsdf_trunc_dist = 0.04f; //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
+    //p.icp_truncate_depth_dist = 0.f;        //meters, disabled
+
+    return p;
+}
+
+KinFu::Params KinFu::Params::coarseParams()
+{
+    Params p = defaultParams();
+
+    // first non-zero numbers are accepted
+    const int iters[] = {5, 3, 2};
+
+    p.icpIterations.clear();
+    for(size_t i = 0; i < sizeof(iters)/sizeof(int); i++)
+    {
+        if(iters[i])
+        {
+            p.icpIterations.push_back(iters[i]);
+        }
+        else
+            break;
+    }
+    p.pyramidLevels = (int)p.icpIterations.size();
+
+    p.volumeDims = 128; //number of voxels
+
+    p.raycast_step_factor = 0.75f;  //in voxel sizes
+
+    return p;
+}
+
+KinFu::KinFuImpl::KinFuImpl(const KinFu::Params &_params) :
+    params(_params),
+    frameGenerator(makeFrameGenerator(params.platform)),
+    icp(makeICP(params.platform, params.intr, params.icpIterations, params.icpAngleThresh, params.icpDistThresh)),
+    volume(makeTSDFVolume(params.platform, params.volumeDims, params.volumeSize, params.volumePose,
+                          params.tsdf_trunc_dist, params.tsdf_max_weight,
+                          params.raycast_step_factor)),
+    frame()
+{
+    reset();
+}
+
+void KinFu::KinFuImpl::reset()
+{
+    frameCounter = 0;
+    pose = Affine3f::Identity();
+    volume->reset();
+}
+
+KinFu::KinFuImpl::~KinFuImpl()
+{
+
+}
+
+const KinFu::Params& KinFu::KinFuImpl::getParams() const
+{
+    return params;
+}
+
+void KinFu::KinFuImpl::setParams(const KinFu::Params& p)
+{
+    params = p;
+}
+
+const Affine3f KinFu::KinFuImpl::getPose() const
+{
+    return pose;
+}
+
+bool KinFu::KinFuImpl::update(InputArray _depth)
+{
+    ScopeTime st("kinfu update");
+
+    CV_Assert(!_depth.empty() && _depth.size() == params.frameSize);
+
+    cv::Ptr<Frame> newFrame = (*frameGenerator)();
+    (*frameGenerator)(newFrame, _depth,
+                      params.intr,
+                      params.pyramidLevels,
+                      params.depthFactor,
+                      params.bilateral_sigma_depth,
+                      params.bilateral_sigma_spatial,
+                      params.bilateral_kernel_size);
+
+    if(frameCounter == 0)
+    {
+        // use depth instead of distance
+        volume->integrate(newFrame, params.depthFactor, pose, params.intr);
+
+        frame = newFrame;
+    }
+    else
+    {
+        Affine3f affine;
+        bool success = icp->estimateTransform(affine, frame, newFrame);
+        if(!success)
+            return false;
+
+        pose = pose * affine;
+
+        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(newFrame, params.depthFactor, pose, params.intr);
+        }
+
+        // raycast and build a pyramid of points and normals
+        volume->raycast(pose, params.intr, params.frameSize,
+                        params.pyramidLevels, frameGenerator, frame);
+    }
+
+    frameCounter++;
+    return true;
+}
+
+
+void KinFu::KinFuImpl::render(OutputArray image, const Affine3f cameraPose) const
+{
+    ScopeTime st("kinfu render");
+
+    const Affine3f id = Affine3f::Identity();
+    if((cameraPose.rotation() == pose.rotation() && cameraPose.translation() == pose.translation()) ||
+       (cameraPose.rotation() == id.rotation()   && cameraPose.translation() == id.translation()))
+    {
+        frame->render(image, 0, params.lightPose);
+    }
+    else
+    {
+        // raycast and build a pyramid of points and normals
+        cv::Ptr<Frame> f = (*frameGenerator)();
+        volume->raycast(cameraPose, params.intr, params.frameSize,
+                        params.pyramidLevels, frameGenerator, f);
+        f->render(image, 0, params.lightPose);
+    }
+}
+
+
+void KinFu::KinFuImpl::getCloud(OutputArray p, OutputArray n) const
+{
+    volume->fetchPointsNormals(p, n);
+}
+
+void KinFu::KinFuImpl::getPoints(OutputArray points) const
+{
+    volume->fetchPointsNormals(points, noArray());
+}
+
+void KinFu::KinFuImpl::getNormals(InputArray points, OutputArray normals) const
+{
+    volume->fetchNormals(points, normals);
+}
+
+// importing class
+
+KinFu::KinFu(const Params& _params)
+{
+    impl = makePtr<KinFu::KinFuImpl>(_params);
+}
+
+KinFu::~KinFu() { }
+
+const KinFu::Params& KinFu::getParams() const
+{
+    return impl->getParams();
+}
+
+void KinFu::setParams(const Params& p)
+{
+    impl->setParams(p);
+}
+
+const Affine3f KinFu::getPose() const
+{
+    return impl->getPose();
+}
+
+void KinFu::reset()
+{
+    impl->reset();
+}
+
+void KinFu::getCloud(cv::OutputArray points, cv::OutputArray normals) const
+{
+    impl->getCloud(points, normals);
+}
+
+void KinFu::getPoints(OutputArray points) const
+{
+    impl->getPoints(points);
+}
+
+void KinFu::getNormals(InputArray points, OutputArray normals) const
+{
+    impl->getNormals(points, normals);
+}
+
+bool KinFu::update(InputArray depth)
+{
+    return impl->update(depth);
+}
+
+void KinFu::render(cv::OutputArray image, const Affine3f cameraPose) const
+{
+    impl->render(image, cameraPose);
+}
+
+} // namespace kinfu
+} // namespace cv

modules/rgbd/src/kinfu_frame.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_KINFU_FRAME_H__
+#define __OPENCV_KINFU_FRAME_H__
+
+#include "precomp.hpp"
+#include "utils.hpp"
+
+namespace cv {
+
+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)
+         };
+};
+
+namespace kinfu {
+
+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);
+}
+
+typedef cv::Mat_< ptype > Points;
+typedef Points Normals;
+
+typedef cv::Mat_< depthType > Depth;
+
+struct Frame
+{
+public:
+    virtual void render(cv::OutputArray image, int level, cv::Affine3f lightPose) const = 0;
+    virtual void getDepth(cv::OutputArray depth) const = 0;
+    virtual ~Frame() { }
+};
+
+struct FrameCPU : Frame
+{
+public:
+    FrameCPU() : points(), normals() { }
+    virtual ~FrameCPU() { }
+
+    virtual void render(cv::OutputArray image, int level, cv::Affine3f lightPose) const override;
+    virtual void getDepth(cv::OutputArray depth) const override;
+
+    std::vector<Points> points;
+    std::vector<Normals> normals;
+    Depth depthData;
+};
+
+struct FrameGPU : Frame
+{
+public:
+    virtual void render(cv::OutputArray image, int level, cv::Affine3f lightPose) const override;
+    virtual void getDepth(cv::OutputArray depth) const override;
+    virtual ~FrameGPU() { }
+};
+
+struct FrameGenerator
+{
+public:
+    virtual cv::Ptr<Frame> operator ()() const = 0;
+    virtual void operator() (cv::Ptr<Frame> frame, cv::InputArray depth, const cv::kinfu::Intr, int levels, float depthFactor,
+                                       float sigmaDepth, float sigmaSpatial, int kernelSize) const = 0;
+    virtual void operator() (cv::Ptr<Frame> frame, cv::InputArray points, cv::InputArray normals, int levels) const = 0;
+    virtual ~FrameGenerator() {}
+};
+
+cv::Ptr<FrameGenerator> makeFrameGenerator(cv::kinfu::KinFu::Params::PlatformType t);
+
+} // namespace kinfu
+} // namespace cv
+#endif

modules/rgbd/src/linemod.cpp

-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                           License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's 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.
-//
-//   * The name of the copyright holders may not 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 Intel Corporation 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.
-//
-//M*/
+// 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
 
 #include "precomp.hpp"
 

modules/rgbd/src/normal.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2012, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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
 
 #include "precomp.hpp"
 
@@ -283,10 +254,13 @@ namespace rgbd
       Vec3T *row_B = B[0];
       for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V)
       {
-        if (cvIsNaN(*row_r))
+          Vec3T val = (*row_V) / (*row_r);
+          if(cvIsInf(val[0]) || cvIsNaN(val[0]) ||
+             cvIsInf(val[1]) || cvIsNaN(val[1]) ||
+             cvIsInf(val[2]) || cvIsNaN(val[2]))
               *row_B = Vec3T();
           else
-          *row_B = (*row_V) / (*row_r);
+              *row_B = val;
       }
 
       // Apply a box filter to B

modules/rgbd/src/odometry.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2012, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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 "precomp.hpp"
+#include "fast_icp.hpp"
 
 #if defined(HAVE_EIGEN) && EIGEN_WORLD_VERSION == 3
 #define HAVE_EIGEN3_HERE
@@ -1095,6 +1069,8 @@ Ptr<Odometry> Odometry::create(const String & odometryType)
         return makePtr<ICPOdometry>();
     else if (odometryType == "RgbdICPOdometry")
         return makePtr<RgbdICPOdometry>();
+    else if (odometryType == "FastICPOdometry")
+        return makePtr<FastICPOdometry>();
     return Ptr<Odometry>();
 }
 
@@ -1441,6 +1417,130 @@ bool RgbdICPOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame, const Ptr<
 
 //
 
+using namespace cv::kinfu;
+
+FastICPOdometry::FastICPOdometry() :
+    maxDistDiff(DEFAULT_MAX_DEPTH_DIFF()),
+    angleThreshold((float)(30. * CV_PI / 180.)),
+    sigmaDepth(0.04f),
+    sigmaSpatial(4.5f),
+    kernelSize(7)
+{
+    setDefaultIterCounts(iterCounts);
+}
+
+FastICPOdometry::FastICPOdometry(const Mat& _cameraMatrix,
+                                 float _maxDistDiff,
+                                 float _angleThreshold,
+                                 float _sigmaDepth,
+                                 float _sigmaSpatial,
+                                 int _kernelSize,
+                                 const std::vector<int>& _iterCounts) :
+    maxDistDiff(_maxDistDiff),
+    angleThreshold(_angleThreshold),
+    sigmaDepth(_sigmaDepth),
+    sigmaSpatial(_sigmaSpatial),
+    kernelSize(_kernelSize),
+    iterCounts(Mat(_iterCounts).clone()),
+    cameraMatrix(_cameraMatrix)
+{
+    if(iterCounts.empty())
+        setDefaultIterCounts(iterCounts);
+}
+
+Ptr<FastICPOdometry> FastICPOdometry::create(const Mat& _cameraMatrix,
+                                             float _maxDistDiff,
+                                             float _angleThreshold,
+                                             float _sigmaDepth,
+                                             float _sigmaSpatial,
+                                             int _kernelSize,
+                                             const std::vector<int>& _iterCounts)
+{
+    return makePtr<FastICPOdometry>(_cameraMatrix, _maxDistDiff, _angleThreshold,
+                                   _sigmaDepth, _sigmaSpatial, _kernelSize, _iterCounts);
+}
+
+Size FastICPOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType) const
+{
+    Odometry::prepareFrameCache(frame, cacheType);
+
+    if(frame->depth.empty())
+    {
+        if(!frame->pyramidDepth.empty())
+            frame->depth = frame->pyramidDepth[0];
+        else if(!frame->pyramidCloud.empty())
+        {
+            Mat cloud = frame->pyramidCloud[0];
+            std::vector<Mat> xyz;
+            split(cloud, xyz);
+            frame->depth = xyz[2];
+        }
+        else
+            CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
+    }
+    checkDepth(frame->depth, frame->depth.size());
+
+    // mask isn't used by FastICP
+
+    Ptr<FrameGenerator> fg = makeFrameGenerator(KinFu::Params::PlatformType::PLATFORM_CPU);
+    Ptr<FrameCPU> f = (*fg)().dynamicCast<FrameCPU>();
+    Intr intr(cameraMatrix);
+    float depthFactor = 1.f; // user should rescale depth manually
+    (*fg)(f, frame->depth, intr, (int)iterCounts.total(), depthFactor,
+          sigmaDepth, sigmaSpatial, kernelSize);
+
+    frame->pyramidCloud.clear();
+    frame->pyramidNormals.clear();
+    for(size_t i = 0; i < f->points.size(); i++)
+    {
+        frame->pyramidCloud.push_back(f->points[i]);
+        frame->pyramidNormals.push_back(f->normals[i]);
+    }
+
+    return frame->depth.size();
+}
+
+void FastICPOdometry::checkParams() const
+{
+    CV_Assert(cameraMatrix.size() == Size(3,3) &&
+              (cameraMatrix.type() == CV_32FC1 ||
+               cameraMatrix.type() == CV_64FC1));
+
+    CV_Assert(maxDistDiff > 0);
+    CV_Assert(angleThreshold > 0);
+    CV_Assert(sigmaDepth > 0 && sigmaSpatial > 0 && kernelSize > 0);
+}
+
+bool FastICPOdometry::computeImpl(const Ptr<OdometryFrame>& srcFrame,
+                                  const Ptr<OdometryFrame>& dstFrame,
+                                  OutputArray Rt, const Mat& /*initRt*/) const
+{
+    kinfu::Intr intr(cameraMatrix);
+    std::vector<int> iterations = iterCounts;
+    Ptr<kinfu::ICP> icp = kinfu::makeICP(kinfu::KinFu::Params::PlatformType::PLATFORM_CPU,
+                                         intr,
+                                         iterations,
+                                         angleThreshold,
+                                         maxDistDiff);
+
+    Affine3f transform;
+    Ptr<FrameCPU> srcF = makePtr<FrameCPU>(), dstF = makePtr<FrameCPU>();
+    for(size_t i = 0; i < srcFrame->pyramidCloud.size(); i++)
+    {
+        srcF-> points.push_back(srcFrame->pyramidCloud  [i]);
+        srcF->normals.push_back(srcFrame->pyramidNormals[i]);
+        dstF-> points.push_back(dstFrame->pyramidCloud  [i]);
+        dstF->normals.push_back(dstFrame->pyramidNormals[i]);
+    }
+    bool result = icp->estimateTransform(transform, dstF, srcF);
+
+    Rt.create(Size(4, 4), CV_64FC1);
+    Mat(Matx44d(transform.matrix)).copyTo(Rt.getMat());
+    return result;
+}
+
+//
+
 void
 warpFrame(const Mat& image, const Mat& depth, const Mat& mask,
           const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
@@ -1453,5 +1553,5 @@ warpFrame(const Mat& image, const Mat& depth, const Mat& mask,
     else
         CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
 }
-}
+} // namespace rgbd
 } // namespace cv

modules/rgbd/src/plane.cpp

-/*
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2012, Willow Garage, Inc.
- *  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 Willow Garage, Inc. 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 OWNER 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.
- *
- */
+// 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
 
 /** This is an implementation of a fast plane detection loosely inspired by
  * Fast Plane Detection and Polygonalization in noisy 3D Range Images

modules/rgbd/src/precomp.hpp

-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-//  By downloading, copying, installing or using the software you agree to this license.
-//  If you do not agree to this license, do not download, install,
-//  copy or use the software.
-//
-//
-//                          License Agreement
-//                For Open Source Computer Vision Library
-//
-// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
-// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-//   * Redistribution's of source code must retain the above copyright notice,
-//     this list of conditions and the following disclaimer.
-//
-//   * Redistribution's 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.
-//
-//   * The name of the copyright holders may not 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 Intel Corporation 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.
-//
-//M*/
+// 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 "opencv2/rgbd.hpp"
-#include "opencv2/calib3d.hpp"
-#include "opencv2/imgproc.hpp"
 #include "opencv2/core/utility.hpp"
 #include "opencv2/core/private.hpp"
+#include "opencv2/core/hal/intrin.hpp"
+#include "opencv2/imgproc.hpp"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/rgbd.hpp"
 #include <iostream>
 #include <list>
 #include <set>