rgbd: colored kinfu - allow fetching colors for pointcloud

modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp

@@ -223,15 +223,16 @@ public:
 
     CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose) const = 0;
 
-    /** @brief Gets points and normals of current 3d mesh
+    /** @brief Gets points, normals and colors 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
+        @param colors vector of colors which are 4-float vectors
      */
-    CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
+    CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals, OutputArray colors = noArray()) const = 0;
 
     /** @brief Gets points of current 3d mesh
 

modules/rgbd/include/opencv2/rgbd/volume.hpp

@@ -39,6 +39,10 @@ class CV_EXPORTS_W Volume
                          OutputArray points, OutputArray normals, OutputArray colors) const    = 0;
     virtual void fetchNormals(InputArray points, OutputArray _normals) const                   = 0;
     virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const             = 0;
+    virtual void fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
+    {
+        CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
+    }
     virtual void reset()                                                                       = 0;
 
    public:

modules/rgbd/src/colored_kinfu.cpp

@@ -144,7 +144,7 @@ public:
     void render(OutputArray image) const CV_OVERRIDE;
     void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
 
-    virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
+    virtual void getCloud(OutputArray points, OutputArray normals, OutputArray colors) const CV_OVERRIDE;
     void getPoints(OutputArray points) const CV_OVERRIDE;
     void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
 
@@ -344,9 +344,9 @@ void ColoredKinFuImpl<MatType>::render(OutputArray image, const Matx44f& _camera
 
 
 template< typename MatType >
-void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
+void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n, OutputArray c) const
 {
-    volume->fetchPointsNormals(p, n);
+    volume->fetchPointsNormalsColors(p, n, c);
 }
 
 

modules/rgbd/src/colored_tsdf.cpp

@@ -74,7 +74,12 @@ public:
         { CV_Error(Error::StsNotImplemented, "Not implemented"); };
 
     virtual void fetchNormals(InputArray points, OutputArray _normals) const override;
-    virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
+    void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
+
+    void fetchPointsNormals(OutputArray points, OutputArray normals) const override
+    {
+        fetchPointsNormalsColors(points, normals, noArray());
+    }
 
     virtual void reset() override;
     virtual RGBTsdfVoxel at(const Vec3i& volumeIdx) const;
@@ -837,17 +842,20 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
     ColorFetchPointsNormalsInvoker(const ColoredTSDFVolumeCPU& _volume,
                               std::vector<std::vector<ptype>>& _pVecs,
                               std::vector<std::vector<ptype>>& _nVecs,
-                              bool _needNormals) :
+                              std::vector<std::vector<ptype>>& _cVecs,
+                              bool _needNormals, bool _needColors) :
         ParallelLoopBody(),
         vol(_volume),
         pVecs(_pVecs),
         nVecs(_nVecs),
-        needNormals(_needNormals)
+        cVecs(_cVecs),
+        needNormals(_needNormals),
+        needColors(_needColors)
     {
         volDataStart = vol.volume.ptr<RGBTsdfVoxel>();
     }
 
-    inline void coord(std::vector<ptype>& points, std::vector<ptype>& normals,
+    inline void coord(std::vector<ptype>& points, std::vector<ptype>& normals, std::vector<ptype>& colors,
                       int x, int y, int z, Point3f V, float v0, int axis) const
     {
         // 0 for x, 1 for y, 2 for z
@@ -897,6 +905,8 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
                         if(needNormals)
                             normals.push_back(toPtype(vol.pose.rotation() *
                                                       vol.getNormalVoxel(p*vol.voxelSizeInv)));
+                        if(needColors)
+                            colors.push_back(toPtype(vol.getColorVoxel(p*vol.voxelSizeInv)));
                     }
                 }
             }
@@ -905,7 +915,7 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
 
     virtual void operator() (const Range& range) const override
     {
-        std::vector<ptype> points, normals;
+        std::vector<ptype> points, normals, colors;
         for(int x = range.start; x < range.end; x++)
         {
             const RGBTsdfVoxel* volDataX = volDataStart + x*vol.volDims[0];
@@ -920,9 +930,9 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
                     {
                         Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize);
 
-                        coord(points, normals, x, y, z, V, v0, 0);
+                        coord(points, normals, colors, x, y, z, V, v0, 0);
-                        coord(points, normals, x, y, z, V, v0, 1);
+                        coord(points, normals, colors, x, y, z, V, v0, 1);
-                        coord(points, normals, x, y, z, V, v0, 2);
+                        coord(points, normals, colors, x, y, z, V, v0, 2);
 
                     } // if voxel is not empty
                 }
@@ -932,32 +942,36 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
         AutoLock al(mutex);
         pVecs.push_back(points);
         nVecs.push_back(normals);
+        cVecs.push_back(colors);
     }
 
     const ColoredTSDFVolumeCPU& vol;
     std::vector<std::vector<ptype>>& pVecs;
     std::vector<std::vector<ptype>>& nVecs;
+    std::vector<std::vector<ptype>>& cVecs;
     const RGBTsdfVoxel* volDataStart;
     bool needNormals;
+    bool needColors;
     mutable Mutex mutex;
 };
 
-void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const
+void ColoredTSDFVolumeCPU::fetchPointsNormalsColors(OutputArray _points, OutputArray _normals, OutputArray _colors) const
 {
     CV_TRACE_FUNCTION();
 
     if(_points.needed())
     {
-        std::vector<std::vector<ptype>> pVecs, nVecs;
+        std::vector<std::vector<ptype>> pVecs, nVecs, cVecs;
-        ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed());
+        ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, cVecs, _normals.needed(), _colors.needed());
         Range range(0, volResolution.x);
         const int nstripes = -1;
         parallel_for_(range, fi, nstripes);
-        std::vector<ptype> points, normals;
+        std::vector<ptype> points, normals, colors;
         for(size_t i = 0; i < pVecs.size(); i++)
         {
             points.insert(points.end(), pVecs[i].begin(), pVecs[i].end());
             normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end());
+            colors.insert(colors.end(), cVecs[i].begin(), cVecs[i].end());
         }
 
         _points.create((int)points.size(), 1, POINT_TYPE);
@@ -970,6 +984,13 @@ void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _
             if(!normals.empty())
                 Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat());
         }
+
+        if(_colors.needed())
+        {
+            _colors.create((int)colors.size(), 1, COLOR_TYPE);
+            if(!colors.empty())
+                Mat((int)colors.size(), 1, COLOR_TYPE, &colors[0]).copyTo(_colors.getMat());
+        }
     }
 }
 

modules/rgbd/src/colored_tsdf.hpp

@@ -40,7 +40,6 @@ class ColoredTSDFVolume : public Volume
     ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
                int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true);
     virtual ~ColoredTSDFVolume() = default;
-
    public:
 
     Point3i volResolution;