mirror of
https://github.com/opencv/opencv.git
synced 2024-12-05 09:49:12 +08:00
Merge pull request #21741 from DumDereDum:odometry_prepareFrame_fix
Odometry prepareFrame fix * fix issue; add tests * img fix * mask fix * minor fix
This commit is contained in:
parent
aa5055261f
commit
3d12581798
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user