Skip to content

Commit 26a3a68

Browse files
PeterBowmanPeterBowman
committed
Implement colored KinFu
1 parent cf3e538 commit 26a3a68

10 files changed

+297
-20
lines changed

‎programs/sceneReconstruction/CMakeLists.txt

+5
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,11 @@ if(ENABLE_sceneReconstruction)
4141
target_compile_definitions(sceneReconstruction PRIVATE HAVE_KINFU_LS)
4242
endif()
4343

44+
if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.5.3)
45+
target_sources(sceneReconstruction PRIVATE ColoredKinFu.cpp)
46+
target_compile_definitions(sceneReconstruction PRIVATE HAVE_COLORED_KINFU)
47+
endif()
48+
4449
install(TARGETS sceneReconstruction
4550
DESTINATION ${CMAKE_INSTALL_BINDIR})
4651

‎programs/sceneReconstruction/ColoredKinFu.cpp

+228
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,228 @@
1+
// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2+
3+
#include "KinectFusionImpl.hpp"
4+
5+
#include <map>
6+
#include <yarp/os/LogStream.h>
7+
#include <opencv2/rgbd/colored_kinfu.hpp>
8+
9+
using namespace roboticslab;
10+
11+
template <>
12+
bool KinectFusionImpl<cv::colored_kinfu::ColoredKinFu>::update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame,
13+
const yarp::sig::FlexImage & rgbFrame)
14+
{
15+
// Cast away constness so that toCvMat accepts the YARP image. This function
16+
// does not alter the inner structure of PixelFloat images anyway.
17+
auto & nonConstDepthFrame = const_cast<yarp::sig::ImageOf<yarp::sig::PixelFloat> &>(depthFrame);
18+
cv::Mat depthMat = yarp::cv::toCvMat(nonConstDepthFrame);
19+
20+
yarp::sig::ImageOf<yarp::sig::PixelBgr> bgrFrame;
21+
bgrFrame.copy(rgbFrame);
22+
cv::Mat bgrMat = yarp::cv::toCvMat(bgrFrame);
23+
24+
cv::UMat depthUmat;
25+
depthMat.convertTo(depthUmat, depthMat.type(), 1000.0); // OpenCV uses milimeters
26+
return handle->update(depthUmat, bgrMat);
27+
}
28+
29+
namespace
30+
{
31+
std::map<std::string, cv::kinfu::VolumeType> stringToCvVolume {
32+
{"tsdf", cv::kinfu::VolumeType::TSDF},
33+
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF},
34+
{"coloredtsdf", cv::kinfu::VolumeType::COLOREDTSDF}
35+
};
36+
}
37+
38+
namespace roboticslab
39+
{
40+
41+
std::unique_ptr<KinectFusion> makeColoredKinFu(const yarp::os::Searchable & config,
42+
const yarp::sig::IntrinsicParams & depthIntrinsic,
43+
const yarp::sig::IntrinsicParams & rgbIntrinsic,
44+
int depthWidth, int depthHeight,
45+
int rgbWidth, int rgbHeight)
46+
{
47+
using Params = cv::colored_kinfu::Params;
48+
49+
auto params = Params::defaultParams();
50+
51+
yInfo() << "--- CAMERA PARAMETERS (depth) ---";
52+
53+
params->frameSize = cv::Size(depthWidth, depthHeight);
54+
yInfo() << "width:" << depthWidth;
55+
yInfo() << "height:" << depthHeight;
56+
57+
params->intr = cv::Matx33f(depthIntrinsic.focalLengthX, 0, depthIntrinsic.principalPointX,
58+
0, depthIntrinsic.focalLengthY, depthIntrinsic.principalPointY,
59+
0, 0, 1);
60+
61+
yInfo() << "focal length (X):" << depthIntrinsic.focalLengthX;
62+
yInfo() << "focal length (Y):" << depthIntrinsic.focalLengthY;
63+
yInfo() << "principal point (X):" << depthIntrinsic.principalPointX;
64+
yInfo() << "principal point (Y):" << depthIntrinsic.principalPointY;
65+
66+
yInfo() << "--- CAMERA PARAMETERS (RGB) ---";
67+
68+
params->rgb_frameSize = cv::Size(rgbWidth, rgbHeight);
69+
yInfo() << "width:" << rgbWidth;
70+
yInfo() << "height:" << rgbHeight;
71+
72+
params->rgb_intr = cv::Matx33f(rgbIntrinsic.focalLengthX, 0, rgbIntrinsic.principalPointX,
73+
0, rgbIntrinsic.focalLengthY, rgbIntrinsic.principalPointY,
74+
0, 0, 1);
75+
76+
yInfo() << "focal length (X):" << rgbIntrinsic.focalLengthX;
77+
yInfo() << "focal length (Y):" << rgbIntrinsic.focalLengthY;
78+
yInfo() << "principal point (X):" << rgbIntrinsic.principalPointX;
79+
yInfo() << "principal point (Y):" << rgbIntrinsic.principalPointY;
80+
81+
yInfo() << "--- ALGORITHM PARAMETERS ---";
82+
83+
yInfo() << "algorithm: colored_kinfu";
84+
85+
updateParam(*params, &Params::bilateral_kernel_size, config, "bilateralKernelSize", "kernel size in pixels for bilateral smooth");
86+
updateParam(*params, &Params::bilateral_sigma_depth, config, "bilateralSigmaDepth", "depth sigma in meters for bilateral smooth");
87+
updateParam(*params, &Params::bilateral_sigma_spatial, config, "bilateralSigmaSpatial", "spatial sigma in pixels for bilateral smooth");
88+
updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");
89+
updateParam(*params, &Params::icpAngleThresh, config, "icpAngleThresh", "angle threshold in radians for ICP");
90+
updateParam(*params, &Params::icpDistThresh, config, "icpDistThresh", "distance threshold in meters for ICP");
91+
92+
if (config.check("icpIterations", "list of iterations per each ICP level"))
93+
{
94+
yarp::os::Bottle * icpIterations = config.find("icpIterations").asList();
95+
96+
if (icpIterations == nullptr)
97+
{
98+
yError() << "Parameter icpIterations must be a list";
99+
return nullptr;
100+
}
101+
102+
params->icpIterations.resize(icpIterations->size());
103+
104+
for (auto i = 0; i < icpIterations->size(); i++)
105+
{
106+
params->icpIterations[i] = icpIterations->get(i).asInt32();
107+
}
108+
109+
yInfo() << "icpIterations:" << icpIterations->toString();
110+
}
111+
else
112+
{
113+
yInfo() << "icpIterations (DEFAULT):" << params->icpIterations;
114+
}
115+
116+
if (config.check("lightPose", "light pose for rendering in meters"))
117+
{
118+
yarp::os::Bottle * lightPose = config.find("lightPose").asList();
119+
120+
if (lightPose == nullptr || lightPose->size() != 3)
121+
{
122+
yError() << "Parameter lightPose must be a list of 3 floats";
123+
return nullptr;
124+
}
125+
126+
params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());
127+
yInfo() << "lightPose:" << lightPose->toString();
128+
}
129+
else
130+
{
131+
const auto & cvLight = params->lightPose;
132+
yInfo() << "lightPose (DEFAULT):" << cvLight[0] << cvLight[1] << cvLight[2];
133+
}
134+
135+
updateParam(*params, &Params::pyramidLevels, config, "pyramidLevels", "number of pyramid levels for ICP");
136+
updateParam(*params, &Params::raycast_step_factor, config, "raycastStepFactor", "a length in voxel sizes for one raycast step");
137+
updateParam(*params, &Params::truncateThreshold, config, "truncateThreshold", "threshold for depth truncation in meters");
138+
updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");
139+
updateParam(*params, &Params::tsdf_min_camera_movement, config, "tsdfMinCameraMovement", "minimal camera movement in meters");
140+
updateParam(*params, &Params::tsdf_trunc_dist, config, "tsdfTruncDist", "distance to truncate in meters");
141+
142+
if (config.check("volumeDims", "resolution of voxel space"))
143+
{
144+
yarp::os::Bottle * volumeDims = config.find("volumeDims").asList();
145+
146+
if (volumeDims == nullptr || volumeDims->size() != 3)
147+
{
148+
yError() << "Parameter volumeDims must be a list of 3 integers";
149+
return nullptr;
150+
}
151+
152+
params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());
153+
yInfo() << "volumeDims:" << volumeDims->toString();
154+
}
155+
else
156+
{
157+
const auto & cvDims = params->volumeDims;
158+
yInfo() << "volumeDims (DEFAULT):" << cvDims[0] << cvDims[1] << cvDims[2];
159+
}
160+
161+
if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))
162+
{
163+
yarp::os::Bottle * volumePoseRot = config.find("volumePoseRot").asList();
164+
165+
if (volumePoseRot == nullptr || volumePoseRot->size() != 9)
166+
{
167+
yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";
168+
return nullptr;
169+
}
170+
171+
auto rot = cv::Matx33f(volumePoseRot->get(0).asFloat32(), volumePoseRot->get(1).asFloat32(), volumePoseRot->get(2).asFloat32(),
172+
volumePoseRot->get(3).asFloat32(), volumePoseRot->get(4).asFloat32(), volumePoseRot->get(5).asFloat32(),
173+
volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());
174+
175+
params->volumePose.rotation(rot);
176+
yInfo() << "volumePoseRot:" << volumePoseRot->toString();
177+
}
178+
else
179+
{
180+
const auto & rot = params->volumePose.rotation();
181+
yInfo() << "volumePoseRot (DEFAULT):" << rot(0,0) << rot(0,1) << rot(0,2) << rot(1,0) << rot(1,1) << rot(1,2) << rot(2,0) << rot(2,1) << rot(2,2);
182+
}
183+
184+
if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))
185+
{
186+
yarp::os::Bottle * volumePoseTransl = config.find("volumePoseTransl").asList();
187+
188+
if (volumePoseTransl == nullptr || volumePoseTransl->size() != 3)
189+
{
190+
yError() << "Parameter volumePoseTransl must be a list of 3 floats";
191+
return nullptr;
192+
}
193+
194+
auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());
195+
params->volumePose.translation(transl);
196+
yInfo() << "volumePoseTransl:" << volumePoseTransl->toString();
197+
}
198+
else
199+
{
200+
const auto & transl = params->volumePose.translation();
201+
yInfo() << "volumePoseTransl (DEFAULT):" << transl[0] << transl[1] << transl[2];
202+
}
203+
204+
if (config.check("volumeType", "type of voxel volume (tsdf, hashtsdf)"))
205+
{
206+
std::string volumeType = config.find("volumeType").asString();
207+
208+
if (stringToCvVolume.find(volumeType) == stringToCvVolume.end())
209+
{
210+
yError() << "Unsupported volume type" << volumeType;
211+
return nullptr;
212+
}
213+
214+
params->volumeType = stringToCvVolume[volumeType];
215+
yInfo() << "volumeType:" << volumeType;
216+
}
217+
else
218+
{
219+
auto res = std::find_if(stringToCvVolume.begin(), stringToCvVolume.end(), [&params](const auto & el) { return el.second == params->volumeType; });
220+
yInfo() << "volumeType (DEFAULT):" << res->first;
221+
}
222+
223+
updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");
224+
225+
return std::make_unique<KinectFusionImpl<cv::colored_kinfu::ColoredKinFu>>(cv::colored_kinfu::ColoredKinFu::create(params));
226+
}
227+
228+
} // namespace roboticslab

