From d69fc657da1491f297494638c907b4d77630ce09 Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Thu, 1 Feb 2018 16:07:36 +0100 Subject: [PATCH 1/7] project_many and transform_many - only for projective camera so far --- opensfm/types.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/opensfm/types.py b/opensfm/types.py index da6edd3f0..570402be6 100644 --- a/opensfm/types.py +++ b/opensfm/types.py @@ -43,6 +43,10 @@ def transform(self, point): """Transform a point from world to this pose coordinates.""" return self.get_rotation_matrix().dot(point) + self.translation + def transform_many(self, points): + """Transform points from world coordinates to this pose.""" + return points.dot(self.get_rotation_matrix().T) + self.translation + def transform_inverse(self, point): """Transform a point from this pose to world coordinates.""" return self.get_rotation_matrix().T.dot(point - self.translation) @@ -201,6 +205,16 @@ def project(self, point): return np.array([self.focal * distortion * xn, self.focal * distortion * yn]) + def project_many(self, points): + """Project 3D points in camera coordinates to the image plane.""" + # Normalized image coordinates + points_n = points[:,:2] / points[:,2,np.newaxis] + + # Radial distortion + r2 = np.sum(np.square(points_n), axis=1, keepdims=True) + distortion = 1.0 + r2 * (self.k1 + self.k2 * r2) + return self.focal * distortion * points_n + def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" point = np.asarray(pixel).reshape((1, 1, 2)) @@ -547,6 +561,11 @@ def project(self, point): camera_point = self.pose.transform(point) return self.camera.project(camera_point) + def project_many(self, points): + """Project 3D points to the image plane.""" + camera_point = self.pose.transform_many(points) + return self.camera.project_many(camera_point) + def back_project(self, pixel, depth): """Project a pixel to a fronto-parallel plane at a given depth. From e390b2c132c08fdb4ae85179ed443ff449ea56c5 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 2 Feb 2018 09:36:19 +0100 Subject: [PATCH 2/7] test: add initial single vs many test --- opensfm/test/test_types.py | 63 ++++++++++++++++++++++++++++++-------- 1 file changed, 51 insertions(+), 12 deletions(-) diff --git a/opensfm/test/test_types.py b/opensfm/test/test_types.py index c14eef0e7..165990147 100644 --- a/opensfm/test/test_types.py +++ b/opensfm/test/test_types.py @@ -90,12 +90,7 @@ def test_reconstruction_class_initialization(): def test_perspective_camera_projection(): """Test perspectiive projection--backprojection loop.""" - camera = types.PerspectiveCamera() - camera.width = 800 - camera.height = 600 - camera.focal = 0.6 - camera.k1 = -0.1 - camera.k2 = 0.01 + camera = _get_perspective_camera() pixel = [0.1, 0.2] bearing = camera.pixel_bearing(pixel) projected = camera.project(bearing) @@ -106,12 +101,7 @@ def test_fisheye_camera_projection(): """Test fisheye projection--backprojection loop.""" if not context.OPENCV3: return - camera = types.FisheyeCamera() - camera.width = 800 - camera.height = 600 - camera.focal = 0.6 - camera.k1 = -0.1 - camera.k2 = 0.01 + camera = _get_fisheye_camera() pixel = [0.1, 0.2] bearing = camera.pixel_bearing(pixel) projected = camera.project(bearing) @@ -146,3 +136,52 @@ def test_pose_inverse(): identity = p.compose(inverse) assert np.allclose(identity.rotation, [0, 0, 0]) assert np.allclose(identity.translation, [0, 0, 0]) + + +def test_single_vs_many(): + points = np.array([[1, 2, 3], [4, 5, 6]], dtype=float) + pixels = np.array([[0.1, 0.2], [0.3, 0.4]], dtype=float) + depths = np.array([1, 2], dtype=float) + + pose = types.Pose([1, 2, 3], [4, 5, 6]) + t_single = [pose.transform(p) for p in points] + t_many = pose.transform_many(points) + assert np.allclose(t_single, t_many) + + t_single = [pose.transform_inverse(p) for p in points] + t_many = pose.transform_inverse_many(points) + assert np.allclose(t_single, t_many) + + cameras = [ + _get_perspective_camera(), + # _get_fisheye_camera(), + ] + for camera in cameras: + p_single = [camera.project(p) for p in points] + p_many = camera.project_many(points) + assert np.allclose(p_single, p_many) + + q_single = [camera.back_project(p, d) + for p, d in zip(pixels, depths)] + q_many = camera.back_project_many(pixels, depths) + assert np.allclose(q_single, q_many) + + +def _get_perspective_camera(): + camera = types.PerspectiveCamera() + camera.width = 800 + camera.height = 600 + camera.focal = 0.6 + camera.k1 = -0.1 + camera.k2 = 0.01 + return camera + + +def _get_fisheye_camera(): + camera = types.FisheyeCamera() + camera.width = 800 + camera.height = 600 + camera.focal = 0.6 + camera.k1 = -0.1 + camera.k2 = 0.01 + return camera From 296b463762ba13be88aa7d14d1cdc60c9d3155e2 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 2 Feb 2018 10:24:41 +0100 Subject: [PATCH 3/7] feat: add projection _many for brown cameras --- opensfm/test/test_types.py | 17 +++++++++++++++++ opensfm/types.py | 13 +++++++++++++ 2 files changed, 30 insertions(+) diff --git a/opensfm/test/test_types.py b/opensfm/test/test_types.py index 165990147..5c6b62b3d 100644 --- a/opensfm/test/test_types.py +++ b/opensfm/test/test_types.py @@ -154,6 +154,7 @@ def test_single_vs_many(): cameras = [ _get_perspective_camera(), + _get_brown_perspective_camera(), # _get_fisheye_camera(), ] for camera in cameras: @@ -177,6 +178,22 @@ def _get_perspective_camera(): return camera +def _get_brown_perspective_camera(): + camera = types.BrownPerspectiveCamera() + camera.width = 800 + camera.height = 600 + camera.focal_x = 0.6 + camera.focal_y = 0.7 + camera.c_x = 0.1 + camera.c_y = -0.05 + camera.k1 = -0.1 + camera.k2 = 0.01 + camera.p1 = 0.001 + camera.p2 = 0.002 + camera.k3 = 0.01 + return camera + + def _get_fisheye_camera(): camera = types.FisheyeCamera() camera.width = 800 diff --git a/opensfm/types.py b/opensfm/types.py index 570402be6..fb010d5cc 100644 --- a/opensfm/types.py +++ b/opensfm/types.py @@ -344,6 +344,13 @@ def project(self, point): return np.array([self.focal_x * x_distorted + self.c_x, self.focal_y * y_distorted + self.c_y]) + def project_many(self, points): + """Project 3D points in camera coordinates to the image plane.""" + distortion = np.array([self.k1, self.k2, self.p1, self.p2, self.k3]) + K, R, t = self.get_K(), np.zeros(3), np.zeros(3) + pixels, _ = cv2.projectPoints(points, R, t, K, distortion) + return pixels.reshape((2, -1)) + def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" point = np.asarray(pixel).reshape((1, 1, 2)) @@ -373,6 +380,12 @@ def back_project(self, pixel, depth): scale = depth / bearing[2] return scale * bearing + def back_project_many(self, pixels, depths): + """Project pixels to fronto-parallel planes at given depths.""" + bearings = self.pixel_bearing_many(pixels) + scales = depths / bearings[:, 2] + return scales[:, np.newaxis] * bearings + def get_K(self): """The calibration matrix.""" return np.array([[self.focal_x, 0., self.c_x], From 8d1ee973b8c0d7aee8d71847aa66d382afa98d71 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 2 Feb 2018 12:00:02 +0100 Subject: [PATCH 4/7] feat: add project_many for fisheye cameras --- opensfm/test/test_types.py | 2 +- opensfm/types.py | 25 ++++++++++++++++++------- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/opensfm/test/test_types.py b/opensfm/test/test_types.py index 5c6b62b3d..e376cdc87 100644 --- a/opensfm/test/test_types.py +++ b/opensfm/test/test_types.py @@ -155,7 +155,7 @@ def test_single_vs_many(): cameras = [ _get_perspective_camera(), _get_brown_perspective_camera(), - # _get_fisheye_camera(), + _get_fisheye_camera(), ] for camera in cameras: p_single = [camera.project(p) for p in points] diff --git a/opensfm/types.py b/opensfm/types.py index fb010d5cc..e06d4578b 100644 --- a/opensfm/types.py +++ b/opensfm/types.py @@ -207,13 +207,10 @@ def project(self, point): def project_many(self, points): """Project 3D points in camera coordinates to the image plane.""" - # Normalized image coordinates - points_n = points[:,:2] / points[:,2,np.newaxis] - - # Radial distortion - r2 = np.sum(np.square(points_n), axis=1, keepdims=True) - distortion = 1.0 + r2 * (self.k1 + self.k2 * r2) - return self.focal * distortion * points_n + distortion = np.array([self.k1, self.k2, 0, 0, 0]) + K, R, t = self.get_K(), np.zeros(3), np.zeros(3) + pixels, _ = cv2.projectPoints(points, R, t, K, distortion) + return pixels.reshape((2, -1)) def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" @@ -448,6 +445,14 @@ def project(self, point): s = self.focal * theta_d / l return np.array([s * x, s * y]) + def project_many(self, points): + """Project 3D points in camera coordinates to the image plane.""" + points = points.reshape((-1, 1, 3)).astype(np.float64) + distortion = np.array([self.k1, self.k2, 0., 0.]) + K, R, t = self.get_K(), np.zeros(3), np.zeros(3) + pixels, _ = cv2.fisheye.projectPoints(points, R, t, K, distortion) + return pixels.reshape((2, -1)) + def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" point = np.asarray(pixel).reshape((1, 1, 2)) @@ -477,6 +482,12 @@ def back_project(self, pixel, depth): scale = depth / bearing[2] return scale * bearing + def back_project_many(self, pixels, depths): + """Project pixels to fronto-parallel planes at given depths.""" + bearings = self.pixel_bearing_many(pixels) + scales = depths / bearings[:, 2] + return scales[:, np.newaxis] * bearings + def get_K(self): """The calibration matrix.""" return np.array([[self.focal, 0., 0.], From 00abae011f070c7a50067c10c52163171bf9b1e2 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 2 Feb 2018 12:20:44 +0100 Subject: [PATCH 5/7] feat: add project_many for spherical cameras --- opensfm/test/test_types.py | 25 ++++++++++++++++++------- opensfm/types.py | 7 +++++++ 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/opensfm/test/test_types.py b/opensfm/test/test_types.py index e376cdc87..64d31a226 100644 --- a/opensfm/test/test_types.py +++ b/opensfm/test/test_types.py @@ -110,9 +110,7 @@ def test_fisheye_camera_projection(): def test_spherical_camera_projection(): """Test spherical projection--backprojection loop.""" - camera = types.SphericalCamera() - camera.width = 800 - camera.height = 600 + camera = _get_spherical_camera() pixel = [0.1, 0.2] bearing = camera.pixel_bearing(pixel) projected = camera.project(bearing) @@ -156,16 +154,22 @@ def test_single_vs_many(): _get_perspective_camera(), _get_brown_perspective_camera(), _get_fisheye_camera(), + _get_spherical_camera(), ] for camera in cameras: p_single = [camera.project(p) for p in points] p_many = camera.project_many(points) assert np.allclose(p_single, p_many) - q_single = [camera.back_project(p, d) - for p, d in zip(pixels, depths)] - q_many = camera.back_project_many(pixels, depths) - assert np.allclose(q_single, q_many) + b_single = [camera.pixel_bearing(p) for p in pixels] + b_many = camera.pixel_bearing_many(pixels) + assert np.allclose(b_single, b_many) + + if hasattr(camera, 'back_project'): + q_single = [camera.back_project(p, d) + for p, d in zip(pixels, depths)] + q_many = camera.back_project_many(pixels, depths) + assert np.allclose(q_single, q_many) def _get_perspective_camera(): @@ -202,3 +206,10 @@ def _get_fisheye_camera(): camera.k1 = -0.1 camera.k2 = 0.01 return camera + + +def _get_spherical_camera(): + camera = types.SphericalCamera() + camera.width = 800 + camera.height = 600 + return camera diff --git a/opensfm/types.py b/opensfm/types.py index e06d4578b..98551939a 100644 --- a/opensfm/types.py +++ b/opensfm/types.py @@ -533,6 +533,13 @@ def project(self, point): lat = np.arctan2(-y, np.sqrt(x**2 + z**2)) return np.array([lon / (2 * np.pi), -lat / (2 * np.pi)]) + def project_many(self, points): + """Project 3D points in camera coordinates to the image plane.""" + x, y, z = points.T + lon = np.arctan2(x, z) + lat = np.arctan2(-y, np.sqrt(x**2 + z**2)) + return np.column_stack([lon / (2 * np.pi), -lat / (2 * np.pi)]) + def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" lon = pixel[0] * 2 * np.pi From 6894009a677e060d571298d10a9e1b55c3950406 Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Fri, 2 Feb 2018 12:29:35 +0100 Subject: [PATCH 6/7] test: disable fisheye camera testing when using opencv 2 --- opensfm/test/test_types.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/opensfm/test/test_types.py b/opensfm/test/test_types.py index 64d31a226..895492bf4 100644 --- a/opensfm/test/test_types.py +++ b/opensfm/test/test_types.py @@ -153,9 +153,11 @@ def test_single_vs_many(): cameras = [ _get_perspective_camera(), _get_brown_perspective_camera(), - _get_fisheye_camera(), _get_spherical_camera(), ] + if context.OPENCV3: + cameras.append(_get_fisheye_camera()) + for camera in cameras: p_single = [camera.project(p) for p in points] p_many = camera.project_many(points) From 602d5c988cceb3948f012de4f094d2734624ec6b Mon Sep 17 00:00:00 2001 From: Manuel Lopez Antequera Date: Fri, 9 Feb 2018 11:59:55 +0100 Subject: [PATCH 7/7] transpose output of project_many --- opensfm/types.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/opensfm/types.py b/opensfm/types.py index 98551939a..e314d5a55 100644 --- a/opensfm/types.py +++ b/opensfm/types.py @@ -210,7 +210,7 @@ def project_many(self, points): distortion = np.array([self.k1, self.k2, 0, 0, 0]) K, R, t = self.get_K(), np.zeros(3), np.zeros(3) pixels, _ = cv2.projectPoints(points, R, t, K, distortion) - return pixels.reshape((2, -1)) + return pixels.reshape((-1, 2)) def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" @@ -346,7 +346,7 @@ def project_many(self, points): distortion = np.array([self.k1, self.k2, self.p1, self.p2, self.k3]) K, R, t = self.get_K(), np.zeros(3), np.zeros(3) pixels, _ = cv2.projectPoints(points, R, t, K, distortion) - return pixels.reshape((2, -1)) + return pixels.reshape((-1, 2)) def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction.""" @@ -451,7 +451,7 @@ def project_many(self, points): distortion = np.array([self.k1, self.k2, 0., 0.]) K, R, t = self.get_K(), np.zeros(3), np.zeros(3) pixels, _ = cv2.fisheye.projectPoints(points, R, t, K, distortion) - return pixels.reshape((2, -1)) + return pixels.reshape((-1, 2)) def pixel_bearing(self, pixel): """Unit vector pointing to the pixel viewing direction."""