Skip to content

Commit 01598b6

Browse files
PeterBowmanPeterBowman
committed
Implement large-scale KinFu
1 parent 3ed7898 commit 01598b6

File tree

5 files changed

+233
-23
lines changed

5 files changed

+233
-23
lines changed

‎programs/sceneReconstruction/CMakeLists.txt

+5
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,11 @@ if(ENABLE_sceneReconstruction)
3131
target_compile_definitions(sceneReconstruction PRIVATE HAVE_DYNAFU)
3232
endif()
3333

34+
if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.5.1)
35+
target_sources(sceneReconstruction PRIVATE KinFuLargeScale.cpp)
36+
target_compile_definitions(sceneReconstruction PRIVATE HAVE_KINFU_LS)
37+
endif()
38+
3439
install(TARGETS sceneReconstruction
3540
DESTINATION ${CMAKE_INSTALL_BINDIR})
3641

‎programs/sceneReconstruction/KinFuLargeScale.cpp

+193
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,193 @@
1+
// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2+
3+
#include "KinectFusionImpl.hpp"
4+
5+
#include <map>
6+
7+
#include <yarp/os/LogStream.h>
8+
9+
#include <opencv2/rgbd/large_kinfu.hpp>
10+
11+
namespace
12+
{
13+
std::map<std::string, cv::kinfu::VolumeType> stringToCvVolume {
14+
{"tsdf", cv::kinfu::VolumeType::TSDF},
15+
{"hashtsdf", cv::kinfu::VolumeType::HASHTSDF}
16+
};
17+
}
18+
19+
namespace roboticslab
20+
{
21+
22+
std::unique_ptr<KinectFusion> makeKinFuLargeScale(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height)
23+
{
24+
using Params = cv::large_kinfu::Params;
25+
using VolParams = cv::kinfu::VolumeParams;
26+
27+
auto params = Params::defaultParams();
28+
29+
yInfo() << "algorithm: DynaFu";
30+
31+
params->frameSize = cv::Size(width, height);
32+
yInfo() << "dimensions: width =" << width << "x height =" << height;
33+
34+
float fx = intrinsic.focalLengthX;
35+
float fy = intrinsic.focalLengthY;
36+
float cx = intrinsic.principalPointX;
37+
float cy = intrinsic.principalPointY;
38+
39+
params->intr = cv::Matx33f(fx, 0, cx,
40+
0, fy, cy,
41+
0, 0, 1);
42+
43+
yInfo() << "intrinsic params: fx =" << fx << "fy =" << fy << "cx =" << cx << "cy =" << cy;
44+
45+
updateParam(*params, &Params::depthFactor, config, "depthFactor", "pre-scale per 1 meter for input values");
46+
updateParam(params->volumeParams, &VolParams::voxelSize, config, "voxelSize", "size of voxel in meters");
47+
updateParam(params->volumeParams, &VolParams::raycastStepFactor, config, "raycastStepFactor", "a length in voxel sizes for one raycast step");
48+
49+
if (config.check("volumeDims", "resolution of voxel space"))
50+
{
51+
yarp::os::Bottle * volumeDims = config.find("volumeDims").asList();
52+
53+
if (volumeDims == nullptr || volumeDims->size() != 3)
54+
{
55+
yError() << "Parameter volumeDims must be a list of 3 integers";
56+
return nullptr;
57+
}
58+
59+
params->volumeParams.resolution = cv::Vec3i(volumeDims->get(0).asInt32(), volumeDims->get(1).asInt32(), volumeDims->get(2).asInt32());
60+
yInfo() << "volumeDims =" << volumeDims->toString();
61+
}
62+
else
63+
{
64+
const auto & cvDims = params->volumeParams.resolution;
65+
yInfo() << "volumeDims (DEFAULT) =" << cvDims[0] << cvDims[1] << cvDims[2];
66+
}
67+
68+
updateParam(params->volumeParams, &VolParams::unitResolution, config, "unitResolution", "resolution of volumeUnit in voxel space");
69+
70+
if (config.check("lightPose", "light pose for rendering in meters"))
71+
{
72+
yarp::os::Bottle * lightPose = config.find("lightPose").asList();
73+
74+
if (lightPose == nullptr || lightPose->size() != 3)
75+
{
76+
yError() << "Parameter lightPose must be a list of 3 floats";
77+
return nullptr;
78+
}
79+
80+
params->lightPose = cv::Vec3f(lightPose->get(0).asFloat32(), lightPose->get(1).asFloat32(), lightPose->get(2).asFloat32());
81+
yInfo() << "lightPose =" << lightPose->toString();
82+
}
83+
else
84+
{
85+
const auto & cvLight = params->lightPose;
86+
yInfo() << "lightPose (DEFAULT) =" << cvLight[0] << cvLight[1] << cvLight[2];
87+
}
88+
89+
if (config.check("volumePoseTransl", "volume pose (translation vector) in meters"))
90+
{
91+
yarp::os::Bottle * volumePoseTransl = config.find("volumePoseTransl").asList();
92+
93+
if (volumePoseTransl == nullptr || volumePoseTransl->size() != 3)
94+
{
95+
yError() << "Parameter volumePoseTransl must be a list of 3 floats";
96+
return nullptr;
97+
}
98+
99+
auto transl = cv::Vec3f(volumePoseTransl->get(0).asFloat32(), volumePoseTransl->get(1).asFloat32(), volumePoseTransl->get(2).asFloat32());
100+
params->volumeParams.pose.translation(transl);
101+
yInfo() << "volumePoseTransl =" << volumePoseTransl->toString();
102+
}
103+
else
104+
{
105+
const auto & transl = params->volumeParams.pose.translation();
106+
yInfo() << "volumePoseTransl (DEFAULT) =" << transl[0] << transl[1] << transl[2];
107+
}
108+
109+
if (config.check("volumePoseRot", "volume pose (rotation matrix) in radians"))
110+
{
111+
yarp::os::Bottle * volumePoseRot = config.find("volumePoseRot").asList();
112+
113+
if (volumePoseRot == nullptr || volumePoseRot->size() != 9)
114+
{
115+
yError() << "Parameter volumePoseRot must be a list of 9 floats (3x3 matrix)";
116+
return nullptr;
117+
}
118+
119+
auto rot = cv::Matx33f(volumePoseRot->get(0).asFloat32(), volumePoseRot->get(1).asFloat32(), volumePoseRot->get(2).asFloat32(),
120+
volumePoseRot->get(3).asFloat32(), volumePoseRot->get(4).asFloat32(), volumePoseRot->get(5).asFloat32(),
121+
volumePoseRot->get(6).asFloat32(), volumePoseRot->get(7).asFloat32(), volumePoseRot->get(8).asFloat32());
122+
123+
params->volumeParams.pose.rotation(rot);
124+
yInfo() << "volumePoseRot =" << volumePoseRot->toString();
125+
}
126+
else
127+
{
128+
const auto & rot = params->volumeParams.pose.rotation();
129+
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);
130+
}
131+
132+
updateParam(*params, &Params::truncateThreshold, config, "truncateThreshold", "threshold for depth truncation in meters");
133+
updateParam(params->volumeParams, &VolParams::depthTruncThreshold, config, "depthTruncThreshold", "threshold for depth truncation in meters");
134+
135+
if (config.check("volumeType", "type of voxel volume (tsdf, hashtsdf)"))
136+
{
137+
std::string volumeType = config.find("volumeType").asString();
138+
139+
if (stringToCvVolume.find(volumeType) == stringToCvVolume.end())
140+
{
141+
yError() << "Unsupported volume type" << volumeType;
142+
return nullptr;
143+
}
144+
145+
params->volumeParams.type = stringToCvVolume[volumeType];
146+
yInfo() << "volumeType =" << volumeType;
147+
}
148+
else
149+
{
150+
auto res = std::find_if(stringToCvVolume.begin(), stringToCvVolume.end(), [&params](const auto & el) { return el.second == params->volumeParams.type; });
151+
yInfo() << "volumeType (DEFAULT) =" << res->first;
152+
}
153+
154+
updateParam(*params, &Params::bilateral_kernel_size, config, "bilateralKernelSize", "kernel size in pixels for bilateral smooth");
155+
updateParam(*params, &Params::bilateral_sigma_depth, config, "bilateralSigmaDepth", "depth sigma in meters for bilateral smooth");
156+
updateParam(*params, &Params::bilateral_sigma_spatial, config, "bilateralSigmaSpatial", "spatial sigma in pixels for bilateral smooth");
157+
158+
updateParam(*params, &Params::icpAngleThresh, config, "icpAngleThresh", "angle threshold in radians for ICP");
159+
updateParam(*params, &Params::icpDistThresh, config, "icpDistThresh", "distance threshold in meters for ICP");
160+
updateParam(*params, &Params::pyramidLevels, config, "pyramidLevels", "number of pyramid levels for ICP");
161+
162+
if (config.check("icpIterations", "list of iterations per each ICP level"))
163+
{
164+
yarp::os::Bottle * icpIterations = config.find("icpIterations").asList();
165+
166+
if (icpIterations == nullptr)
167+
{
168+
yError() << "Parameter icpIterations must be a list";
169+
return nullptr;
170+
}
171+
172+
params->icpIterations.resize(icpIterations->size());
173+
174+
for (auto i = 0; i < icpIterations->size(); i++)
175+
{
176+
params->icpIterations[i] = icpIterations->get(i).asInt32();
177+
}
178+
179+
yInfo() << "icpIterations =" << icpIterations->toString();
180+
}
181+
else
182+
{
183+
yInfo() << "icpIterations (DEFAULT) =" << params->icpIterations;
184+
}
185+
186+
updateParam(params->volumeParams, &VolParams::maxWeight, config, "tsdfMaxWeight", "max number of frames per voxel");
187+
updateParam(*params, &Params::tsdf_min_camera_movement, config, "tsdfMinCameraMovement", "minimal camera movement in meters");
188+
updateParam(params->volumeParams, &VolParams::tsdfTruncDist, config, "tsdfTruncDist", "distance to truncate in meters");
189+
190+
return std::make_unique<KinectFusionImpl<cv::large_kinfu::LargeKinfu>>(cv::large_kinfu::LargeKinfu::create(params));
191+
}
192+
193+
} // namespace roboticslab

