Add monocular pose estimation example

- add display for projection error cost surface
- add estimation via direct linear transform with good results

examples/display_monocular_mapping_problem.py

+import numpy as np
+from scene_elements.scene import Scene
+from scene_elements.sphere import Sphere
+from scene_elements.water import Water
+from scene_elements.camera import Camera
+from misc.interpolation import build_nd_interpolator
+from scipy.interpolate import InterpolatedUnivariateSpline
+from rigid_body_models import boat
+from vis import vis_mounted_thruster_model
+import misc.matrix_building as mb
+import matplotlib.pyplot as plt
+
+
+def xyplane_projection_error_cost_surface(P_array, uv_array, xyz_true, z_level=0):
+    """
+    Projection error model:
+
+    (u_i; v_i; 1) = P_i (x; y; z; 1) + e_i   [with e_i = (-, -, 0)]
+
+    Plot projection error (sum of squared e_i) surface by calculating error at various xyz.
+    Grid xyz by centering xy on true xy, and using z_level for all z.
+    :param P_array: k, 3, 4
+    :param uv_array: k, 2
+    :param xyz_true: 3,
+    :param z_level:
+    :return:
+        x_grid: n, m | x coordinates of tested xyz
+        y_grid: n, m | y coordinates of tested xyz
+        sse_grid: n, m | sum of squared errors at each tested xyz
+    """
+    xyz1_test = np.mgrid[-10:10:.1, -10:10:.1]
+    xyz1_test[0] += xyz_true[0]
+    xyz1_test[1] += xyz_true[1]
+    m, n = xyz1_test.shape[1:]
+    xyz1 = np.concatenate((
+        xyz1_test, z_level * xyz1_test[[0]], 1 + 0 * xyz1_test[[0]]))
+    xyz1 = xyz1.reshape(4, -1)  # 4, m*n
+    sse = np.zeros((m*n))
+    for i in range(uv_array.shape[0]):
+        uv_proj = P_array[i].dot(xyz1).T  # m*n, 3
+        np.clip(uv_proj[:, 2], 1e-5, None, out=uv_proj[:, 2])  # avoid / 0
+        uv_proj = uv_proj[:, :2] / uv_proj[:, [2]]
+        sse += np.linalg.norm(uv_proj - uv_array[i], axis=1) ** 2
+    return xyz1_test[0], xyz1_test[1], sse.reshape(m, n)
+
+
+def main():
+    target_sphere = Sphere([2, 6], color='yellow')
+
+    scene = Scene([
+        Sphere([1, 2], color='green'),
+        Sphere([-1, 2], color='red'),
+        target_sphere,
+        Sphere([0, 7], color='black'),
+        Water(),
+    ])
+
+    cam_f = 420.
+    cam_rows = 480
+    cam_cols = 640
+    cam_K = np.array([
+        [cam_f, 0, cam_rows/2, 0],
+        [0, cam_f, cam_cols/2, 0],
+        [0, 0, 1., 0],
+    ])
+    cam_Rt = np.array([
+        [0, 0, 1, -.2],
+        [0, -1, 0, 0],
+        [1, 0, 0, 0],
+        [0, 0, 0, 1],
+    ])
+    camera = Camera(K=cam_K, Rt=cam_Rt, cam_cols=cam_cols, cam_rows=cam_rows)
+
+    DT = 0.5
+    cam_traj_xytheta_t_points = np.array([
+        [0, 0, np.pi / 2, 0],
+        [.5, 5, np.pi / (2 + .5), 10],
+    ])
+    cam_traj_spline = build_nd_interpolator(
+        cam_traj_xytheta_t_points[:, :-1], cam_traj_xytheta_t_points[:, -1],
+        InterpolatedUnivariateSpline, k=1,
+    )
+    xytheta = cam_traj_spline(np.mgrid[0:cam_traj_xytheta_t_points[-1, -1]:DT])
+
+    # For displaying the trajectory
+    # u = np.empty((xytheta.shape[0], 0))
+    # boat_model = boat.DiamondMountedThrusters()
+    # vis_mounted_thruster_model.loop_camera_sim(
+    #     boat_model, xytheta[:, [0, 1]], xytheta[:, 2], u, DT, scene, camera)
+    # -
+
+    n_steps = xytheta.shape[0]
+    P_array = np.zeros((n_steps, 3, 4))
+    uv_array = np.zeros((n_steps, 2))
+    for i in range(n_steps):
+        body2world_Rt = mb.Rt_planar_xyz1(xytheta[i, -1], xytheta[i, :2])
+        camera.set_body2world_Rt(body2world_Rt)
+        P_array[i] = camera.get_world_projection_matrix()
+        uv_array[i] = target_sphere.calculate_image_view_center(camera)
+
+    # Select indices where target is in image
+    mask = ~np.isnan(uv_array).any(axis=1)
+    P_array, uv_array = P_array[mask], uv_array[mask]
+    mask = (0 <= uv_array[:, 0]) & (uv_array[:, 0] < cam_rows) & \
+           (0 <= uv_array[:, 1]) & (uv_array[:, 1] < cam_cols)
+    P_array, uv_array = P_array[mask], uv_array[mask]
+
+    n_obs = uv_array.shape[0]
+    print('{} observations of target collected'.format(n_obs))
+
+    xyz_true = np.hstack([target_sphere.xy, target_sphere.z])
+    x_grid, y_grid, sse_grid = xyplane_projection_error_cost_surface(
+        P_array, uv_array, xyz_true, z_level=0)
+    sse_grid /= 1e6  # scale by a fictitious variance
+    sse_grid[sse_grid > 1e2] = 1e2
+
+    # 2d display
+    fig, ax = plt.subplots()
+    ax.imshow(sse_grid, extent=(x_grid.min(), x_grid.max(), y_grid.min(), y_grid.max()),
+              origin='lower', vmin=0, vmax=1)
+    ax.set_xlabel('x [m]')
+    ax.set_ylabel('y [m]')
+    ax.scatter(*target_sphere.xy, marker='+', color='w')
+    plt.show()
+
+    # 3d display
+    # from mpl_toolkits.mplot3d import Axes3D
+    # fig = plt.figure()
+    # ax = fig.add_subplot(111, projection='3d')
+    # surf = ax.plot_surface(x_grid, y_grid, sse_grid, cmap='jet',
+    #                        linewidth=0, antialiased=False, vmin=0, vmax=50)
+    # ax.set_xlabel('x [m]')
+    # ax.set_ylabel('y [m]')
+    # plt.show()
+
+    # Estimate x
+    import examples.monocular_mapper as mm
+
+    x_hat = mm.dlt_estimate(P_array, uv_array)
+    print('Estimated position: {}'.format(x_hat.round(2)))
+
+
+if __name__ == '__main__':
+    main()

