diff --git a/examples/display_monocular_mapping_problem.py b/examples/display_monocular_mapping_problem.py
new file mode 100644
index 0000000000000000000000000000000000000000..cb68eda71cdaf42148ccea13c3f72d934772445b
--- /dev/null
+++ b/examples/display_monocular_mapping_problem.py
@@ -0,0 +1,144 @@
+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()
diff --git a/examples/monocular_mapper.py b/examples/monocular_mapper.py
new file mode 100644
index 0000000000000000000000000000000000000000..1084fc01ed4991f1ae4b321965d211d38adf0f90
--- /dev/null
+++ b/examples/monocular_mapper.py
@@ -0,0 +1,33 @@
+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
diff --git a/scene_elements/camera.py b/scene_elements/camera.py
index 7413e2859ee163961765c895f02c77ae4027a3ea..075171eba3f284130bb64008d1def36a6b4d30b0 100644
--- a/scene_elements/camera.py
+++ b/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)
diff --git a/scene_elements/scene.py b/scene_elements/scene.py
index 3fd3f57c4e44a5906afb599838c65614c6cee80c..341dc7a81f972eff5adfc5c464c9b7b0438eacb6 100644
--- a/scene_elements/scene.py
+++ b/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]
diff --git a/scene_elements/sphere.py b/scene_elements/sphere.py
index 813521216ca17fcde8feef83e449132eac87ed43..4f692d72f450533624255207c9675e8dc6c97976 100644
--- a/scene_elements/sphere.py
+++ b/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)