Add camera trajectory example

- add interpolation for creating n dimensional trajectories
- fix bug in camera projection not accounting for translation

examples/display_scene_camera_trajectory.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
+
+
+def main():
+    scene = Scene([
+        Sphere([1, 2], color='green'),
+        Sphere([-1, 2], color='red'),
+        Sphere([2, 6], color='yellow'),
+        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.1
+    cam_traj_xytheta_t_points = np.array([
+        [0, 0, np.pi / 2, 0],
+        [0, 0, np.pi / (2 + .2), 1],
+        [0, 0, np.pi / (2 - .3), 3],
+        [0, 0, np.pi / 2, 5],
+        [.5, 5, np.pi / (2 + .5), 20],
+    ])
+    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])
+    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)
+
+
+if __name__ == '__main__':
+    main()

misc/interpolation.py

+import numpy as np
+
+
+def build_nd_interpolator(x, t, interp_builder_fcn, **kwargs):
+    """
+
+    :param x: k, n | n-dimensional states over k steps
+    :param t: k, | time at each step
+    :param interp_builder_fcn: for y, t -> f, build f, where y = f(t) and y is 1d
+    :return: spl where x = spl(t) and x is n-dimensional
+    """
+    spl_1d_list = [interp_builder_fcn(t, x[:, i], **kwargs)
+                   for i in range(x.shape[1])]
+
+    def spl(t_):
+        return np.array([spl_i(t_) for spl_i in spl_1d_list]).T
+    return spl

misc/matrix_building.py

@@ -22,5 +22,16 @@ def rot_yaw_xyz1(theta):
     return mat
 
 
+def Rt_planar_xyz1(theta, t):
+    """
+    :param theta: yaw rad
+    :param t: 2, | planar translation
+    :return: 4, 4 | homogeneous ccw rotation matrix with translation
+    """
+    mat = rot_yaw_xyz1(theta)
+    mat[:2, -1] = t
+    return mat
+
+
 
 

scene_elements/camera.py

@@ -24,7 +24,10 @@ class Camera(object):
         self.body2world_Rt = Rt
 
     def project(self, x):
-        return self.P.dot(self.body2world_Rt.T.dot(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))
 
     def get_camera_center_world(self):
         return self.body2world_Rt[:3, :3].dot(self.cam_center)

scene_elements/sphere.py

 import numpy as np
-import matplotlib.pyplot as plt
 import matplotlib.patches as pa
 
 

vis/vis_mounted_thruster_model.py

@@ -2,7 +2,51 @@ import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
 import matplotlib.patches as pa
-from misc.matrix_building import rot_2d
+import misc.matrix_building as mb
+
+
+def loop_camera_sim(model, xy, theta, u, dt, scene, camera, **kwargs):
+    """
+
+    :param model:
+    :param xy: k, 2 | positions in xy of (center of mass of) rigid body
+    :param theta: k, | orientation of rigid body
+    :param u: k, n | control inputs
+    :param dt: time between sampled positions
+    :param scene:
+    :param camera:
+    :return:
+    """
+    fig, ax = plt.subplots(1, 2)
+    k = theta.size
+    ax[0].set_xticks(np.mgrid[-50:50:1])
+    ax[0].set_yticks(np.mgrid[-50:50:1])
+    ax[0].set_xlim([-3, 3])
+    ax[0].set_ylim([-3, 3])
+    ax[0].set_aspect('equal')
+    ax[1].set_aspect('equal')
+    ax[0].grid(True)
+
+    artists = draw_model(ax[0], model, xy[0], theta[0])
+    scene.draw_top_view(ax[0])
+    time_text = ax[0].text(
+        0.5, 1.05, 't = {:2.2f}s'.format(0), fontsize=14,
+        horizontalalignment='center',
+        verticalalignment='center', transform=ax[0].transAxes)
+
+    def draw_fcn(i, artists):
+        ax[0].set_xlim(xy[i, 0] + [-3, 3])
+        ax[0].set_ylim(xy[i, 1] + [-3, 3])
+        time_text.set_text('t = {:2.2f}s'.format(i * dt))
+        draw_model(ax[0], model, xy[i], theta[i], u[i], artists=artists)
+        ax[1].clear()
+        body2world_Rt = mb.Rt_planar_xyz1(theta[i], xy[i])
+        camera.set_body2world_Rt(body2world_Rt)
+        scene.draw_image_view(ax[1], camera)
+
+    ani = animation.FuncAnimation(
+        fig, draw_fcn, k, fargs=(artists,), interval=50, repeat_delay=500, blit=False)
+    plt.show()
 
 
 def loop_sim(model, xy, theta, u, dt, **kwargs):
@@ -54,7 +98,7 @@ def draw_model(ax, model, xy, theta, u=(), artists=None):
     if not artists:
         artists = {}
 
-    body_rect_xy = xy - rot_2d(theta).dot(np.array([model.length, model.width])/2)
+    body_rect_xy = xy - mb.rot_2d(theta).dot(np.array([model.length, model.width])/2)
     body_rect_angle = theta * 180/np.pi
     if 'body_rect' in artists:
         artists['body_rect'].set_xy(body_rect_xy)
@@ -104,7 +148,7 @@ def draw_model_thrust(ax, model, xy, theta, u, index):
     :param index: index of thruster in model
     :return:
     """
-    rot = rot_2d(theta)
+    rot = mb.rot_2d(theta)
     f_world = rot.dot(model.f[:, index])
     xy0 = rot.dot(model.r[:, index]) + xy
     dxy0 = f_world * u