Beispiel #1
0
def edge_quality(n, edges, Tstar, R):
    good_edges = 0.0
    bad_edges = 0.0
    err_bad = 0.0
    err_good = 0.0
    for edge in edges:
        if edge['weight'] < 0.01:
            continue
        sid = edge['src']
        tid = edge['tgt']
        Ti = Tstar[sid]
        Tj = Tstar[tid]
        Tij_gt = Tj.dot(inverse(Ti))
        Rij_in = edge['R']
        tij_in = edge['t']
        Tij_in = pack(Rij_in, tij_in)
        aerr_gt = angular_distance_np(Tij_in[np.newaxis, :3, :3],
                                      Tij_gt[np.newaxis, :3, :3]).sum()
        Rij = R[tid].dot(R[sid].T)
        aerr = np.linalg.norm(Rij - Rij_in, 'fro')**2
        if aerr_gt > 30.0:
            bad_edges += edge['weight']
            err_bad += aerr * edge['weight']
        else:
            good_edges += edge['weight']
            err_good += aerr * edge['weight']
    print('Edge Quality: #good=%f, #bad=%f, mean aerr=(%f, %f)' %
          (good_edges, bad_edges, err_good / good_edges, err_bad / bad_edges))
Beispiel #2
0
def main():
    depth_paths, T, pose_paths = getData(args.shapeid)
    n = len(depth_paths)
    print('found %d clean depth images...' % n)
    intrinsic = np.array([[525.0,0,319.5],[0,525.0,239.5],[0,0,1]])
    np.random.seed(816)
    indices = np.random.permutation(n)
    print(indices[:100])
    #indices = sorted(indices)
    make_dirs(PATH_MAT.format(args.shapeid, 0))
    import open3d
    pcd_combined = open3d.PointCloud()
    for i, idx in enumerate(indices):
        import ipdb; ipdb.set_trace()
        print('%d / %d' % (i, len(indices)))
        mesh = Mesh.read(depth_paths[idx], mode='depth', intrinsic = intrinsic)
        pcd = open3d.PointCloud()
        pcd.points = open3d.Vector3dVector(mesh.vertex.T)
        pcd.transform(inverse(T[idx]))
        #pcd = open3d.voxel_down_sample(pcd, voxel_size=0.02)
        pcd_combined += pcd
        pcd_combined = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
        sio.savemat(PATH_MAT.format(args.shapeid, i), mdict={
            'vertex': mesh.vertex, 
            'validIdx_rowmajor': mesh.validIdx, 
            'pose': T[idx], 
            'depth_path': depth_paths[idx], 
            'pose_path': pose_paths[idx]})
        if i <= 50 and i >= 40:
            pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
            open3d.draw_geometries([pcd_combined_down])
            

    pcd_combined_down = open3d.voxel_down_sample(pcd_combined, voxel_size=0.02)
    open3d.draw_geometries([pcd_combined_down])
Beispiel #3
0
    def __init__(self, center, radius, normal, I, scene, loops = 1, pitch = 1):
        Shape.__init__(self, scene)

        self.center = center
        self.radius = radius
        self.normal = normal

        self.loops = loops
        self.pitch = pitch
        self.length = loops*pitch

        self.I = I

        # some consonants
        self.C = mu_0*I/pi
        self.oner = [[1,0,0], [0,1,0], [0,0,1]]

        # rotation matrices for this coil, rotating so norm becomes z axis
        self.rotate = self.find_rotmatrix(normal.norm(), vector(0, 0, 1))
        self.antiRotate = inverse(self.rotate)


        if loops == 1:
            self.obj = ring(pos = center, axis = normal*self.length,
                    radius = radius, thickness = 0.1, display = scene,
                    material = materials.chrome)
        else:
            self.obj = helix(pos = center, axis = normal*self.length,
                    radius = radius, coils = loops, thickness = 0.1,
                    display = scene, material = materials.chrome)
