示例#1
0
 def testtuple_matrix(self):
     X = (0, 0, 0, 0, 0, 0)
     Y = poseutil.posetuple(poseutil.mat(X))
     self.assertEquals(X, Y)
     X = (0.1, -0.2, 0, 0.45, 0.9, -1.1)
     Y = poseutil.posetuple(poseutil.mat(X))
     self.assertTrue(np.allclose(X, Y))
示例#2
0
 def testSegmentationMethods(self):
     print "Test Segmentation Methods"
     # Test the segmentation independantly of the aligment using the "true" pose
     # report the confusion matrix or summary statustic (false alarm rate, etc?)
     temp_outliers = util.get_scan_points_from_file(os.path.join(self.datadir, '1271124950-new.txt'))
     dists = pointcloud.nearest_neighbour(self.Pall,temp_outliers)
     outliers_inds2 = dists < 0.01
     temp_inliers = self.Pall[np.logical_not(outliers_inds2)]
     self.assertTrue((temp_outliers.shape[0] + temp_inliers.shape[0]) == self.Pall.shape[0])
     self.trueoutliers = poseutil.transformPoints(temp_outliers, poseutil.mat(self.truepose))
     self.trueinliers = poseutil.transformPoints(temp_inliers, poseutil.mat(self.truepose))
     for method in self.segmenters:
         seg_name = method.__name__.replace('_', ' ')
         seg_name = seg_name.replace('segment ', '')
         print 5*'-', seg_name, 5*'-'
         # Execute the segmentation algorithm and time it
         start_seg = time.time()
         inliers,outliers = method(self.truePoints)
         taken_seg = time.time() - start_seg
         # check accuracy and timeliness
         true_pos, true_neg, false_neg, false_pos = util.segmentation_stats(inliers, outliers, self.trueinliers, self.trueoutliers)
         self.assertTrue(sum(true_pos) > 0)
         self.assertTrue(sum(true_neg) > 0)
         print sum(true_pos), sum(false_pos)
         print sum(false_neg), sum(true_neg)
         FPR, MDR, accuracy, specificity = util.segmentation_summary(true_pos, true_neg, false_neg, false_pos, self.trueoutliers.shape[0], self.Pall.shape[0])
         print "False +ve rate: ", FPR
         self.assertTrue(FPR < 0.05)
         print "Missed detection rate: ", MDR
         self.assertTrue(MDR < 0.15)
         print "accuracy: ", accuracy
         self.assertTrue(accuracy > 0.9)
         print "specifity: ", specificity
         self.assertTrue(specificity > 0.9)
def align(initialpose,
          P,
          Q,
          terminate_threshold,
          reject_threshold=0.5,
          iterations=50,
          quiet=True):
    '''Given two sets of corresponding points (P, Q) and an initial guess 
    transform calculate the transform from P to Q. (i.e. P is scan, Q is map)'''
    assert (type(P[0][0]) == type(
        Q[0][0])), 'P and Q must be of the same data type'
    P = poseutil.transformPoints(P, poseutil.mat(initialpose))
    # looping without re-associating does improve but I think it is negligible,
    # compared to error due to associations
    initialposemat = poseutil.mat(initialpose)
    bestposemat = poseutil.mat((0, 0, 0, 0, 0, 0))
    buildindex(Q)

    if visualise:
        vis.clear()
        Qvis = Q.copy()
        if len(Qvis) > 1e5:
            print "resampling for visualiser"
            Qvis = util.volumetricsample(Qvis, 0.1)
        vis.addmappts(Qvis)
        vis.setleftpts(P)
    last_dist = 0.0
    # TODO decide whether to bail early from this loop
    for iteration in range(iterations):
        Qinds, Pinds, dists = __associate(P,
                                          reject_threshold,
                                          return_dists=True)
        associated_pts = np.array([Q[ind] for ind in Qinds])
        dist = np.sqrt(np.mean(dists[Pinds]))
        if len(P[Pinds]) == 0 or len(associated_pts) == 0:
            if not quiet:
                print "No points within association distance"
            return np.array([0, 0, 0, 0, 0, 0]), 0
        X = __calctransform(P[Pinds], associated_pts)
        bestposemat = np.dot(bestposemat, poseutil.mat(X))
        P = poseutil.transformPoints(P, poseutil.mat(X))
        if visualise:
            # TODO set this threshold
            inds = dists < terminate_threshold
            vis.setrightpts(P[inds])
            vis.setleftpts(P[np.logical_not(inds)])
        if not quiet:
            print iteration, dist
        if dist < terminate_threshold: break
        if np.abs(dist - last_dist) < 1e-9:
            if not quiet:
                print "Warning, no longer converging and termination threshold not reached. Possible local minima. Try changing the rejection threshold."
            break
        if iteration == iterations - 1:
            print "Warning, maximum iterations reached."
        last_dist = dist
    bestposemat = np.dot(bestposemat, initialposemat)
    #inlier_pts, outlier_pts = classify_pts(P, Pinds)
    return np.array(
        poseutil.posetuple(bestposemat)), dist  #, inlier_pts, outlier_pts
 def testtuple_matrix(self):
     X = (0, 0, 0, 0, 0, 0)
     Y = poseutil.posetuple(poseutil.mat(X))
     self.assertEquals(X, Y)
     X = (0.1, -0.2, 0, 0.45, 0.9, -1.1)
     Y = poseutil.posetuple(poseutil.mat(X))
     self.assertTrue(np.allclose(X, Y))
