diff --git a/modules/3d/misc/python/warp_test.py b/modules/3d/misc/python/warp_test.py new file mode 100644 index 0000000000..40fc844506 --- /dev/null +++ b/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)) + + diff --git a/modules/3d/src/rgbd/odometry.cpp b/modules/3d/src/rgbd/odometry.cpp index 7afe1eca5a..ac0b184c92 100644 --- a/modules/3d/src/rgbd/odometry.cpp +++ b/modules/3d/src/rgbd/odometry.cpp @@ -381,14 +381,18 @@ bool Odometry::compute(InputArray srcDepthFrame, InputArray srcRGBFrame, template 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 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(p2d) > transformed_z) + if ((!mask_row || mask_row[x]) && transformed_z > 0 && + rect.contains(p2d) && /*!cvIsNaN(cloud_row[x].z) && */zBuffer.at(p2d) > transformed_z) { warpedImage.at(p2d) = image_row[x]; zBuffer.at(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(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask); + warpFrameImpl(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), + warpedImage, warpedDepth, warpedMask); else if (image.type() == CV_8UC3) - warpFrameImpl >(image, depth, mask, Rt.getMat(), cameraMatrix.getMat(), distCoeff.getMat(), warpedImage, warpedDepth, warpedMask); + warpFrameImpl>(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"); } diff --git a/modules/3d/test/test_odometry.cpp b/modules/3d/test/test_odometry.cpp index d58b1d596b..d8f81fa13d 100644 --- a/modules/3d/test/test_odometry.cpp +++ b/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::quiet_NaN(), srcDepth < FLT_EPSILON); + warpedDepth.setTo(std::numeric_limits::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