From 5e4d4319d67c89d83483e3ae0802cfb0b4c257de Mon Sep 17 00:00:00 2001 From: SoshinK Date: Sun, 9 May 2021 21:08:27 +0300 Subject: [PATCH] FIX: transposed rays in undistortion --- .../distortion/undistort.py | 26 +++++++++---------- kinect_telepresence/requirements.txt | 6 +++-- scripts/reprojection_demo.py | 18 +++++++++---- 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/kinect_telepresence/kinect_telepresence/distortion/undistort.py b/kinect_telepresence/kinect_telepresence/distortion/undistort.py index 3a3eb59..1492472 100644 --- a/kinect_telepresence/kinect_telepresence/distortion/undistort.py +++ b/kinect_telepresence/kinect_telepresence/distortion/undistort.py @@ -36,16 +36,17 @@ def _create_undistortion_lut(intrinsics: Intrinsics, interpolation_type): ii, jj = np.meshgrid(np.arange(width), np.arange(height)) - rays = np.array([(np.float32(ii.ravel()) - px) / fx, - (np.float32(jj.ravel()) - py) / fy, - np.ones(width * height, dtype=np.float32)]) - - distorted, _ = cv2.projectPoints(rays, np.zeros(3), np.zeros(3), intrinsics.cam_mtrx_dist, intrinsics.dist_coeffs) + rays = np.array([(ii.ravel() - px) / fx, + (jj.ravel() - py) / fy, + np.ones(width * height)]) + + # next line with argument 'rays' works on my machine, + # however on venv it requires 'rays' to have Nx3 shape, i.e. rays should be transposed + # idk why + distorted, _ = cv2.projectPoints(rays.T, np.zeros(3), np.zeros(3), intrinsics.cam_mtrx_dist, intrinsics.dist_coeffs) + distorted = distorted[:, 0, :].T # (2, N) - # print("dist", distorted.shape, rays.shape)#, y, x) - # exit() - # src = np.array([0, 0]) - + src = np.array([]) if interpolation_type == 'nearest': @@ -56,7 +57,7 @@ def _create_undistortion_lut(intrinsics: Intrinsics, interpolation_type): ValueError("Unknown interpolation type") lut_data = np.array([[0, 0] for _ in range(height * width)]).T - # print("!!", lut_data.shape) + mask1 = src[0] >= 0 mask2 = src[1] >= 0 mask3 = src[0] < width @@ -103,12 +104,10 @@ def __call__(self, depthmap): dst_img = np.zeros_like(depthmap_flattened) mask = self._undistortion_lut[:, 0] != DepthDistortionRectifier.INVALID_LUT_DATA - # print(self._undistortion_lut.shape, depthmap_flattened.shape, self._undistortion_lut[mask].shape) + if self._interpolation_type == 'nearest': dst_img[mask] = depthmap_flattened[self._undistortion_lut[mask:, 1] * width + self._undistortion_lut[mask:, 0]] elif self._interpolation_type == 'bilinear': - # print(depthmap_flattened[np.int32(self._undistortion_lut[mask, 1] * width + self._undistortion_lut[mask, 0])]) - # exit() neighbors = np.zeros((depthmap_flattened.shape[0], 4)) neighbors[mask] = np.array([depthmap_flattened[np.int32(self._undistortion_lut[mask, 1] * width + self._undistortion_lut[mask, 0])], depthmap_flattened[np.int32(self._undistortion_lut[mask, 1] * width + self._undistortion_lut[mask, 0] + 1)], @@ -118,7 +117,6 @@ def __call__(self, depthmap): idxs_where_eq_zero = np.unique(np.argwhere(neighbors == 0)[:, 0]) mask_where_not_eq_zero = np.ones(depthmap_flattened.shape[0], dtype=bool) mask_where_not_eq_zero[idxs_where_eq_zero] = False - # print(mask_where_not_eq_zero[mask_where_not_eq_zero == True].shape) mask_where_calc = mask_where_not_eq_zero dst_img[mask_where_calc] = np.sum(neighbors[mask_where_calc] * self._undistortion_lut[mask_where_calc, 2:], axis=1) + 0.5 diff --git a/kinect_telepresence/requirements.txt b/kinect_telepresence/requirements.txt index e1512ea..29bfe8d 100644 --- a/kinect_telepresence/requirements.txt +++ b/kinect_telepresence/requirements.txt @@ -1,5 +1,7 @@ numpy==1.17.0 matplotlib==3.3.4 -open3d==0.12.0 +open3d==0.11.2 numba==0.47.0 -opencv-python==3.4.2.17 \ No newline at end of file +opencv-python==3.4.2.17 +llvmlite==0.32.1 + diff --git a/scripts/reprojection_demo.py b/scripts/reprojection_demo.py index 923d0d1..cc00c05 100644 --- a/scripts/reprojection_demo.py +++ b/scripts/reprojection_demo.py @@ -12,6 +12,12 @@ from kinect_telepresence.distortion.undistort import DepthDistortionRectifier, RgbDistortionRectifier from kinect_telepresence.filtering.jbu import JBU +from pathlib import Path + +SCRIPT_PATH = Path(__file__).parent.absolute() +TRUNK_PATH = SCRIPT_PATH.parent +print(SCRIPT_PATH) + def main(): path_input_depth = '../depth0592.png' path_input_color = '../color0592.png' @@ -22,17 +28,19 @@ def main(): with open(json_path) as f: params = json.load(f) - depth_intrinsics = DepthIntrinsics.from_meta_dict(params) + depth_intrinsics = DepthIntrinsics.from_meta_dict(params) rgb_intrinsics = RgbIntrinsics.from_meta_dict(params) depth_dist_rectifier = DepthDistortionRectifier(depth_intrinsics, 'bilinear') rgb_dist_rectifier = RgbDistortionRectifier(rgb_intrinsics, 'bilinear') - depthmap = cv2.imread(path_input_depth, -1) - rgb = cv2.cvtColor(cv2.imread(path_input_color), cv2.COLOR_BGR2RGB) - # rgb = rgb_dist_rectifier(rgb) - gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY) + with open(path_input_depth) as f: + depthmap = cv2.imread(path_input_depth, -1) + with open(path_input_color) as f: + rgb = cv2.cvtColor(cv2.imread(path_input_color), cv2.COLOR_BGR2RGB) + # rgb = rgb_dist_rectifier(rgb) + gray = cv2.cvtColor(rgb, cv2.COLOR_RGB2GRAY) t = np.array(params['depth_to_rgb']['t'])