Skip to content

rgbd: colored kinfu - allow fetching colors for pointcloud #3121

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 1 commit into from
Dec 7, 2021
Merged

rgbd: colored kinfu - allow fetching colors for pointcloud #3121

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
4 changes: 4 additions & 0 deletions modules/rgbd/include/opencv2/rgbd/volume.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
6 changes: 3 additions & 3 deletions modules/rgbd/src/colored_kinfu.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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);
}


Expand Down
45 changes: 33 additions & 12 deletions modules/rgbd/src/colored_tsdf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)));
}
}
}
Expand All @@ -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];
Expand All @@ -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
}
Expand All @@ -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;
ColorFetchPointsNormalsInvoker fi(*this, pVecs, nVecs, _normals.needed());
std::vector<std::vector<ptype>> 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<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);
Expand All @@ -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());
}
}
}

Expand Down
1 change: 0 additions & 1 deletion modules/rgbd/src/colored_tsdf.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down