def transformation_error(**kwargs):
root_dir = kwargs.get('root_dir')
descriptor_type = kwargs.get('descriptor_type')
radius = kwargs.get('radius', 30)
percentile = kwargs.get('percentile', 99)
nr_iterations = kwargs.get('nr_iterations', 500)
aux_output_string = kwargs.get('aux_output_string', "")
descriptor_string = kwargs.get('descriptor_string', "descriptors")
gt_fname = kwargs.get('gt_fname', "Hs.txt")
geo_tfile = kwargs.get('geo_tfile', "")
compute_scale = kwargs.get('compute_scale', False)
src_features_dir = root_dir + "/" + descriptor_type + "_" + str(radius)
#************GEO**************"
#load the geo tranformation
GEO = reg3d_T.geo_transformation(geo_tfile)
#************Hs**************#
#read source to target "Ground Truth" Transformation
Tfile = root_dir + "/" + gt_fname;
GT_Tform = reg3d_T.gt_transformation(Tfile)
#************PCL IA and ICP**************
#read source to target "Initial Alignment" Transformation
Tfile_ia = src_features_dir + "/ia_transformation_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string + ".txt";
Tfile_icp = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string + ".txt"
REG_Tform = reg3d_T.pcl_transformation(Tfile_ia, Tfile_icp, (not compute_scale))
#Initial Alignment errors
#Rotation error - half angle between the normalized quaternions
quat_ia = tf.unit_vector(tf.quaternion_from_matrix(REG_Tform.Rs_ia));
Rs_error_ia_norm = math.acos(abs(np.dot(quat_ia, GT_Tform.quat)))* 180/np.pi;
#Translation error
# x = REG_Tform.Rs_ia*x_ia + Ts_ia = REG_Tform.Rs_ia(x_ia + np.dot(REG_Tform.Rs_ia.T(), Ts_ia)
# np.dot(REG_Tform.Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_ia = (REG_Tform.Rs_ia.T).dot(REG_Tform.Ts_ia) - (GT_Tform.Rs.T).dot(GT_Tform.Ts)
Ts_error_ia_norm = GEO.scale_geo*GT_Tform.scale*LA.norm(Ts_error_ia)
scale_error_ia = 1.0 - REG_Tform.scale_ia/GT_Tform.scale
print "Error (S,R,T)", scale_error_ia, Rs_error_ia_norm , Ts_error_ia_norm
#ICP errors
#Rotation error - half angle between the normalized quaternions
quat_icp = tf.unit_vector(tf.quaternion_from_matrix(REG_Tform.Rs_icp))
Rs_error_icp_norm = math.acos(abs(np.dot(quat_icp, GT_Tform.quat)))* 180/np.pi
#Translation error
# x = REG_Tform.Rs_ia*x_ia + Ts_ia = REG_Tform.Rs_ia(x_ia + np.dot(REG_Tform.Rs_ia.T(), Ts_ia)
# np.dot(REG_Tform.Rs_ia.T(), Ts_ia) correspond to trans on ia coordinate system
Ts_error_icp = (REG_Tform.Rs_icp.T).dot(REG_Tform.Ts_icp) - (GT_Tform.Rs.T).dot(GT_Tform.Ts)
Ts_error_icp_norm = GEO.scale_geo*GT_Tform.scale*LA.norm(Ts_error_icp)
scale_error_icp = 1.0 - (REG_Tform.scale_icp*REG_Tform.scale_ia)/GT_Tform.scale
print "Error (S, R,T)", scale_error_icp, Rs_error_icp_norm , Ts_error_icp_norm
IA_error = np.array([scale_error_ia, Rs_error_ia_norm, Ts_error_ia_norm])
ICP_error = np.array([scale_error_icp, Rs_error_icp_norm, Ts_error_icp_norm])
# import code; code.interact(local=locals())
return IA_error, ICP_error
def compute_geo_accuracy(fid_path, original_corrs_path,
geo_tform, trials_root, desc_name,
niter, ntrials, percentile=99):
#Load fiducial .ply
fid = open(fid_path, 'r')
fid_points = np.genfromtxt(fid, delimiter=' ',
skip_header=9)
fid.close()
#Load original corrs .ply
fid = open(original_corrs_path, 'r')
original_corrs = np.genfromtxt(fid, delimiter=' ', skip_header=9)
fid.close()
#load the geo tranformation
GEO = reg3d_T.geo_transformation(geo_tform);
#Compute the "reference error"
#i.e. fiducial points - geo registered correspondances
npoints, c = fid_points.shape
if npoints != 30:
LOG.warn("Number of fiducial point is NOT 30")
if c != 3:
LOG.error("Fiducial points has the wrong number of dimensions")
# import code; code.interact(local=locals())
fid_points_hom = np.hstack((fid_points, np.ones([npoints, 1]))).T
original_corrs_hom = np.hstack((original_corrs, np.ones([npoints, 1]))).T
geo_corrs_hom = GEO.transform_points(original_corrs_hom)
geo_ref_diff = geo_corrs_hom - fid_points_hom
# import pdb; pdb.set_trace()
delta_z = (geo_ref_diff[2, :] **2) ** (1./2.)
delta_r = (geo_ref_diff[0, :] **2 + geo_ref_diff[1, :] **2 )** (1./2.)
delta_z_ia = np.zeros([ntrials, npoints])
delta_r_ia = np.zeros([ntrials, npoints])
delta_z_icp = np.zeros([ntrials, npoints])
delta_r_icp = np.zeros([ntrials, npoints])
for trial in range(0, ntrials):
print "********Trial", trial, "**********"
#Load the transformations for this trial
#************Hs**************#
#read source to target "Ground Truth" Transformation
Tfile = trials_root + "/trial_" + str(trial) + "/Hs_inv.txt"
GT_Tform = reg3d_T.gt_transformation(Tfile)
src_features_dir = (trials_root + "/trial_" + str(trial) +
"/" + desc_name)
Tfile_ia = (src_features_dir + "/ia_transformation_" +
str(percentile) + "_" + str(niter) + ".txt")
Tfile_icp = (src_features_dir + "/icp_transformation_" +
str(percentile) + "_" + str(niter) + ".txt")
REG_Tform = reg3d_T.pcl_transformation(Tfile_ia, Tfile_icp)
Hs_ia_error = REG_Tform.Hs_ia.dot(GT_Tform.Hs)
Hs_icp_error = REG_Tform.Hs_icp.dot(GT_Tform.Hs)
# transform the points with the residual transformations
ia_corrs_hom = Hs_ia_error.dot(original_corrs_hom)
icp_corrs_hom = Hs_icp_error.dot(original_corrs_hom)
# geo-register
geo_ia_corrs_hom = GEO.transform_points(ia_corrs_hom)
geo_icp_corrs_hom = GEO.transform_points(icp_corrs_hom)
# distances
geo_ia_ref_diff = geo_ia_corrs_hom - fid_points_hom
geo_icp_ref_diff = geo_icp_corrs_hom - fid_points_hom
delta_z_ia[trial, :] = np.sqrt(geo_ia_ref_diff[2, :] ** 2)
delta_r_ia[trial, :] = np.sqrt(geo_ia_ref_diff[0, :] ** 2 +
geo_ia_ref_diff[1, :] ** 2 )
delta_z_icp[trial, :] = np.sqrt(geo_icp_ref_diff[2, :] ** 2)
delta_r_icp[trial, :] = np.sqrt(geo_icp_ref_diff[0, :] ** 2 +
geo_icp_ref_diff[1, :] ** 2)
# import pdb; pdb.set_trace()
return delta_z, delta_r,\
delta_z_ia, delta_r_ia, \
delta_z_icp, delta_r_icp