Skip to content

Commit 44c8df7

Browse files
PeterBowmanPeterBowman
committed
Merge branch 'scene'
2 parents 20b9242 + 68bd41b commit 44c8df7

14 files changed

+1083
-0
lines changed

‎programs/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ add_subdirectory(colorRegionDetection)
55
add_subdirectory(haarDetection)
66
add_subdirectory(kinectPxToReal)
77
add_subdirectory(pointAtObjectServer)
8+
add_subdirectory(sceneReconstruction)
89
add_subdirectory(switchDetection2D)
910
add_subdirectory(tensorflowDetection2D)
1011
add_subdirectory(voxelOccupancyDetection)

‎programs/sceneReconstruction/CMakeLists.txt

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
if(NOT DEFINED ENABLE_sceneReconstruction OR ENABLE_sceneReconstruction)
2+
if(NOT TARGET opencv_rgbd OR NOT OpenCV_VERSION VERSION_GREATER_EQUAL 4.0)
3+
message(WARNING "OpenCV 4.0+/rgbd module not found, disabling sceneReconstruction program")
4+
elseif(NOT YARP_cv_FOUND)
5+
message(WARNING "YARP_cv package not found, disabling sceneReconstruction program")
6+
endif()
7+
endif()
8+
9+
cmake_dependent_option(ENABLE_sceneReconstruction "Enable/disable sceneReconstruction program" ON
10+
"TARGET opencv_rgbd;OpenCV_VERSION VERSION_GREATER_EQUAL 4.0;YARP_cv_FOUND" OFF)
11+
12+
if(ENABLE_sceneReconstruction)
13+
14+
# sceneReconstruction
15+
add_executable(sceneReconstruction main.cpp
16+
SceneReconstruction.hpp
17+
SceneReconstruction.cpp
18+
KinectFusion.hpp
19+
KinectFusionImpl.hpp
20+
KinFu.cpp)
21+
22+
target_link_libraries(sceneReconstruction YARP::YARP_os
23+
YARP::YARP_init
24+
YARP::YARP_dev
25+
YARP::YARP_sig
26+
YARP::YARP_cv
27+
opencv_rgbd
28+
ROBOTICSLAB::ColorDebug)
29+
30+
if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.2)
31+
target_sources(sceneReconstruction PRIVATE DynaFu.cpp)
32+
target_compile_definitions(sceneReconstruction PRIVATE HAVE_DYNAFU)
33+
endif()
34+
35+
install(TARGETS sceneReconstruction
36+
DESTINATION ${CMAKE_INSTALL_BINDIR})
37+
38+
else()
39+
40+
set(ENABLE_sceneReconstruction OFF CACHE BOOL "Enable/disable sceneReconstruction program" FORCE)
41+
42+
endif()

‎programs/sceneReconstruction/DynaFu.cpp

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

0 commit comments

Comments
 (0)