Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

New odometry Pipeline#3055

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to ourterms of service andprivacy statement. We’ll occasionally send you account related emails.

Already on GitHub?Sign in to your account

Merged
alalek merged 19 commits intoopencv:5.xfromDumDereDum:new_odometry
Dec 2, 2021
Merged
Show file tree
Hide file tree
Changes fromall commits
Commits
Show all changes
19 commits
Select commitHold shift + click to select a range
10d1020
Merge pull request #2972 from Channingss:date_type_fixer
ChanningssJun 15, 2021
55a0efc
Merge branch 'next' of https://github.com/opencv/opencv_contrib
DumDereDumOct 6, 2021
249520c
try to build it
DumDereDumOct 6, 2021
24d8a3c
colored kinfu works
DumDereDumOct 7, 2021
f4cf615
minor fixes
DumDereDumOct 8, 2021
7ab93ae
try to make it workable
DumDereDumOct 12, 2021
fac9a9a
it works
DumDereDumOct 12, 2021
ee82985
colored kinfu works
DumDereDumOct 12, 2021
0390826
update large_kinfu
DumDereDumOct 13, 2021
7f118cb
replace ICP with DEPTH; delete extra comments
DumDereDumOct 20, 2021
38ba603
comments fix
DumDereDumOct 21, 2021
3c513e4
minor fix
DumDereDumOct 28, 2021
9aed206
minor fix
DumDereDumNov 1, 2021
f7fc819
review fix
DumDereDumNov 3, 2021
d6380c6
docs fix
DumDereDumNov 3, 2021
c01e6fc
minor fix
DumDereDumNov 3, 2021
1301517
remove extra changes
DumDereDumNov 3, 2021
dc6487d
createOdometryFrame fix
DumDereDumNov 9, 2021
667f76e
add comment
DumDereDumNov 15, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletionmodules/rgbd/samples/kinfu_demo.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -207,7 +207,6 @@ int main(int argc, char **argv)
if(!idle)
{
imshow("depth", cvt8);

if(!kf->update(frame))
{
kf->reset();
Expand Down
72 changes: 39 additions & 33 deletionsmodules/rgbd/src/colored_kinfu.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -155,12 +155,13 @@ class ColoredKinFuImpl : public ColoredKinFu
private:
Params params;

cv::Ptr<FastICPOdometry> icp;
Odometry icp;
cv::Ptr<Volume> volume;

int frameCounter;
Matx44f pose;
cv::Ptr<OdometryFrame> prevFrame;
OdometryFrame renderFrame;
OdometryFrame prevFrame;
};


Expand All@@ -172,13 +173,13 @@ ColoredKinFuImpl<MatType>::ColoredKinFuImpl(const Params &_params) :
params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold,
params.volumeDims[0], params.volumeDims[1], params.volumeDims[2]);

icp = FastICPOdometry::create(Mat(params.intr), params.icpDistThresh, params.icpAngleThresh,
params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
params.icpIterations, params.depthFactor, params.truncateThreshold);
OdometrySettings ods;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others.Learn more.

What about other settings passed inFastICP constructor? Are they already set by default?

Copy link
MemberAuthor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others.Learn more.

yes

ods.setCameraMatrix(Mat(params.intr));
ods.setMaxRotation(30.f);
ods.setMaxTranslation(params.voxelSize * (float)params.volumeDims[0] * 0.5f);
ods.setIterCounts(params.icpIterations);

// TODO: make these tunable algorithm parameters
icp->setMaxRotation(30.f);
icp->setMaxTranslation(params.voxelSize * (float)params.volumeDims[0] * 0.5f);
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);

reset();
}
Expand DownExpand Up@@ -272,29 +273,32 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
else
rgb = _rgb;

cv::Ptr<OdometryFrame> newFrame = icp->makeOdometryFrame(rgb, depth, noArray());
//TODO: fix it
// This workaround is needed because we want to keep color image in newFrame
// FastICP doesn't use color info and doesn't prepare its pyramids
newFrame->setPyramidLevels(params.icpIterations.size());
OdometryFrame newFrame = icp.createOdometryFrame();

icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);
newFrame.setImage(rgb);
newFrame.setDepth(depth);

if(frameCounter == 0)
{
icp.prepareFrame(newFrame);

// use depth instead of distance
volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr);
newFrame->setPyramidAt(rgb, OdometryFrame::PYR_IMAGE, 0);
// TODO: try to move setPyramidLevel from kinfu to volume
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others.Learn more.

I guesssetPyramidLevel is called because all pyramid vectors are empty by default.
I left a comment in main repo's PR on that.

newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
}
else
{
Affine3f affine;
Matx44d mrt;
bool success = icp->compute(newFrame, prevFrame, mrt);
if(!success)
return false;
affine.matrix = mrt;
Mat Rt;
icp.prepareFrames(newFrame, prevFrame);
bool success = icp.compute(newFrame, prevFrame, Rt);

if (!success)
return false;
affine.matrix = Matx44f(Rt);
pose = (Affine3f(pose) * affine).matrix;

float rnorm = (float)cv::norm(affine.rvec());
Expand All@@ -304,23 +308,24 @@ bool ColoredKinFuImpl<MatType>::updateT(const MatType& _depth, const MatType& _r
{
// use depth instead of distance
volume->integrate(depth, rgb, params.depthFactor, pose, params.intr, params.rgb_intr);
newFrame.setPyramidLevel(params.icpIterations.size(), OdometryFramePyramidType::PYR_IMAGE);
newFrame.setPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);
}
MatType points, normals, colors;
newFrame->getPyramidAt(points, OdometryFrame::PYR_CLOUD, 0);
newFrame->getPyramidAt(normals, OdometryFrame::PYR_NORM, 0);
newFrame->getPyramidAt(colors, OdometryFrame::PYR_IMAGE, 0);
newFrame.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.getPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
newFrame.getPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);

volume->raycast(pose, params.intr, params.frameSize, points, normals, colors);
//TODO: fix it
// This workaround relates to specific process of pyramid building
newFrame->setDepth(noArray());

newFrame->setPyramidAt(points, OdometryFrame::PYR_CLOUD, 0);
newFrame->setPyramidAt(normals, OdometryFrame::PYR_NORM, 0);
newFrame->setPyramidAt(colors, OdometryFrame::PYR_IMAGE, 0);
icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);

newFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
newFrame.setPyramidAt(colors, OdometryFramePyramidType::PYR_IMAGE, 0);
}

renderFrame = newFrame;
prevFrame = newFrame;

frameCounter++;
return true;
}
Expand All@@ -331,9 +336,10 @@ void ColoredKinFuImpl<MatType>::render(OutputArray image) const
{
CV_TRACE_FUNCTION();
MatType pts, nrm, rgb;
prevFrame->getPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
prevFrame->getPyramidAt(nrm, OdometryFrame::PYR_NORM, 0);
prevFrame->getPyramidAt(rgb, OdometryFrame::PYR_IMAGE, 0);
renderFrame.getPyramidAt(pts, OdometryFramePyramidType::PYR_CLOUD, 0);
renderFrame.getPyramidAt(nrm, OdometryFramePyramidType::PYR_NORM, 0);
renderFrame.getPyramidAt(rgb, OdometryFramePyramidType::PYR_IMAGE, 0);

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

Expand Down
57 changes: 33 additions & 24 deletionsmodules/rgbd/src/dynafu.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -112,13 +112,13 @@ class DynaFuImpl : public DynaFu
private:
Params params;

cv::Ptr<FastICPOdometry> icp;
Odometry icp;
cv::Ptr<NonRigidICP> dynafuICP;
cv::Ptr<TSDFVolume> volume;

int frameCounter;
Affine3f pose;
cv::Ptr<OdometryFrame> frame;
OdometryFrame frame;

WarpField warpfield;

Expand DownExpand Up@@ -171,10 +171,13 @@ DynaFuImpl<T>::DynaFuImpl(const Params &_params) :
glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_COLOR_ATTACHMENT0_EXT, GL_RENDERBUFFER_EXT, fbo_color);

#endif
OdometrySettings ods;
ods.setCameraMatrix(Mat(params.intr));
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);

icp = FastICPOdometry::create(Mat(params.intr), params.icpDistThresh, params.icpAngleThresh,
params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
params.icpIterations, params.depthFactor, params.truncateThreshold);
//icp = FastICPOdometry::create(Mat(params.intr), params.icpDistThresh, params.icpAngleThresh,
// params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
// params.icpIterations, params.depthFactor, params.truncateThreshold);