def align(initialpose, P, Q, terminate_threshold, reject_threshold=0.5, iterations=50, quiet=True):
    '''Given two sets of corresponding points (P, Q) and an initial guess 
    transform calculate the transform from P to Q. (i.e. P is scan, Q is map)'''
    assert(type(P[0][0]) == type(Q[0][0])), 'P and Q must be of the same data type'
    P = poseutil.transformPoints(P, poseutil.mat(initialpose))
    # looping without re-associating does improve but I think it is negligible, 
    # compared to error due to associations
    initialposemat = poseutil.mat(initialpose)
    bestposemat = poseutil.mat((0, 0, 0, 0, 0, 0))
    buildindex(Q)

    if visualise:
        vis.clear()
        Qvis = Q.copy()
        if len(Qvis) > 1e5:
            print "resampling for visualiser"
            Qvis = util.volumetricsample(Qvis, 0.1)
        vis.addmappts(Qvis)
        vis.setleftpts(P)
    last_dist = 0.0
    # TODO decide whether to bail early from this loop
    for iteration in range(iterations):
        Qinds, Pinds, dists = __associate(P, reject_threshold, return_dists=True)
        associated_pts = np.array([Q[ind] for ind in Qinds])
        dist = np.sqrt(np.mean(dists[Pinds]))        
        if len(P[Pinds]) == 0 or len(associated_pts) == 0:
            if not quiet:
                print "No points within association distance"
            return np.array([0,0,0,0,0,0]), 0
        X = __calctransform(P[Pinds], associated_pts)
        bestposemat = np.dot(bestposemat, poseutil.mat(X))
        P = poseutil.transformPoints(P, poseutil.mat(X))
        if visualise:
            # TODO set this threshold
            inds = dists < terminate_threshold 
            vis.setrightpts(P[inds])
            vis.setleftpts(P[np.logical_not(inds)])
        if not quiet:
            print iteration, dist
        if dist < terminate_threshold: break
        if np.abs(dist - last_dist) < 1e-9:
            if not quiet:
                print "Warning, no longer converging and termination threshold not reached. Possible local minima. Try changing the rejection threshold."
            break
        if iteration == iterations-1:
            print "Warning, maximum iterations reached."
        last_dist = dist
    bestposemat = np.dot(bestposemat, initialposemat)
    #inlier_pts, outlier_pts = classify_pts(P, Pinds)
    return np.array(poseutil.posetuple(bestposemat)), dist #, inlier_pts, outlier_pts
示例#6
0
 def setUp(self):
     self.datadir = 'data/stillscans/'
     self.Pall, self.Q, self.guesspose, self.res = generate_tst_data_from_file(self.datadir + 'empty-map.txt', self.datadir + '1271124950-scan.txt', self.datadir + '1271124950-pose.txt')
     # override guess pose to make it harder for the algorithms
     self.guesspose = np.array([-2, 42, 80, 0.2, 0.1, 2.58])
     # sampling
     self.P = util.volumetricsample(self.Pall, self.res) # shuffles P as well
     # setup the mrol for the alignment
     self.mapmrol = mrol.MROL(self.res, levels=5)
     self.mapmrol.addpoints(self.Q)
     # setup the occupiedlist for the segmentation
     mv = occupiedlist.pointstovoxels(self.Q, self.res)
     self.mapvoxels = {} 
     occupiedlist.increment(self.mapvoxels, mv)
     # setup a dilated occupiedlist for segmentation
     self.dilated = {} 
     mv = occupiedlist.dilate(self.mapmrol.getvoxels().keys())
     occupiedlist.increment(self.dilated, mv)
     # setup a mapset for fast alignment
     self.mapset = set(occupiedlist.hashrows(self.mapvoxels.keys()))
     # Ground truths and tolerances
     self.lin_tol = 1e-1
     self.ang_tol = np.deg2rad(2.0)
     self.truepose = np.array([-1.8572, 41.7913, 79.3341, np.deg2rad(12.4607), np.deg2rad(4.3733), np.deg2rad(147.2011)]) # Overlap: 6094, Pose (m, degrees): -1.8572 41.7913 79.3341 12.4607 4.3733 147.2011 
     self.truePoints = poseutil.transformPoints(self.Pall, poseutil.mat(self.truepose))
     # list the aligners to be tested
     self.aligners = [ 
                       self.align_mrol_optimize_00001,
                       self.align_mrol_optimize_11111,
                     ]     
     # list the segmenters to be tested
     self.segmenters = [ self.segment_ovl,
                         self.segment_ovl_dilated
                       ]
