From 04cafeb2447ce86cbc9bd291f59aefdbeafc032e Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Wed, 24 Nov 2021 00:09:36 +0100 Subject: [PATCH] rgbd: colored kinfu - allow fetching colors for pointcloud --- .../include/opencv2/rgbd/colored_kinfu.hpp | 5 ++- modules/rgbd/include/opencv2/rgbd/volume.hpp | 4 ++ modules/rgbd/src/colored_kinfu.cpp | 6 +-- modules/rgbd/src/colored_tsdf.cpp | 45 ++++++++++++++----- modules/rgbd/src/colored_tsdf.hpp | 1 - 5 files changed, 43 insertions(+), 18 deletions(-) diff --git a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp index 861b9db2ed..68b23124c2 100644 --- a/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp +++ b/modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp @@ -223,15 +223,16 @@ class CV_EXPORTS_W ColoredKinFu 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 diff --git a/modules/rgbd/include/opencv2/rgbd/volume.hpp b/modules/rgbd/include/opencv2/rgbd/volume.hpp index 3fb1d98ef4..8dc0dce5fa 100644 --- a/modules/rgbd/include/opencv2/rgbd/volume.hpp +++ b/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: diff --git a/modules/rgbd/src/colored_kinfu.cpp b/modules/rgbd/src/colored_kinfu.cpp index 8205596d63..0aaa58cc79 100644 --- a/modules/rgbd/src/colored_kinfu.cpp +++ b/modules/rgbd/src/colored_kinfu.cpp @@ -144,7 +144,7 @@ class ColoredKinFuImpl : public ColoredKinFu 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::render(OutputArray image, const Matx44f& _camera template< typename MatType > -void ColoredKinFuImpl::getCloud(OutputArray p, OutputArray n) const +void ColoredKinFuImpl::getCloud(OutputArray p, OutputArray n, OutputArray c) const { - volume->fetchPointsNormals(p, n); + volume->fetchPointsNormalsColors(p, n, c); } diff --git a/modules/rgbd/src/colored_tsdf.cpp b/modules/rgbd/src/colored_tsdf.cpp index 116136a7f2..0247f66d01 100644 --- a/modules/rgbd/src/colored_tsdf.cpp +++ b/modules/rgbd/src/colored_tsdf.cpp @@ -74,7 +74,12 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume { 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>& _pVecs, std::vector>& _nVecs, - bool _needNormals) : + std::vector>& _cVecs, + bool _needNormals, bool _needColors) : ParallelLoopBody(), vol(_volume), pVecs(_pVecs), nVecs(_nVecs), - needNormals(_needNormals) + cVecs(_cVecs), + needNormals(_needNormals), + needColors(_needColors) { volDataStart = vol.volume.ptr(); } - inline void coord(std::vector& points, std::vector& normals, + inline void coord(std::vector& points, std::vector& normals, std::vector& 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 points, normals; + std::vector 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, x, y, z, V, v0, 1); - coord(points, normals, x, y, z, V, v0, 2); + coord(points, normals, colors, x, y, z, V, v0, 0); + coord(points, normals, colors, x, y, z, V, v0, 1); + 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>& pVecs; std::vector>& nVecs; + std::vector>& 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> pVecs, nVecs; - ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed()); + std::vector> pVecs, nVecs, cVecs; + 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 points, normals; + std::vector 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()); + } } } diff --git a/modules/rgbd/src/colored_tsdf.hpp b/modules/rgbd/src/colored_tsdf.hpp index d6b09b0156..8f81376ac5 100644 --- a/modules/rgbd/src/colored_tsdf.hpp +++ b/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;