def transformation_error_general(**kwargs):
fname = kwargs.get('fname') #filename trans. file
gt_fname = kwargs.get('gt_fname', "Hs.txt") #filename ground truth trans. file
geo_fname = kwargs.get('geo_fname', "")
#************Hs**************#
#read source to target "Ground Truth" Transformation
GT_Tform = reg3d_T.gt_transformation(gt_fname)
#**************************
#read source to target estimated Transformation
Tform = reg3d_T.gt_transformation(fname)
#load the geo tranformation
print "geo_fname", geo_fname
if geo_fname == "":
geo_scale = 1;
else:
GEO = reg3d_T.geo_transformation(geo_fname)
geo_scale = GEO.scale_geo;
#Initial Alignment errors
#Rotation error - half angle between the normalized quaternions
Rs_error_norm = math.acos(abs(np.dot(Tform.quat, GT_Tform.quat)))* 180/np.pi;
#Translation error
# import code; code.interact(local=locals())
Ts_error = (Tform.Rs.T).dot(Tform.Ts) - (GT_Tform.Rs.T).dot(GT_Tform.Ts)
Ts_error_norm = geo_scale*GT_Tform.scale*LA.norm(Ts_error)
scale_error = 1.0 - Tform.scale/GT_Tform.scale
print "Error (S,R,T)", scale_error, Rs_error_norm , Ts_error_norm
error = np.array([scale_error, Rs_error_norm, Ts_error_norm])
# import code; code.interact(local=locals())
return error
# Tgeo_2011 = reg3d_T.gt_transformation(Hs_geo_2011) # Tgeo_2011.save_to_file(root_2011 + "/Hs_geo") #******************** Downtown ******************** root_cvg ="/Users/isa/Experiments/reg3d_eval/cvg_eo_data/flight5_sites/site_5" # root_2011 = "/Users/isa/Experiments/reg3d_eval/downtown_2011/original" root_2006 = "/Users/isa/Experiments/reg3d_eval/downtown_2006/original" # #Load 2011-2006 # Hfile_2011_2006 = root_2011 + "/2011-2006_Hs.txt" # T_2011_2006 = reg3d_T.gt_transformation(Hfile_2011_2006) Hfile_cvg_2006 = root_cvg + "/f5-2006_Hs.txt" T_cvg_2006 = reg3d_T.gt_transformation(Hfile_cvg_2006) #Load 2006-lidar Hfile_geo_2006 = "/data/lidar_providence/downtown_offset-1-financial-dan-Hs.txt" Tgeo_2006 = reg3d_T.geo_transformation(Hfile_geo_2006) # Hs_geo_2011 = Tgeo_2006.Hs_geo.dot(T_2011_2006.Hs) # Tgeo_2011 = reg3d_T.gt_transformation(Hs_geo_2011) # Tgeo_2011.save_to_file(root_2011 + "/Hs_geo") Hs_geo_cvg = Tgeo_2006.Hs_geo.dot(T_cvg_2006.Hs) Tgeo_cvg = reg3d_T.gt_transformation(Hs_geo_cvg) Tgeo_cvg.save_to_file(root_cvg + "/Hs_geo") # import pdb; pdb.set_trace()
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
# f4_f2_file = cvg_root + "/flight4_sites/site_1/f4-f2_Hs.txt" # f4_f2 = reg3d_T.gt_transformation(f4_f2_file) # #Load f4-2006 # f4_2006_file = cvg_root + "/flight4_sites/site_1/f4-2006_Hs.txt" # f4_2006 = reg3d_T.gt_transformation(f4_2006_file) # #Load cvg-f4-2006 # f4_2006_file = cvg_root + "/flight4_sites/site_1/f4-2006_Hs.txt" # f4_2006 = reg3d_T.gt_transformation(f4_2006_file) #Load vsi-2006 this is only used for vishal's paper set up f4_dan_file = vsi_root + "/vsi-2006_Hs.txt" f4_dan = reg3d_T.gt_transformation(f4_dan_file) dan_lidar_file = dan_root + "/Hs_geo.txt" dan_geo = reg3d_T.geo_transformation(dan_lidar_file) vsi_geo_Hs = dan_geo.Hs_geo.dot(f4_dan.Hs) vsi_geo = reg3d_T.gt_transformation(vsi_geo_Hs) vsi_geo.save_to_file(vsi_root + "/Hs_geo.txt") # #Compute missing ones # #2006-lidar # dan_geo_Hs = f2_geo.Hs_geo.dot(f4_f2.Hs.dot(LA.inv(f4_2006.Hs))) # dan_geo = reg3d_T.gt_transformation(dan_geo_Hs) # dan_geo.save_to_file(dan_root + "/Hs_geo") # import pdb; pdb.set_trace() #******************** Capitol ********************
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