fixed odometry tests (changes ported from PR #1627)

modules/rgbd/src/normal.cpp

@@ -283,10 +283,13 @@ namespace rgbd
       Vec3T *row_B = B[0];
       for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V)
       {
-        if (cvIsNaN(*row_r))
+        Vec3T val = (*row_V) / (*row_r);
+        if(cvIsInf(val[0]) || cvIsNaN(val[0]) ||
+           cvIsInf(val[1]) || cvIsNaN(val[1]) ||
+           cvIsInf(val[2]) || cvIsNaN(val[2]))
             *row_B = Vec3T();
         else
-          *row_B = (*row_V) / (*row_r);
+            *row_B = val;
       }
 
       // Apply a box filter to B

modules/rgbd/test/test_odometry.cpp

@@ -150,10 +150,13 @@ class CV_OdometryTest : public cvtest::BaseTest
 public:
     CV_OdometryTest(const Ptr<Odometry>& _odometry,
                     double _maxError1,
-                    double _maxError5) :
+                    double _maxError5,
+                    double _idError = DBL_EPSILON) :
         odometry(_odometry),
         maxError1(_maxError1),
-        maxError5(_maxError5) {}
+        maxError5(_maxError5),
+        idError(_idError)
+    { }
 
 protected:
     bool readData(Mat& image, Mat& depth) const;
@@ -164,6 +167,7 @@ protected:
     Ptr<Odometry> odometry;
     double maxError1;
     double maxError5;
+    double idError;
 };
 
 bool CV_OdometryTest::readData(Mat& image, Mat& depth) const
@@ -248,7 +252,7 @@ void CV_OdometryTest::run(int)
         ts->set_failed_test_info(cvtest::TS::FAIL_INVALID_OUTPUT);
     }
     double diff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
-    if(diff > DBL_EPSILON)
+    if(diff > idError)
     {
         ts->printf(cvtest::TS::LOG, "Incorrect transformation between the same frame (not the identity matrix), diff = %f", diff);
         ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
@@ -258,8 +262,8 @@ void CV_OdometryTest::run(int)
     // On each iteration an input frame is warped using generated transformation.
     // Odometry is run on the following pair: the original frame and the warped one.
     // Comparing a computed transformation with an applied one we compute 2 errors:
-    // better_1time_count - count of poses which error is less than ground thrush pose,
-    // better_5times_count - count of poses which error is 5 times less than ground thrush pose.
+    // better_1time_count - count of poses which error is less than ground truth pose,
+    // better_5times_count - count of poses which error is 5 times less than ground truth pose.
     int iterCount = 100;
     int better_1time_count = 0;
     int better_5times_count = 0;
@@ -271,7 +275,8 @@ void CV_OdometryTest::run(int)
         warpFrame(image, depth, rvec, tvec, K, warpedImage, warpedDepth);
         dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
         
-        isComputed = odometry->compute(image, depth, Mat(), warpedImage, warpedDepth, Mat(), calcRt);
+        Mat imageMask(image.size(), CV_8UC1, Scalar(255));
+        isComputed = odometry->compute(image, depth, imageMask, warpedImage, warpedDepth, imageMask, calcRt);
         if(!isComputed)
             continue;
                 
@@ -329,17 +334,17 @@ void CV_OdometryTest::run(int)
 
 TEST(RGBD_Odometry_Rgbd, algorithmic)
 {
-    CV_OdometryTest test(cv::rgbd::Odometry::create("RgbdOdometry"), 0.99, 0.94);
+    CV_OdometryTest test(cv::rgbd::Odometry::create("RgbdOdometry"), 0.99, 0.89);
     test.safe_run();
 }
 
-TEST(DISABLED_RGBD_Odometry_ICP, algorithmic)
+TEST(RGBD_Odometry_ICP, algorithmic)
 {
     CV_OdometryTest test(cv::rgbd::Odometry::create("ICPOdometry"), 0.99, 0.99);
     test.safe_run();
 }
 
-TEST(DISABLED_RGBD_Odometry_RgbdICP, algorithmic)
+TEST(RGBD_Odometry_RgbdICP, algorithmic)
 {
     CV_OdometryTest test(cv::rgbd::Odometry::create("RgbdICPOdometry"), 0.99, 0.99);
     test.safe_run();