Merge pull request #2629 from DumDereDum:tsdf_optimization

TSDF Optimization

* getMat using fix

* min/max fix

* create WeightType

* create normals test

* bug fix

* complete normals test

* fix makeVolume and rewrite tests

* minor fixes

* add new normal tests

* replace operator() on lambda expressions

* make a valid points test

* minor fixes

* getNormalVoxel fix in tsdf and hashTsdf

* create renderPointsNormals

* replace Affine3f with Matx44f oin make volume

* minor fixes

* minor fix

* tmp

* create function interpolateVoxel for hashTSDF

* tmp

* right interpolation for HashTSDF

* rewrite intrinsics normalize

* minor fix

* rewrite GPU normalize

* start to write perf tests

* make Volume fix

* GPU normalize fix

* minor fix

* create perf test for raycast

* fix LNK2019 problem in perf test

* made all perf tests

* replace all Affine3f with Matx44f

* replace Point3i with Vec3i

* minor fix

* minor fix

* add CV_EXPORT_W

* build fix 1

* build fix 2

* build fix 3

* warning fix

* build test

* win test

* tests without HashTSDF

* create noparallel normals checking

* test without fetch

* test without fetch points normals

* add end line

* revert rotation() in hash_tsdf

* fix matrix multiplication order

* fetch points normals invoker fix

* warning fix

* warning fix

* Docs fix

* Hash push normals fix

* replace operator() with lambda in PushNormals

* warning fix

* create half type and types conversion

* error fix and preparation for CPU optimization

* replace all TsdfType with half

* minor fixes

* minor fix

* raycast fix

* conversion bug fix

* delete cout

* it's alive!

* improve conversion CPU

* warning fix

* warning fix 1

* intrinsics optimization

* minor fixes

* warning fix

* interpolate improve

* start to optimize GPU version

* vectorize tsdfToFloat

* CPU optimization

* GPU optimization

* minor fix

* minor fix

* Docs fix

* minor fixes

* add perf tests HashTSDF

* hashTSDF optimization

* minor fix

* interpolate improvement

* getNormalVoxel improvement

* added a new calculation pixNorm

* tsdfToFloat improve

* add flag USE_INTERPOLATION_IN_GETNORMAL to HashTSDF

* minor fix

* minor fix

* replace int with uchar

* getNormal improve

* minor fix

* minor fixes

* inline _at()

* inline _at()

* vectorize interpolation

