Skip to content

Commit d9d0d07

Browse files
PeterBowmanPeterBowman
committed
Implement DynaFu
1 parent 095e7d2 commit d9d0d07

File tree

4 files changed

+301
-1
lines changed

4 files changed

+301
-1
lines changed

‎programs/sceneReconstruction/CMakeLists.txt

+5
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,11 @@ if(ENABLE_sceneReconstruction)
2626
opencv_rgbd
2727
ROBOTICSLAB::ColorDebug)
2828

29+
if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.2)
30+
target_sources(sceneReconstruction PRIVATE DynaFu.cpp)
31+
target_compile_definitions(sceneReconstruction PRIVATE HAVE_DYNAFU)
32+
endif()
33+
2934
install(TARGETS sceneReconstruction
3035
DESTINATION ${CMAKE_INSTALL_BINDIR})
3136

‎programs/sceneReconstruction/DynaFu.cpp

+286
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,286 @@
1+
// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2+
3+
#include "KinectFusionAdapter.hpp"
4+
5+
#include <algorithm>
6+
#include <iterator>
7+
#include <sstream>
8+
#include <type_traits>
9+
10+
#include <opencv2/rgbd/dynafu.hpp>
11+
12+
#include <yarp/cv/Cv.h>
13+
14+
#include <ColorDebug.h>
15+
16+
namespace
17+
{
18+
template <typename T, std::enable_if_t<std::is_integral<T>::value, bool> = true>
19+
inline T getValue(const yarp::os::Value & v)
20+
{
21+
return v.asInt32();
22+
}
23+
24+
template <typename T, std::enable_if_t<std::is_floating_point<T>::value, bool> = true>
25+
inline T getValue(const yarp::os::Value & v)
26+
{
27+
return v.asFloat32();
28+
}
29+
30+
template <typename T>
31+
void updateParam(cv::dynafu::Params & params, T cv::dynafu::Params::* param, const yarp::os::Searchable & config,
32+
const std::string & name, const std::string & description)
33+
{
34+
std::stringstream ss;
35+
ss << name;
36+
37+
if (config.check(name, description))
38+
{
39+
params.*param = getValue<T>(config.find(name));
40+
ss << ": ";
41+
}
42+
else
43+
{
44+
ss << " (DEFAULT): ";
45+
}
46+
47+
ss << params.*param << "\n";
48+
CD_INFO(ss.str().c_str());
49+
}
50+
51+
template <typename T>
52+
inline std::string vectorToString(const std::vector<T> & vec, const std::string & delimiter = " ")
53+
{
54+
// https://stackoverflow.com/a/8581865
55+
std::ostringstream oss;
56+
std::copy(vec.begin(), vec.end() - 1, std::ostream_iterator<T>(oss, delimiter.c_str()));
57+
oss << vec.back();
58+
return oss.str();
59+
}
60+
}
61+
62+
namespace roboticslab
63+
{
64+
65+
class DynaFu : public KinectFusionAdapter
66+
{
67+
public:
68+
DynaFu(const cv::Ptr<cv::dynafu::Params> & params) : handle(cv::dynafu::DynaFu::create(params))
69+
{
70+
cv::setUseOptimized(true);
71+
}
72+
73+
void getCloud(yarp::sig::PointCloudXYZNormal & cloudWithNormals) const override
74+
{
75+
cv::Mat points, normals;
76+
handle->getCloud(points, normals);
77+
cloudWithNormals.resize(points.rows);
78+
79+
for (auto i = 0; i < points.rows; i++)
80+
{
81+
const auto & point = points.at<cv::Vec4f>(i);
82+
const auto & normal = normals.at<cv::Vec4f>(i);
83+
cloudWithNormals(i) = {{point[0], point[1], point[2]}, {normal[0], normal[1], normal[2]}};
84+
}
85+
}
86+
87+
void getPoints(yarp::sig::PointCloudXYZ & cloud) const override
88+
{
89+
cv::Mat points;
90+
handle->getPoints(points);
91+
cloud.resize(points.rows);
92+
93+
for (auto i = 0; i < points.rows; i++)
94+
{
95+
const auto & point = points.at<cv::Vec4f>(i);
96+
cloud(i) = {point[0], point[1], point[2]};
97+
}
98+
}
99+
100+
void getPose(yarp::sig::Matrix & pose) const override
101+
{
102+
const auto & affine = handle->getPose().matrix;
103+
pose.resize(4, 4);
104+
105+
for (int i = 0; i < 4; i++)
106+
{
107+
for (int j = 0; j < 4; j++)
108+
{
109+
pose(i, j) = affine(i, j);
110+
}
111+
}
112+
}
113+
114+
bool update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame) override
115+
{
116+
// Cast away constness so that toCvMat accepts the YARP image. This function
117+
// does not alter the inner structure of PixelFloat images anyway.
118+
auto & nonConstDepthFrame = const_cast<yarp::sig::ImageOf<yarp::sig::PixelFloat> &>(depthFrame);
119+
cv::Mat mat = yarp::cv::toCvMat(nonConstDepthFrame);
120+
cv::UMat umat;
121+
mat.convertTo(umat, mat.type(), 1000.0); // OpenCV uses milimeters
122+
return handle->update(umat);
123+
}
124+
125+
void reset() override
126+
{
127+
handle->reset();
128+
}
129+
130+
void render(yarp::sig::ImageOf<yarp::sig::PixelRgb> & image) const override
131+
{
132+
cv::UMat umat;
133+
handle->render(umat);
134+
cv::Mat mat = umat.getMat(cv::ACCESS_RW);
135+
image = yarp::cv::fromCvMat<yarp::sig::PixelRgb>(mat);
136+
}
137+
138+
private:
139+
cv::Ptr<cv::dynafu::DynaFu> handle;
140+
};
141+
142+
std::unique_ptr<KinectFusionAdapter> makeDynaFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height)
143+
{
144+
using Params = cv::dynafu::Params;
145+
146+
auto params = Params::defaultParams();
147+
148+
params->frameSize = cv::Size(width, height);
149+
CD_INFO("dimensions: width = %d, height = %d\n", width, height);
150+
151+
float fx = intrinsic.focalLengthX;
152+
float fy = intrinsic.focalLengthY;
153+
float cx = intrinsic.principalPointX;
154+
float cy = intrinsic.principalPointY;
155+
156+
params->intr = cv::Matx33f(fx, 0, cx,
157+
0, fy, cy,
158+
0, 0, 1);
159+
160+
CD_INFO("intrinsic params: fx = %f, fy = %f, cx = %f, cy = %f\n", fx, fy, cx, cy);
161+
162+
updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");
163+
updateParam(*params, &Params::voxelSize, config, "voxelSize", "size of voxel in meters");
164+
updateParam(*params, &Params::raycast_step_factor, config, "raycastStepFactor", "a length in voxel sizes for one raycast step");
165+
166+
if (config.check("volumeDims", "resolution of voxel space"))
167+
{
168+
yarp::os::Bottle * volumeDims = config.find("volumeDims").asList();
169+
170+
if (volumeDims == nullptr || volumeDims->size() != 3)
171+
{
172+
CD_ERROR("Parameter volumeDims must be a list of 3 integers.\n");
173+
return nullptr;
174+
}
175+
176+
params->volumeDims = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());
177+
CD_INFO("volumeDims: %s\n", volumeDims->toString().c_str());
178+
}
179+
else
180+
{
181+
const auto & cvDims = params->volumeDims;
182+
CD_INFO("volumeDims (DEFAULT): (%d %d %d)\n", cvDims[0], cvDims[1], cvDims[2]);
183+
}
184+
185+
if (config.check("lightPose", "light pose for rendering in meters"))
186+
{
187+
yarp::os::Bottle * lightPose = config.find("lightPose").asList();
188+
189+
if (lightPose == nullptr || lightPose->size() != 3)
190+
{
191+
CD_ERROR("Parameter lightPose must be a list of 3 floats.\n");
192+
return nullptr;
193+
}
194+
195+
params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());
196+
CD_INFO("lightPose: %s\n", lightPose->toString().c_str());
197+
}
198+
else
199+
{
200+
const auto & cvLight = params->lightPose;
201+
CD_INFO("lightPose (DEFAULT): (%f %f %f)\n", cvLight[0], cvLight[1], cvLight[2]);
202+
}
203+
204+
if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))
205+
{
206+
yarp::os::Bottle * volumePoseTransl = config.find("volumePoseTransl").asList();
207+
208+
if (volumePoseTransl == nullptr || volumePoseTransl->size() != 3)
209+
{
210+
CD_ERROR("Parameter volumePoseTransl must be a list of 3 floats.\n");
211+
return nullptr;
212+
}
213+
214+
auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());
215+
params->volumePose.translation(transl);
216+
CD_INFO("volumePoseTransl: %s\n", volumePoseTransl->toString().c_str());
217+
}
218+
else
219+
{
220+
const auto & transl = params->volumePose.translation();
221+
CD_INFO("volumePoseTransl (DEFAULT): (%f %f %f)\n", transl[0], transl[1], transl[2]);
222+
}
223+
224+
if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))
225+
{
226+
yarp::os::Bottle * volumePoseRot = config.find("volumePoseRot").asList();
227+
228+
if (volumePoseRot == nullptr || volumePoseRot->size() != 9)
229+
{
230+
CD_ERROR("Parameter volumePoseRot must be a list of 9 floats (3x3 matrix).\n");
231+
return nullptr;
232+
}
233+
234+
auto rot = cv::Matx33f(volumePoseRot->get(0).asFloat32(), volumePoseRot->get(1).asFloat32(), volumePoseRot->get(2).asFloat32(),
235+
volumePoseRot->get(3).asFloat32(), volumePoseRot->get(4).asFloat32(), volumePoseRot->get(5).asFloat32(),
236+
volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());
237+
238+
params->volumePose.rotation(rot);
239+
CD_INFO("volumePoseRot: %s\n", volumePoseRot->toString().c_str());
240+
}
241+
else
242+
{
243+
const auto & rot = params->volumePose.rotation();
244+
CD_INFO("volumePoseRot (DEFAULT): (%f %f %f %f %f %f %f %f %f)\n", 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));
245+
}
246+
247+
updateParam(*params, &Params::truncateThreshold, config, "truncateThreshold", "threshold for depth truncation in meters");
248+
249+
updateParam(*params, &Params::bilateral_kernel_size, config, "bilateralKernelSize", "kernel size in pixels for bilateral smooth");
250+
updateParam(*params, &Params::bilateral_sigma_depth, config, "bilateralSigmaDepth", "depth sigma in meters for bilateral smooth");
251+
updateParam(*params, &Params::bilateral_sigma_spatial, config, "bilateralSigmaSpatial", "spatial sigma in pixels for bilateral smooth");
252+
253+
updateParam(*params, &Params::icpAngleThresh, config, "icpAngleThresh", "angle threshold in radians for ICP");
254+
updateParam(*params, &Params::icpDistThresh, config, "icpDistThresh", "distance threshold in meters for ICP");
255+
updateParam(*params, &Params::pyramidLevels, config, "pyramidLevels", "number of pyramid levels for ICP");
256+
257+
if (config.check("icpIterations", "list of iterations per each ICP level"))
258+
{
259+
yarp::os::Bottle * icpIterations = config.find("icpIterations").asList();
260+
261+
if (icpIterations == nullptr)
262+
{
263+
CD_ERROR("Parameter icpIterations must be a list.\n");
264+
return nullptr;
265+
}
266+
267+
for (auto i = 0; i < icpIterations->size(); i++)
268+
{
269+
params->icpIterations.push_back(icpIterations->get(i).asInt32());
270+
}
271+
272+
CD_INFO("icpIterations: %s\n", icpIterations->toString().c_str());
273+
}
274+
else
275+
{
276+
CD_INFO("icpIterations (DEFAULT): (%s)\n", vectorToString(params->icpIterations).c_str());
277+
}
278+
279+
updateParam(*params, &Params::tsdf_max_weight, config, "tsdfMaxWeight", "max number of frames per voxel");
280+
updateParam(*params, &Params::tsdf_min_camera_movement, config, "tsdfMinCameraMovement", "minimal camera movement in meters");
281+
updateParam(*params, &Params::tsdf_trunc_dist, config, "tsdfTruncDist", "distance to truncate in meters");
282+
283+
return std::make_unique<DynaFu>(params);
284+
}
285+
286+
} // namespace roboticslab

