From 4d57438cb6894b43f6bc97a7598fbc91fff90f74 Mon Sep 17 00:00:00 2001
From: Ayush Garg <ayushgargdroid@gmail.com>
Date: Tue, 17 Mar 2020 15:12:22 -0400
Subject: [PATCH] Merge pull request #2460 from
 ayushgargdroid:fixDepthRegisteration

* Fix for Depth Registration. Issue #2234

* Fix style

* Incorporating suggestions. 1. Trailing whitespace 2. Matx33f 3. EXPECT_EQ

* test: use Matx33f ctor
---
 modules/rgbd/src/depth_registration.cpp |  7 +++----
 modules/rgbd/test/test_registration.cpp | 21 +++++++++++++++++++++
 2 files changed, 24 insertions(+), 4 deletions(-)

diff --git a/modules/rgbd/src/depth_registration.cpp b/modules/rgbd/src/depth_registration.cpp
index f6bcf7d74..4cf13e3c8 100644
--- a/modules/rgbd/src/depth_registration.cpp
+++ b/modules/rgbd/src/depth_registration.cpp
@@ -185,14 +185,14 @@ namespace rgbd
         // Apply the initial projection to the input depth
         Mat_<Point3f> transformedCloud;
         {
-            Mat_<Point3f> point_tmp(outputImagePlaneSize);
+            Mat_<Point3f> point_tmp(outputImagePlaneSize,Point3f(0.,0.,0.));
 
-            for(int j = 0; j < point_tmp.rows; ++j)
+            for(int j = 0; j < unregisteredDepth.rows; ++j)
             {
                 const DepthDepth *unregisteredDepthPtr = unregisteredDepth[j];
 
                 Point3f *point = point_tmp[j];
-                for(int i = 0; i < point_tmp.cols; ++i, ++unregisteredDepthPtr, ++point)
+                for(int i = 0; i < unregisteredDepth.cols; ++i, ++unregisteredDepthPtr, ++point)
                 {
                     float rescaled_depth = float(*unregisteredDepthPtr) * inputDepthToMetersScale;
 
@@ -309,7 +309,6 @@ namespace rgbd
 
             } // iterate cols
         } // iterate rows
-
     }
 
 
diff --git a/modules/rgbd/test/test_registration.cpp b/modules/rgbd/test/test_registration.cpp
index f88998367..5b35a444d 100644
--- a/modules/rgbd/test/test_registration.cpp
+++ b/modules/rgbd/test/test_registration.cpp
@@ -151,5 +151,26 @@ TEST(Rgbd_DepthRegistration, compute)
   test.safe_run();
 }
 
+TEST(Rgbd_DepthRegistration, issue_2234)
+{
+    Matx33f intrinsicsDepth(100, 0,  50, 0, 100, 50, 0, 0, 1);
+    Matx33f intrinsicsColor(100, 0, 200, 0, 100, 50, 0, 0, 1);
+
+    Mat_<float> depthMat(100, 100, (float)0.);
+    for(int i = 1; i <= 100; i++)
+    {
+        for(int j = 1; j <= 100; j++)
+            depthMat(i-1,j-1) = (float)j;
+    }
+
+    Mat registeredDepth;
+    registerDepth(intrinsicsDepth, intrinsicsColor, Mat(), Matx44f::eye(), depthMat, Size(400, 100), registeredDepth);
+
+    Rect roi( 150, 0, 100, 100 );
+    Mat subM(registeredDepth,roi);
+
+    EXPECT_EQ(0, cvtest::norm(subM, depthMat, NORM_INF));
+}
+
 
 }} // namespace