def post_process_in_mem(self, se3):
rot = SE3.extract_rotation(se3)
euler = SE3.rotationMatrixToEulerAngles(rot)
rot_new = SE3.makeS03(euler[0], euler[1], -euler[2])
se3[0:3, 0:3] = rot_new
#se3[0, 3] = -se3[0, 3]
se3[1, 3] = -se3[1, 3]
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 post_process_in_mem(self, se3):
x = se3[0, 3]
y = se3[1, 3]
z = se3[2, 3]
rot = SE3.extract_rotation(se3)
euler = SE3.rotationMatrixToEulerAngles(rot)
rot_new = SE3.makeS03(euler[2], -euler[1], euler[0])
#se3[0:4,0:4] = se3_new[0:4,0:4]
se3[0:3, 0:3] = rot_new[0:3, 0:3]
se3[0, 3] = z
#se3[1, 3] = y
se3[2, 3] = -x
def post_process_in_mem(self, se3):
rot = SE3.extract_rotation(se3)
euler = SE3.rotationMatrixToEulerAngles(rot)
#rot_new = SE3.makeS03(euler[1], -euler[2], euler[0])
rot_new = SE3.makeS03(euler[0], euler[2], euler[1])
#se3[0:3, 0:3] = rot_new
x = se3[0, 3]
y = se3[1, 3]
z = se3[2, 3]
#se3[0, 3] = -y
#se3[1, 3] = z
#se3[2, 3] = -x
se3[0, 3] = x
se3[1, 3] = z
se3[2, 3] = y
def render(self):
(height, width) = self.resolution
for x in range(0, width):
for y in range(0, height):
ray_direction_camera_space = self.camera.camera_ray_direction_camera_space(
x, y, width, height, self.fov)
camera_to_world = self.camera.se3_inv
rot = SE3.extract_rotation(camera_to_world)
ray_world_space = Utils.normalize(
np.matmul(rot, ray_direction_camera_space))
ray = Geometry.Ray(self.camera.origin_ws[0:3], ray_world_space)
(b, t, sphere) = self.find_closest_intersection(ray)
if sphere.is_intersection_acceptable(b, t):
intersection_point = Geometry.point_for_ray(ray, t)
normal = sphere.normal_for_point(intersection_point)
depth = intersection_point[2]
self.depth_buffer[y, x] = fabs(depth)
self.frame_buffer[y, x] = phong_shading(
self.light_ws, intersection_point, normal)
# We only need the gradients of the target frame
frame_reference = Frame.Frame(im_greyscale_reference, depth_reference,
camera_reference, False)
frame_target = Frame.Frame(im_greyscale_target, depth_target, camera_target,
True)
#visualizer = Visualizer.Visualizer(photometric_solver)
SE3_est = Solver.solve_photometric(frame_reference,
frame_target,
threadLock=None,
pose_estimate_list=None,
max_its=20000,
eps=0.001,
alpha_step=1.0,
gradient_monitoring_window_start=5.0,
image_range_offset_start=0,
twist_prior=None,
hessian_prior=None,
use_ndc=use_ndc,
use_robust=True,
track_pose_estimates=False,
debug=False)
euler_angles_XYZ = SE3.rotationMatrixToEulerAngles(
SE3.extract_rotation(SE3_est))
print(SE3_est)
print(Utils.radians_to_degrees(euler_angles_XYZ[0]),
Utils.radians_to_degrees(euler_angles_XYZ[1]),
Utils.radians_to_degrees(euler_angles_XYZ[2]))
threadLock=None,
pose_estimate_list=None,
max_its=20000,
eps=0.0005,
alpha_step=1.0,
gradient_monitoring_window_start=20,
image_range_offset_start=0,
twist_prior=None,
hessian_prior=None,
use_ndc=use_ndc,
use_robust=True,
track_pose_estimates=False,
debug=False)
euler_angles_XYZ = SE3.rotationMatrixToEulerAngles(
SE3.extract_rotation(SE3_est))
euler_angles_gt_XYZ = SE3.rotationMatrixToEulerAngles(
SE3.extract_rotation(SE3_ref_target))
print('*' * 80)
print('GROUND TRUTH\n')
print(SE3_ref_target)
print(Utils.radians_to_degrees(euler_angles_gt_XYZ[0]),
Utils.radians_to_degrees(euler_angles_gt_XYZ[1]),
Utils.radians_to_degrees(euler_angles_gt_XYZ[2]))
print('*' * 80)
print(SE3_est)
print(Utils.radians_to_degrees(euler_angles_XYZ[0]),
Utils.radians_to_degrees(euler_angles_XYZ[1]),
Utils.radians_to_degrees(euler_angles_XYZ[2]))
#depth_target /= depth_factor
#depth_reference /= depth_factor
se3_identity = np.identity(4, dtype=Utils.matrix_data_type)
# fx and fy affect the resulting coordiante system of the se3 matrix
intrinsic_identity = Intrinsic.Intrinsic(-1, -1, image_width/2, image_height/2)
if use_ndc:
intrinsic_identity = Intrinsic.Intrinsic(-1, -1, 1/2, 1/2) # for ndc
# reference frame is assumed to be the origin
# target frame SE3 is unknown i.e. what we are trying to solve
camera_reference = Camera.Camera(intrinsic_identity, se3_identity)
camera_target = Camera.Camera(intrinsic_identity, se3_identity)
# We only need the gradients of the target frame
frame_reference = Frame.Frame(im_greyscale_reference, depth_reference, camera_reference, False)
frame_target = Frame.Frame(im_greyscale_target, depth_target, camera_target, True)
SE3_est = Solver.solve_photometric(frame_reference, frame_target, 20000, 0.33, alpha_step=1.0, use_ndc = use_ndc, debug = False)
euler_angles_XYZ = SE3.rotationMatrixToEulerAngles(SE3.extract_rotation(SE3_est))
print(SE3_est)
print(Utils.radians_to_degrees(euler_angles_XYZ[0]),
Utils.radians_to_degrees(euler_angles_XYZ[1]),
Utils.radians_to_degrees(euler_angles_XYZ[2]))