‎programs/sceneReconstruction/KinectFusionAdapter.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,8 @@ class KinectFusionAdapter
3434

3535
std::unique_ptr<KinectFusionAdapter> makeKinFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
3636

37+
std::unique_ptr<KinectFusionAdapter> makeDynaFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
38+
3739
} // namespace roboticslab
3840

3941
#endif // __KINECT_FUSION_ADAPTER_HPP__

‎programs/sceneReconstruction/SceneReconstruction.cpp

+8-1
Original file line numberDiff line numberDiff line change
@@ -114,9 +114,16 @@ bool SceneReconstruction::configure(yarp::os::ResourceFinder & rf)
114114
const auto & config = rf.findGroup("kinfu");
115115
kinfu = makeKinFu(config, depthIntrinsic, width, height);
116116
}
117+
#ifdef HAVE_DYNAFU
118+
else if (algorithm == "dynafu")
119+
{
120+
const auto & config = rf.findGroup("dynafu");
121+
kinfu = makeDynaFu(config, depthIntrinsic, width, height);
122+
}
123+
#endif
117124
else
118125
{
119-
CD_ERROR("Unsupported or unrecognized algorithm: %s (available: kinfu).\n", algorithm.c_str());
126+
CD_ERROR("Unsupported or unrecognized algorithm: %s (available: kinfu, dynafu).\n", algorithm.c_str());
120127
return false;
121128
}
122129

0 commit comments

Comments
 (0)