예제 #1
0
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"
예제 #2
0
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
예제 #3
0
  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 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"