RGBD tests refactoring

modules/rgbd/perf/perf_tsdf.cpp

@@ -32,12 +32,13 @@ template<class Scene>
 struct RenderInvoker : ParallelLoopBody
 {
     RenderInvoker(Mat_<float>& _frame, Affine3f _pose,
-        Reprojector _reproj,
-        float _depthFactor) : ParallelLoopBody(),
+        Reprojector _reproj, float _depthFactor, bool _onlySemisphere)
+        : ParallelLoopBody(),
         frame(_frame),
         pose(_pose),
         reproj(_reproj),
-        depthFactor(_depthFactor)
+        depthFactor(_depthFactor),
+        onlySemisphere(_onlySemisphere)
     { }
 
     virtual void operator ()(const cv::Range& r) const
@@ -64,7 +65,7 @@ struct RenderInvoker : ParallelLoopBody
                 for (int step = 0; step < maxSteps && t < maxDepth; step++)
                 {
                     Point3f p = orig + dir * t;
-                    float d = Scene::map(p);
+                    float d = Scene::map(p, onlySemisphere);
                     if (d < 0.000001f)
                     {
                         float depth = std::sqrt(t * t * xyt);
@@ -83,53 +84,50 @@ struct RenderInvoker : ParallelLoopBody
     Affine3f pose;
     Reprojector reproj;
     float depthFactor;
+    bool onlySemisphere;
 };
 
 struct Scene
 {
     virtual ~Scene() {}
-    static Ptr<Scene> create(Size sz, Matx33f _intr, float _depthFactor);
+    static Ptr<Scene> create(Size sz, Matx33f _intr, float _depthFactor, bool onlySemisphere);
     virtual Mat depth(Affine3f pose) = 0;
     virtual std::vector<Affine3f> getPoses() = 0;
 };
 
 struct SemisphereScene : Scene
 {
     const int framesPerCycle = 72;
-    const float nCycles = 1.0f;
-    const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -1.5f));
+    const float nCycles = 0.25f;
+    const Affine3f startPose = Affine3f(Vec3f(0.f, 0.f, 0.f), Vec3f(1.5f, 0.3f, -2.1f));
 
     Size frameSize;
     Matx33f intr;
     float depthFactor;
+    bool onlySemisphere;
 
-    SemisphereScene(Size sz, Matx33f _intr, float _depthFactor) :
-        frameSize(sz), intr(_intr), depthFactor(_depthFactor)
+    SemisphereScene(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere) :
+        frameSize(sz), intr(_intr), depthFactor(_depthFactor), onlySemisphere(_onlySemisphere)
     { }
 
-    static float map(Point3f p)
+    static float map(Point3f p, bool onlySemisphere)
     {
         float plane = p.y + 0.5f;
-
-        Point3f boxPose = p - Point3f(-0.0f, 0.3f, 0.5f);
-        float boxSize = 0.5f;
-        float roundness = 0.08f;
-        Point3f boxTmp;
-        boxTmp.x = max(abs(boxPose.x) - boxSize, 0.0f);
-        boxTmp.y = max(abs(boxPose.y) - boxSize, 0.0f);
-        boxTmp.z = max(abs(boxPose.z) - boxSize, 0.0f);
-        float roundBox = (float)cv::norm(boxTmp) - roundness;
-
-        Point3f spherePose = p - Point3f(-0.0f, 0.3f, 0.0f);
+        Point3f spherePose = p - Point3f(-0.0f, 0.3f, 1.1f);
         float sphereRadius = 0.5f;
         float sphere = (float)cv::norm(spherePose) - sphereRadius;
-        float sphereMinusBox = max(sphere, -roundBox);
+        float sphereMinusBox = sphere;
 
         float subSphereRadius = 0.05f;
         Point3f subSpherePose = p - Point3f(0.3f, -0.1f, -0.3f);
         float subSphere = (float)cv::norm(subSpherePose) - subSphereRadius;
 
-        float res = min({ sphereMinusBox, subSphere, plane });
+        float res;
+        if (!onlySemisphere)
+            res = min({ sphereMinusBox, subSphere, plane });
+        else
+            res = sphereMinusBox;
+
         return res;
     }
 
