Skip to content

Commit 744a459

Browse files
alalekalalek
committed
Merge pull request #3121 from paroj:colorfu
2 parents 41b70b0 + 04cafeb commit 744a459

File tree

5 files changed

+43
-18
lines changed

5 files changed

+43
-18
lines changed

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

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -223,15 +223,16 @@ class CV_EXPORTS_W ColoredKinFu
223223

224224
CV_WRAP virtual void render(OutputArray image, const Matx44f& cameraPose) const = 0;
225225

226-
/** @brief Gets points and normals of current 3d mesh
226+
/** @brief Gets points, normals and colors of current 3d mesh
227227
228228
The order of normals corresponds to order of points.
229229
The order of points is undefined.
230230
231231
@param points vector of points which are 4-float vectors
232232
@param normals vector of normals which are 4-float vectors
233+
@param colors vector of colors which are 4-float vectors
233234
*/
234-
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals) const = 0;
235+
CV_WRAP virtual void getCloud(OutputArray points, OutputArray normals, OutputArray colors = noArray()) const = 0;
235236

236237
/** @brief Gets points of current 3d mesh
237238

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

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,10 @@ class CV_EXPORTS_W Volume
3939
OutputArray points, OutputArray normals, OutputArray colors) const = 0;
4040
virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0;
4141
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
42+
virtual void fetchPointsNormalsColors(OutputArray, OutputArray, OutputArray) const
43+
{
44+
CV_Error(cv::Error::StsBadFunc, "This volume doesn't support vertex colors");
45+
}
4246
virtual void reset() = 0;
4347

4448
public:

‎modules/rgbd/src/colored_kinfu.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,7 @@ class ColoredKinFuImpl : public ColoredKinFu
144144
void render(OutputArray image) const CV_OVERRIDE;
145145
void render(OutputArray image, const Matx44f& cameraPose) const CV_OVERRIDE;
146146

147-
virtual void getCloud(OutputArray points, OutputArray normals) const CV_OVERRIDE;
147+
virtual void getCloud(OutputArray points, OutputArray normals, OutputArray colors) const CV_OVERRIDE;
148148
void getPoints(OutputArray points) const CV_OVERRIDE;
149149
void getNormals(InputArray points, OutputArray normals) const CV_OVERRIDE;
150150

@@ -344,9 +344,9 @@ void ColoredKinFuImpl<MatType>::render(OutputArray image, const Matx44f& _camera
344344

345345

346346
template< typename MatType >
347-
void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
347+
void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n, OutputArray c) const
348348
{
349-
volume->fetchPointsNormals(p, n);
349+
volume->fetchPointsNormalsColors(p, n, c);
350350
}
351351

352352

‎modules/rgbd/src/colored_tsdf.cpp‎

Lines changed: 33 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,12 @@ class ColoredTSDFVolumeCPU : public ColoredTSDFVolume
7474
{ CV_Error(Error::StsNotImplemented, "Not implemented"); };
7575

7676
virtual void fetchNormals(InputArray points, OutputArray _normals) const override;
77-
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const override;
77+
void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const override;
78+
79+
void fetchPointsNormals(OutputArray points, OutputArray normals) const override
80+
{
81+
fetchPointsNormalsColors(points, normals, noArray());
82+
}
7883

7984
virtual void reset() override;
8085
virtual RGBTsdfVoxel at(const Vec3i& volumeIdx) const;
@@ -837,17 +842,20 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
837842
ColorFetchPointsNormalsInvoker(const ColoredTSDFVolumeCPU& _volume,
838843
std::vector<std::vector<ptype>>& _pVecs,
839844
std::vector<std::vector<ptype>>& _nVecs,
840-
bool _needNormals) :
845+
std::vector<std::vector<ptype>>& _cVecs,
846+
bool _needNormals, bool _needColors) :
841847
ParallelLoopBody(),
842848
vol(_volume),
843849
pVecs(_pVecs),
844850
nVecs(_nVecs),
845-
needNormals(_needNormals)
851+
cVecs(_cVecs),
852+
needNormals(_needNormals),
853+
needColors(_needColors)
846854
{
847855
volDataStart = vol.volume.ptr<RGBTsdfVoxel>();
848856
}
849857

850-
inline void coord(std::vector<ptype>& points, std::vector<ptype>& normals,
858+
inline void coord(std::vector<ptype>& points, std::vector<ptype>& normals, std::vector<ptype>& colors,
851859
int x, int y, int z, Point3f V, float v0, int axis) const
852860
{
853861
// 0 for x, 1 for y, 2 for z
@@ -897,6 +905,8 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
897905
if(needNormals)
898906
normals.push_back(toPtype(vol.pose.rotation() *
899907
vol.getNormalVoxel(p*vol.voxelSizeInv)));
908+
if(needColors)
909+
colors.push_back(toPtype(vol.getColorVoxel(p*vol.voxelSizeInv)));
900910
}
901911
}
902912
}
@@ -905,7 +915,7 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
905915

906916
virtual void operator() (const Range& range) const override
907917
{
908-
std::vector<ptype> points, normals;
918+
std::vector<ptype> points, normals, colors;
909919
for(int x = range.start; x < range.end; x++)
910920
{
911921
const RGBTsdfVoxel* volDataX = volDataStart + x*vol.volDims[0];
@@ -920,9 +930,9 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
920930
{
921931
Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f)*vol.voxelSize);
922932

923-
coord(points, normals, x, y, z, V, v0, 0);
924-
coord(points, normals, x, y, z, V, v0, 1);
925-
coord(points, normals, x, y, z, V, v0, 2);
933+
coord(points, normals, colors, x, y, z, V, v0, 0);
934+
coord(points, normals, colors, x, y, z, V, v0, 1);
935+
coord(points, normals, colors, x, y, z, V, v0, 2);
926936

927937
} // if voxel is not empty
928938
}
@@ -932,32 +942,36 @@ struct ColorFetchPointsNormalsInvoker : ParallelLoopBody
932942
AutoLock al(mutex);
933943
pVecs.push_back(points);
934944
nVecs.push_back(normals);
945+
cVecs.push_back(colors);
935946
}
936947

937948
const ColoredTSDFVolumeCPU& vol;
938949
std::vector<std::vector<ptype>>& pVecs;
939950
std::vector<std::vector<ptype>>& nVecs;
951+
std::vector<std::vector<ptype>>& cVecs;
940952
const RGBTsdfVoxel* volDataStart;
941953
bool needNormals;
954+
bool needColors;
942955
mutable Mutex mutex;
943956
};
944957

945-
void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const
958+
void ColoredTSDFVolumeCPU::fetchPointsNormalsColors(OutputArray _points, OutputArray _normals, OutputArray _colors) const
946959
{
947960
CV_TRACE_FUNCTION();
948961

949962
if(_points.needed())
950963
{
951-
std::vector<std::vector<ptype>> pVecs, nVecs;
952-
ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed());
964+
std::vector<std::vector<ptype>> pVecs, nVecs, cVecs;
965+
ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, cVecs, _normals.needed(), _colors.needed());
953966
Range range(0, volResolution.x);
954967
const int nstripes = -1;
955968
parallel_for_(range, fi, nstripes);
956-
std::vector<ptype> points, normals;
969+
std::vector<ptype> points, normals, colors;
957970
for(size_t i = 0; i < pVecs.size(); i++)
958971
{
959972
points.insert(points.end(), pVecs[i].begin(), pVecs[i].end());
960973
normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end());
974+
colors.insert(colors.end(), cVecs[i].begin(), cVecs[i].end());
961975
}
962976

963977
_points.create((int)points.size(), 1, POINT_TYPE);
@@ -970,6 +984,13 @@ void ColoredTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _
970984
if(!normals.empty())
971985
Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat());
972986
}
987+
988+
if(_colors.needed())
989+
{
990+
_colors.create((int)colors.size(), 1, COLOR_TYPE);
991+
if(!colors.empty())
992+
Mat((int)colors.size(), 1, COLOR_TYPE, &colors[0]).copyTo(_colors.getMat());
993+
}
973994
}
974995
}
975996

‎modules/rgbd/src/colored_tsdf.hpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,6 @@ class ColoredTSDFVolume : public Volume
4040
ColoredTSDFVolume(float _voxelSize, Matx44f _pose, float _raycastStepFactor, float _truncDist,
4141
int _maxWeight, Point3i _resolution, bool zFirstMemOrder = true);
4242
virtual ~ColoredTSDFVolume() = default;
43-
4443
public:
4544

4645
Point3i volResolution;

0 commit comments

Comments
 (0)