diff --git a/testdata/cv/rgbd/warpedDepth.png b/testdata/cv/rgbd/warpedDepth.png new file mode 100644 index 000000000..cfc7de534 Binary files /dev/null and b/testdata/cv/rgbd/warpedDepth.png differ diff --git a/testdata/cv/rgbd/warpedRgb.png b/testdata/cv/rgbd/warpedRgb.png new file mode 100644 index 000000000..eb3e807f7 Binary files /dev/null and b/testdata/cv/rgbd/warpedRgb.png differ diff --git a/testdata/cv/rgbd/warped_depth_generator/requirements.txt b/testdata/cv/rgbd/warped_depth_generator/requirements.txt new file mode 100644 index 000000000..a4567695c --- /dev/null +++ b/testdata/cv/rgbd/warped_depth_generator/requirements.txt @@ -0,0 +1,3 @@ +numpy>=1.17.4 +scipy>=1.3.3 +PIL>=7.0.0 diff --git a/testdata/cv/rgbd/warped_depth_generator/warp_test.py b/testdata/cv/rgbd/warped_depth_generator/warp_test.py new file mode 100644 index 000000000..b8dda4fd5 --- /dev/null +++ b/testdata/cv/rgbd/warped_depth_generator/warp_test.py @@ -0,0 +1,111 @@ +import numpy as np +from scipy.spatial.transform import Rotation +from PIL import Image +import os + +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]]))) + +testDataPath = os.getenv("OPENCV_TEST_DATA_PATH", default=None) +srcDepth = np.asarray(Image.open(testDataPath + "/cv/rgbd/depth.png")) +srcRgb = np.asarray(Image.open(testDataPath + "/cv/rgbd/rgb.png")) + +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 + +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]) + + if z > 0: + projected[y, x, :] = v[:] + + return projected + +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 +def splat(projected, maxv, rgb): + xsz, ysz = projected.shape[1], projected.shape[0] + depth = np.full((ysz, xsz), maxv, np.float32) + colors = np.full((ysz, xsz, 3), 0, np.uint8) + 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 + colors[v, u, :] = rgb[y, x, :] + return depth, colors + +maxv = depthFactor +dstDepth, dstRgb = splat(reprojectRtProject(srcDepth, K, depthFactor, rmat, tmat), maxv, srcRgb) +dstDepth[dstDepth >= maxv] = 0 +dstDepth = (dstDepth*depthFactor).astype(np.uint16) + +Image.fromarray(dstDepth).save(testDataPath + "/cv/rgbd/warpedDepth.png") +Image.fromarray(dstRgb ).save(testDataPath + "/cv/rgbd/warpedRgb.png") + +# debug +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() + +outObj = False +if outObj: + objdir = "/path/to/objdir/" + outFile(objdir + "reproj_rot_proj.obj", reproject(dstDepth, depthFactor, K)) + outFile(objdir + "rotated.obj", reprojectRt(srcDepth, K, depthFactor, rmat, tmat)) +