@@ -139,7 +137,7 @@ struct SemisphereScene : Scene
         Reprojector reproj(intr);
 
         Range range(0, frame.rows);
-        parallel_for_(range, RenderInvoker<SemisphereScene>(frame, pose, reproj, depthFactor));
+        parallel_for_(range, RenderInvoker<SemisphereScene>(frame, pose, reproj, depthFactor, onlySemisphere));
 
         return std::move(frame);
     }
@@ -152,7 +150,7 @@ struct SemisphereScene : Scene
             float angle = (float)(CV_2PI * i / framesPerCycle);
             Affine3f pose;
             pose = pose.rotate(startPose.rotation());
-            pose = pose.rotate(Vec3f(0.f, -1.f, 0.f) * angle);
+            pose = pose.rotate(Vec3f(0.f, -0.5f, 0.f) * angle);
             pose = pose.translate(Vec3f(startPose.translation()[0] * sin(angle),
                 startPose.translation()[1],
                 startPose.translation()[2] * cos(angle)));
@@ -164,11 +162,117 @@ struct SemisphereScene : Scene
 
 };
 
-Ptr<Scene> Scene::create(Size sz, Matx33f _intr, float _depthFactor)
+Ptr<Scene> Scene::create(Size sz, Matx33f _intr, float _depthFactor, bool _onlySemisphere)
+{
+    return makePtr<SemisphereScene>(sz, _intr, _depthFactor, _onlySemisphere);
+}
+
+// this is a temporary solution
+// ----------------------------
+
+typedef cv::Vec4f ptype;
+typedef cv::Mat_< ptype > Points;
+typedef Points Normals;
+typedef Size2i Size;
+
+template<int p>
+inline float specPow(float x)
+{
+    if (p % 2 == 0)
+    {
+        float v = specPow<p / 2>(x);
+        return v * v;
+    }
+    else
+    {
+        float v = specPow<(p - 1) / 2>(x);
+        return v * v * x;
+    }
+}
+
+template<>
+inline float specPow<0>(float /*x*/)
 {
-    return makePtr<SemisphereScene>(sz, _intr, _depthFactor);
+    return 1.f;
 }
 
+template<>
+inline float specPow<1>(float x)
+{
+    return x;
+}
+
+inline cv::Vec3f fromPtype(const ptype& x)
+{
+    return cv::Vec3f(x[0], x[1], x[2]);
+}
+
+inline Point3f normalize(const Vec3f& v)
+{
+    double nv = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
+    return v * (nv ? 1. / nv : 0.);
+}
+
+void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, Affine3f lightPose)
+{
+    Size sz = _points.size();
+    image.create(sz, CV_8UC4);
+
+    Points  points = _points.getMat();
+    Normals normals = _normals.getMat();
+
+    Mat_<Vec4b> img = image.getMat();
+
+    Range range(0, sz.height);
+    const int nstripes = -1;
+    parallel_for_(range, [&](const Range&)
+        {
+            for (int y = range.start; y < range.end; y++)
+            {
+                Vec4b* imgRow = img[y];
+                const ptype* ptsRow = points[y];
+                const ptype* nrmRow = normals[y];
+
+                for (int x = 0; x < sz.width; x++)
+                {
+                    Point3f p = fromPtype(ptsRow[x]);
+                    Point3f n = fromPtype(nrmRow[x]);
+
+                    Vec4b color;
+
+                    if (cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z) )
+                    {
+                        color = Vec4b(0, 32, 0, 0);
+                    }
+                    else
+                    {
+                        const float Ka = 0.3f;  //ambient coeff
+                        const float Kd = 0.5f;  //diffuse coeff
+                        const float Ks = 0.2f;  //specular coeff
+                        const int   sp = 20;  //specular power
+
+                        const float Ax = 1.f;   //ambient color,  can be RGB
+                        const float Dx = 1.f;   //diffuse color,  can be RGB
+                        const float Sx = 1.f;   //specular color, can be RGB
+                        const float Lx = 1.f;   //light color
+
+                        Point3f l = normalize(lightPose.translation() - Vec3f(p));
+                        Point3f v = normalize(-Vec3f(p));
+                        Point3f r = normalize(Vec3f(2.f * n * n.dot(l) - l));
+
+                        uchar ix = (uchar)((Ax * Ka * Dx + Lx * Kd * Dx * max(0.f, n.dot(l)) +
+                            Lx * Ks * Sx * specPow<sp>(max(0.f, r.dot(v)))) * 255.f);
+                        color = Vec4b(ix, ix, ix, 0);
+                    }
+
+                    imgRow[x] = color;
+                }
+            }
+        }, nstripes);
+}
+
+// ----------------------------
+
 class Settings
 {
 public:
@@ -188,11 +292,27 @@ class Settings
             _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);
+        scene = Scene::create(_params->frameSize, _params->intr, _params->depthFactor, true);
         poses = scene->getPoses();
     }
 };
 
+void displayImage(Mat depth, UMat _points, UMat _normals, float depthFactor, Vec3f lightPose)
+{
+    Mat  points, normals, image;
+    AccessFlag af = ACCESS_READ;
+    normals = _normals.getMat(af);
+    points = _points.getMat(af);
+    patchNaNs(points);
+
+    imshow("depth", depth * (1.f / depthFactor / 4.f));
+    renderPointsNormals(points, normals, image, lightPose);
+    imshow("render", image);
+    waitKey(2000);
+}
+
+static const bool display = false;
+
 PERF_TEST(Perf_TSDF, integrate)
 {
     Settings settings(false);
@@ -220,6 +340,9 @@ PERF_TEST(Perf_TSDF, raycast)
         startTimer();
         settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals);
         stopTimer();
+
+        if (display)
+            displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose);
     }
     SANITY_CHECK_NOTHING();
 }
@@ -252,6 +375,9 @@ PERF_TEST(Perf_HashTSDF, raycast)
         startTimer();
         settings.volume->raycast(pose, settings._params->intr, settings._params->frameSize, _points, _normals);
         stopTimer();
+
+        if (display)
+            displayImage(depth, _points, _normals, settings._params->depthFactor, settings._params->lightPose);
     }
     SANITY_CHECK_NOTHING();
 }

modules/rgbd/src/opencl/tsdf.cl

@@ -227,8 +227,8 @@ inline float3 getNormalVoxel(float3 p, __global const struct TsdfVoxel* volumePt
 
         float vaz[8];
         for(int i = 0; i < 8; i++)
-            vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf -
-                                 volumePtr[nco[i] - dim].tsdf);
+            vaz[i] = tsdfToFloat(volumePtr[nco[i] + dim].tsdf) -
+                     tsdfToFloat(volumePtr[nco[i] - dim].tsdf);
 
         float8 vz = vload8(0, vaz);
 

modules/rgbd/src/tsdf.cpp

@@ -259,13 +259,13 @@ inline v_float32x4 TSDFVolumeCPU::getNormalVoxel(const v_float32x4& p) const
         const int dim = volDims[c];
         float& nv     = an[c];
 
-        TsdfType vx[8];
+        float vx[8];
         for(int i = 0; i < 8; i++)
-            vx[i] = volData[neighbourCoords[i] + coordBase + 1*dim].tsdf -
-                    volData[neighbourCoords[i] + coordBase - 1*dim].tsdf;
+            vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf) -
+                    tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1*dim].tsdf);
 
-        v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6]));
-        v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7]));
+        v_float32x4 v0246 (vx[0], vx[2], vx[4], vx[6]);
+        v_float32x4 v1357 (vx[1], vx[3], vx[5], vx[7]);
         v_float32x4 vxx = v0246 + v_setall_f32(tz)*(v1357 - v0246);
 
         v_float32x4 v00_10 = vxx;
@@ -312,9 +312,9 @@ inline Point3f TSDFVolumeCPU::getNormalVoxel(const Point3f& p) const
         float& nv = an[c];
 
         float vx[8];
-        for(int i = 0; i < 8; i++)
-            vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1*dim].tsdf -
-                                volData[neighbourCoords[i] + coordBase - 1*dim].tsdf);
+        for (int i = 0; i < 8; i++)
+            vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) -
+                    tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf);
 
         float v00 = vx[0] + tz*(vx[1] - vx[0]);
         float v01 = vx[2] + tz*(vx[3] - vx[2]);