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

Add monocular pose estimation example

- add display for projection error cost surface
- add estimation via direct linear transform with good results
parent dc79c5ad
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
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()
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
......@@ -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)
......@@ -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]
......
......@@ -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)
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