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 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 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 extract_pc(self): # pc_range = np.array([self.pc['x'],self.pc['y'], self.pc['z']]) # pc_color = np.array([self.pc['r'],self.pc['g'], self.pc['b']]) pc_range = np.array([self.pc[:,0],self.pc[:,1], self.pc[:,2]]) pc_color = np.array([self.pc[:,3],self.pc[:,4], self.pc[:,5]]) # Get Transform from local to sesor pc = poseutil.transformPoints(pc_range.T, self.truth_T) # Filters out data outside of the region of interest accept = (pc[:,2]>0.01) * (np.abs(pc[:,0]) < .2)* (np.abs(pc[:,1]) < .2) pos_vec = pc.T * accept color_vec = pc_color * accept self.img_pos = np.reshape(pos_vec.T, (480, 640, 3)) self.img_color = np.reshape(color_vec.T, (480, 640, 3)) pc_tmp = np.sum(pc_range, axis=0) ROI_ind = np.nonzero(-np.isnan(pc_tmp))[0] self.pc_ROI = np.empty((ROI_ind.shape[0], 6)) self.pc_ROI[:,:3] = pos_vec[:,ROI_ind].T self.pc_ROI[:,3:6] = color_vec[:,ROI_ind].T
def match_all(self, current_pc, current_centroid=[], name_list = []): vmap = mapper.VoxelMap(self.res, levels = self.mrolLevels) pose_zero = poseutil.Pose3D() pose_zero.set((0,0,0,0,0,0)) poseList = [] costList = [] centroid_input = 0 current_centroid_local = np.zeros(3) if current_centroid==[]: centroid_input = 1 if centroid_input==[]: current_centroid = np.mean(current_pc, axis=0) current_pc -= current_centroid current_pc_vox = pointcloud.PointCloud(current_pc) rad = .2 current_pc_vox.boxfilter(xmin=-rad, xmax=rad, ymin=-rad, ymax=rad, zmin=-rad, zmax=rad) if centroid_input: current_centroid_local = np.mean(current_pc_vox.points, axis=0) current_pc_vox -= current_centroid_local vmap.add_points(current_pc_vox, pose_zero) for i in range(self.modelCount): #Check to see if for j in range(len(name_list)): # print self.all_names[i] # print name_list[j] # print self.all_names[i]==name_list[j] if (name_list == [] and j == 0) or self.all_names[i] == name_list[j]: pc_ground_vox = self.models_vox[i] bestPose, maxCost = vmap.align_points(pc_ground_vox, pose_zero) # print "Pose: ", bestPose # print "Max cost: ", maxCost bestPose_inv = poseutil.inverse(bestPose.get()) bestPose_inv = poseutil.Pose3D(bestPose_inv) modelPose_T = current_centroid + current_centroid_local + bestPose_inv.get()[0:3] modelPose = np.reshape([modelPose_T, bestPose_inv.get()[3:]], 6) poseList.append(modelPose) costList.append(maxCost) # mat = poseutil.Pose3D() # mat.set(bestPose.get()) if 0: xyzs = poseutil.transformPoints(pc_ground_vox.points, bestPose.getMatrix()) pc = pointcloud.PointCloud(xyzs) pdb.set_trace() return costList, poseList
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 create_model(self):
PCs_added = 0
if self.fileCount > 1:
for j in range(0, self.fileCount, 1):
data = np.load(self.PC_files[j]) #should be self.PC_files_reg
xyzs = np.array((data['x'], data['y'], data['z'])).T
xyzs = xyzs[(np.nonzero(np.nansum(xyzs < 100, axis=1) > 0)[0])]
pose = poseutil.Pose3D(self.pose_list[j])
pose_zero = poseutil.Pose3D()
pose_zero.set((0, 0, 0, 0, 0, 0))
xyzs = poseutil.transformPoints(xyzs, pose.getMatrix())
pc = pointcloud.PointCloud(xyzs)
pc.pose = pose_zero #pose
# pc.boxfilter(xmin=-.1, xmax=.1, ymin=-.1, ymax=.1, zmin=-.1, zmax=.1)
# pc.boxfilter(xmin=-0.05, xmax=.05, ymin=-.05, ymax=.05, zmin=-.05, zmax=.05)
pc.boxfilter(xmin=self.boxFilter[0],
xmax=self.boxFilter[1],
ymin=self.boxFilter[2],
ymax=self.boxFilter[3],
zmin=self.boxFilter[4],
zmax=self.boxFilter[5])
# print pose
# pdb.set_trace()
if j == 0: #Don't try to align first pose
self.vmap.add_points(pc, pc.pose)
else:
guessPose = pose_zero #self.pose_list[i]#np.load(self.PC_files_reg_all[i])['pose']
bestPose = guessPose
# print guessPose
bestPose = pose_zero
# bestPose, maxcost = self.vmap.align_points(pc, guessPose)
# bestPose, maxcost = self.vmap.align_points(pc, guessPose, True) #only 2D alignment
disp = np.array(bestPose.getTuple())
if 1: #all(np.abs(disp[:3]) < self.transThresh) and all(np.abs(disp[3:])<self.rotThresh*3.14/180.):
self.vmap.add_points(pc, bestPose)
PCs_added += 1
# print '%i PCs added'%PCs_added
# print "Best Pose: ", bestPose
# guessPose = bestpose
# print "Pose from ground: ", guessPose
print '%i of %i point clouds were added' % (PCs_added, self.fileCount)
def merge_PC_ground(self): if self.fileCount > 1: for j in range(self.fileCount): data = np.load(self.PC_files[j]) #should be self.PC_files_reg xyzs = np.array((data['x'], data['y'] , data['z'] )).T # pose = self.pose_list[j]#np.load(self.PC_files_reg_all[j])['pose'] pose_zero = poseutil.Pose3D() pose_zero.set((0,0,0,0,0,0)) xyzs = poseutil.transformPoints(xyzs, pose.getMatrix()) pc = pointcloud.PointCloud(xyzs) pc.pose = pose_zero#pose pc.boxfilter(xmin=self.boxFilter[0], xmax=self.boxFilter[1],ymin=self.boxFilter[2], ymax=self.boxFilter[3],zmin=self.boxFilter[4], zmax=self.boxFilter[5]) print pose self.vmap.add_points(pc, pc.pose)
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
def create_model(self): PCs_added = 0 if self.fileCount > 1: for j in range(0,self.fileCount, 1): data = np.load(self.PC_files[j]) #should be self.PC_files_reg xyzs = np.array((data['x'], data['y'], data['z'])).T xyzs = xyzs[(np.nonzero(np.nansum(xyzs<100, axis=1)>0)[0])] pose = poseutil.Pose3D(self.pose_list[j]) pose_zero = poseutil.Pose3D() pose_zero.set((0,0,0,0,0,0)) xyzs = poseutil.transformPoints(xyzs, pose.getMatrix()) pc = pointcloud.PointCloud(xyzs) pc.pose = pose_zero#pose # pc.boxfilter(xmin=-.1, xmax=.1, ymin=-.1, ymax=.1, zmin=-.1, zmax=.1) # pc.boxfilter(xmin=-0.05, xmax=.05, ymin=-.05, ymax=.05, zmin=-.05, zmax=.05) pc.boxfilter(xmin=self.boxFilter[0], xmax=self.boxFilter[1],ymin=self.boxFilter[2], ymax=self.boxFilter[3],zmin=self.boxFilter[4], zmax=self.boxFilter[5]) # print pose # pdb.set_trace() if j==0: #Don't try to align first pose self.vmap.add_points(pc, pc.pose) else: guessPose = pose_zero#self.pose_list[i]#np.load(self.PC_files_reg_all[i])['pose'] bestPose = guessPose # print guessPose bestPose = pose_zero # bestPose, maxcost = self.vmap.align_points(pc, guessPose) # bestPose, maxcost = self.vmap.align_points(pc, guessPose, True) #only 2D alignment disp = np.array(bestPose.getTuple()) if 1: #all(np.abs(disp[:3]) < self.transThresh) and all(np.abs(disp[3:])<self.rotThresh*3.14/180.): self.vmap.add_points(pc, bestPose) PCs_added +=1 # print '%i PCs added'%PCs_added # print "Best Pose: ", bestPose # guessPose = bestpose # print "Pose from ground: ", guessPose print '%i of %i point clouds were added' %(PCs_added, self.fileCount)
def merge_PC_ground(self):
if self.fileCount > 1:
for j in range(self.fileCount):
data = np.load(self.PC_files[j]) #should be self.PC_files_reg
xyzs = np.array((data['x'], data['y'], data['z'])).T
# pose = self.pose_list[j]#np.load(self.PC_files_reg_all[j])['pose']
pose_zero = poseutil.Pose3D()
pose_zero.set((0, 0, 0, 0, 0, 0))
xyzs = poseutil.transformPoints(xyzs, pose.getMatrix())
pc = pointcloud.PointCloud(xyzs)
pc.pose = pose_zero #pose
pc.boxfilter(xmin=self.boxFilter[0],
xmax=self.boxFilter[1],
ymin=self.boxFilter[2],
ymax=self.boxFilter[3],
zmin=self.boxFilter[4],
zmax=self.boxFilter[5])
print pose
self.vmap.add_points(pc, pc.pose)
def extract_pc(self):
# pc_range = np.array([self.pc['x'],self.pc['y'], self.pc['z']])
# pc_color = np.array([self.pc['r'],self.pc['g'], self.pc['b']])
pc_range = np.array([self.pc[:, 0], self.pc[:, 1], self.pc[:, 2]])
pc_color = np.array([self.pc[:, 3], self.pc[:, 4], self.pc[:, 5]])
# Get Transform from local to sesor
pc = poseutil.transformPoints(pc_range.T, self.truth_T)
# Filters out data outside of the region of interest
accept = (pc[:, 2] > 0.01) * (np.abs(pc[:, 0]) < .2) * (np.abs(
pc[:, 1]) < .2)
pos_vec = pc.T * accept
color_vec = pc_color * accept
self.img_pos = np.reshape(pos_vec.T, (480, 640, 3))
self.img_color = np.reshape(color_vec.T, (480, 640, 3))
pc_tmp = np.sum(pc_range, axis=0)
ROI_ind = np.nonzero(-np.isnan(pc_tmp))[0]
self.pc_ROI = np.empty((ROI_ind.shape[0], 6))
self.pc_ROI[:, :3] = pos_vec[:, ROI_ind].T
self.pc_ROI[:, 3:6] = color_vec[:, ROI_ind].T
'''Create 3D model using MROL align -- NIST data''' start = time.time() if 1: PCs_added = 0 if file_count > 1: for j in range(0,file_count, 1): print 'File %i of %i' %(j, file_count) data = np.load(good_file_pc[j]) #should be good_file_reg xyzs = np.array((data['x']*scale, data['y']*scale, data['z']*scale)).T pose = pose_list[j]#np.load(good_file_reg_all[j])['pose'] pose_zero = poseutil.Pose3D() pose_zero.set((0,0,0,0,0,0)) xyzs = poseutil.transformPoints(xyzs, pose.getMatrix()) pc = pointcloud.PointCloud(xyzs) pc.pose = pose_zero#pose # pdb.set_trace() pc.boxfilter(xmin=bb_xmin, xmax=bb_xmax,ymin=bb_ymin, ymax=bb_ymax,zmin=bb_zmin, zmax=bb_zmax) # print pose if j==0: #Don't try to align first pose vmap.add_points(pc, pc.pose) else: guessPose = pose_zero#pose_list[i]#np.load(good_file_reg_all[i])['pose'] bestPose = guessPose # print guessPose bestPose, maxcost = vmap.align_points(pc, guessPose)