reset();
}
Expand DownExpand Up@@ -312,9 +315,10 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
else
depth = _depth;

cv::Ptr<OdometryFrame> newFrame = icp->makeOdometryFrame(noArray(), depth, noArray());
OdometryFrame newFrame = icp.createOdometryFrame();
newFrame.setDepth(depth);

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

if(frameCounter == 0)
{
Expand All@@ -334,19 +338,22 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
renderSurface(_depthRender, _vertRender, _normRender, false);
_depthRender.convertTo(estdDepth, DEPTH_TYPE);

Ptr<OdometryFrame> estdFrame = icp->makeOdometryFrame(noArray(), estdDepth, noArray());
icp->setDepthFactor(1.f);
icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
icp->setDepthFactor(params.depthFactor);
OdometryFrame estdFrame = icp.createOdometryFrame();
estdFrame.setDepth(estdDepth);
//icp->setDepthFactor(1.f);
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
//icp->setDepthFactor(params.depthFactor);

frame = estdFrame;

Affine3f affine;
Matx44d mrt;
bool success = icp->compute(newFrame, frame, mrt);
if(!success)
Mat Rt;
icp.prepareFrames(frame, newFrame);
bool success = icp.compute(newFrame, frame, Rt);
if (!success)
return false;
affine.matrix =mrt;
affine =Affine3f(Rt);

pose = pose * affine;

Expand All@@ -355,16 +362,18 @@ bool DynaFuImpl<T>::updateT(const T& _depth)
renderSurface(_depthRender, _vertRender, _normRender);
_depthRender.convertTo(estdDepth, DEPTH_TYPE);

estdFrame = OdometryFrame::create(noArray(), estdDepth, noArray(), noArray(), -1);
icp->setDepthFactor(1.f);
icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
icp->setDepthFactor(params.depthFactor);
estdFrame = icp.createOdometryFrame();
estdFrame.getDepth(estdDepth);
//estdFrame = OdometryFrame::create(noArray(), estdDepth, noArray(), noArray(), -1);
//icp->setDepthFactor(1.f);
//icp->prepareFrameCache(estdFrame, OdometryFrame::CACHE_SRC);
//icp->setDepthFactor(params.depthFactor);

T estdPts, estdNrm, newPts, newNrm;
estdFrame->getPyramidAt(estdPts,OdometryFrame::PYR_CLOUD, 0);
estdFrame->getPyramidAt(estdNrm,OdometryFrame::PYR_NORM, 0);
newFrame->getPyramidAt(newPts,OdometryFrame::PYR_CLOUD, 0);
newFrame->getPyramidAt(newNrm,OdometryFrame::PYR_NORM, 0);
estdFrame.getPyramidAt(estdPts,OdometryFramePyramidType::PYR_CLOUD, 0);
estdFrame.getPyramidAt(estdNrm,OdometryFramePyramidType::PYR_NORM, 0);
newFrame.getPyramidAt(newPts,OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.getPyramidAt(newNrm,OdometryFramePyramidType::PYR_NORM, 0);
success = dynafuICP->estimateWarpNodes(warpfield, pose, _vertRender, estdPts, estdNrm, newPts, newNrm);
if(!success)
return false;
Expand DownExpand Up@@ -397,8 +406,8 @@ void DynaFuImpl<T>::render(OutputArray image, const Matx44f& _cameraPose) const
(cameraPose.rotation() == id.rotation() && cameraPose.translation() == id.translation()))
{
T pts, nrm;
frame->getPyramidAt(pts,OdometryFrame::PYR_CLOUD, 0);
frame->getPyramidAt(nrm,OdometryFrame::PYR_NORM, 0);
frame.getPyramidAt(pts,OdometryFramePyramidType::PYR_CLOUD, 0);
frame.getPyramidAt(nrm,OdometryFramePyramidType::PYR_NORM, 0);

detail::renderPointsNormals(pts, nrm, image, params.lightPose);
}
Expand Down
48 changes: 23 additions & 25 deletionsmodules/rgbd/src/kinfu.cpp
View file
Open in desktop
Original file line numberDiff line numberDiff line change
Expand Up@@ -143,12 +143,13 @@ class KinFuImpl : public KinFu
private:
Params params;

cv::Ptr<FastICPOdometry> icp;
Odometry icp;
cv::Ptr<Volume> volume;

int frameCounter;
Matx44f pose;
cv::Ptr<OdometryFrame> prevFrame;
OdometryFrame prevFrame;
OdometryFrame renderFrame;
};