‎programs/sceneReconstruction/DynaFu.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,10 @@ namespace
1414
{
1515
std::map<std::string, cv::kinfu::VolumeType> stringToCvVolume {
1616
{"tsdf", cv::kinfu::VolumeType::TSDF},
17-
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF}
17+
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF},
18+
#if HAVE_COLORED_KINFU
19+
{"coloredtsdf", cv::kinfu::VolumeType::COLOREDTSDF}
20+
#endif
1821
};
1922
}
2023
#endif

‎programs/sceneReconstruction/KinFu.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,10 @@ namespace
1414
{
1515
std::map<std::string, cv::kinfu::VolumeType> stringToCvVolume {
1616
{"tsdf", cv::kinfu::VolumeType::TSDF},
17-
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF}
17+
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF},
18+
#if HAVE_COLORED_KINFU
19+
{"coloredtsdf", cv::kinfu::VolumeType::COLOREDTSDF}
20+
#endif
1821
};
1922
}
2023
#endif

‎programs/sceneReconstruction/KinFuLargeScale.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,10 @@ namespace
1212
{
1313
std::map<std::string, cv::kinfu::VolumeType> stringToCvVolume {
1414
{"tsdf", cv::kinfu::VolumeType::TSDF},
15-
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF}
15+
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF},
16+
#if HAVE_COLORED_KINFU
17+
{"coloredtsdf", cv::kinfu::VolumeType::COLOREDTSDF}
18+
#endif
1619
};
1720
}
1821

