warpFrame() test draft + script generating test data

modules/3d/misc/python/warp_test.py

@@ -0,0 +1,113 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+import imageio
+# optional, works slower w/o it
+from numba import jit
+
+depthFactor = 5000
+psize = (640, 480)
+fx = 525.0
+fy = 525.0
+cx = psize[0]/2-0.5
+cy = psize[1]/2-0.5
+K = np.array([[fx,  0, cx],
+              [ 0, fy, cy],
+              [ 0,  0,  1]])
+# some random transform
+rmat = Rotation.from_rotvec(np.array([0.1, 0.2, 0.3])).as_dcm()
+tmat = np.array([[-0.04, 0.05, 0.6]]).T
+rtmat = np.vstack((np.hstack((rmat, tmat)), np.array([[0, 0, 0, 1]])))
+
+#TODO: warp rgb image as well
+testDataPath = "/path/to/sources/opencv_extra/testdata"
+srcDepth = imageio.imread(testDataPath + "/cv/rgbd/depth.png")
+
+@jit
+def reproject(image, df, K):
+    Kinv = np.linalg.inv(K)
+    xsz, ysz = image.shape[1], image.shape[0]
+    reprojected = np.zeros((ysz, xsz, 3))
+    for y in range(ysz):
+        for x in range(xsz):
+            z = image[y, x]/df
+
+            v = Kinv @ np.array([x*z, y*z, z]).T
+
+            #xv = (x - cx)/fx * z
+            #yv = (y - cy)/fy * z
+            #zv = z
+
+            reprojected[y, x, :] = v[:]
+    return reprojected
+
+@jit
+def reprojectRtProject(image, K, depthFactor, rmat, tmat):
+    Kinv = np.linalg.inv(K)
+    xsz, ysz = image.shape[1], image.shape[0]
+    projected = np.zeros((ysz, xsz, 3))
+    for y in range(ysz):
+        for x in range(xsz):
+            z = image[y, x]/depthFactor
+
+            v = K @ (rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0])
+
+            projected[y, x, :] = v[:]
+    
+    return projected
+
+@jit
+def reprojectRt(image, K, depthFactor, rmat, tmat):
+    Kinv = np.linalg.inv(K)
+    xsz, ysz = image.shape[1], image.shape[0]
+    rotated = np.zeros((ysz, xsz, 3))
+    for y in range(ysz):
+        for x in range(xsz):
+            z = image[y, x]/depthFactor
+
+            v = rmat @ Kinv @ np.array([x*z, y*z, z]).T + tmat[:, 0]
+
+            rotated[y, x, :] = v[:]
+    
+    return rotated
+
+# put projected points on a depth map
+@jit
+def splat(projected, maxv):
+    xsz, ysz = projected.shape[1], projected.shape[0]
+    depth = np.full((ysz, xsz), maxv, np.float32)
+    for y in range(ysz):
+        for x in range(xsz):
+            p = projected[y, x, :]
+            z = p[2]
+            if z > 0:
+                u, v = int(p[0]/z), int(p[1]/z)
+                okuv = (u >= 0 and v >= 0 and u < xsz and v < ysz)
+                if okuv and depth[v, u] > z:
+                    depth[v, u] = z
+    return depth
+
+maxv = depthFactor
+im2 = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv)
+im2[im2 >= maxv] = 0
+im2 = im2*depthFactor
+
+imageio.imwrite(testDataPath + "/cv/rgbd/warped.png", im2)
+
+# debug
+
+outObj = False
+def outFile(path, ptsimg):
+    f = open(path, "w")
+    for y in range(ptsimg.shape[0]):
+        for x in range(ptsimg.shape[1]):
+            v = ptsimg[y, x, :]
+            if v[2] > 0:
+                f.write(f"v {v[0]} {v[1]} {v[2]}\n")
+    f.close()
+
+if outObj:
+    objdir = "/path/to/objdir/"
+    outFile(objdir + "reproj_rot_proj.obj", reproject(im2, depthFactor, K))
+    outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat))
+
+

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

@@ -381,14 +381,18 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame,
 template<class ImageElemType>
 static void
 warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
-    const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
-    OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
+              const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
+              OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
 {
+    //TODO: take into account that image can be empty
+    //TODO: mask can also be empty
+
     CV_Assert(_image.size() == depth.size());
 
     Mat cloud;
     depthTo3d(depth, cameraMatrix, cloud);
 
+    //TODO: replace this by channel split/merge after the function is covered by tests
     Mat cloud3;
     cloud3.create(cloud.size(), CV_32FC3);
     for (int y = 0; y < cloud3.rows; y++)
@@ -400,11 +404,13 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
         }
     }
 
