Merge pull request #27821 from savuor:rv/fix_nan_depth_scale

Fixed wrong NaN handling when scaling depth for OdometryFrame #27821

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake

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

@@ -47,14 +47,26 @@ static UMat prepareScaledDepth(OdometryFrame& frame)
     UMat depth;
     frame.getDepth(depth);
     CV_Assert(!depth.empty());
+    bool isFloat = (depth.type() == CV_32FC1 || depth.type() == CV_64FC1);
 
     // Odometry works well with depth values in range [0, 10)
     // If it's bigger, let's scale it down by 5000, a typical depth factor
-    double maxv;
-    cv::minMaxLoc(depth, nullptr, &maxv);
     UMat depthFlt;
-    depth.convertTo(depthFlt, CV_32FC1, maxv > 10 ? (1.f / 5000.f) : 1.f);
-    patchNaNs(depthFlt, 0);
+    if (isFloat)
+    {
+        depth.convertTo(depthFlt, CV_32FC1);
+        patchNaNs(depthFlt, 0);
+    }
+
+    double maxv;
+    cv::minMaxLoc(isFloat ? depthFlt : depth, nullptr, &maxv);
+
+    if (!isFloat || maxv > 10)
+    {
+        depth.convertTo(depthFlt, CV_32FC1, maxv > 10 ? (1.f / 5000.f) : 1.f);
+        patchNaNs(depthFlt, 0);
+    }
+
     frame.impl->scaledDepth = depthFlt;
 
     return depthFlt;

modules/3d/test/test_odometry.cpp

@@ -79,6 +79,7 @@ public:
     void run();
     void checkUMats();
     void prepareFrameCheck();
+    void processedDepthCheck();
 
     OdometryType otype;
     OdometryAlgoType algtype;
@@ -428,6 +429,32 @@ void OdometryTest::prepareFrameCheck()
     }
 }
 
+
+void OdometryTest::processedDepthCheck()
+{
+    Mat K = getCameraMatrix();
+
+    Mat gtImage, gtDepth;
+    readData(gtImage, gtDepth);
+
+    gtDepth *= 5000.0;
+
+    OdometrySettings ods;
+    ods.setCameraMatrix(K);
+    Odometry odometry = Odometry(otype, ods, algtype);
+    OdometryFrame odf(gtDepth, gtImage);
+
+    odometry.prepareFrame(odf);
+
+    Mat scaled;
+    odf.getProcessedDepth(scaled);
+
+    //TODO: remove this check when depth rescaling is removed
+    double pmax;
+    cv::minMaxLoc(scaled, nullptr, &pmax);
+    EXPECT_LT(pmax, 10.0);
+}
+
 /****************************************************************************************\
 *                                Tests registrations                                     *
 \****************************************************************************************/
@@ -508,6 +535,31 @@ TEST(RGBD_Odometry_FastICP, prepareFrame)
 }
 
 
+TEST(RGBD_Odometry_Rgb, processedDepth)
+{
+    OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_ICP, processedDepth)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_RgbdICP, processedDepth)
+{
+    OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
+    test.processedDepthCheck();
+}
+
+TEST(RGBD_Odometry_FastICP, processedDepth)
+{
+    OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON);
+    test.processedDepthCheck();
+}
+
+
 struct WarpFrameTest
 {
     WarpFrameTest() :