Beispiel #4
0
def error(T, G):
    aerrs = []
    n = T.shape[0]
    terrs = []
    for i in range(n):
        for j in range(i + 1, n):
            Ti = T[i, :, :]
            Tj = T[j, :, :]
            Gi = G[i, :, :]
            Gj = G[j, :, :]
            Tij = Tj.dot(inverse(Ti))
            Gij = Gj.dot(inverse(Gi))
            Rij = Tij[:3, :3]
            Rij_gt = Gij[:3, :3]
            fro = np.linalg.norm(Rij - Rij_gt, 'fro')
            aerr = angular_distance_np(Rij[np.newaxis, :, :],
                                       Rij_gt[np.newaxis, :, :]).sum()
            terr = np.linalg.norm(Tij[:3, 3] - Gij[:3, 3], 2)
            aerrs.append(aerr)
            terrs.append(terr)

    return np.mean(aerrs), np.mean(terrs)
Beispiel #5
0
def __get_label__(Rij, tij, Ti, Tj):
    """ Measure Quality of Edge """
    Ristar, tistar = decompose(Ti)
    Rjstar, tjstar = decompose(Tj)
    Tij_gt = Tj.dot(inverse(Ti))
    Tij_in = pack(Rij, tij)

    label = 0.0
    err_R = angular_distance_np(Rij[np.newaxis, :, :],
                                Rjstar.dot(Ristar.T)[np.newaxis, :, :]).sum()
    err_T = np.linalg.norm(Tij_gt[:3, 3] - Tij_in[:3, 3], 2)
    if (err_R < 30.0) and (err_T < 0.2):
        label = 1.0
    else:
        label = 0.0
    return label
def getData(shapeid):
    depth_paths = []
    poses = []
    pose_paths = []
    frames = glob.glob(PATH_DEPTH.format(shapeid, '*'))
    frames.sort()
    for i, frame in enumerate(frames):
        frameid = frame.split('/')[-1].split('.')[0]
        depth_path = PATH_DEPTH.format(shapeid, frameid)
        #tmp = cv2.resize(cv2.imread(imgsPath, 2)/1000., (64,64))
        #AuthenticdepthMap.append(tmp.reshape(1,tmp.shape[0],tmp.shape[1],1))

        pose_fp = PATH_POSE.format(shapeid, frameid)
        flag = True
        try:
            tmp = np.loadtxt(pose_fp)
            assert abs(tmp[3, 3] -
                       1.0) < 1e-4, 'bottom right corner should be one'
            assert (abs(tmp[3, :3]) < 1e-4).all(), '[3, :3] should be zero'
            R = tmp[:3, :3]
            assert np.linalg.det(R) > 0.01, 'determinant should be 1'
            assert np.linalg.norm(
                R.dot(R.T) - np.eye(3),
                'fro')**2 < 1e-4, 'should be a rotation matrix'
            project_R = project_so(R)
            assert np.linalg.norm(
                R - project_R,
                'fro')**2 < 1e-4, 'projection onto SO3 should be identical'
            tmp[:3, :3] = project_R
            tmp = inverse(tmp)
        except Exception as e:
            print('error on {}: {}'.format(pose_fp, e))
            #print(R.dot(R.T))
            #print(np.linalg.norm(R.dot(R.T) - np.eye(3), 'fro'))
            flag = False
        if not flag:
            print('ignoring frame {}'.format(frameid))
            continue
        poses.append(tmp)
        depth_paths.append(depth_path)
        pose_paths.append(pose_fp)
    if len(poses) == 0:
        T = None
    else:
        T = np.concatenate(poses).reshape(-1, 4, 4)
    return depth_paths, T, pose_paths