Expand All@@ -160,13 +161,11 @@ KinFuImpl<MatType>::KinFuImpl(const Params &_params) :
params.tsdf_trunc_dist, params.tsdf_max_weight, params.truncateThreshold,
params.volumeDims[0], params.volumeDims[1], params.volumeDims[2]);

icp = FastICPOdometry::create(Mat(params.intr), params.icpDistThresh, params.icpAngleThresh,
params.bilateral_sigma_depth, params.bilateral_sigma_spatial, params.bilateral_kernel_size,
params.icpIterations, params.depthFactor, params.truncateThreshold);

// TODO: make these tunable algorithm parameters
icp->setMaxRotation(30.f);
icp->setMaxTranslation(params.voxelSize * params.volumeDims[0] * 0.5f);
OdometrySettings ods;
ods.setMaxRotation(30.f);
ods.setMaxTranslation(params.voxelSize * params.volumeDims[0] * 0.5f);
ods.setCameraMatrix(Mat(params.intr));
icp = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);

reset();
}
Expand DownExpand Up@@ -243,24 +242,25 @@ bool KinFuImpl<MatType>::updateT(const MatType& _depth)
else
depth = _depth;

cv::Ptr<OdometryFrame> newFrame = icp->makeOdometryFrame(noArray(), depth, noArray());

icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);
OdometryFrame newFrame = icp.createOdometryFrame();
newFrame.setDepth(depth);

if(frameCounter == 0)
{
icp.prepareFrame(newFrame);
// use depth instead of distance
volume->integrate(depth, params.depthFactor, pose, params.intr);
}
else
{
Affine3f affine;
Matx44d mrt;
bool success = icp->compute(newFrame, prevFrame, mrt);
Mat Rt;
icp.prepareFrames(newFrame, prevFrame);
bool success = icp.compute(newFrame, prevFrame, Rt);
if(!success)
return false;
affine.matrix = mrt;

affine.matrix = Matx44f(Rt);
pose = (Affine3f(pose) * affine).matrix;

float rnorm = (float)cv::norm(affine.rvec());
Expand All@@ -273,19 +273,17 @@ bool KinFuImpl<MatType>::updateT(const MatType& _depth)
}

MatType points, normals;
newFrame->getPyramidAt(points, OdometryFrame::PYR_CLOUD, 0);
newFrame->getPyramidAt(normals,OdometryFrame::PYR_NORM, 0);
newFrame.getPyramidAt(points,OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.getPyramidAt(normals,OdometryFramePyramidType::PYR_NORM, 0);
volume->raycast(pose, params.intr, params.frameSize, points, normals);
//TODO: fix it
// This workaround relates to specific process of pyramid building
newFrame->setDepth(noArray());

newFrame->setPyramidAt(points, OdometryFrame::PYR_CLOUD, 0);
newFrame->setPyramidAt(normals, OdometryFrame::PYR_NORM, 0);
icp->prepareFrameCache(newFrame, OdometryFrame::CACHE_SRC);
newFrame.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
newFrame.setPyramidAt(normals, OdometryFramePyramidType::PYR_NORM, 0);
}

renderFrame = newFrame;
prevFrame = newFrame;

frameCounter++;
return true;
}
Expand All@@ -296,8 +294,8 @@ void KinFuImpl<MatType>::render(OutputArray image) const
{
CV_TRACE_FUNCTION();
MatType pts, nrm;
prevFrame->getPyramidAt(pts,OdometryFrame::PYR_CLOUD, 0);
prevFrame->getPyramidAt(nrm,OdometryFrame::PYR_NORM, 0);
renderFrame.getPyramidAt(pts,OdometryFramePyramidType::PYR_CLOUD, 0);
renderFrame.getPyramidAt(nrm,OdometryFramePyramidType::PYR_NORM, 0);
detail::renderPointsNormals(pts, nrm, image, params.lightPose);
}

Expand Down
Loading
[8]ページ先頭
©2009-2025 Movatter.jp