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

Author: savuor

modules/rgbd/include/opencv2/rgbd/kinfu.hpp

@@ -102,8 +102,11 @@ struct CV_EXPORTS_W Params
     /** number of ICP iterations for each pyramid level */
     CV_PROP std::vector<int> icpIterations;
 
-    // depth truncation is not used by default
-    // float icp_truncate_depth_dist; //meters
+    /** @brief Threshold for depth truncation in meters
+
+    All depth values beyond this threshold will be set to zero
+    */
+    CV_PROP_RW float truncateThreshold;
 };
 
 /** @brief KinectFusion implementation

modules/rgbd/samples/kinfu_demo.cpp

@@ -95,6 +95,14 @@ namespace Kinect2Params
 struct DepthSource
 {
 public:
+    enum Type
+    {
+        DEPTH_LIST,
+        DEPTH_KINECT2_LIST,
+        DEPTH_KINECT2,
+        DEPTH_REALSENSE
+    };
+
     DepthSource(int cam) :
         DepthSource("", cam)
     { }
@@ -106,11 +114,31 @@ struct DepthSource
     DepthSource(String fileListName, int cam) :
         depthFileList(fileListName.empty() ? vector<string>() : readDepth(fileListName)),
         frameIdx(0),
-        vc( cam >= 0 ? VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam) : VideoCapture()),
         undistortMap1(),
-        undistortMap2(),
-        useKinect2Workarounds(true)
-    { }
+        undistortMap2()
+    {
+        if(cam >= 0)
+        {
+            vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
+            if(vc.isOpened())
+            {
+                sourceType = Type::DEPTH_KINECT2;
+            }
+            else
+            {
+                vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
+                if(vc.isOpened())
+                {
+                    sourceType = Type::DEPTH_REALSENSE;
+                }
+            }
+        }
+        else
+        {
+            vc = VideoCapture();
+            sourceType = Type::DEPTH_KINECT2_LIST;
+        }
+    }
 
     UMat getDepth()
     {
@@ -130,10 +158,21 @@ struct DepthSource
         else
         {
             vc.grab();
-            vc.retrieve(out, CAP_OPENNI_DEPTH_MAP);
+            switch (sourceType)
+            {
+            case Type::DEPTH_KINECT2:
+                vc.retrieve(out, CAP_OPENNI_DEPTH_MAP);
+                break;
+            case Type::DEPTH_REALSENSE:
+                vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP);
+                break;
+            default:
+                // unknown depth source
+                vc.retrieve(out);
+            }
 
             // workaround for Kinect 2
-            if(useKinect2Workarounds)
+            if(sourceType == Type::DEPTH_KINECT2)
             {
                 out = out(Rect(Point(), Kinect2Params::frameSize));
 
@@ -163,12 +202,11 @@ struct DepthSource
             int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH);
             int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT);
 
-            float focal = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);
-
             // it's recommended to calibrate sensor to obtain its intrinsics
             float fx, fy, cx, cy;
+            float depthFactor = 1000.f;
             Size frameSize;
-            if(useKinect2Workarounds)
+            if(sourceType == Type::DEPTH_KINECT2)
             {
                 fx = fy = Kinect2Params::focal;
                 cx = Kinect2Params::cx;
@@ -178,7 +216,17 @@ struct DepthSource
             }
             else
             {
-                fx = fy = focal;
+                if(sourceType == Type::DEPTH_REALSENSE)
+                {
+                    fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ);
+                    fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT);
+                    depthFactor = 1.f/(float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE);
+                }
+                else
+                {
+                    fx = fy = (float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);
+                }
+
                 cx = w/2 - 0.5f;
                 cy = h/2 - 0.5f;
 
@@ -191,24 +239,42 @@ struct DepthSource
 
             params.frameSize = frameSize;
             params.intr = camMatrix;
-            params.depthFactor = 1000.f;
+            params.depthFactor = depthFactor;
+
+            // RealSense has shorter depth range, some params should be tuned
+            if(sourceType == Type::DEPTH_REALSENSE)
+            {
+                // all sizes in meters
+                float cubeSize = 1.f;
+                params.voxelSize = cubeSize/params.volumeDims[0];
+                params.tsdf_trunc_dist = 0.01f;
+                params.icpDistThresh = 0.01f;
+                params.volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f,
+                                                               -cubeSize/2.f,
+                                                               0.05f));
+                params.truncateThreshold = 2.5f;
+                params.bilateral_sigma_depth = 0.01f;
+            }
+
+            if(sourceType == Type::DEPTH_KINECT2)
+            {
+                Matx<float, 1, 5> distCoeffs;
+                distCoeffs(0) = Kinect2Params::k1;
+                distCoeffs(1) = Kinect2Params::k2;
+                distCoeffs(4) = Kinect2Params::k3;
 
-            Matx<float, 1, 5> distCoeffs;
-            distCoeffs(0) = Kinect2Params::k1;
-            distCoeffs(1) = Kinect2Params::k2;
-            distCoeffs(4) = Kinect2Params::k3;
-            if(useKinect2Workarounds)
                 initUndistortRectifyMap(camMatrix, distCoeffs, cv::noArray(),
                                         camMatrix, frameSize, CV_16SC2,
                                         undistortMap1, undistortMap2);
+            }
         }
     }
 
     vector<string> depthFileList;
     size_t frameIdx;
     VideoCapture vc;
     UMat undistortMap1, undistortMap2;
-    bool useKinect2Workarounds;
+    Type sourceType;
 };
 
 #ifdef HAVE_OPENCV_VIZ
@@ -323,7 +389,11 @@ int main(int argc, char **argv)
     cv::setUseOptimized(true);
 
     // Scene-specific params should be tuned for each scene individually
-    //params->volumePose = params->volumePose.translate(Vec3f(0.f, 0.f, 0.5f));
+    //float cubeSize = 1.f;
+    //params->voxelSize = cubeSize/params->volumeDims[0]; //meters
+    //params->tsdf_trunc_dist = 0.01f; //meters
+    //params->icpDistThresh = 0.01f; //meters
+    //params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
     //params->tsdf_max_weight = 16;
 
     if(!idle)

modules/rgbd/src/kinfu.cpp

@@ -60,8 +60,8 @@ Ptr<Params> Params::defaultParams()
     //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
+    // depth truncation is not used by default but can be useful in some scenes
+    p.truncateThreshold = 0.f; //meters
 
     return makePtr<Params>(p);
 }
@@ -209,7 +209,8 @@ bool KinFuImpl<T>::updateT(const T& _depth)
                        params.depthFactor,
                        params.bilateral_sigma_depth,
                        params.bilateral_sigma_spatial,
-                       params.bilateral_kernel_size);
+                       params.bilateral_kernel_size,
+                       params.truncateThreshold);
 
     if(frameCounter == 0)
     {

modules/rgbd/src/kinfu_frame.cpp

@@ -304,7 +304,8 @@ void computePointsNormals(const Intr intr, float depthFactor, const Depth depth,
 static bool ocl_renderPointsNormals(const UMat points, const UMat normals, UMat image, Affine3f lightPose);
 static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
                                    const Intr intr, int levels, float depthFactor,
-                                   float sigmaDepth, float sigmaSpatial, int kernelSize);
+                                   float sigmaDepth, float sigmaSpatial, int kernelSize,
+                                   float truncateThreshold);
 static bool ocl_buildPyramidPointsNormals(const UMat points, const UMat normals,
                                           OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
                                           int levels);
@@ -490,20 +491,22 @@ static bool ocl_renderPointsNormals(const UMat points, const UMat normals,
 
 static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
                                    const Intr intr, int levels, float depthFactor,
-                                   float sigmaDepth, float sigmaSpatial, int kernelSize)
+                                   float sigmaDepth, float sigmaSpatial, int kernelSize,
+                                   float truncateThreshold)
 {
     CV_TRACE_FUNCTION();
 
     // looks like OpenCV's bilateral filter works the same as KinFu's
     UMat smooth;
     //TODO: fix that
-    // until 32f isn't implemented in OpenCV, we should use our workarounds
+    // until 32f isn't implemented in OpenCV in OpenCL, we should use our workarounds
     //bilateralFilter(udepth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial);
     if(!customBilateralFilterGpu(depth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial))
-        return  false;
+        return false;
 
-    // depth truncation is not used by default
-    //if (p.icp_truncate_depth_dist > 0) kfusion::cuda::depthTruncation(curr_.depth_pyr[0], p.icp_truncate_depth_dist);
+    // depth truncation can be used in some scenes
+    if(truncateThreshold > 0.f)
+        threshold(depth, depth, truncateThreshold*depthFactor, 0.0, THRESH_TOZERO_INV);
 
     UMat scaled = smooth;
     Size sz = smooth.size();
@@ -596,7 +599,8 @@ void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray im
 void makeFrameFromDepth(InputArray _depth,
                         OutputArray pyrPoints, OutputArray pyrNormals,
                         const Intr intr, int levels, float depthFactor,
-                        float sigmaDepth, float sigmaSpatial, int kernelSize)
+                        float sigmaDepth, float sigmaSpatial, int kernelSize,
+                        float truncateThreshold)
 {
     CV_TRACE_FUNCTION();
 
@@ -605,7 +609,8 @@ void makeFrameFromDepth(InputArray _depth,
     CV_OCL_RUN(_depth.isUMat() && pyrPoints.isUMatVector() && pyrNormals.isUMatVector(),
                ocl_makeFrameFromDepth(_depth.getUMat(), pyrPoints, pyrNormals,
                                       intr, levels, depthFactor,
-                                      sigmaDepth, sigmaSpatial, kernelSize));
+                                      sigmaDepth, sigmaSpatial, kernelSize,
+                                      truncateThreshold));
 
     int kp = pyrPoints.kind(), kn = pyrNormals.kind();
     CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT);
@@ -618,8 +623,9 @@ void makeFrameFromDepth(InputArray _depth,
 
     bilateralFilter(depth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial);
 
-    // depth truncation is not used by default
-    //if (p.icp_truncate_depth_dist > 0) kfusion::cuda::depthTruncation(curr_.depth_pyr[0], p.icp_truncate_depth_dist);
+    // depth truncation can be used in some scenes
+    if(truncateThreshold > 0.f)
+        threshold(depth, depth, truncateThreshold, 0.0, THRESH_TOZERO_INV);
 
     // we don't need depth pyramid outside this method
     // if we do, the code is to be refactored

modules/rgbd/src/kinfu_frame.hpp

@@ -83,7 +83,8 @@ typedef cv::Mat_< depthType > Depth;
 void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Affine3f lightPose);
 void makeFrameFromDepth(InputArray depth, OutputArray pyrPoints, OutputArray pyrNormals,
                         const Intr intr, int levels, float depthFactor,
-                        float sigmaDepth, float sigmaSpatial, int kernelSize);
+                        float sigmaDepth, float sigmaSpatial, int kernelSize,
+                        float truncateThreshold);
 void buildPyramidPointsNormals(InputArray _points, InputArray _normals,
                                OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
                                int levels);

modules/rgbd/src/odometry.cpp

@@ -1492,8 +1492,9 @@ Size FastICPOdometry::prepareFrameCache(Ptr<OdometryFrame>& frame, int cacheType
     // mask isn't used by FastICP
     Intr intr(cameraMatrix);
     float depthFactor = 1.f; // user should rescale depth manually
+    float truncateThreshold = 0.f; // disabled
     makeFrameFromDepth(frame->depth, frame->pyramidCloud, frame->pyramidNormals, intr, (int)iterCounts.total(),
-                       depthFactor, sigmaDepth, sigmaSpatial, kernelSize);
+                       depthFactor, sigmaDepth, sigmaSpatial, kernelSize, truncateThreshold);
 
     return frame->depth.size();
 }