def cost(pose, P, Q, threshold=0.5, rebuild=True):
    '''Cost function based on root mean squared distance to approximate nearest 
    neighbour. '''
    if rebuild:
        buildindex(Q)
    # TODO refactor this into a class so make the requirement to call 
    # buildindex less error prone
    P = poseutil.transformPoints(P, poseutil.mat(pose))
    Qinds, Pinds, dist = __associate(P, threshold)
    return dist
def cost(pose, P, Q, threshold=0.5, rebuild=True):
    '''Cost function based on root mean squared distance to approximate nearest 
    neighbour. '''
    if rebuild:
        buildindex(Q)
    # TODO refactor this into a class so make the requirement to call
    # buildindex less error prone
    P = poseutil.transformPoints(P, poseutil.mat(pose))
    Qinds, Pinds, dist = __associate(P, threshold)
    return dist
示例#9
0
def generate_data(true_pose, npts=100, pextra=10, qextra=100, noise_bool=True):
    '''Returns map points Q, scan points P and the true pose to move from P to Q (scan to map)'''
    true_pose = np.array(true_pose, dtype=float)

    np.random.seed(6)
    P = 10 * np.random.ranf((npts, 3))  # scan points
    noise = 0.05 * np.random.standard_normal(P.shape)
    Q = poseutil.transformPoints(P, poseutil.mat(true_pose))
    # move the map points away from the scan points by pose so that moving the
    # scan points by pose will match the map
    if noise_bool:
        Q = Q + noise  # not sure why we add noise ot the map instead of the scan, but tests fail otherwise?
    # add some points that are unrelated to both P and Q
    X = 10 * np.random.ranf((pextra, 3))
    P = np.vstack((P, X))
    X = 10 * np.random.ranf((qextra, 3))
    Q = np.vstack((Q, X))
    return P, Q
示例#10
0
def generate_data(true_pose, npts=100, pextra=10, qextra=100, noise_bool=True):
    '''Returns map points Q, scan points P and the true pose to move from P to Q (scan to map)'''
    true_pose = np.array(true_pose, dtype=float)

    np.random.seed(6)
    P = 10*np.random.ranf((npts, 3)) # scan points
    noise = 0.05 * np.random.standard_normal(P.shape)
    Q = poseutil.transformPoints(P, poseutil.mat(true_pose))
    # move the map points away from the scan points by pose so that moving the 
    # scan points by pose will match the map
    if noise_bool:
        Q = Q + noise # not sure why we add noise ot the map instead of the scan, but tests fail otherwise?
    # add some points that are unrelated to both P and Q
    X = 10*np.random.ranf((pextra, 3))
    P = np.vstack((P, X))
    X = 10*np.random.ranf((qextra, 3))
    Q = np.vstack((Q, X))
    return P, Q
示例#11
0
                    T[0:3, 3] = trans

                    P = poseutil.Pose3D()
                    P.setMatrix(T)
                    new_pose = poseutil.inverse(P.get())

                    pose_list.append(new_pose)

                outDir = baseDir + objectID + "/"
                bag.save_pts(pts_pos, pts_color, objectID, out_folder=outDir)

                pc_6D = np.empty((pts_pos.shape[1], 6))
                pc_6D[:, 0:3] = pts_pos.T
                pc_6D[:, 3:6] = pts_color

                feature = featureExtract(objectID, poseutil.mat(new_pose), pointcloud=pc_6D)

                poses = np.array([[0, 0, 0, 0, 0, 0]])
                T = trans
                R = np.array(rot_mat)
                oID = 0
                dID = objectID

                # data is a dictionary with elements time, frame, dID, oID, R (3x3), T (3x1)
                data = {"time": time_out, "frame": frame, "oID": oID, "dID": dID, "R": R, "T": T}
                output.write_data(data)

                # Visualize
                if viz:
                    pc = pointcloud.PointCloud(pts_pos.T)
                    P = poseutil.Pose3D()
                    P = poseutil.Pose3D()
                    P.setMatrix(T)
                    new_pose = poseutil.inverse(P.get())

                    pose_list.append(new_pose)

                outDir = baseDir + objectID + '/'
                bag.save_pts(pts_pos, pts_color, objectID, out_folder=outDir)

                pc_6D = np.empty((pts_pos.shape[1], 6))
                pc_6D[:, 0:3] = pts_pos.T
                pc_6D[:, 3:6] = pts_color

                feature = featureExtract(objectID,
                                         poseutil.mat(new_pose),
                                         pointcloud=pc_6D)

                poses = np.array([[0, 0, 0, 0, 0, 0]])
                T = trans
                R = np.array(rot_mat)
                oID = 0
                dID = objectID

                # data is a dictionary with elements time, frame, dID, oID, R (3x3), T (3x1)
                data = {
                    'time': time_out,
                    'frame': frame,
                    'oID': oID,
                    'dID': dID,
                    'R': R,