‎programs/sceneReconstruction/KinectFusion.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,10 @@ std::unique_ptr<KinectFusion> makeKinFu(const yarp::os::Searchable & config, con
3838
std::unique_ptr<KinectFusion> makeDynaFu(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
3939
#endif
4040

41+
#ifdef HAVE_KINFU_LS
42+
std::unique_ptr<KinectFusion> makeKinFuLargeScale(const yarp::os::Searchable & config, const yarp::sig::IntrinsicParams & intrinsic, int width, int height);
43+
#endif
44+
4145
} // namespace roboticslab
4246

4347
#endif // __KINECT_FUSION_HPP__

‎programs/sceneReconstruction/SceneReconstruction.cpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -118,10 +118,16 @@ bool SceneReconstruction::configure(yarp::os::ResourceFinder & rf)
118118
{
119119
kinfu = makeDynaFu(params, depthIntrinsic, width, height);
120120
}
121+
#endif
122+
#ifdef HAVE_KINFU_LS
123+
else if (algorithm == "kinfu_ls")
124+
{
125+
kinfu = makeKinFuLargeScale(params, depthIntrinsic, width, height);
126+
}
121127
#endif
122128
else
123129
{
124-
yError() << "Unsupported or unrecognized algorithm:" << algorithm << "(available: kinfu, dynafu)";
130+
yError() << "Unsupported or unrecognized algorithm:" << algorithm << "(available: kinfu, dynafu, kinfu_ls)";
125131
return false;
126132
}
127133

‎programs/sceneReconstruction/main.cpp

+24-22
Original file line numberDiff line numberDiff line change
@@ -27,28 +27,30 @@
2727
*
2828
* @section sceneReconstructionKinectFusion Kinect Fusion options
2929
*
30-
* | PROPERTY | DESCRIPTION | DEFAULT |
31-
* |-----------------------|----------------------------------------------|---------------------------------------|
32-
* | algorithm | Kinect Fusion algorithm ("kinfu", "dynafu") | kinfu |
33-
* | bilateralKernelSize | kernel size in pixels for bilateral smooth | 7 |
34-
* | bilateralSigmaDepth | depth sigma in meters for bilateral smooth | 0.04 |
35-
* | bilateralSigmaSpatial | spatial sigma in pixels for bilateral smooth | 4.5 |
36-
* | depthFactor | pre-scale per 1 meter for input values | 5000 |
37-
* | icpAngleThresh | angle threshold in radians for ICP | 0.523599 |
38-
* | icpDistThresh | distance threshold in meters for ICP | 0.1 |
39-
* | icpIterations | iterations per each ICP level | (10 5 4) |
40-
* | lightPose | light pose for rendering in meters | (0.0 0.0 0.0) |
41-
* | pyramidLevels | number of pyramid levels for ICP | 3 |
42-
* | raycastStepFactor | a lenght in voxel sizes for one raycast step | 0.25 |
43-
* | truncateThreshold | threshold for depth truncation in meters | 0.0 |
44-
* | tsdfMaxWeight | max number of frames per voxel | 64 |
45-
* | tsdfMinCameraMovement | minimal camera movement in meters | 0.0 |
46-
* | tsdfTruncDist | distance to truncate in meters | 0.0410156 |
47-
* | volumeDims | resolution of voxel space | (512 512 512) |
48-
* | volumePoseRot | volume pose (rotation matrix) in radians | (1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 1.0) |
49-
* | volumePoseTransl | volume pose (translation vector) in meters | (-1.5 -1.5 0.5) |
50-
* | volumeType | type of voxel volume ("tsdf", "hashtsdf") | tsdf |
51-
* | voxelSize | size of voxel in meters | 0.00585938 |
30+
* | PROPERTY | DESCRIPTION | DEFAULT |
31+
* |-----------------------|-------------------------------------------------------------|---------------------------------------|
32+
* | algorithm | Kinect Fusion algorithm ("kinfu", "dynafu", "kinfu_ls") | kinfu |
33+
* | bilateralKernelSize | kernel size in pixels for bilateral smooth | 7 |
34+
* | bilateralSigmaDepth | depth sigma in meters for bilateral smooth | 0.04 |
35+
* | bilateralSigmaSpatial | spatial sigma in pixels for bilateral smooth | 4.5 |
36+
* | depthFactor | pre-scale per 1 meter for input values | 5000 |
37+
* | depthTruncThreshold | distance to truncate in meters (kinfu_ls/hashtsdf) | 0.0410156 |
38+
* | icpAngleThresh | angle threshold in radians for ICP | 0.523599 |
39+
* | icpDistThresh | distance threshold in meters for ICP | 0.1 |
40+
* | icpIterations | iterations per each ICP level | (10 5 4) |
41+
* | lightPose | light pose for rendering in meters | (0.0 0.0 0.0) |
42+
* | pyramidLevels | number of pyramid levels for ICP | 3 |
43+
* | raycastStepFactor | a lenght in voxel sizes for one raycast step | 0.25 |
44+
* | truncateThreshold | threshold for depth truncation in meters | 0.0 |
45+
* | tsdfMaxWeight | max number of frames per voxel | 64 |
46+
* | tsdfMinCameraMovement | minimal camera movement in meters | 0.0 |
47+
* | tsdfTruncDist | distance to truncate in meters | 0.0410156 |
48+
* | unitResolution | resolution of volumeUnit in voxel space (kinfu_ls/hashtsdf) | 0 |
49+
* | volumeDims | resolution of voxel space | (512 512 512) |
50+
* | volumePoseRot | volume pose (rotation matrix) in radians | (1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 1.0) |
51+
* | volumePoseTransl | volume pose (translation vector) in meters | (-1.5 -1.5 0.5) |
52+
* | volumeType | type of voxel volume ("tsdf", "hashtsdf") | tsdf |
53+
* | voxelSize | size of voxel in meters | 0.00585938 |
5254
*
5355
* @section sceneReconstructionLicense License
5456
*

0 commit comments

Comments
 (0)