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