Merge branch 'vectorizations'

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)
@@ -120,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)
@@ -146,3 +134,84 @@ 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_brown_perspective_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)
+        assert np.allclose(p_single, p_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():
+    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_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
+    camera.height = 600
+    camera.focal = 0.6
+    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

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,13 @@ 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."""
+        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((-1, 2))
+
     def pixel_bearing(self, pixel):
         """Unit vector pointing to the pixel viewing direction."""
         point = np.asarray(pixel).reshape((1, 1, 2))
@@ -330,6 +341,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((-1, 2))
+
     def pixel_bearing(self, pixel):
         """Unit vector pointing to the pixel viewing direction."""
         point = np.asarray(pixel).reshape((1, 1, 2))
@@ -359,6 +377,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],
@@ -421,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((-1, 2))
+
     def pixel_bearing(self, pixel):
         """Unit vector pointing to the pixel viewing direction."""
         point = np.asarray(pixel).reshape((1, 1, 2))
@@ -450,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.],
@@ -495,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
@@ -547,6 +592,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.