Beispiel #7
0
def compute_sigma(mat_file1, mat_file2, txt, output_mat):
  mat1 = sio.loadmat(mat_file1)
  mat2 = sio.loadmat(mat_file2)
  v1 = mat1['vertex'] # [3, n]
  v2 = mat2['vertex'] # [3, n]
  Tij = read_super4pcs(txt)
  Tij_gt = mat2['pose'].dot(inverse(mat1['pose']))
  Rij = Tij[:3, :3]
  tij = Tij[:3, 3]
  v1 = Rij.dot(v1) + tij[:, np.newaxis]
  
  tree = NN(n_neighbors=1, algorithm='kd_tree').fit(v1.T)
  distances, _ = tree.kneighbors(v2.T)
  distances = distances[distances < 0.2]
  d = {}
  d['sigma'] = np.median(distances)
  d['Tij'] = Tij
  d['aerr'] = angular_distance_np(Tij[np.newaxis, :3, :3], Tij_gt[np.newaxis, :3, :3]).sum()
  d['terr'] = np.linalg.norm(Tij[:3, 3]- Tij_gt[:3, 3], 2)
  d['src'] = mat_file1
  d['tgt'] = mat_file2
  sio.savemat(output_mat, mdict=d, do_compression=True)
Beispiel #8
0
def reweightEdges(n, edges, R, t, sigma_r, sigma_t, plot=False):
    theta1 = 4
    terrs = []
    if plot:
        plots = []
    for edge in edges:
        i = edge['src']
        j = edge['tgt']
        Rij = edge['R']
        tij = edge['t']

        Tij_in = pack(Rij, tij)
        Ti = pack(R[i], t[i])
        Tj = pack(R[j], t[j])
        Tij_rec = Tj.dot(inverse(Ti))

        weight = edge['predicted_weight']
        if weight < 0.01:
            weight = 0.00
        if weight > 1.0:
            weight = 1.0

        aerr_fro = np.linalg.norm(Tij_rec[:3, :3] - Tij_in[:3, :3],
                                  'fro') * sigmoid((0.5 - weight) * 5)
        terr_fro = np.linalg.norm(Tij_rec[:3, 3] - Tij_in[:3, 3], 2) * sigmoid(
            (0.5 - weight) * 5)
        #fro2 = aerr_fro ** 2 + terr_fro ** 2
        edge['rotation_weight'] = (sigma_r**theta1) / (sigma_r**theta1 +
                                                       aerr_fro**(theta1))
        edge['translation_weight'] = (sigma_t**theta1) / (sigma_t**theta1 +
                                                          terr_fro**(theta1))
        if plot:
            plots.append([aerr_fro, edge['rotation_weight']])
    if plot:
        plots = np.array(plots)
        import matplotlib.pyplot as plt
        plt.scatter(plots[:, 0], plots[:, 1])
        plt.show()
Beispiel #9
0
def generate_synthetic(n, sigma):
    T = np.zeros((n, 4, 4))
    X = so.rvs(dim=3, size=n)
    T[:, :3, :3] = X
    # u, sigma, v = np.linalg.svd(T[0])
    T[:, :3, 3] = np.random.randn(n, 3)
    T[0, :3, 3] = 0.0
    T[:, 3, 3] = 1
    edges = []
    for i in range(n):
        for j in range(n):
            if i <= j:
                continue
            Tij = T[j].dot(inverse(T[i]))
            Rij, tij = decompose(Tij)
            Rij = Rij + np.random.randn(3, 3) * sigma
            Rij = project_so(Rij)
            tij = tij + np.random.randn(3) * sigma
            Tij = pack(Rij, tij)
            edge = {'src': i, 'tgt': j, 'R': Rij, 't': tij, 'weight': 1.0}
            edges.append(edge)
    edges = np.array(edges)
    return n, edges, T
Beispiel #10
0
    def __init__(self, center, radius, normal, I, scene, loops=1, pitch=1):
        Shape.__init__(self, scene)

        self.center = center
        self.radius = radius
        self.normal = normal

        self.loops = loops
        self.pitch = pitch
        self.length = loops * pitch

        self.I = I

        # some consonants
        self.C = mu_0 * I / pi
        self.oner = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]

        # rotation matrices for this coil, rotating so norm becomes z axis
        self.rotate = self.find_rotmatrix(normal.norm(), vector(0, 0, 1))
        self.antiRotate = inverse(self.rotate)

        if loops == 1:
            self.obj = ring(pos=center,
                            axis=normal * self.length,
                            radius=radius,
                            thickness=0.1,
                            display=scene,
                            material=materials.chrome)
        else:
            self.obj = helix(pos=center,
                             axis=normal * self.length,
                             radius=radius,
                             coils=loops,
                             thickness=0.1,
                             display=scene,
                             material=materials.chrome)
