mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-12-05 01:39:13 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #21741 from DumDereDum:odometry_prepareFrame_fix

Browse Source 
Odometry prepareFrame fix

* fix issue; add tests

* img fix

* mask fix

* minor fix
This commit is contained in:
Artem Saratovtsev 2022-04-05 00:14:31 +03:00 committed by GitHub
parent aa5055261f
commit 3d12581798
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 71 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
modules/3d/src/rgbd/odometry_functions.cpp
View File

@ -64,6 +64,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool u
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_IMAGE, 0);
frame.setImage(pyr0);
frame.getGrayImage(image);
}
else
CV_Error(Error::StsBadSize, "Image or pyramidImage have to be set.");
@ -71,10 +72,9 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool u
checkImage(image);
TMat depth;
if (useDepth)
{
frame.getDepth(depth);
frame.getScaledDepth(depth);
if (depth.empty())
{
if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_DEPTH) > 0)
@ -90,6 +90,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool u
std::vector<TMat> xyz;
split(cloud, xyz);
frame.setDepth(xyz[2]);
frame.getScaledDepth(depth);
}
else
CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
@ -106,6 +107,7 @@ void prepareRGBFrameBase(OdometryFrame& frame, OdometrySettings settings, bool u
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0);
frame.setMask(pyr0);
frame.getMask(mask);
}
checkMask(mask, image.size());
@ -186,6 +188,7 @@ void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_DEPTH, 0);
frame.setDepth(pyr0);
frame.getScaledDepth(depth);
}
else if (frame.getPyramidLevels(OdometryFramePyramidType::PYR_CLOUD) > 0)
{
@ -194,10 +197,12 @@ void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
std::vector<TMat> xyz;
split(cloud, xyz);
frame.setDepth(xyz[2]);
frame.getScaledDepth(depth);
}
else
CV_Error(Error::StsBadSize, "Depth or pyramidDepth or pyramidCloud have to be set.");
}
checkDepth(depth, depth.size());
TMat mask;
@ -207,6 +212,7 @@ void prepareICPFrameBase(OdometryFrame& frame, OdometrySettings settings)
TMat pyr0;
frame.getPyramidAt(pyr0, OdometryFramePyramidType::PYR_MASK, 0);
frame.setMask(pyr0);
frame.getMask(mask);
}
checkMask(mask, depth.size());
@ -268,6 +274,7 @@ void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings)
TMat n0;
frame.getPyramidAt(n0, OdometryFramePyramidType::PYR_NORM, 0);
frame.setNormals(n0);
frame.getNormals(normals);
}
else
{
@ -288,8 +295,8 @@ void prepareICPFrameDst(OdometryFrame& frame, OdometrySettings settings)
frame.getPyramidAt(c0, OdometryFramePyramidType::PYR_CLOUD, 0);
normalsComputer->apply(c0, normals);
frame.setNormals(normals);
frame.getNormals(normals);
}
}
std::vector<TMat> npyramids;

61
modules/3d/test/test_odometry.cpp
View File

@ -148,6 +148,7 @@ public:
void run();
void checkUMats();
void prepareFrameCheck();
OdometryType otype;
OdometryAlgoType algtype;
@ -348,6 +349,41 @@ void OdometryTest::run()
}
}
void OdometryTest::prepareFrameCheck()
{
Mat K = getCameraMatrix();
Mat image, depth;
readData(image, depth);
OdometrySettings ods;
ods.setCameraMatrix(K);
Odometry odometry = Odometry(otype, ods, algtype);
OdometryFrame odf = odometry.createOdometryFrame();
odf.setImage(image);
odf.setDepth(depth);
odometry.prepareFrame(odf);
Mat points, mask;
odf.getPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
odf.getPyramidAt(mask, OdometryFramePyramidType::PYR_MASK, 0);
OdometryFrame todf = odometry.createOdometryFrame();
if (otype != OdometryType::DEPTH)
{
Mat img;
odf.getPyramidAt(img, OdometryFramePyramidType::PYR_IMAGE, 0);
todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_IMAGE);
todf.setPyramidAt(img, OdometryFramePyramidType::PYR_IMAGE, 0);
}
todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_CLOUD);
todf.setPyramidAt(points, OdometryFramePyramidType::PYR_CLOUD, 0);
todf.setPyramidLevel(1, OdometryFramePyramidType::PYR_MASK);
todf.setPyramidAt(mask, OdometryFramePyramidType::PYR_MASK, 0);
odometry.prepareFrame(todf);
}
/****************************************************************************************\
* Tests registrations *
\****************************************************************************************/
@ -401,4 +437,29 @@ TEST(RGBD_Odometry_FastICP, UMats)
test.checkUMats();
}
TEST(RGBD_Odometry_Rgbd, prepareFrame)
{
OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.89);
test.prepareFrameCheck();
}
TEST(RGBD_Odometry_ICP, prepareFrame)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
test.prepareFrameCheck();
}
TEST(RGBD_Odometry_RgbdICP, prepareFrame)
{
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
test.prepareFrameCheck();
}
TEST(RGBD_Odometry_FastICP, prepareFrame)
{
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.89, FLT_EPSILON);
test.prepareFrameCheck();
}
}} // namespace