def __init__(self, camera_model, image_shape):
self.image_shape = image_shape
us = image_coordinates(image_shape)
xs = camera_model.normalize(us)
self._xs_map_0 = xs[:, 0].reshape(image_shape)
self._xs_map_1 = xs[:, 1].reshape(image_shape)
def __call__(self, I0, D0, I1, pose10, weights=None):
def warn():
warnings.warn("Camera pose change is too large.", RuntimeWarning)
error = PhotometricError(self.camera_model0, self.camera_model1,
I0, D0, I1)
us0 = image_coordinates(I0.shape)
xs0 = self.camera_model0.normalize(us0)
P0 = inv_pi(xs0, D0.flatten())
GX1, GY1 = calc_image_gradient(I1)
residuals = (I0 - I1).flatten()
prev_error = error(pose10)
for k in range(self.max_iter):
P1 = transform(pose10.R, pose10.t, P0)
xi = calc_pose_update(self.camera_model1, residuals,
GX1, GY1, P1, weights)
if xi is None:
warn()
return pose10
dpose = Pose.from_se3(xi)
candidate = dpose * pose10
curr_error = error(candidate)
if curr_error > prev_error:
break
prev_error = curr_error
pose10 = candidate
return pose10
def test_image_coordinates():
width, height = 3, 4
coordinates = image_coordinates(image_shape=[height, width])
assert_array_equal(
coordinates,
# x y
[[0, 0], [1, 0], [2, 0], [0, 1], [1, 1], [2, 1], [0, 2], [1, 2],
[2, 2], [0, 3], [1, 3], [2, 3]])
def photometric_error(warp, gray_image0, depth_map0, gray_image1):
# TODO change the argument order
# gray_image0, depth_map0, gray_image1
# -> gray_image0, gray_image1, depth_map0
# assert(isinstance(warp, LocalWarp2D))
us0 = image_coordinates(depth_map0.shape)
us1, depths1 = warp(us0, depth_map0.flatten())
mask = is_in_image_range(us1, depth_map0.shape)
i0 = get(gray_image0, us0[mask])
i1 = interpolation(gray_image1, us1[mask])
return calc_error_(i0, i1)
def plot_warp(warp2d, gray_image0, depth_map0, gray_image1):
from tadataka.interpolation import interpolation
from tadataka.coordinates import image_coordinates
from tadataka.utils import is_in_image_range
from matplotlib import pyplot as plt
us0 = image_coordinates(depth_map0.shape)
depths0 = depth_map0.flatten()
us1, depths1 = warp2d(us0, depths0)
mask = is_in_image_range(us1, depth_map0.shape)
fig = plt.figure()
E = photometric_error(warp2d, gray_image0, depth_map0, gray_image1)
fig.suptitle("photometric error = {:.3f}".format(E))
ax = fig.add_subplot(221)
ax.set_title("t0 intensities")
ax.imshow(gray_image0, cmap="gray")
ax = fig.add_subplot(223)
ax.set_title("t0 depth")
ax.imshow(depth_map0, cmap="gray")
ax = fig.add_subplot(222)
ax.set_title("t1 intensities")
ax.imshow(gray_image1, cmap="gray")
ax = fig.add_subplot(224)
ax.set_title("predicted t1 intensities")
height, width = gray_image1.shape
ax.scatter(us1[mask, 0], us1[mask, 1],
c=gray_image0[us0[mask, 1], us0[mask, 0]],
s=0.5,
cmap="gray")
ax.set_xlim(0, width)
ax.set_ylim(height, 0)
ax.set_aspect('equal')
plt.show()
max_inv_depth = safe_invert(min_depth)
keyframe_, refframe_ = dataset[0]
image_key = rgb2gray(keyframe_.image)
estimate = InvDepthEstimator(keyframe_.camera_model, image_key,
[min_inv_depth, max_inv_depth],
sigma_i=0.1, sigma_l=0.2,
step_size_ref=0.01, min_gradient=0.2)
pose_wk = keyframe_.pose
pose_wr = refframe_.pose
pose_rk = pose_wr.inv() * pose_wk
T_rk = pose_rk.T
image_ref = rgb2gray(refframe_.image)
prior = Hypothesis(prior_inv_depth, prior_variance)
inv_depth_map = prior.inv_depth * np.ones(image_key.shape)
variance_map = prior.variance * np.ones(image_key.shape)
flag_map = np.full(image_key.shape, FLAG.NOT_PROCESSED)
for u_key in tqdm(image_coordinates(image_key.shape)):
(inv_depth, variance), flag = estimate(refframe_.camera_model, image_ref,
T_rk, u_key, prior)
x, y = u_key
inv_depth_map[y, x] = inv_depth
variance_map[y, x] = variance
flag_map[y, x] = flag
# plot_depth(image_key, np.zeros(image_key.shape),
# flag_map, keyframe_.depth_map,
# safe_invert(inv_depth_map), variance_map)
def flag_to_color_map(flag_map):
color_map = np.empty((*flag_map.shape, 3))
for x, y in image_coordinates(flag_map.shape):
color_map[y, x] = flag_to_rgb(flag_map[y, x])
return color_map
def plot_depth(image_key, pixel_age, flag_map, # validity_map,
depth_map_true, depth_map_pred, variance_map,
image_cmap="gray", depth_cmap='RdBu'):
fig = plt.figure()
ax = fig.add_subplot(2, 4, 1)
ax.set_title("keyframe")
ax.imshow(image_key, cmap=image_cmap)
ax = fig.add_subplot(2, 4, 2)
ax.set_title("pixel age")
im = ax.imshow(pixel_age, cmap=image_cmap)
plot_with_bar(ax, im)
ax = fig.add_subplot(2, 4, 3)
ax.set_title("flag map")
ax.imshow(flag_to_color_map(flag_map))
patches = [Patch("black", flag_to_rgb(f), label=f.name) for f in FLAG]
ax.legend(handles=patches, loc='center left', bbox_to_anchor=(1.05, 0.5))
mask = flag_map==FLAG.SUCCESS
if mask.sum() == 0: # no success pixel
plt.show()
return
depths_pred = depth_map_pred[mask]
depths_true = depth_map_true[mask]
depths_diff = np.abs(depths_pred - depths_true)
us = image_coordinates(depth_map_pred.shape)[mask.flatten()]
vmax = np.percentile(
np.concatenate((depth_map_true.flatten(), depths_pred)), 98
)
norm = Normalize(vmin=0.0, vmax=vmax)
mapper = ScalarMappable(norm=norm, cmap=depth_cmap)
ax = fig.add_subplot(2, 4, 5)
ax.set_title("ground truth depth")
ax.axis("off")
im = ax.imshow(depth_map_true, norm=norm, cmap=depth_cmap)
plot_with_bar(ax, im)
height, width = image_key.shape[0:2]
ax = fig.add_subplot(2, 4, 6)
ax.set_title("predicted depth map")
ax.axis("off")
im = ax.imshow(image_key, cmap=image_cmap)
ax.scatter(us[:, 0], us[:, 1], s=0.5, c=mapper.to_rgba(depths_pred))
ax.set_xlim(0, width)
ax.set_ylim(height, 0)
ax = fig.add_subplot(2, 4, 7)
ax.set_title("error = abs(pred - true)")
im = ax.imshow(image_key, cmap=image_cmap)
ax.scatter(us[:, 0], us[:, 1], s=0.5, c=mapper.to_rgba(depths_diff))
ax.set_xlim(0, width)
ax.set_ylim(height, 0)
ax = fig.add_subplot(2, 4, 8)
ax.set_title("variance map")
norm = Normalize(vmin=0.0, vmax=np.percentile(variance_map.flatten(), 98))
im = ax.imshow(variance_map, norm=norm, cmap=depth_cmap)
plot_with_bar(ax, im)
plt.show()