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