def run_check_pixels(self,
log_file,
post_process=True,
debug_mode=False,
skip_guards=False):
if log_file is None:
return
stereo_pair = self.stereo_pairs[0]
stereo_pair_builder = self.stereo_pair_builder
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, stereo_pair_builder.runtime_settings)
(good_pixels, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(
post_processing_enabled=post_process,
early_exit=False,
debug_mode=debug_mode,
skip_guards=skip_guards)
match_count = 0
error_count = 0
for (x, y, error_code) in marked_keypoints:
if error_code == 0 or error_code == 10 or error_code == 20:
match_count += 1
else:
error_count += 1
out = "Success: " + str(match_count) + " - Discarded: " + str(
error_count) + "\n"
log_file.write(out)
print(out)
def run_compare_gradients(self, segment_step_size, skip_guards=False):
stereo_pair = self.stereo_pairs[0]
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, self.stereo_pair_builder.runtime_settings)
(successes, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(
post_processing_enabled=False,
early_exit=False,
skip_guards=skip_guards)
match_amount = len(matches)
for idx in range(0, match_amount, segment_step_size):
(x, y, match_x, match_y, epx, epy, p_close_ref, p_far_ref,
rescale_factor) = matches[idx]
p_close_ref_x = p_close_ref[0, 0]
p_close_ref_y = p_close_ref[1, 0]
p_far_ref_x = p_far_ref[0, 0]
p_far_ref_y = p_far_ref[1, 0]
ep_ref = (p_close_ref_x, p_close_ref_y, p_far_ref_x, p_far_ref_y)
VISUALIZE.visualize_compare_gradients(
stereo_pair.key_frame.get_image(),
stereo_pair.reference_frame.get_image(), (x, y),
(match_x, match_y), ep_ref, (epx, epy, rescale_factor),
self.root_dir + 'grad_comp/', str(idx))
def check_gradient(self,
log_file,
post_process=True,
debug_mode=False,
skip_guards=False):
if log_file is None:
return
stereo_pair = self.stereo_pairs[0]
stereo_pair_builder = self.stereo_pair_builder
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, stereo_pair_builder.runtime_settings)
(good_pixels, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(
post_processing_enabled=post_process,
early_exit=False,
debug_mode=debug_mode,
skip_guards=skip_guards)
match_amount = len(matches)
for idx in range(0, match_amount, 1):
(x, y, match_x, match_y, epx, epy, p_close_ref, p_far_ref,
rescale_factor) = matches[idx]
p_close_ref_x = p_close_ref[0, 0]
p_close_ref_y = p_close_ref[1, 0]
p_far_ref_x = p_far_ref[0, 0]
p_far_ref_y = p_far_ref[1, 0]
ep_ref = (p_close_ref_x, p_close_ref_y, p_far_ref_x, p_far_ref_y)
inc_x = p_close_ref[0, 0] - p_far_ref[0, 0]
inc_y = p_close_ref[1, 0] - p_far_ref[1, 0]
key_ratio_y = epy / epx
ref_ratio_y = inc_y / inc_x
info = 'Gradient Ratio (x to) in Key Frame: 1:' + str(
key_ratio_y) + ' in reference frame: 1:' + str(
ref_ratio_y) + '\n'
log_file.write(info)
print(info)
def run_show_epipolar_segments(self, segment_step_size, skip_guards=False):
stereo_pair = self.stereo_pairs[0]
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, self.stereo_pair_builder.runtime_settings)
(successes, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(
post_processing_enabled=False,
early_exit=False,
skip_guards=skip_guards)
match_amount = len(matches)
disp_image_1 = stereo_pair.key_frame.get_image()
disp_image_2 = stereo_pair.reference_frame.get_image()
disp_image_1 = cv2.cvtColor(disp_image_1, cv2.COLOR_GRAY2BGR)
disp_image_2 = cv2.cvtColor(disp_image_2, cv2.COLOR_GRAY2BGR)
for idx in range(0, match_amount, segment_step_size):
(x, y, match_x, match_y, epx, epy, p_close_ref, p_far_ref,
rescale_factor) = matches[idx]
p_close_ref_x = p_close_ref[0, 0]
p_close_ref_y = p_close_ref[1, 0]
p_far_ref_x = p_far_ref[0, 0]
p_far_ref_y = p_far_ref[1, 0]
ep_ref = (p_close_ref_x, p_close_ref_y, p_far_ref_x, p_far_ref_y)
VISUALIZE.visualize_match_and_epipolar(disp_image_1, disp_image_2,
(x, y), (match_x, match_y),
ep_ref,
(epx, epy, rescale_factor),
self.root_dir + 'epipolar/',
str(idx))
res = np.zeros((disp_image_1.shape[0], disp_image_1.shape[1] * 2,
disp_image_1.shape[2]))
res[:, 0:disp_image_1.shape[1], :] = disp_image_1
res[:, disp_image_1.shape[1]:, :] = disp_image_2
cv2.imwrite(self.root_dir + 'epipolar/' + 'epipoloar.png', res)
runtime_settings = libs.settings.Settings(cm.focal_length[0],
cm.focal_length[1],
cm.principal_point[0],
cm.principal_point[1], height, width)
source_frame = libs.frame.Frame(pic_rect_source, source)
dest_frame = libs.frame.Frame(pic_rect_dest, dest)
# source_frame_re = libs.frame.Frame(pic_source_resize,source)
# dest_frame_re = libs.frame.Frame(pic_dest_resize,dest)
se3 = vo.buildSE3(source, dest)
K = runtime_settings.build_K_matrix()
stereo_pair = libs.stereo_pair.StereoPair(dest_frame, source_frame, se3, K)
depth_estimation = depth_est.DepthEstimation(stereo_pair, runtime_settings)
## Plot Initial Values ##
#libs.visualize.show(stereo_pair.key_frame.get_depth_map(),normalize=True)
#libs.visualize.show(stereo_pair.key_frame.get_image())
#libs.visualize.show_prob_custom(stereo_pair.key_frame.get_depth_map(),0.4999,0.50001)
## Visualize Good Pixels ##
#(successes, good_pixels) = depth_estimation.search_for_good_pixels()
#libs.visualize.visualize_keypoints(stereo_pair.key_frame.get_image(),good_pixels)
## 1 iteration of depth estimation ##
#(successes, matches,epipolar_matches) = depth_estimation.depth_estimation()
def run(self,
visualize_enum,
ground_truth,
normalize=True,
calc_error_metrics=False,
post_process=False,
regularize=True,
show_frame=True,
debug_mode=False,
skip_guards=False):
iteration = 1
stereo_pair_builder = self.stereo_pair_builder
runtime_settings = stereo_pair_builder.runtime_settings
for stereo_pair in self.stereo_pairs:
it_string = str(iteration)
early_exit = False
if visualize_enum == VISUALIZE.visualize_enum.SHOW_INITIAL_GOOD_PIXELS:
early_exit = True
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, runtime_settings)
(good_pixels, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(
post_processing_enabled=regularize,
early_exit=early_exit,
debug_mode=debug_mode,
skip_guards=skip_guards)
hypothesis_map = stereo_pair_builder.key_frame.get_hypothesis_map()
post_processed_depth = stereo_pair_builder.key_frame.get_depth_map(
)
if post_process:
# post_processed_depth = POST.Evaluation.threshold(post_processed_depth, 1e-2,
# 5)
post_processed_depth = POST.Evaluation.scale_matrix(
post_processed_depth, runtime_settings.min_depth,
runtime_settings.max_depth, hypothesis_map)
if calc_error_metrics and not ground_truth is None:
mse = POST.Evaluation.compute_rms(ground_truth,
post_processed_depth,
hypothesis_map)
self.mse_list.append(mse)
print(mse)
if visualize_enum == VISUALIZE.visualize_enum.SHOW_DEPTH:
VISUALIZE.show(stereo_pair_builder.key_frame.get_image(),
post_processed_depth,
hypothesis_map,
stereo_pair_builder.runtime_settings,
normalize=normalize,
iteration=it_string,
path=self.root_dir,
color_map='nipy_spectral',
show_keyframe=show_frame)
elif visualize_enum == VISUALIZE.visualize_enum.SHOW_VARIANCE:
VISUALIZE.show(
stereo_pair_builder.key_frame.get_image(),
stereo_pair_builder.key_frame.get_variance_map(),
hypothesis_map,
stereo_pair_builder.runtime_settings,
normalize=normalize,
iteration=it_string,
path=self.root_dir,
color_map='magma')
elif visualize_enum == VISUALIZE.visualize_enum.SHOW_SUCCESSES_AND_DISCARDED_PIXELS:
VISUALIZE.visualize_successes_and_discarded_pixels(
stereo_pair_builder.key_frame.get_image(),
marked_keypoints, self.root_dir, iteration)
elif visualize_enum == VISUALIZE.visualize_enum.SHOW_INITIAL_GOOD_PIXELS:
VISUALIZE.visualize_keypoints(
stereo_pair_builder.key_frame.get_image(), good_pixels,
self.root_dir)
iteration = iteration + 1
if calc_error_metrics:
# VISUALIZE.line_graph(self.mse_list, self.root_dir, 'line_graph')
self.save_list(self.mse_list)
# source_frame = libs.frame.Frame(pic_rect_source,source)
# dest_frame = libs.frame.Frame(pic_rect_dest,dest)
dest_frame_re = libs.frame.Frame(pic_source_resize, source)
source_frame_re = libs.frame.Frame(pic_dest_resize, dest)
se3 = vo.buildSE3(source, dest)
K = runtime_settings.build_K_matrix()
stereo_pair_re = libs.stereo_pair.StereoPair(source_frame_re,
dest_frame_re,
se3,
K,
invert_se3_matricies=True)
depth_estimation_re = depth_est.DepthEstimation(stereo_pair_re,
runtime_settings)
## Plot Initial Values ##
# libs.visualize.show(stereo_pair.key_frame.get_depth_map(),normalize=True)
# libs.visualize.show(stereo_pair.key_frame.get_image())
# libs.visualize.show_prob_custom(stereo_pair.key_frame.get_depth_map(),0.4999,0.50001)
## Visualize Good Pixels For Resized ##
# (successes, good_pixels) = depth_estimation_re.search_for_good_pixels()
# libs.visualize.visualize_keypoints(stereo_pair_re.key_frame.get_image(),good_pixels)
## Visualize All Pixels For Resized ##
(successes, matches, matches_for_large_disparities_in_x,
cm.principal_point[0],
cm.principal_point[1], 12, 0.01, 0.3, 1.0)
ref_list = [ref_1, ref_2, ref_3]
stereo_pair_builder = STEREO_PAIR_BUILDER.StereoPairBuilder(
cm, image_loader, vo, 25, runtime_settings)
stereo_pairs = stereo_pair_builder.generate_stereo_pairs(key,
ref_list,
invert=True)
iteration = 1
for stereo_pair in stereo_pairs:
#libs.visualize.show_frame(stereo_pair.key_frame.get_image(),root_dir,'keyframe')
it_string = str(iteration)
depth_estimation_re = DEPTH_ESTIMATION.DepthEstimation(
stereo_pair, stereo_pair_builder.runtime_settings)
(successes, matches, matches_for_large_disparities_in_x,
marked_keypoints) = depth_estimation_re.depth_estimation(True)
libs.visualize.show(stereo_pair_builder.key_frame.get_depth_map(),
stereo_pair_builder.key_frame.get_hypothesis_map(),
normalize=True,
iteration=it_string,
show=False,
path=root_dir,
color_map='nipy_spectral')
#libs.visualize.show(stereo_pair_builder.key_frame.get_depth_map(),normalize=True,iteration=it_string,show=False,path=root_dir,color_map='gray')
# libs.visualize.show_color(stereo_pair_builder.key_frame.get_depth_map(),stereo_pair_builder.key_frame.get_hypothesis_map(), iteration=it_string, path=root_dir)
# libs.visualize.show_color_with_keyframe(stereo_pair_builder.key_frame.get_image(),stereo_pair_builder.key_frame.get_depth_map(),stereo_pair_builder.key_frame.get_hypothesis_map(), iteration=it_string, path=root_dir)
iteration = iteration + 1
# for stereo_pair in stereo_pairs: