warpFrame() rewritten, interface changed

modules/3d/include/opencv2/3d/depth.hpp

@@ -124,20 +124,20 @@ CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d
  */
 CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double depth_factor = 1000.0);
 
-/** Warp the image: compute 3d points from the depth, transform them using given transformation,
- * then project color point cloud to an image plane.
- * This function can be used to visualize results of the Odometry algorithm.
- * @param image The image (of CV_8UC1 or CV_8UC3 type)
- * @param depth The depth (of type used in depthTo3d fuction)
- * @param mask The mask of used pixels (of CV_8UC1), it can be empty
- * @param Rt The transformation that will be applied to the 3d points computed from the depth
- * @param cameraMatrix Camera matrix
- * @param warpedImage The warped image.
- * @param warpedDepth The warped depth.
- * @param warpedMask The warped mask.
+/** Warps depth or RGB-D image by reprojecting it in 3d, applying Rt transformation
+ * and then projecting it back onto the image plane.
+ * This function can be used to visualize the results of the Odometry algorithm.
+ * @param depth Depth data, should be 1-channel CV_16U, CV_16S, CV_32F or CV_64F
+ * @param image RGB image (optional), should be 1-, 3- or 4-channel CV_8U
+ * @param mask Mask of used pixels (optional), should be CV_8UC1
+ * @param Rt Rotation+translation matrix (3x4 or 4x4) to be applied to depth points
+ * @param cameraMatrix Camera intrinsics matrix (3x3)
+ * @param warpedDepth The warped depth data (optional)
+ * @param warpedImage The warped RGB image (optional)
+ * @param warpedMask The mask of valid pixels in warped image (optional)
  */
-CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix,
-                            OutputArray warpedImage, OutputArray warpedDepth = noArray(), OutputArray warpedMask = noArray());
+CV_EXPORTS_W void warpFrame(InputArray depth, InputArray image, InputArray mask, InputArray Rt, InputArray cameraMatrix,
+                            OutputArray warpedDepth = noArray(), OutputArray warpedImage = noArray(), OutputArray warpedMask = noArray());
 
 enum RgbdPlaneMethod
 {

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

@@ -362,92 +362,219 @@ 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,
-              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());
+void warpFrame(InputArray depth, InputArray image, InputArray mask,
+               InputArray Rt, InputArray cameraMatrix,
+               OutputArray warpedDepth, OutputArray warpedImage, OutputArray warpedMask)
+{
+    CV_Assert(cameraMatrix.size() == Size(3, 3));
+    CV_Assert(cameraMatrix.depth() == CV_32F || cameraMatrix.depth() == CV_64F);
+    Matx33d K, Kinv;
+    cameraMatrix.getMat().convertTo(K, CV_64F);
+    std::vector<bool> camPlaces { /* fx */ true, false, /* cx */ true, false, /* fy */ true, /* cy */ true,  false, false, /* 1 */ true};
+    for (int i = 0; i < 9; i++)
+    {
+        CV_Assert(camPlaces[i] == (K.val[i] > DBL_EPSILON));
+    }
+    Kinv = K.inv();
+
+    CV_Assert((Rt.cols() == 4) && (Rt.rows() == 3 || Rt.rows() == 4));
+    CV_Assert(Rt.depth() == CV_32F || Rt.depth() == CV_64F);
+    Mat rtmat;
+    Rt.getMat().convertTo(rtmat, CV_64F);
+    Affine3d rt(rtmat);
+
+    CV_Assert(!depth.empty());
+    CV_Assert(depth.channels() == 1);
+    double maxDepth = 0;
+    int depthDepth = depth.depth();
+    switch (depthDepth)
+    {
+    case CV_16U:
+        maxDepth = std::numeric_limits<unsigned short>::max();
+        break;
+    case CV_32F:
+        maxDepth = std::numeric_limits<float>::max();
+        break;
+    case CV_64F:
+        maxDepth = std::numeric_limits<double>::max();
+        break;
+    default:
+        CV_Error(Error::StsBadArg, "Unsupported depth data type");
+    }
+    Mat_<double> depthDbl;
+    depth.getMat().convertTo(depthDbl, CV_64F);
+    Size sz = depth.size();
 
-    Mat cloud;
-    depthTo3d(depth, cameraMatrix, cloud);
+    Mat_<uchar> maskMat;
+    if (!mask.empty())
+    {
+        CV_Assert(mask.type() == CV_8UC1);
+        CV_Assert(mask.size() == sz);
+        maskMat = mask.getMat();
+    }
 
-    //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++)
+    int imageType = -1;
+    Mat imageMat;
+    if (!image.empty())
     {
-        for (int x = 0; x < cloud3.cols; x++)
+        imageType = image.type();
+        CV_Assert(imageType == CV_8UC1 || imageType == CV_8UC3 || imageType == CV_8UC4);
+        CV_Assert(image.size() == sz);
+        CV_Assert(warpedImage.needed());
+        imageMat = image.getMat();
+    }
+
+    CV_Assert(warpedDepth.needed() || warpedImage.needed() || warpedMask.needed());
+
+    // Getting new coords for depth point
+
+    // see the explanation in the loop below
+    Matx33d krki = K * rt.rotation() * Kinv;
+    Matx32d krki_cols01 = krki.get_minor<3, 2>(0, 0);
+    Vec3d krki_col2(krki.col(2).val);
+
+    Vec3d ktmat = K * rt.translation();
+    Mat_<Vec3d> reprojBack(depth.size());
+    for (int y = 0; y < sz.height; y++)
+    {
+        const uchar* maskRow = maskMat.empty() ? nullptr : maskMat[y];
+        const double* depthRow = depthDbl[y];
+        Vec3d* reprojRow = reprojBack[y];
+        for (int x = 0; x < sz.width; x++)
         {
-            Vec4f p = cloud.at<Vec4f>(y, x);
-            cloud3.at<Vec3f>(y, x) = Vec3f(p[0], p[1], p[2]);
+            double z = depthRow[x];
+            bool badz = cvIsNaN(z) || cvIsInf(z) || z <= 0 || z >= maxDepth || (maskRow && !maskRow[x]);
+            Vec3d v;
+            if (!badz)
+            {
+                // Reproject pixel (x, y) using known z, rotate+translate and project back
+                // getting new pixel in projective coordinates:
+                // v = K * Rt * K^-1 * ([x, y, 1] * z) = [new_x*new_z, new_y*new_z, new_z]
+                // v = K * (R * K^-1 * ([x, y, 1] * z) + t) =
+                // v = krki * [x, y, 1] * z + ktmat =
+                // v = (krki_cols01 * [x, y] + krki_col2) * z + K * t
+                v = (krki_cols01 * Vec2d(x, y) + krki_col2) * z + ktmat;
+            }
+            else
+            {
+                v = Vec3d();
+            }
+            reprojRow[x] = v;
         }
     }
 
-    //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,
-                  Mat::zeros(1, 5, CV_64FC1), points2d);
-
-    Mat image = _image.getMat();
-    Size sz = _image.size();
-    Mat mask = _mask.getMat();
-    _warpedImage.create(sz, image.type());
-    Mat warpedImage = _warpedImage.getMat();
-
-    Mat zBuffer(sz, CV_32FC1, std::numeric_limits<float>::max());
+    // Draw new depth in z-buffer manner
+
+    Mat warpedImageMat;
+    if (warpedImage.needed())
+    {
+        warpedImage.create(sz, imageType);
+        warpedImage.setZero();
+        warpedImageMat = warpedImage.getMat();
+    }
+
+    const double infinity = std::numeric_limits<double>::max();
+
+    Mat zBuffer(sz, CV_32FC1, infinity);
+    
     const Rect rect = Rect(Point(), sz);
 
     for (int y = 0; y < sz.height; y++)
     {
-        //const Point3f* cloud_row = cloud.ptr<Point3f>(y);
-        const Point3f* transformedCloud_row = transformedCloud.ptr<Point3f>(y);
-        const Point2f* points2d_row = &points2d[y * sz.width];
-        const ImageElemType* image_row = image.ptr<ImageElemType>(y);
-        const uchar* mask_row = mask.empty() ? 0 : mask.ptr<uchar>(y);
+        uchar* imageRow1ch = nullptr;
+        Vec3b* imageRow3ch = nullptr;
+        Vec4b* imageRow4ch = nullptr;
+        switch (imageType)
+        {
+        case -1:
+            break;
+        case CV_8UC1:
+            imageRow1ch = imageMat.ptr<uchar>(y);
+            break;
+        case CV_8UC3:
+            imageRow3ch = imageMat.ptr<Vec3b>(y);
+            break;
+        case CV_8UC4:
+            imageRow4ch = imageMat.ptr<Vec4b>(y);
+            break;
+        default:
+            break;
+        }
+
+        const Vec3d* reprojRow = reprojBack[y];
         for (int x = 0; x < sz.width; x++)
         {
-            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)
+            Vec3d v = reprojRow[x];
+            double z = v[2];
+
+            if (z > 0)
             {
-                warpedImage.at<ImageElemType>(p2d) = image_row[x];
-                zBuffer.at<float>(p2d) = transformed_z;
+                Point uv(cvFloor(v[0] / z), cvFloor(v[1] / z));
+                if (rect.contains(uv))
+                {
+                    float oldz = zBuffer.at<float>(uv);
+
+                    if (z < oldz)
+                    {
+                        zBuffer.at<float>(uv) = z;
+
+                        switch (imageType)
+                        {
+                        case -1:
+                            break;
+                        case CV_8UC1:
+                            warpedImageMat.at<uchar>(uv) = imageRow1ch[x];
+                            break;
+                        case CV_8UC3:
+                            warpedImageMat.at<Vec3b>(uv) = imageRow3ch[x];
+                            break;
+                        case CV_8UC4:
+                            warpedImageMat.at<Vec4b>(uv) = imageRow4ch[x];
+                            break;
+                        default:
+                            break;
+                        }
+                    }
+                }
             }
         }
     }
 
