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 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
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
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
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
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,
