#im_depth_reference = np.add(im_depth_reference,depth_t)
#depth_t = (im_depth_target != 0).astype(Utils.depth_data_type_float)
#im_depth_target = np.add(im_depth_target,depth_t)
# We only need the gradients of the target frame
frame_reference = Frame.Frame(im_greyscale_reference, im_depth_reference, camera_reference, False)
frame_target = Frame.Frame(im_greyscale_target, im_depth_target, camera_target, True)
solver_manager = SolverThreadManager.Manager(1,
"Solver Manager",
frame_reference,
frame_target,
max_its=50,
eps=0.002, #0.001, 0.00001, 0.00005, 0.00000001
alpha_step=0.63, # 0.1, 0.04, 0.005, 0.55 - motion prior
gradient_monitoring_window_start=0,
image_range_offset_start=0,
twist_prior=twist_prior,
motion_cov_inv = motion_cov_inv,
use_ndc=use_ndc,
use_robust=True,
track_pose_estimates=True,
use_motion_prior=True,
debug=False)
solver_manager.start()
solver_manager.join() # wait to complete
motion_cov_inv = solver_manager.motion_cov_inv_final
#motion_cov_inv = np.add(motion_cov_inv,solver_manager.motion_cov_inv_final)
twist_prior = solver_manager.twist_final
#twist_prior = np.add(twist_prior,solver_manager.twist_final)
linear_cov = linear_cov_list[i]
linear_cov_large = Linear.generate_6DOF_cov_from_motion_model_cov(linear_cov)
linear_cov_large_inv = np.linalg.inv(linear_cov_large)
linear_twist = linear_motion.pose_delta_list[i].get_6dof_twist(normalize=False)
motion_cov_inv = linear_cov_large_inv
solver_manager = SolverThreadManager.Manager(1,
"Solver Manager",
frame_reference,
frame_target,
max_its=max_its,
eps=eps, #0.001, 0.00001, 0.00005, 0.00000001
alpha_step=alpha_step, # 0.001, 0.008 - motion pri
gradient_monitoring_window_start=gradient_monitoring_window_start,
image_range_offset_start=image_range_offset_start,
max_depth=max_depth,
twist_prior=twist_prior,
motion_cov_inv = motion_cov_inv,
use_ndc=use_ndc,
use_robust=use_robust,
track_pose_estimates=track_pose_estimates,
use_motion_prior=use_motion_prior,
ackermann_pose_prior=linear_twist,
use_ackermann=use_ackermann,
debug=debug)
if calc_vo:
solver_manager.start()
solver_manager.join() # wait to complete
print('iteration ', i + 1, ' complete')