Merge pull request #21741 from DumDereDum:odometry_prepareFrame_fix

Odometry prepareFrame fix

* fix issue; add tests

* img fix

* mask fix

* minor fix

modules/3d/src/rgbd/odometry_functions.cpp

@@ -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;

modules/3d/test/test_odometry.cpp

@@ -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