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 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
# 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]))
frame_target,
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]))