Example #1
0
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
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