Skip to content

Commit 119fb7e

Browse files
DumDereDumDumDereDum
authored
Merge pull request #3162 from DumDereDum:new_volume
new Volume pipeline
1 parent 92dccac commit 119fb7e

File tree

6 files changed

+312
-56
lines changed

6 files changed

+312
-56
lines changed

‎modules/rgbd/include/opencv2/rgbd/colored_kinfu.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,7 +87,7 @@ struct CV_EXPORTS_W Params
8787
/** @brief rgb frame size in pixels */
8888
CV_PROP_RW Size rgb_frameSize;
8989

90-
CV_PROP_RW int volumeKind;
90+
CV_PROP_RW VolumeType volumeKind;
9191

9292
/** @brief camera intrinsics */
9393
CV_PROP_RW Matx33f intr;

‎modules/rgbd/include/opencv2/rgbd/kinfu.hpp‎

Lines changed: 56 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,61 @@ namespace kinfu {
1616
//! @addtogroup kinect_fusion
1717
//! @{
1818

19+
struct CV_EXPORTS_W VolumeParams
20+
{
21+
/** @brief Kind of Volume
22+
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
23+
*/
24+
CV_PROP_RW VolumeType kind = VolumeType::TSDF;
25+
26+
/** @brief Resolution of voxel space
27+
Number of voxels in each dimension.
28+
Applicable only for TSDF Volume.
29+
HashTSDF volume only supports equal resolution in all three dimensions
30+
*/
31+
CV_PROP_RW int resolutionX = 128;
32+
CV_PROP_RW int resolutionY = 128;
33+
CV_PROP_RW int resolutionZ = 128;
34+
35+
/** @brief Resolution of volumeUnit in voxel space
36+
Number of voxels in each dimension for volumeUnit
37+
Applicable only for hashTSDF.
38+
*/
39+
CV_PROP_RW int unitResolution = 0;
40+
41+
/** @brief Size of volume in meters */
42+
CV_PROP_RW float volumSize = 3.f;
43+
44+
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
45+
CV_PROP_RW Matx44f pose = Affine3f().translate(Vec3f(-volumSize / 2.f, -volumSize / 2.f, 0.5f)).matrix;
46+
47+
/** @brief Length of voxels in meters */
48+
CV_PROP_RW float voxelSize = volumSize / 512.f;
49+
50+
/** @brief TSDF truncation distance
51+
Distances greater than value from surface will be truncated to 1.0
52+
*/
53+
CV_PROP_RW float tsdfTruncDist = 7.f * voxelSize;
54+
55+
/** @brief Max number of frames to integrate per voxel
56+
Represents the max number of frames over which a running average
57+
of the TSDF is calculated for a voxel
58+
*/
59+
CV_PROP_RW int maxWeight = 64;
60+
61+
/** @brief Threshold for depth truncation in meters
62+
Truncates the depth greater than threshold to 0
63+
*/
64+
CV_PROP_RW float depthTruncThreshold = 0.f;
65+
66+
/** @brief Length of single raycast step
67+
Describes the percentage of voxel length that is skipped per march
68+
*/
69+
CV_PROP_RW float raycastStepFactor = 0.25f;
70+
};
71+
72+
73+
1974
struct CV_EXPORTS_W Params
2075
{
2176
CV_WRAP Params()
@@ -85,7 +140,7 @@ struct CV_EXPORTS_W Params
85140
CV_PROP_RW Size frameSize;
86141

87142
/** @brief Volume kind */
88-
int volumeKind;
143+
VolumeType volumeKind;
89144

90145
/** @brief camera intrinsics */
91146
CV_PROP_RW Matx33f intr;

‎modules/rgbd/include/opencv2/rgbd/large_kinfu.hpp‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,20 +17,89 @@ namespace cv
1717
{
1818
namespace large_kinfu
1919
{
20+
struct CV_EXPORTS_W VolumeParams
21+
{
22+
/** @brief Kind of Volume
23+
24+
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
25+
*/
26+
CV_PROP_RW VolumeType kind = VolumeType::TSDF;
27+
28+
/** @brief Resolution of voxel space
29+
30+
Number of voxels in each dimension.
31+
Applicable only for TSDF Volume.
32+
HashTSDF volume only supports equal resolution in all three dimensions
33+
*/
34+
CV_PROP_RW int resolutionX = 128;
35+
CV_PROP_RW int resolutionY = 128;
36+
CV_PROP_RW int resolutionZ = 128;
37+
38+
/** @brief Resolution of volumeUnit in voxel space
39+
40+
Number of voxels in each dimension for volumeUnit
41+
Applicable only for hashTSDF.
42+
*/
43+
CV_PROP_RW int unitResolution = 0;
44+
45+
/** @brief Size of volume in meters */
46+
CV_PROP_RW float volumSize = 3.f;
47+
48+
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
49+
CV_PROP_RW Matx44f pose = Affine3f().translate(Vec3f(-volumSize / 2.f, -volumSize / 2.f, 0.5f)).matrix;
50+
51+
/** @brief Length of voxels in meters */
52+
CV_PROP_RW float voxelSize = volumSize / 512.f;
53+
54+
/** @brief TSDF truncation distance
55+
56+
Distances greater than value from surface will be truncated to 1.0
57+
*/
58+
CV_PROP_RW float tsdfTruncDist = 7.f * voxelSize;
59+
60+
/** @brief Max number of frames to integrate per voxel
61+
62+
Represents the max number of frames over which a running average
63+
of the TSDF is calculated for a voxel
64+
*/
65+
CV_PROP_RW int maxWeight = 64;
66+
67+
/** @brief Threshold for depth truncation in meters
68+
69+
Truncates the depth greater than threshold to 0
70+
*/
71+
CV_PROP_RW float depthTruncThreshold = 0.f;
72+
73+
/** @brief Length of single raycast step
74+
75+
Describes the percentage of voxel length that is skipped per march
76+
*/
77+
CV_PROP_RW float raycastStepFactor = 0.25f;
78+
79+
/** @brief Default set of parameters that provide higher quality reconstruction at the cost of slow performance.*/
80+
CV_WRAP static Ptr<VolumeParams> defaultParams(VolumeType volumeType);
81+
82+
/** @brief Coarse set of parameters that provides relatively higher performance at the cost of reconstrution quality.*/
83+
CV_WRAP static Ptr<VolumeParams> coarseParams(VolumeType volumeType);
84+
};
85+
2086
struct CV_EXPORTS_W Params
2187
{
2288
/** @brief Default parameters
89+
2390
A set of parameters which provides better model quality, can be very slow.
2491
*/
2592
CV_WRAP static Ptr<Params> defaultParams();
2693

2794
/** @brief Coarse parameters
95+
2896
A set of parameters which provides better speed, can fail to match frames
2997
in case of rapid sensor motion.
3098
*/
3199
CV_WRAP static Ptr<Params> coarseParams();
32100

33101
/** @brief HashTSDF parameters
102+
34103
A set of parameters suitable for use with HashTSDFVolume
35104
*/
36105
CV_WRAP static Ptr<Params> hashTSDFParams(bool isCoarse);
@@ -45,6 +114,7 @@ struct CV_EXPORTS_W Params
45114
CV_PROP_RW Matx33f rgb_intr;
46115

47116
/** @brief pre-scale per 1 meter for input values
117+
48118
Typical values are:
49119
* 5000 per 1 meter for the 16-bit PNG files of TUM database
50120
* 1000 per 1 meter for Kinect 2 device
@@ -63,6 +133,7 @@ struct CV_EXPORTS_W Params
63133
CV_PROP_RW int pyramidLevels;
64134

65135
/** @brief Minimal camera movement in meters
136+
66137
Integrate new depth frame only if camera movement exceeds this value.
67138
*/
68139
CV_PROP_RW float tsdf_min_camera_movement;
@@ -78,6 +149,7 @@ struct CV_EXPORTS_W Params
78149
CV_PROP_RW std::vector<int> icpIterations;
79150

80151
/** @brief Threshold for depth truncation in meters
152+
81153
All depth values beyond this threshold will be set to zero
82154
*/
83155
CV_PROP_RW float truncateThreshold;

‎modules/rgbd/src/colored_kinfu.cpp‎

Lines changed: 33 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ Ptr<Params> Params::defaultParams()
2727

2828
p.frameSize = Size(640, 480);
2929

30-
p.volumeKind = VolumeParams::VolumeKind::TSDF;
30+
p.volumeKind = VolumeType::TSDF;
3131

3232
float fx, fy, cx, cy;
3333
fx = fy = 525.f;
@@ -110,7 +110,7 @@ Ptr<Params> Params::hashTSDFParams(bool isCoarse)
110110
p = coarseParams();
111111
else
112112
p = defaultParams();
113-
p->volumeKind = VolumeParams::VolumeKind::HASHTSDF;
113+
p->volumeKind = VolumeType::HashTSDF;
114114
p->truncateThreshold = 4.f;
115115
return p;
116116
}
@@ -122,7 +122,7 @@ Ptr<Params> Params::coloredTSDFParams(bool isCoarse)
122122
p = coarseParams();
123123
else
124124
p = defaultParams();
125-
p->volumeKind = VolumeParams::VolumeKind::COLOREDTSDF;
125+
p->volumeKind = VolumeType::ColorTSDF;
126126

127127
return p;
128128
}
@@ -135,6 +135,7 @@ class ColoredKinFuImpl : public ColoredKinFu
135135
ColoredKinFuImpl(const Params& _params);
136136
virtual ~ColoredKinFuImpl();
137137

138+
static VolumeSettings paramsToSettings(const Params& params);
138139
const Params& getParams() const CV_OVERRIDE;
139140

140141
void render(OutputArray image) const CV_OVERRIDE;
@@ -154,25 +155,40 @@ class ColoredKinFuImpl : public ColoredKinFu
154155

155156
private:
156157
Params params;
158+
VolumeSettings settings;
157159

158160
Odometry icp;
159-
cv::Ptr<Volume> volume;
161+
Volume volume;
160162

161163
int frameCounter;
162164
Matx44f pose;
163165
OdometryFrame renderFrame;
164166
OdometryFrame prevFrame;
165167
};
166168

169+
template< typename MatType >
170+
VolumeSettings ColoredKinFuImpl<MatType>::paramsToSettings(const Params& params)
171+
{
172+
VolumeSettings vs(VolumeType::TSDF);
173+
vs.setVoxelSize(params.voxelSize);
174+
vs.setVolumePose(params.volumePose);
175+
vs.setRaycastStepFactor(params.raycast_step_factor);
176+
vs.setTsdfTruncateDistance(params.tsdf_trunc_dist);
177+
vs.setMaxWeight(params.tsdf_max_weight);
178+
vs.setMaxDepth(params.truncateThreshold);
179+
vs.setCameraIntegrateIntrinsics(params.intr);
180+
vs.setDepthFactor(params.depthFactor);
181+
vs.setVolumeResolution(params.volumeDims);
182+
183+
return vs;
184+
}
167185

168186
template< typename MatType >
169187
ColoredKinFuImpl<MatType>::ColoredKinFuImpl(const Params &_params) :
170-
params(_params)
188+
params(_params),
189+
settings(paramsToSettings(params)),
190+
volume(VolumeType::ColorTSDF, settings)
171191
{
172-
volume = makeVolume(params.volumeKind, params.voxelSize, params.volumePose, params.raycast_step_factor,
173-
params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold,
174-
params.volumeDims[0], params.volumeDims[1], params.volumeDims[2]);
175-
176192
OdometrySettings ods;
177193
ods.setCameraMatrix(Mat(params.intr));
178194
ods.setMaxRotation(30.f);
@@ -189,7 +205,7 @@ void ColoredKinFuImpl<MatType >::reset()
189205
{
190206
frameCounter = 0;
191207
pose = Affine3f::Identity().matrix;
192-
volume->reset();
208+
volume.reset();
193209
}
194210

195211
template< typename MatType >
@@ -283,7 +299,7 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
283299
icp.prepareFrame(newFrame);
284300

285301
// use depth instead of distance
286-
volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr);
302+
volume.integrate(depth, rgb, pose);
287303
// TODO: try to move setPyramidLevel from kinfu to volume
288304
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
289305
newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
@@ -307,7 +323,7 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
307323
if((rnorm + tnorm)/2 >= params.tsdf_min_camera_movement)
308324
{
309325
// use depth instead of distance
310-
volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr);
326+
volume.integrate(depth, rgb, pose);
311327
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
312328
newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
313329
}
@@ -316,7 +332,7 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
316332
newFrame.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
317333
newFrame.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
318334

319-
volume->raycast(pose, params.intr, params.frameSize, points, normals, colors);
335+
volume.raycast(pose, points, normals, colors);
320336

321337
newFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
322338
newFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
@@ -350,29 +366,29 @@ void ColoredKinFuImpl<MatType>::render(OutputArray image, const Matx44f& _camera
350366

351367
Affine3f cameraPose(_cameraPose);
352368
MatType points, normals, colors;
353-
volume->raycast(_cameraPose, params.intr, params.frameSize, points, normals, colors);
369+
volume.raycast(_cameraPose, points, normals, colors);
354370
detail::renderPointsNormalsColors(points, normals, colors, image);
355371
}
356372

357373

358374
template< typename MatType >
359375
void ColoredKinFuImpl<MatType>::getCloud(OutputArray p, OutputArray n) const
360376
{
361-
volume->fetchPointsNormals(p, n);
377+
volume.fetchPointsNormals(p, n);
362378
}
363379

364380

365381
template< typename MatType >
366382
void ColoredKinFuImpl<MatType>::getPoints(OutputArray points) const
367383
{
368-
volume->fetchPointsNormals(points, noArray());
384+
volume.fetchPointsNormals(points, noArray());
369385
}
370386

371387

372388
template< typename MatType >
373389
void ColoredKinFuImpl<MatType>::getNormals(InputArray points, OutputArray normals) const
374390
{
375-
volume->fetchNormals(points, normals);
391+
volume.fetchNormals(points, normals);
376392
}
377393

378394
// importing class

0 commit comments

Comments
 (0)