Skip to content
Snippets Groups Projects
Commit dc79c5ad authored by andersct's avatar andersct
Browse files

Add camera trajectory example

- add interpolation for creating n dimensional trajectories
- fix bug in camera projection not accounting for translation
parent 81bf57f0
No related branches found
No related tags found
No related merge requests found
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()
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
......@@ -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
......@@ -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)
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.patches as pa
......
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment