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