示例#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))
 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
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