examples/monocular_mapper.py

+import numpy as np
+
+
+def dlt_estimate(P_array, u_array):
+    """
+    Solve for unknown position x \in \reals^3
+    Observe u_i \in \reals^2 for pixel values
+    and projection matrices P_i \reals^{3,4}.
+
+    u_i * d_i = P_i (x; 1)
+
+    where d_i is the scale factor for the observation (depth).
+    The direct linear transform (DLT) method uses a skew symmetric matrix H
+    to remove the d_i from each equation.
+
+    Take H = [0 1; -1 0] so u_i'Hu_i = 0 by skew symmetry. Hence
+    (taking the {2,4} relevant section of P_i, that is P[:2, :]):
+    -
+
+    0 = (u_i'H)u_i * d_i = (u_i'H) P_i (x; 1)
+                         = (u_i'H) (P_i[:2, :3]x + P_i[:2, 3])
+
+    now solve via least squares.
+
+    :param P_array: k, 3, 4
+    :param u_array: k, 2
+    :return:
+    """
+    uH = u_array.dot(np.array([[0, -1], [1, 0.]]))  # k, 2
+    uHP = np.einsum('li,lik->lk', uH, P_array[:, :2, :3])  # k, 3
+    uH_pvec = np.einsum('li,li->l', uH, P_array[:, :2, 3])  # k,
+    x = np.linalg.lstsq(uHP, -uH_pvec, rcond=None)[0]
+    return x

scene_elements/camera.py

