From dc79c5add074e61414f31bd32f4388966a52cd7e Mon Sep 17 00:00:00 2001
From: andersct <andersct@umich.edu>
Date: Sun, 16 Aug 2020 15:13:43 -0400
Subject: [PATCH] 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 | 58 +++++++++++++++++++++
misc/interpolation.py | 17 ++++++
misc/matrix_building.py | 11 ++++
scene_elements/camera.py | 5 +-
scene_elements/sphere.py | 1 -
vis/vis_mounted_thruster_model.py | 50 ++++++++++++++++--
6 files changed, 137 insertions(+), 5 deletions(-)
create mode 100644 examples/display_scene_camera_trajectory.py
create mode 100644 misc/interpolation.py
diff --git a/examples/display_scene_camera_trajectory.py b/examples/display_scene_camera_trajectory.py
new file mode 100644
index 0000000..0e76196
--- /dev/null
+++ b/examples/display_scene_camera_trajectory.py
@@ -0,0 +1,58 @@
+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()
diff --git a/misc/interpolation.py b/misc/interpolation.py
new file mode 100644
index 0000000..4d54218
--- /dev/null
+++ b/misc/interpolation.py
@@ -0,0 +1,17 @@
+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
diff --git a/misc/matrix_building.py b/misc/matrix_building.py
index d1e32be..e8f267b 100644
--- a/misc/matrix_building.py
+++ b/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
+
+
diff --git a/scene_elements/camera.py b/scene_elements/camera.py
index ffbec9f..7413e28 100644
--- a/scene_elements/camera.py
+++ b/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)
diff --git a/scene_elements/sphere.py b/scene_elements/sphere.py
index f549f2a..8135212 100644
--- a/scene_elements/sphere.py
+++ b/scene_elements/sphere.py
@@ -1,5 +1,4 @@
import numpy as np
-import matplotlib.pyplot as plt
import matplotlib.patches as pa
diff --git a/vis/vis_mounted_thruster_model.py b/vis/vis_mounted_thruster_model.py
index 564c20f..1926eed 100644
--- a/vis/vis_mounted_thruster_model.py
+++ b/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
--
GitLab