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()