* i tried :(

* minor fix

* try to improve _getNormalVoxel

* minor fix

* create new struct for tests

* minor test fix

* minor fix

* minor fix 1

* minor fix 2

* minor fix 3

* minor fix 4

* minor fix 5

* minor fix 6

* minor fix 7

* monor test fix

* monor test fix 1

* integrate improve

* rewrite interpolation in getNormalVoxel in HashTSDF

* intergrate improve tsdf

* minor Docs fix

* change autolock mutex place

* reduce interpolation calls

* new getNormal

* minor fix

* tmp mutex optimization

* rewrite mutex lock

* rewrite Settings structure as class

* minor Docs fix

* new inteprolateVoxel debug

* new inteprolateVoxel

* minor fix

* add new voxelToVolumeUnitIdx

* new integrate with lambda functions

* new integrate minor fix

* minor fix

* minor fix 1

* pixNorm, I try :(

* need to rewrite [][] part

* It's Alive

* omg? it works!

* minor fix

* minor fix 1

* minor fix

* add new environment check

* minor fix

* minor fix 1

* Docs fix

* minor fix 3

* minor fix

* minor fix 1

Co-authored-by: arsaratovtsev <artem.saratovtsev@intel.com>

modules/rgbd/perf/perf_tsdf.cpp

@@ -169,25 +169,39 @@ Ptr<Scene> Scene::create(Size sz, Matx33f _intr, float _depthFactor)
     return makePtr<SemisphereScene>(sz, _intr, _depthFactor);
 }
 
+class Settings
+{
+public:
+    Ptr<kinfu::Params> _params;
+    Ptr<kinfu::Volume> volume;
+    Ptr<Scene> scene;
+    std::vector<Affine3f> poses;
+
+    Settings(bool useHashTSDF)
+    {
+        if (useHashTSDF)
+            _params = kinfu::Params::hashTSDFParams(true);
+        else
+            _params = kinfu::Params::coarseParams();
+
+        volume = kinfu::makeVolume(_params->volumeType, _params->voxelSize, _params->volumePose.matrix,
+            _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight,
+            _params->truncateThreshold, _params->volumeDims);
+
+        scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor);
+        poses = scene->getPoses();
+    }
+};
+
 PERF_TEST(Perf_TSDF, integrate)
 {
-    Ptr<kinfu::Params> _params;
-    _params = kinfu::Params::coarseParams();
-
-    Ptr<kinfu::Volume> volume = kinfu::makeVolume(_params->volumeType, _params->voxelSize, _params->volumePose.matrix,
-        _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight,
-        _params->truncateThreshold, _params->volumeDims);
-
-    Ptr<Scene> scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor);
-    std::vector<Affine3f> poses = scene->getPoses();
-
-    for (size_t i = 0; i < poses.size(); i++)
+    Settings settings(false);
+    for (size_t i = 0; i < settings.poses.size(); i++)
     {
-        UMat _points, _normals;
-        Matx44f pose = poses[i].matrix;
-        Mat depth = scene->depth(pose);
+        Matx44f pose = settings.poses[i].matrix;
+        Mat depth = settings.scene->depth(pose);
         startTimer();
-        volume->integrate(depth, _params->depthFactor, pose, _params->intr);
+        settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr);
         stopTimer();
     }
     SANITY_CHECK_NOTHING();
@@ -195,25 +209,48 @@ PERF_TEST(Perf_TSDF, integrate)
 
 PERF_TEST(Perf_TSDF, raycast)
 {
-    Ptr<kinfu::Params> _params;
-    _params = kinfu::Params::coarseParams();
-
-    Ptr<kinfu::Volume> volume = kinfu::makeVolume(_params->volumeType, _params->voxelSize, _params->volumePose.matrix,
-        _params->raycast_step_factor, _params->tsdf_trunc_dist, _params->tsdf_max_weight,
-        _params->truncateThreshold, _params->volumeDims);
-
-    Ptr<Scene> scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor);
-    std::vector<Affine3f> poses = scene->getPoses();
-
-    for (size_t i = 0; i < poses.size(); i++)
+    Settings settings(false);
+    for (size_t i = 0; i < settings.poses.size(); i++)
     {
         UMat _points, _normals;
-        Matx44f pose = poses[i].matrix;
-        Mat depth = scene->depth(pose);
+        Matx44f pose = settings.poses[i].matrix;
+        Mat depth = settings.scene->depth(pose);
 
-        volume->integrate(depth, _params->depthFactor, pose, _params->intr);
+        settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr);
         startTimer();
-        volume->raycast(pose, _params->intr, _params->frameSize, _points, _normals);
+        settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals);
+        stopTimer();
+    }
+    SANITY_CHECK_NOTHING();
+}
+
+PERF_TEST(Perf_HashTSDF, integrate)
+{
+    Settings settings(true);
+
+    for (size_t i = 0; i < settings.poses.size(); i++)
+    {
+        Matx44f pose = settings.poses[i].matrix;
+        Mat depth = settings.scene->depth(pose);
+        startTimer();
+        settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr);
+        stopTimer();
+    }
+    SANITY_CHECK_NOTHING();
+}
+
+PERF_TEST(Perf_HashTSDF, raycast)
+{
+    Settings settings(true);
+    for (size_t i = 0; i < settings.poses.size(); i++)
+    {
+        UMat _points, _normals;
+        Matx44f pose = settings.poses[i].matrix;
+        Mat depth = settings.scene->depth(pose);
+
+        settings.volume->integrate(depth, settings._params->depthFactor, pose, settings._params->intr);
+        startTimer();
+        settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals);
         stopTimer();
     }
     SANITY_CHECK_NOTHING();