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
vi = 0 bboxF2 = np.zeros((4,8)) for i in range(0,len(XF2)): for j in range(0,len(YF2)): for k in range(0,len(ZF2)): bboxF2[:,vi] = np.array([XF2[i],YF2[j],ZF2[k],1]) vi = vi+1 maxF2 = np.max(bboxF2, axis=1) meanF2 = np.mean(bboxF2, axis=1) # import pdb; pdb.set_trace() #Load f4-f2 f4_f2_file = cvg_root + "/flight4_sites/site_2/f4-f2_Hs.txt" f4_f2 = reg3d_T.gt_transformation(f4_f2_file) meanF4 = LA.inv(f4_f2.Hs).dot(meanF2) maxF4 = LA.inv(f4_f2.Hs).dot(maxF2) #Load f5-f2 f5_f2_file = cvg_root + "/flight5_sites/site_2/f5-f2_Hs.txt" f5_f2 = reg3d_T.gt_transformation(f5_f2_file) meanF5 = LA.inv(f5_f2.Hs).dot(meanF2) maxF5 = LA.inv(f5_f2.Hs).dot(maxF2) #Crop and threshold f2 sceneroot = cvg_root + "/flight2_sites/site_2"; file_in = sceneroot + "/gauss_233_normals.ply";
print "Output dir not found - creating one ", dir_out os.makedirs(dir_out) #read the centroid fid = open(centroid_file, 'r') centroid = np.genfromtxt(fid); fid.close() print "Centroid: " print centroid #read the GEO transfomation tform_file_out = dir_out + "/Hs_geo" GEO_Tform = reg3d_T.gt_transformation(geo_Tfile); print "Initial Translation: " print GEO_Tform.Ts print "Initial scale: " print GEO_Tform.scale GEO_Tform.Ts = GEO_Tform.Ts - centroid/GEO_Tform.scale print "Demeaned Translation: " print GEO_Tform.Ts GEO_Tform.Hs[:3, 3] = GEO_Tform.Hs[:3, 3] - centroid print "Demeaned Transfomation: " print GEO_Tform.Hs
percent = 95; for site in sites: for trial in range(0,5): print "Processing Site: " + site + " Trial: " + str(trial) noise_Tfile_base = reg_dir + "/ground_truth/rand_t/H_" + str(trial) inv_noise_Tfile_base = noise_Tfile_base + "_inv" noise_Tfile = noise_Tfile_base + ".txt" matrix_noise_Tfile = reg_dir + "/ground_truth/rand_t/H_" + str(trial) +"_matrix.txt" noise_Tform = reg3d_T.gt_transformation(noise_Tfile); #save as matrix noise_Tform.save_to_file(noise_Tfile_base) #inverse transformation inv_noise_Tform = reg3d_T.gt_transformation(inv(noise_Tform.Hs)) inv_noise_Tform.save_to_file(inv_noise_Tfile_base) # S_inv = (1.0/noise_Tform.scale); # R_inv = noise_Tform.Rs.T; # T_inv = R_inv.dot((noise_Tform.scale*noise_Tform.Ts)); # print inv_noise_Tform.Hs # print S_inv # print R_inv
# 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") #******************** 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")
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
print "Processing Site: " + site + " Trial: " + str(trial) pc_aerial_file = reg_dir + "/data/point_clouds_aerial/" + site + "/gauss_233_normals_pvn_cropped_"+ str(percent)+ ".ply" geo_Tfile = reg_dir + "/ground_truth/aerial-lidar-demeaned/" + site + "/Hs_geo.txt" noise_Tfile = reg_dir + "/ground_truth/rand_t/H_" + str(trial) +".txt" dir_out = file_out = reg_dir + "/data/point_clouds_aerial_geo_noise/" + site + "/H_" + str(trial) if not os.path.exists(dir_out): print "Output dir not found - creating one ", dir_out os.makedirs(dir_out) file_out = dir_out + "/gauss_233_normals_pvn_cropped_"+ str(percent)+ ".ply" tform_file_out = dir_out +"/Hs" GEO_Tform = reg3d_T.gt_transformation(geo_Tfile); noise_Tform = reg3d_T.gt_transformation(noise_Tfile); final_Hs = noise_Tform.Hs.dot(GEO_Tform.Hs) final_Tform = reg3d_T.gt_transformation(final_Hs) #save the transform final_Tform.save_to_file(tform_file_out); #Load original corrs .ply fid = open(pc_aerial_file, 'r') pc_aerial = np.genfromtxt(fid, dtype=float, delimiter=' ', skip_header=16, usecols={0, 1, 2}); fid.close()
# #Load f4-f2 # 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()
for i in range(0,len(XCVGF4)): for j in range(0,len(YCVGF4)): for k in range(0,len(ZCVGF4)): bboxCVGF4[:,vi] = np.array([XCVGF4[i],YCVGF4[j],ZCVGF4[k],1]) vi = vi+1 minCVGF4 = np.min(bboxCVGF4, axis=1) maxCVGF4 = np.max(bboxCVGF4, axis=1) print "CVGF4" print minCVGF4 print maxCVGF4 #Load f4-CVGF4 f4_2006_file = cvg_root + "/f4-2006_Hs.txt" f4_2006 = reg3d_T.gt_transformation(f4_2006_file) bbox2006 = (f4_2006.Hs).dot(bboxCVGF4) min2006 = np.min(bbox2006, axis=1) max2006 = np.max(bbox2006, axis=1) print "2006" print bbox2006 print bbox2006 #Crop and threshold CVGF4 sceneroot = cvg_root; file_in = sceneroot + "/gauss_233_normals.ply"; file_out = sceneroot + "/gauss_233_normals_cropped.ply" if thresh:
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