@@ -5,6 +5,12 @@ class Camera(object):
 
     def __init__(self, K, Rt, cam_cols=None, cam_rows=None):
         """
+        Standard:
+        ^ x
+        |             z out from camera, towards world
+        |
+         --------> y
+
         Coordinates:
         [cam] <--Rt-- [body] <--body2world_Rt-- [world]
         :param K: 3, 4 | intrinsic matrix
@@ -19,15 +25,25 @@ class Camera(object):
         self.P = K.dot(Rt)
         self.cam_center = Rt[:3, :3].T.dot(-Rt[:-1, -1])
         self.body2world_Rt = np.eye(4)
+        self.world2body_Rt = np.eye(4)
 
     def set_body2world_Rt(self, Rt):
         self.body2world_Rt = Rt
-
-    def project(self, x):
         world2body_Rt = np.eye(4)
         world2body_Rt[:3, :3] = self.body2world_Rt[:3, :3].T
         world2body_Rt[:-1, -1] = world2body_Rt[:3, :3].dot(-self.body2world_Rt[:-1, -1])
-        return self.P.dot(world2body_Rt.dot(x))
+        self.world2body_Rt = world2body_Rt
+
+    def get_world_projection_matrix(self):
+        P_world = self.P.dot(self.world2body_Rt)
+        return P_world
+
+    def get_world2cam_Rt(self):
+        return self.Rt.dot(self.world2body_Rt)
+
+    def project(self, x):
+        P_world = self.get_world_projection_matrix()
+        return P_world.dot(x)
 
     def get_camera_center_world(self):
         return self.body2world_Rt[:3, :3].dot(self.cam_center)

scene_elements/scene.py

@@ -12,9 +12,14 @@ class Scene(object):
             element.draw_top_view(ax)
 
     def draw_image_view(self, ax, camera):
+        """
+        Draw all elements in the scene
+        :param ax:
+        :param camera:
+        :return:
+        """
         ax.set_xlim([0, camera.cam_cols])
         ax.set_ylim([0, camera.cam_rows])
-        # ax.grid(True)
 
         # apply world to cam body Rt
         cam_center_world = camera.get_camera_center_world()[:2]

scene_elements/sphere.py

@@ -14,27 +14,52 @@ class Sphere(object):
         circle = pa.Circle(tuple(self.xy), radius=self.radius, facecolor=self.color)
         ax.add_artist(circle)
 
-    def draw_image_view(self, ax, camera, zorder=0):
+    def calculate_image_view_shape(self, camera):
         """
-        Do not draw if behind camera.
-        (Out of frame objects can be drawn, they will just be cut off)
-        :param ax:
         :param camera:
-        :param zorder: ordering for fake depth
         :return:
+            pos: 2, | [u, v] position of element in image
+            - not in image -> nans
+            - may be outside of image bounds, use image size to filter these
+            radius: | in pixels
         """
         xyz1 = np.hstack([self.xy, self.z, 1.])
         top_bot_xyz1 = np.array([xyz1, xyz1]).T
         top_bot_xyz1[2, :] += [self.radius, -self.radius]
         top_bot_xy1 = camera.project(top_bot_xyz1)
         if np.any(top_bot_xy1[-1] <= 0):
-            return
+            return np.array([np.nan] * 2), np.nan
         top_bot_xy1 /= top_bot_xy1[-1]
         xy = top_bot_xy1[:-1].mean(axis=1)
         r = np.linalg.norm(top_bot_xy1[:-1, 0] - xy)/2
+        return xy, r
+
+    def calculate_image_view_center(self, camera):
+        """
+        :param camera:
+        :return:
+            pos: 2, | [u, v] position of element in image
+            - not in image -> nans
+            - may be outside of image bounds, use image size to filter these
+        """
+        xy = self.calculate_image_view_shape(camera)[0]
+        return xy
+
+    def draw_image_view(self, ax, camera, zorder=0):
+        """
+        Do not draw if behind camera.
+        (Out of frame objects can be drawn, they will just be cut off)
+        :param ax:
+        :param camera:
+        :param zorder: ordering for fake depth
+        :return:
+        """
+        xy, r = self.calculate_image_view_shape(camera)
+        if np.isnan(xy).any():
+            return
         # reverse xy to place into plot coordinates, since cam x is vertical
         circle = pa.Circle(
-            tuple(xy)[::-1], radius=r,
+            tuple(xy[::-1]), radius=r,
             facecolor=self.color, zorder=zorder
         )
         ax.add_artist(circle)