Beispiel #11
0
def IterativeTransfSync(n,
                        edges,
                        eps0=-1,
                        decay=0.8,
                        Tstar=None,
                        max_iter=10000,
                        cheat=False,
                        scheme='reweight'):

    if scheme == 'reweight':
        reweight = True
    else:
        reweight = False

    if cheat:
        for edge in edges:
            #if edge['weight'] < 0.5:
            #    continue
            sid = edge['src']
            tid = edge['tgt']
            Ti = Tstar[sid]
            Tj = Tstar[tid]
            Tij_gt = Tj.dot(inverse(Ti))
            Rij = edge['R']
            tij = edge['t']
            Tij = pack(Rij, tij)
            aerr = angular_distance_np(Tij[np.newaxis, :3, :3],
                                       Tij_gt[np.newaxis, :3, :3]).sum()
            terr = np.linalg.norm(Tij[:3, 3] - Tij_gt[:3, 3], 2)
            #p = 0.9
            if aerr > 30.0 or terr > 0.2:
                weight = 0.0  #coin(0.02)
            else:
                weight = 1.0  #coin(0.9)
            edge['predicted_weight'] = weight
            edge['translation_weight'] = weight
            edge['rotation_weight'] = weight
    """ Edge Quality """
    good_edges = 0.0
    bad_edges = 0.0
    err_bad = 0.0
    err_good = 0.0
    for edge in edges:
        if edge['predicted_weight'] < 0.5:
            continue
        sid = edge['src']
        tid = edge['tgt']
        Ti = Tstar[sid]
        Tj = Tstar[tid]
        Tij_gt = Tj.dot(inverse(Ti))
        Rij = edge['R']
        tij = edge['t']
        Tij = pack(Rij, tij)
        aerr = angular_distance_np(Tij[np.newaxis, :3, :3],
                                   Tij_gt[np.newaxis, :3, :3]).sum()
        terr = np.linalg.norm(Tij[:3, 3] - Tij_gt[:3, 3], 2)
        if aerr > 30.0 or terr > 0.2:
            #print(sid, tid, edge['predicted_weight'])
            bad_edges += 1
            err_bad += aerr * edge['predicted_weight']
        else:
            good_edges += 1
            err_good += aerr * edge['predicted_weight']
    print('Edge Quality: #good=%f, #bad=%f' % (good_edges, bad_edges))
    itr = 0
    while itr < max_iter:
        R, t, eigengap = TransfSync(n, edges)
        #edge_quality(n, edges, Tstar, R)
        T = np.array([pack(R[i], t[i]) for i in range(n)])
        err_gt = -1.0
        if Tstar is not None:
            aerr_gt, terr_gt = error(T, Tstar)

        if not reweight:
            if eps0 < -0.5:
                eps0 = max_existing_err(n, edges, R, t)

        if reweight:
            reweightEdges(n, edges, R, t, sigma_r=0.01, sigma_t=0.01)
        else:
            truncatedWeightPredict(n, edges, R, t, eps0)

        mindeg, numedges, err_sum = computeStats(n, edges, R, t)
        print(
            'iter=%d, avg(err^2)=%f, eigengap=%f, #edges=%d, min_deg=%f, eps0=%f, aerr_gt=%f, terr_gt=%f'
            % (itr, err_sum / numedges, eigengap, numedges, mindeg, eps0,
               aerr_gt, terr_gt))
        """ Skip idle iterations """
        if reweight:
            itr += 1
        else:
            max_err = max_existing_err(n, edges, R, t)
            while (itr < max_iter) and (eps0 > max_err):
                eps0 = eps0 * decay
                itr += 1
        if mindeg == 0:
            break
        if err_sum <= 1e-2:
            break
    return T
Beispiel #12
0