+    //TODO: do the following instead of the functions: K*R*Kinv*[uv1]*z + K*t
+    //TODO: optimize it using K's structure
     std::vector<Point2f> points2d;
     Mat transformedCloud;
     perspectiveTransform(cloud3, transformedCloud, Rt);
     projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix,
-        distCoeff, points2d);
+                  distCoeff, points2d);
 
     Mat image = _image.getMat();
     Size sz = _image.size();
@@ -426,7 +432,8 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
         {
             const float transformed_z = transformedCloud_row[x].z;
             const Point2i p2d = points2d_row[x];
-            if ((!mask_row || mask_row[x]) && transformed_z > 0 && rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at<float>(p2d) > transformed_z)
+            if ((!mask_row || mask_row[x]) && transformed_z > 0 &&
+                rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at<float>(p2d) > transformed_z)
             {
                 warpedImage.at<ImageElemType>(p2d) = image_row[x];
                 zBuffer.at<float>(p2d) = transformed_z;
@@ -444,14 +451,17 @@ warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask,
     }
 }
 
+
 void warpFrame(InputArray image, InputArray depth, InputArray mask,
-    InputArray Rt, InputArray cameraMatrix, InputArray distCoeff,
-    OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
+               InputArray Rt, InputArray cameraMatrix, InputArray distCoeff,
+               OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
 {
     if (image.type() == CV_8UC1)
-        warpFrameImpl<uchar>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask);
+        warpFrameImpl<uchar>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(),
+                             warpedImage, warpedDepth, warpedMask);
     else if (image.type() == CV_8UC3)
-        warpFrameImpl<Point3_<uchar> >(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask);
+        warpFrameImpl<Point3_<uchar>>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(),
+                                      warpedImage, warpedDepth, warpedMask);
     else
         CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
 }

modules/3d/test/test_odometry.cpp

@@ -491,4 +491,61 @@ TEST(RGBD_Odometry_FastICP, prepareFrame)
     test.prepareFrameCheck();
 }
 
+TEST(RGBD_Odometry_WarpFrame, compareToGold)
+{
+    //TODO: identity transform
+    //TODO: finish it
+
+    std::string dataPath = cvtest::TS::ptr()->get_data_path();
+    std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png";
+    // The depth was generated using the script at 3d/misc/python/warp_test.py
+    std::string warpedDepthFilename = dataPath + "/cv/rgbd/warped.png";
+
+    Mat srcDepth, warpedDepth;
+
+    srcDepth = imread(srcDepthFilename, -1);
+    if(srcDepth.empty())
+    {
+        FAIL() << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl;
+    }
+
+    warpedDepth = imread(warpedDepthFilename, -1);
+    if(warpedDepth.empty())
+    {
+        FAIL() << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl;
+    }
+
+    CV_DbgAssert(srcDepth.type() == CV_16UC1);
+    CV_DbgAssert(warpedDepth.type() == CV_16UC1);
+
+    double fx = 525.0, fy = 525.0,
+           cx = 319.5, cy = 239.5;
+    Matx33d K(fx,  0, cx,
+               0, fy, cy,
+               0,  0,  1);
+    cv::Affine3d rt(cv::Vec3d(0.1, 0.2, 0.3), cv::Vec3d(-0.04, 0.05, 0.6));
+
+    //TODO: check with and without scaling
+    Mat srcDepthCvt, warpedDepthCvt;
+    srcDepth.convertTo(srcDepthCvt, CV_32FC1, 1.f/5000.f);
+    srcDepth = srcDepthCvt;
+    warpedDepth.convertTo(warpedDepthCvt, CV_32FC1, 1.f/5000.f);
+    warpedDepth = warpedDepthCvt;
+
+    srcDepth.setTo(std::numeric_limits<float>::quiet_NaN(), srcDepth < FLT_EPSILON);
+    warpedDepth.setTo(std::numeric_limits<float>::quiet_NaN(), warpedDepth < FLT_EPSILON);
+
+    //TODO: check with and without image
+    //TODO: check with and without mask
+    //TODO: check with and without distCoeff
+    Mat image, mask, distCoeff, dstImage, dstDepth, dstMask;
+    warpFrame(image, srcDepth, mask, rt.matrix, K, distCoeff,
+              dstImage, dstDepth, dstMask);
+
+    //TODO: check this norm
+    double depthDiff = cv::norm(dstDepth, warpedDepth, NORM_L2);
+    //TODO: find true threshold, maybe based on pixcount
+    ASSERT_LE(0.1, depthDiff);
+}
+
 }} // namespace