def register_icp(**kwargs):
gt_root_dir = kwargs.get('gt_root_dir')
trial_root_dir = kwargs.get('trial_root_dir')
descriptor_type = kwargs.get('descriptor_type')
radius = kwargs.get('radius', 30)
percentile = kwargs.get('percentile' ,99)
nr_iterations = kwargs.get('nr_iterations', 200)
rej_normals = kwargs.get('rej_normals', False)
compute_scale = kwargs.get('compute_scale', False)
use_max_nr_iter = kwargs.get('use_max_nr_iter', False)
verbose = kwargs.get('verbose', True)
aux_output_string = kwargs.get('aux_output_string', "")
descriptor_string = kwargs.get('descriptor_string', 'descriptors')
basename_in = kwargs.get('basename_in',"gauss_233_normals_pvn")
for key in kwargs:
print "Arguments: %s: %s" % (key, kwargs[key])
#path to where all scenes are
src_scene_root=trial_root_dir;
tgt_scene_root=gt_root_dir;
src_features_dir = src_scene_root + "/" + descriptor_type + "_" + str(radius);
src_fname = src_features_dir + "/ia_cloud_" + str(percentile) +"_" + str(nr_iterations) + "_" + aux_output_string + ".pcd";
tgt_fname = tgt_scene_root + "/" + basename_in + "_" + str(percentile) + ".ply"
tgt_features_dir = tgt_scene_root + "/" + descriptor_type + "_" + str(radius);
if rej_normals:
output_cloud_fname = src_features_dir + "/icp_cloud_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string +"_n.pcd"
tform_fname = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string +"_n.txt"
else:
output_cloud_fname = src_features_dir + "/icp_cloud_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string + ".pcd"
tform_fname = src_features_dir + "/icp_transformation_" + str(percentile) + "_" + str(nr_iterations) + "_" + aux_output_string + ".txt"
tgt_scene_info = tgt_scene_root + "/scene_info.xml"
tgt_scene_res = parse_scene_resolution(tgt_scene_info);
if not verbose:
py_vpcl.set_stdout(src_features_dir+ "/log_icp_" + str(percentile) +"_" + str(nr_iterations) + "_" + aux_output_string +'.log')
max_dist = 4*radius*tgt_scene_res;
translation_threshold = 0.001 * tgt_scene_res #used to be 0.1
rotation_threshold = 0.001; #0.1 degree
#set this flag to let ICP run for 500 - nr_iterations is used for naming conventions, but ignored here
if use_max_nr_iter:
nr_iterations = 500
vpcl_adaptor.register_icp( srcFname = src_fname,
tgtFname = tgt_fname,
outCloud = output_cloud_fname,
tformFname = tform_fname,
maxCorrDist = max_dist,
epsTrans = translation_threshold,
epsRot = rotation_threshold,
numIter = nr_iterations,
rejectNormals = rej_normals,
computeScale = compute_scale);
if not verbose:
py_vpcl.reset_stdout();
py_vpcl.clear();
print "Done with ICP"
def register_ia(**kwargs):
gt_root_dir = kwargs.get('gt_root_dir')
trial_root_dir = kwargs.get('trial_root_dir')
descriptor_type = kwargs.get('descriptor_type')
radius = kwargs.get('radius', 30)
percentile = kwargs.get('percentile' ,99)
nr_iterations = kwargs.get('nr_iterations', 200)
nsamples = kwargs.get('nsamples', 3)
min_sample_distance = kwargs.get('sample_distance', 1)
#min_sample_distance gets multiplied by radius and resolution
compute_scale = kwargs.get('compute_scale', False)
verbose = kwargs.get('verbose', True)
aux_output_string = kwargs.get('aux_output_string', "")
descriptor_string = kwargs.get('descriptor_string', 'descriptors')
basename_in = kwargs.get('basename_in',"gauss_233_normals_pvn")
gt_fname = kwargs.get('gt_fname',"Hs.txt")
scale = kwargs.get('scale', 0)
bound_scale = kwargs.get('bound_scale', False)
bound_percentile = kwargs.get('bound_percentile', 100)
for key in kwargs:
print "Arguments: %s: %s" % (key, kwargs[key])
print descriptor_string
print aux_output_string
max_scale=0;
min_scale=0;
#read the scale from file
if(not compute_scale and scale == 0):
Tfile = trial_root_dir + "/" + gt_fname
try:
Tfis = open(Tfile, 'r')
except:
scale = 1
print "Failed to read " , Tfile
else:
lines = []
lines = Tfis.readlines()
scale = float(lines[0])
Tfis.close()
print "Scale read from file: " , scale
if compute_scale and bound_scale:
Tfile = trial_root_dir + "/" + gt_fname
try:
Tfis = open(Tfile, 'r')
except:
scale = 1
print "Failed to read " , Tfile
else:
lines = []
lines = Tfis.readlines()
scale = float(lines[0])
Tfis.close()
print "Scale read from file: ", scale
max_scale = scale + scale*bound_percentile*(1.0/100.0)
min_scale = scale - scale*bound_percentile*(1.0/100.0)
print "Scale bounds Percentile: " , bound_percentile
print "Min Scale: ", min_scale
print "max_scale: ", max_scale
print "Using Scale: " , scale
#path to where all scenes are
src_scene_root=trial_root_dir
tgt_scene_root=gt_root_dir
src_fname = src_scene_root + "/" + basename_in + "_" + str(percentile) + ".ply"
src_features_dir = src_scene_root + "/" + descriptor_type + "_" + str(radius)
src_features_fname = src_features_dir + "/" + descriptor_string + "_" + str(percentile) + ".pcd"
tgt_fname = tgt_scene_root + "/" + basename_in + "_" + str(percentile) + ".ply"
tgt_features_dir = tgt_scene_root + "/" + descriptor_type + "_" + str(radius)
tgt_features_fname = tgt_features_dir + "/" + descriptor_string + "_" + str(percentile) + ".pcd"
output_cloud_fname = src_features_dir + "/ia_cloud_" + str(percentile) +"_" + str(nr_iterations) + "_" + aux_output_string + ".pcd";
tform_fname = src_features_dir + "/ia_transformation_" + str(percentile) +"_" + str(nr_iterations) + "_" + aux_output_string + ".txt";
tgt_scene_info = tgt_scene_root + "/scene_info.xml"
tgt_scene_res = parse_scene_resolution(tgt_scene_info)
print "Here"
if not verbose:
py_vpcl.set_stdout(src_features_dir+ "/log_ia_" + str(percentile) +"_" + str(nr_iterations) + "_" + aux_output_string +'.log')
#****PARAMETERS*******#
min_sample_distance = min_sample_distance*radius*tgt_scene_res
max_dist = 4*radius*tgt_scene_res
ransac_scale, avg_scale = vpcl_adaptor.register_ia_sac( srcFname = src_fname,
tgtFname = tgt_fname,
srcFeatures = src_features_fname,
tgtFeatures = tgt_features_fname,
outCloud = output_cloud_fname,
tformFname = tform_fname,
descType = descriptor_type,
minSampleDist= min_sample_distance,
maxCorrDist = max_dist,
numIter = nr_iterations,
numSamples = nsamples,
computeScale = compute_scale,
scale = scale,
boundScale = bound_scale,
minScale = min_scale,
maxScale = max_scale)
if not verbose:
py_vpcl.reset_stdout();
py_vpcl.clear();
print "Done with SAC_IA";
return ransac_scale, avg_scale
nr_iterations = 500
vpcl_adaptor.register_icp( srcFname = src_fname,
tgtFname = tgt_fname,
outCloud = output_cloud_fname,
tformFname = tform_fname,
maxCorrDist = max_dist,
epsTrans = translation_threshold,
epsRot = rotation_threshold,
numIter = nr_iterations,
rejectNormals = rej_normals,
computeScale = compute_scale);
if not verbose:
py_vpcl.reset_stdout();
py_vpcl.clear();
print "Done with ICP"
def visualize_reg_ia(gt_root_dir,trial_root_dir, descriptor, radius = 30,
percentile = 99, nr_iterations=200, geo=False,
aux_output_string = "", basename_in="gauss_233_normals_pvn"):
if geo:
tgtRoot=gt_root_dir
tgt_cloud= tgtRoot+ "/" + basename_in +"_" +str(percentile) + "_XYZ_geo.pcd"
src_cloud=trial_root_dir + "/" + descriptor + "_" + str(radius) + "/ia_cloud_" + str(percentile) + "_" + str(nr_iterations) + "_geo.pcd"
else:
tgtRoot=gt_root_dir
tgt_cloud=tgtRoot + "/" + basename_in +"_" + str(percentile) + ".ply"
src_cloud=trial_root_dir + "/" + descriptor + "_" + str(radius) + "/ia_cloud_" + str(percentile) + "_" + str(nr_iterations) + "_" +aux_output_string + ".pcd"