if __name__ == "__main__":
    voxel_size = 0.02  # means 5cm for the dataset
    import argparse
    parser = argparse.ArgumentParser(
        description='Baseline Algorithm: Fast Global Registration')
    parser.add_argument('files', type=str, nargs='+', help='src, tgt, output')
    args = parser.parse_args()
    src_mat = sio.loadmat(args.files[0])
    tgt_mat = sio.loadmat(args.files[1])
    src_pose = src_mat['pose']
    tgt_pose = tgt_mat['pose']
    src = src_mat['vertex']
    tgt = tgt_mat['vertex']
    Tij = tgt_pose.dot(inverse(src_pose))
    src_pc = open3d.PointCloud()
    src_pc.points = open3d.Vector3dVector(src.T)
    tgt_pc = open3d.PointCloud()
    tgt_pc.points = open3d.Vector3dVector(tgt.T)
    #source, target, source_down, target_down, source_fpfh, target_fpfh = \
    #        prepare_dataset(voxel_size)
    #draw_registration_result(src_pc, tgt_pc, Tij)
    open3d.estimate_normals(src_pc,
                            search_param=open3d.KDTreeSearchParamHybrid(
                                radius=0.2, max_nn=60))
    open3d.estimate_normals(tgt_pc,
                            search_param=open3d.KDTreeSearchParamHybrid(
                                radius=0.2, max_nn=60))

    source_down, source_fpfh = preprocess_point_cloud(src_pc, voxel_size)
Beispiel #13
0
sigmas = []

for line in lines:
  summary_mat = '%s/relative_pose/summary/%s/%s/%s.mat' % (data_path, dataset, source, line)
  summary_mat = sio.loadmat(summary_mat)
  T = summary_mat['T']
  Tstar = summary_mat['Tstar']
  aerr = summary_mat['aerr']
  terr = summary_mat['terr']
  sigma = summary_mat['sigma']
  n = Tstar.shape[0]
  n = 30
  for i in range(n):
    for j in range(i+1, n):
      Tij = T[i*4:(i+1)*4, j*4:(j+1)*4]
      Tij_gt = Tstar[j, :, :].dot(inverse(Tstar[i, :, :]))
      terr_ij = np.linalg.norm((Tij_gt - Tij)[:3, 3], 2)
      assert abs(terr_ij - terr[i, j]) < 1e-4
      terrs.append(terr_ij)
      aerr_ij = angular_distance_np(Tij_gt[np.newaxis, :3, :3], Tij[np.newaxis, :3, :3]).sum()
      assert abs(aerr_ij - aerr[i, j]) < 1e-4
      aerrs.append(aerr_ij)
      sigmas.append(sigma[i, j])

aerrs = np.array(aerrs)
terrs = np.array(terrs)
sigmas = np.array(sigmas)

for sigma_threshold in [0.1, 0.2]:
  valid_indices = np.where(sigmas < sigma_threshold)[0]
  
Beispiel #14
0
    terr = np.zeros((n, n)) + 10000000.0
    RLlist = reader.list_relative_poses(dataset, source, sceneid)

    if len(RLlist) < 4950:
      continue
    top = 0
    bottom = 0
    for mat in reader.list_relative_poses(dataset, source, sceneid):
        s = sio.loadmat(mat)
        src, tgt = mat.split('/')[-1].split('.')[0].split('_')[:2]
        sid = scanid_map[src]
        tid = scanid_map[tgt]
        Tij = s['Tij']
        if sid > tid:
            tmp = sid; sid = tid; tid = tmp
            Tij = inverse(Tij)
        assert sid < tid
        sigma[sid, tid] = s['sigma']
        aerr[sid, tid] = s['aerr']
        terr[sid, tid] = s['terr']
        if s['aerr'] < 15.0 and s['terr'] < 0.2:
          top += 1.0
        bottom += 1.0
        T[sid*4:(sid+1)*4, tid*4:(tid+1)*4] = Tij
    
    Tstar = np.zeros((n, 4, 4))
    
    for i, scanid in enumerate(scanids):
        scan = reader.read_scan(dataset, sceneid, scanid, variable_names=['pose'])
        Tstar[i, :, :] = scan['pose']
    print(sceneid, 'good ratio=', top / bottom)