def lie_ackermann_correction(gradient_step, motion_cov_inv, ackermann_twist,
vo_twist, twist_size):
# ack_prior = np.multiply(Gradient_step_manager.current_alpha,ackermann_pose_prior)
ack_prior = ackermann_twist
#ack_prior = np.matmul(motion_cov_inv, ack_prior) # 1
#ack_prior = np.multiply(gradient_step, ack_prior) # 2
R_w, t_w = exp(vo_twist, twist_size)
R_ack, t_ack = exp(ack_prior, twist_size)
SE_3_w = np.append(np.append(R_w, t_w, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE_3_ack = np.append(np.append(R_ack, t_ack, axis=1),
Utils.homogenous_for_SE3(),
axis=0)
SE3_w_ack = SE3.pose_pose_composition_inverse(SE_3_w, SE_3_ack)
w_inc = ln(SE3.extract_rotation(SE3_w_ack),
SE3.extract_translation(SE3_w_ack), twist_size)
w_inc = np.multiply(gradient_step, np.matmul(motion_cov_inv, w_inc)) # 1
#w_inc = np.matmul(motion_cov_inv, w_inc) # 2
#w_inc = np.multiply(gradient_step, w_inc)
return w_inc
def __init__(self, intrinsic: Intrinsic, se3: np.ndarray):
if intrinsic.extract_fx() > 0 or intrinsic.extract_fy() > 0:
warnings.warn(
"focal length is positive in right handed coordinate system, may lead to inverted image",
RuntimeWarning)
self.intrinsic = intrinsic
self.se3 = se3
self.se3_inv = SE3.invert(se3)
self.origin_ws = SE3.extract_translation(self.se3_inv)
def __init__(self, intrinsic: Intrinsic, se3: np.ndarray):
self.intrinsic = intrinsic
self.se3 = se3
self.se3_inv = SE3.invert(se3)
self.origin_ws = SE3.extract_translation(self.se3_inv)