FIX: transposed rays in undistortion

SoshinK · May 9, 2021 · 5e4d431 · 5e4d431
1 parent 4ecf6dd
commit 5e4d431
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 21 deletions.
diff --git 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
@@ -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
+opencv-python==3.4.2.17
+llvmlite==0.32.1
+
diff --git 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'])