‎programs/sceneReconstruction/KinectFusion.hpp

+19-5
Original file line numberDiff line numberDiff line change
@@ -25,21 +25,35 @@ class KinectFusion
2525

2626
virtual void getPose(yarp::sig::Matrix & pose) const = 0;
2727

28-
virtual bool update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame) = 0;
28+
virtual bool update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame, const yarp::sig::FlexImage & rgbFrame) = 0;
2929

3030
virtual void reset() = 0;
3131

32-
virtual void render(yarp::sig::ImageOf<yarp::sig::PixelMono> & image) const = 0;
32+
virtual void render(yarp::sig::ImageOf<yarp::sig::PixelRgb> & image) const = 0;
3333
};
3434

35-
std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
35+
std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config,
36+
const yarp::sig::IntrinsicParams & intrinsic,
37+
int width, int height);
3638

3739
#ifdef HAVE_DYNAFU
38-
std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
40+
std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config,
41+
const yarp::sig::IntrinsicParams & intrinsic,
42+
int width, int height);
3943
#endif
4044

4145
#ifdef HAVE_KINFU_LS
42-
std::unique_ptr<KinectFusion> makeKinFuLargeScale(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
46+
std::unique_ptr<KinectFusion> makeKinFuLargeScale(const yarp::os::Searchable & config,
47+
const yarp::sig::IntrinsicParams & intrinsic,
48+
int width, int height);
49+
#endif
50+
51+
#ifdef HAVE_COLORED_KINFU
52+
std::unique_ptr<KinectFusion> makeColoredKinFu(const yarp::os::Searchable & config,
53+
const yarp::sig::IntrinsicParams & depthIntrinsic,
54+
const yarp::sig::IntrinsicParams & rgbIntrinsic,
55+
int depthWidth, int depthHeight,
56+
int rgbWidth, int rgbHeight);
4357
#endif
4458

4559
} // namespace roboticslab

‎programs/sceneReconstruction/KinectFusionImpl.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ class KinectFusionImpl : public KinectFusion
6363
}
6464
}
6565

66-
bool update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame) override
66+
bool update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame, const yarp::sig::FlexImage & rgbFrame) override
6767
{
6868
// Cast away constness so that toCvMat accepts the YARP image. This function
6969
// does not alter the inner structure of PixelFloat images anyway.
@@ -79,7 +79,7 @@ class KinectFusionImpl : public KinectFusion
7979
handle->reset();
8080
}
8181

82-
void render(yarp::sig::ImageOf<yarp::sig::PixelMono> & image) const override
82+
void render(yarp::sig::ImageOf<yarp::sig::PixelRgb> & image) const override
8383
{
8484
cv::UMat umat;
8585
handle->render(umat);

0 commit comments

Comments
 (0)