diff --git a/modules/3d/include/opencv2/3d/depth.hpp b/modules/3d/include/opencv2/3d/depth.hpp index c0150384d4..0279a26f03 100644 --- a/modules/3d/include/opencv2/3d/depth.hpp +++ b/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 { diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index a8ae22721b..ba9fcd57c5 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -362,92 +362,219 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, } -template -static void -warpFrameImpl(InputArray _image, InputArray depth, InputArray _mask, - const Mat& Rt, const Mat& cameraMatrix, - OutputArray _warpedImage, OutputArray warpedDepth, OutputArray warpedMask) + +void warpFrame(InputArray depth, InputArray image, InputArray mask, + InputArray Rt, InputArray cameraMatrix, + OutputArray warpedDepth, OutputArray warpedImage, 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++) + 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 camPlaces { /* fx */ true, false, /* cx */ true, false, /* fy */ true, /* cy */ true, false, false, /* 1 */ true}; + for (int i = 0; i < 9; i++) { - for (int x = 0; x < cloud3.cols; x++) + 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::max(); + break; + case CV_32F: + maxDepth = std::numeric_limits::max(); + break; + case CV_64F: + maxDepth = std::numeric_limits::max(); + break; + default: + CV_Error(Error::StsBadArg, "Unsupported depth data type"); + } + Mat_ depthDbl; + depth.getMat().convertTo(depthDbl, CV_64F); + Size sz = depth.size(); + + Mat_ maskMat; + if (!mask.empty()) + { + CV_Assert(mask.type() == CV_8UC1); + CV_Assert(mask.size() == sz); + maskMat = mask.getMat(); + } + + int imageType = -1; + Mat imageMat; + if (!image.empty()) + { + 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_ 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(y, x); - cloud3.at(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 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); + // Draw new depth in z-buffer manner - Mat image = _image.getMat(); - Size sz = _image.size(); - Mat mask = _mask.getMat(); - _warpedImage.create(sz, image.type()); - Mat warpedImage = _warpedImage.getMat(); + Mat warpedImageMat; + if (warpedImage.needed()) + { + warpedImage.create(sz, imageType); + warpedImage.setZero(); + warpedImageMat = warpedImage.getMat(); + } - Mat zBuffer(sz, CV_32FC1, std::numeric_limits::max()); + const double infinity = std::numeric_limits::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(y); - const Point3f* transformedCloud_row = transformedCloud.ptr(y); - const Point2f* points2d_row = &points2d[y * sz.width]; - const ImageElemType* image_row = image.ptr(y); - const uchar* mask_row = mask.empty() ? 0 : mask.ptr(y); + uchar* imageRow1ch = nullptr; + Vec3b* imageRow3ch = nullptr; + Vec4b* imageRow4ch = nullptr; + switch (imageType) + { + case -1: + break; + case CV_8UC1: + imageRow1ch = imageMat.ptr(y); + break; + case CV_8UC3: + imageRow3ch = imageMat.ptr(y); + break; + case CV_8UC4: + imageRow4ch = imageMat.ptr(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(p2d) > transformed_z) + Vec3d v = reprojRow[x]; + double z = v[2]; + + if (z > 0) { - warpedImage.at(p2d) = image_row[x]; - zBuffer.at(p2d) = transformed_z; + Point uv(cvFloor(v[0] / z), cvFloor(v[1] / z)); + if (rect.contains(uv)) + { + float oldz = zBuffer.at(uv); + + if (z < oldz) + { + zBuffer.at(uv) = z; + + switch (imageType) + { + case -1: + break; + case CV_8UC1: + warpedImageMat.at(uv) = imageRow1ch[x]; + break; + case CV_8UC3: + warpedImageMat.at(uv) = imageRow3ch[x]; + break; + case CV_8UC4: + warpedImageMat.at(uv) = imageRow4ch[x]; + break; + default: + break; + } + } + } } } } - if (warpedMask.needed()) - Mat(zBuffer != std::numeric_limits::max()).copyTo(warpedMask); - - if (warpedDepth.needed()) + if (warpedDepth.needed() || warpedMask.needed()) { - zBuffer.setTo(std::numeric_limits::quiet_NaN(), zBuffer == std::numeric_limits::max()); - zBuffer.copyTo(warpedDepth); + Mat goodMask = (zBuffer < infinity); + + if (warpedDepth.needed()) + { + warpedDepth.create(sz, depthDepth); + + double badVal; + switch (depthDepth) + { + case CV_16U: + badVal = 0; + break; + case CV_32F: + badVal = std::numeric_limits::quiet_NaN(); + break; + case CV_64F: + badVal = std::numeric_limits::quiet_NaN(); + break; + default: + break; + } + + zBuffer.convertTo(warpedDepth, depthDepth); + warpedDepth.setTo(badVal, ~goodMask); + } + + if (warpedMask.needed()) + { + warpedMask.create(sz, CV_8UC1); + goodMask.copyTo(warpedMask); + } } } - -void warpFrame(InputArray image, InputArray depth, InputArray mask, - InputArray Rt, InputArray cameraMatrix, - OutputArray warpedImage, OutputArray warpedDepth, OutputArray warpedMask) -{ - if (image.type() == CV_8UC1) - warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), - warpedImage, warpedDepth, warpedMask); - else if (image.type() == CV_8UC3) - warpFrameImpl>(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"); -} - }