-    if (warpedMask.needed())
-        Mat(zBuffer != std::numeric_limits<float>::max()).copyTo(warpedMask);
-
-    if (warpedDepth.needed())
+    if (warpedDepth.needed() || warpedMask.needed())
     {
-        zBuffer.setTo(std::numeric_limits<float>::quiet_NaN(), zBuffer == std::numeric_limits<float>::max());
-        zBuffer.copyTo(warpedDepth);
-    }
-}
+        Mat goodMask = (zBuffer < infinity);
 
+        if (warpedDepth.needed())
+        {
+            warpedDepth.create(sz, depthDepth);
 
-void warpFrame(InputArray image, InputArray depth, InputArray mask,
-               InputArray Rt, InputArray cameraMatrix,
-               OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask)
-{
-    if (image.type() == CV_8UC1)
-        warpFrameImpl<uchar>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(),
-                             warpedImage, warpedDepth, warpedMask);
-    else if (image.type() == CV_8UC3)
-        warpFrameImpl<Point3_<uchar>>(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(),
-                                      warpedImage, warpedDepth, warpedMask);
-    else
-        CV_Error(Error::StsBadArg, "Image has to be type of CV_8UC1 or CV_8UC3");
+            double badVal;
+            switch (depthDepth)
+            {
+            case CV_16U:
+                badVal = 0;
+                break;
+            case CV_32F:
+                badVal = std::numeric_limits<float>::quiet_NaN();
+                break;
+            case CV_64F:
+                badVal = std::numeric_limits<double>::quiet_NaN();
+                break;
+            default:
+                break;
+            }
+
+            zBuffer.convertTo(warpedDepth, depthDepth);
+            warpedDepth.setTo(badVal, ~goodMask);
+        }
+
+        if (warpedMask.needed())
+        {
+            warpedMask.create(sz, CV_8UC1);
+            goodMask.copyTo(warpedMask);
+        }
+    }
 }
 
 }