Skip to content

Commit 5a9d7fb

Browse files
savuorsavuor
authored
Merge pull request #3366 from savuor:icp_oframe_readonly
Changes Complement PR for #22598@main *KinFu classes updated to comply with a new semantics.
1 parent f2f05c2 commit 5a9d7fb

File tree

6 files changed

+27
-42
lines changed

6 files changed

+27
-42
lines changed

‎modules/rgbd/src/colored_kinfu.cpp

Lines changed: 8 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -162,7 +162,8 @@ class ColoredKinFuImpl : public ColoredKinFu
162162

163163
int frameCounter;
164164
Matx44f pose;
165-
OdometryFrame renderFrame;
165+
// Mutable because it contains cache updated by icp.prepareFrame() during rendering
166+
mutable OdometryFrame renderFrame;
166167
OdometryFrame prevFrame;
167168
};
168169

@@ -289,20 +290,14 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
289290
else
290291
rgb = _rgb;
291292

292-
OdometryFrame newFrame = icp.createOdometryFrame();
293-
294-
newFrame.setImage(rgb);
295-
newFrame.setDepth(depth);
293+
OdometryFrame newFrame(rgb, depth);
296294

297295
if(frameCounter == 0)
298296
{
299297
icp.prepareFrame(newFrame);
300298

301299
// use depth instead of distance
302300
volume.integrate(depth, rgb, pose);
303-
// TODO: try to move setPyramidLevel from kinfu to volume
304-
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
305-
newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
306301
}
307302
else
308303
{
@@ -324,19 +319,13 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
324319
{
325320
// use depth instead of distance
326321
volume.integrate(depth, rgb, pose);
327-
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
328-
newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
329322
}
330323
MatType points, normals, colors;
331-
newFrame.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
332-
newFrame.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
333-
newFrame.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
334324

335325
volume.raycast(pose, points, normals, colors);
336-
337-
newFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
338-
newFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
339-
newFrame.setPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
326+
std::vector<MatType> pch(3);
327+
split(points, pch);
328+
newFrame = OdometryFrame(colors, pch[2], noArray(), normals);
340329
}
341330

342331
renderFrame = newFrame;
@@ -352,9 +341,10 @@ void ColoredKinFuImpl<MatType>::render(OutputArray image) const
352341
{
353342
CV_TRACE_FUNCTION();
354343
MatType pts, nrm, rgb;
344+
icp.prepareFrame(renderFrame);
355345
renderFrame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
356346
renderFrame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
357-
renderFrame.getPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
347+
renderFrame.getImage(rgb);
358348

359349
detail::renderPointsNormalsColors(pts, nrm, rgb, image);
360350
}

‎modules/rgbd/src/dynafu.cpp

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -315,8 +315,7 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
315315
else
316316
depth = _depth;
317317

318-
OdometryFrame newFrame = icp.createOdometryFrame();
319-
newFrame.setDepth(depth);
318+
OdometryFrame newFrame(noArray(), depth);
320319

321320
//icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);
322321

@@ -338,8 +337,7 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
338337
renderSurface(_depthRender, _vertRender, _normRender, false);
339338
_depthRender.convertTo(estdDepth, DEPTH_TYPE);
340339

341-
OdometryFrame estdFrame = icp.createOdometryFrame();
342-
estdFrame.setDepth(estdDepth);
340+
OdometryFrame estdFrame(noArray(), estdDepth);
343341
//icp->setDepthFactor(1.f);
344342
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
345343
//icp->setDepthFactor(params.depthFactor);
@@ -362,8 +360,8 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
362360
renderSurface(_depthRender, _vertRender, _normRender);
363361
_depthRender.convertTo(estdDepth, DEPTH_TYPE);
364362

365-
estdFrame = icp.createOdometryFrame();
366-
estdFrame.getDepth(estdDepth);
363+
estdFrame = OdometryFrame(noArray(), estdDepth);
364+
icp.prepareFrame(estdFrame);
367365
//estdFrame = OdometryFrame::create(noArray(), estdDepth, noArray(), noArray(), -1);
368366
//icp->setDepthFactor(1.f);
369367
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);

‎modules/rgbd/src/kinfu.cpp

Lines changed: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -258,8 +258,7 @@ bool KinFuImpl<MatType>::updateT(const MatType& _depth)
258258
else
259259
depth = _depth;
260260

261-
OdometryFrame newFrame = icp.createOdometryFrame();
262-
newFrame.setDepth(depth);
261+
OdometryFrame newFrame(noArray(), depth);
263262

264263
if(frameCounter == 0)
265264
{
@@ -289,12 +288,11 @@ bool KinFuImpl<MatType>::updateT(const MatType& _depth)
289288
}
290289

291290
MatType points, normals;
292-
newFrame.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
293-
newFrame.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
294291
volume.raycast(pose, points, normals);
295292

296-
newFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
297-
newFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
293+
std::vector<MatType> pch(3);
294+
split(points, pch);
295+
newFrame = OdometryFrame(noArray(), pch[2], noArray(), normals);
298296
}
299297

300298
renderFrame = newFrame;

‎modules/rgbd/src/large_kinfu.cpp

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -321,8 +321,7 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
321321
else
322322
depth = _depth;
323323

324-
OdometryFrame newFrame = icp.createOdometryFrame();
325-
newFrame.setDepth(depth);
324+
OdometryFrame newFrame(noArray(), depth);
326325

327326
CV_LOG_INFO(NULL, "Current frameID: " << frameCounter);
328327
for (const auto& it : submapMgr->activeSubmaps)
@@ -370,7 +369,7 @@ bool LargeKinfuImpl<MatType>::updateT(const MatType& _depth)
370369
}
371370

372371
//3. Raycast
373-
currTrackingSubmap->raycast(this->icp, currTrackingSubmap->cameraPose, params.frameSize);
372+
currTrackingSubmap->raycast(currTrackingSubmap->cameraPose, params.frameSize);
374373

375374
CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Total allocated blocks: " << currTrackingSubmap->getTotalAllocatedBlocks());
376375
CV_LOG_INFO(NULL, "Submap: " << currTrackingId << " Visible blocks: " << currTrackingSubmap->getVisibleBlocks(frameCounter));
@@ -422,7 +421,7 @@ void LargeKinfuImpl<MatType>::render(OutputArray image, const Matx44f& _cameraPo
422421
Affine3f cameraPose(_cameraPose);
423422
auto currSubmap = submapMgr->getCurrentSubmap();
424423
MatType points, normals;
425-
currSubmap->raycast(this->icp, cameraPose, params.frameSize, points, normals);
424+
currSubmap->raycast(cameraPose, params.frameSize, points, normals);
426425
detail::renderPointsNormals(points, normals, image, params.lightPose);
427426
}
428427

‎modules/rgbd/test/test_colored_kinfu.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -361,9 +361,9 @@ Ptr<Scene> Scene::create(int nScene, Size sz, Matx33f _intr, float _depthFactor)
361361
return makePtr<CubeSpheresScene>(sz, _intr, _depthFactor);
362362
}
363363

364-
static inline void CheckFrequency(Mat image)
364+
static inline void checkRenderedColors(Mat image)
365365
{
366-
float all = (float)image.size().height * image.size().width;
366+
float area = (float)image.total();
367367
int cc1 = 0, cc2 = 0, cc3 = 0, cc4 = 0;
368368
Vec3b c1 = Vec3b(200, 0, 0), c2 = Vec3b(0, 200, 0);
369369
Vec3b c3 = Vec3b(0, 0, 200), c4 = Vec3b(100, 100, 100);
@@ -378,10 +378,10 @@ static inline void CheckFrequency(Mat image)
378378
if (color == c4) cc4++;
379379
}
380380
}
381-
ASSERT_LT(float(cc1) / all, 0.2);
382-
ASSERT_LT(float(cc2) / all, 0.2);
383-
ASSERT_LT(float(cc3) / all, 0.2);
384-
ASSERT_LT(float(cc4) / all, 0.2);
381+
ASSERT_LT(float(cc1) / area, 0.2);
382+
ASSERT_LT(float(cc2) / area, 0.2);
383+
ASSERT_LT(float(cc3) / area, 0.2);
384+
ASSERT_LT(float(cc4) / area, 0.2);
385385
}
386386

387387
static const bool display = false;
@@ -427,7 +427,7 @@ void flyTest(bool hiDense, bool test_colors)
427427
{
428428
Mat rendered;
429429
kf->render(rendered);
430-
CheckFrequency(rendered);
430+
checkRenderedColors(rendered);
431431
return;
432432
}
433433
}

‎modules/rgbd/test/test_kinfu.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -326,7 +326,7 @@ void flyTest(bool hiDense, bool inequal, bool hashTsdf = false)
326326

327327
double rvecThreshold = hiDense ? 0.01 : 0.02;
328328
ASSERT_LT(cv::norm(kfPose.rvec() - pose.rvec()), rvecThreshold);
329-
double poseThreshold = hiDense ? 0.1 : 0.2;
329+
double poseThreshold = hiDense ? 0.11 : 0.2;
330330
ASSERT_LT(cv::norm(kfPose.translation() - pose.translation()), poseThreshold);
331331
}
332332

0 commit comments

Comments
 (0)