def testeqandhash(self): P = self.p P = poseutil.Pose3D() P.set([1.2, 2.3, 0.2, 0.12, 0.63, 0.9]) Q = poseutil.Pose3D() Q.setMatrix(P.getMatrix()) self.assertEquals(P, Q)
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 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 load_poses(fname): # skip comment lines poses = {} P = poseutil.Pose3D() for line in open(fname): if line.strip().startswith('#'): continue fields = line.split(None, 1) fname = fields[0] mat = np.asarray(eval(fields[1])) P = poseutil.Pose3D() # need to transpose from blender? P.setMatrix(mat.T) poses[fname] = P return poses
def OFFtest_mapper_global_localisation(self): dx = 0.5 dq = np.radians(15) spread_pose = poseutil.Pose3D((dx, dx, 0, 0, 0, dq)) res = 0.04 map_mrol = mrol.MROL(res, levels=4) #map_mrol.set_feature_range(8) self.P1.volumetricsample(self.res + 0.01111) self.P2.volumetricsample(0.08) map_mrol.addpoints(self.P1) pc1 = get_PC(1)[0] guesspose = pc1.pose bestpose, maxo = map_mrol.match(pc1, guesspose, spread_pose) truepose = self.P2.pose if visualise: self.P1.display(color=(1, 0, 0)) self.P2.pose = bestpose self.P2.display(color=(0, 1, 0)) print 'Best:', bestpose print 'True:', truepose poseerror = truepose.getCombined(bestpose.inverse()) pos_error = np.sum(np.abs(poseerror.get()[:3])) ang_error = np.sum(np.abs(poseerror.get()[3:])) print print 'Total Position error (m): ', pos_error print 'Total Angular error (degs):', np.degrees(ang_error) angular_tol = np.radians(2) position_tol = 0.05 self.assertTrue(bestpose.is_close(truepose, position_tol, angular_tol))
def get_scan_pose_from_file(voxel_test_query_pose_file): Ppose = np.loadtxt(voxel_test_query_pose_file) #T = transformations.translation_from_matrix(Ppose) #R = transformations.euler_from_matrix(Ppose) #return np.hstack((T, R)) newPose = poseutil.Pose3D() newPose.setMatrix(Ppose) return newPose.getTuple()
def testcombined(self): p = self.p[0] combo = p.getCombined(p) self.assertTrue(np.allclose(combo.get(), p.get())) p.set([1, 1, 0, 0, 0, np.radians(45)]) combo = p.getCombined(p) expected = poseutil.Pose3D() expected.set([1, 1 + 2**0.5, 0, 0, 0, np.radians(90)]) self.assertTrue(np.allclose(expected.get(), combo.get()))
def makePose3D(transform): p = poseutil.Pose3D() R = tf.transformations.quaternion_matrix(transform[1]) T = transform[0] #M = [R,T;0,0,0,0] M = np.zeros([4,4]) M = R M[0:3,3:4] = np.asarray(T).reshape(3,1) p.setMatrix(M) return p
def randomsearch(mapset,res,Pall,guesspose): ''' Exhaustive search for ground truth This function is used to generate the "true pose" which is used to generate the check data sets for the segmentation. It should not be used widely until it is implemented properly (see notes in implementation) and has more generic input.''' poseobj = poseutil.Pose3D() # TODO this is only valid for small poses should do it properly by using # random rotations from transformations.py dx = 0.1 dq = np.radians(1) bestoverlap = 0 while True: #start = time.time() delta = np.hstack((np.random.normal(0, dx, 3), np.random.normal(0, dq, 3))) poseobj.set(guesspose + delta) overlap = occupiedlist.calccollisions(poseobj.getTuple(), mapset, res, Pall) if overlap > bestoverlap: bestoverlap = overlap # TODO now search around new pose and refine? alternatively just do this randomly print bestoverlap, ', ', poseobj
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 __init__(self, objectName, poses, boxFilter=[-.08, .08,-.08, .08, 0.012, .4] , \ mrolLevels = 4, res = .002, transThresh=.04, rotThresh=55.0, viz = 0): self.objectName = objectName self.boxFilter = boxFilter #[-x, x, -y, y, -z, z] for boxFilter self.mrolLevels = mrolLevels #number of self.resolution layers for MROL self.res = res #self.resolution in meters self.transThresh = transThresh # Threshold for merging PCs -- in meters self.rotThresh = rotThresh # Threshold for merging PCs -- in degrees # self.baseDir = 'data/'+objectName+'/' self.baseDir = sys.argv[0][0:sys.argv[0].rfind('/') + 1] + 'data/' + objectName + '/' self.pose_list = [] # if poses == []: # self.truth_T = truth.getTransformations() # for i in range(self.truth_T.shape[2]): # P = poseutil.Pose3D() # P.setMatrix(self.truth_T[:,:,i]) # self.pose_list.append(P) # else: self.pose_list = poses print self.pose_list P = poseutil.Pose3D() point_clouds = {} PCs = [] self.vmap = mapper.VoxelMap(self.res, levels=self.mrolLevels) if viz: self.vmap.display(npts=4e5) self.run()
import mrol_mapping.mapper as mapper import mrol_mapping.mrol as mrol import mrol_mapping.poseutil as poseutil import mrol_mapping.pointcloud as pointcloud import time import pdb res = .002 n_levels = 4 if len(sys.argv) > 1: input_file = sys.argv[1] else: input_file = '/home/colin/repo/perception_repo/data/obj16_best.npy' vmap = mapper.VoxelMap(res, levels=n_levels) #vmap.display(npts=4e5) input_pc = np.load(input_file) pc = pointcloud.PointCloud(input_pc) vmap.add_points(pc, poseutil.Pose3D()) point1 = np.array([-.2, 0, 0]) point2 = np.array([0, 0, 0]) tmp = vmap.ray_trace_pts(point1, point2, return_extras=False) pdb.set_trace()
### Pose Estimation potential_names = [dIDs[i]] # potential_names.append(dIDs[i]) scores, poses = poseEstimator.run( pc, pose_centroid, potential_names) # scores = 0 # poses = np.array([[0,0,0,0,0,0]]) bestPose = poses[0] # print bestPose # pdb.set_trace() # bestPose = poses[np.argmin(scores)] if bestPose != []: T = bestPose[0:3] p = poseutil.Pose3D() p.set(bestPose) R = p.getMatrix()[0:3, 0:3] except: print "Error getting pose for ", dIDs[i] poses = np.array([[0, 0, 0, 0, 0, 0]]) T = bestPose[0:3] p = poseutil.Pose3D() p.set(bestPose) R = p.getMatrix()[0:3, 0:3] # data is a dictionary with elements time, frame, dID, oID, R (3x3), T (3x1) data = { 'time': time_out, 'frame': frame, 'oID': oID,
def testuniformposes(self): centrepose = poseutil.Pose3D() spreadpose = poseutil.Pose3D() spreadpose.set([1, 1, 0, 0, 0, 1]) poses = poseutil.uniformposes(centrepose, spreadpose, 0.2, 0.1) self.assertEquals(len(poses), 2541)
good_file_pc.append(root[i]+'/'+files[i][j]) elif files[i][j].find(".npz")>-1 and files[i][j].find('reg_')>-1: good_file_reg_all.append(root[i]+'/'+files[i][j]) good_file_reg.sort() good_file_reg_all.sort() good_file_pc.sort() file_count = len(good_file_pc) pose_list = [] truth_T = truth.getTransformations() for i in range(truth_T.shape[2]): P = poseutil.Pose3D() P.setMatrix(truth_T[:,:,i]) # pose_list.append(np.array([0,0,0,0,0,0])) pose_list.append(P) #Set centroid to zero #for i in range(file_count): # pose_list.append(np.load(good_file_reg_all[i])['pose']) # #centroid = np.mean(pose_list, axis=0) #pose_list = np.array(pose_list) #pose_list[:,0] -= centroid[0] #pose_list[:,1] -= centroid[1] #pose_list[:,2] -= centroid[2]
if load_pc: fnames = [ datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc') ] else: fnames = [ datadir + '/' + fname for fname in fnames if fname.endswith('.png') ] except: print 'Need to specify a valid directory for input' sys.exit() visualise = True long_term = False bestpose = poseutil.Pose3D(X=(0, 0, 0, 0, 0, 0)) pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2., 0, -np.pi / 2.)) res = 0.01 segment_map = mapper.VoxelMap(res, levels=3) pc = pointcloud.load(fnames.pop(0)) #pc = pointcloud.PointCloud(pc) pc = pointcloud.PointCloud(pc_xform.transformPoints(pc)) segment_map.add_points(pc, bestpose) object_map = mapper.VoxelMap(res / 4., levels=1) #freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3) #freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res free_space_res = res pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
def setUp(self): self.p = [] self.p.append(poseutil.Pose3D()) self.p.append(poseutil.Pose3D())
if __name__ == '__main__': try: datadir = sys.argv[1] outfname = sys.argv[2]+".map" fnames = os.listdir(datadir) fnames.sort() fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')] except: print 'Need to specify a valid directory for input and file name for output' sys.exit() # variable initialisation iros_map = mapper.VoxelMap(res,levels=5) iros_free_map = mapper.VoxelMap(res,levels=1) bestpose = poseutil.Pose3D() pc = get_pointcloud(fnames.pop(0)) iros_map.add_points(pc, bestpose) if visualise: iros_map.display(changes=True) if make_free: freepts = get_freespace(pc.points, bestpose, iros_map) pcfree = pointcloud.PointCloud(freepts) iros_free_map.add_points(pcfree,None) if visualise: iros_free_map.display(npts=1000000) for fname in fnames: print fname, pc = get_pointcloud(fname) bestpose = iros_map.align_points(pc, bestpose)[0]
#sensor_model = mapper._build_sensor_model(0.1) # #SM_points = sensor_model.getpoints() #inds = ol.calccollisions(bestpose, SM_points, query=True) #inds = np.array(inds) #hits = pointcloud.PointCloud(SM_points[inds > 0]) # # #import pydb; pydb.set_trace() #print ol #absent_map.display() segment_map = absent_map bestpose = poseutil.Pose3D(X=(0.5,-0.5,0,0,0,np.pi/4)) for fname in fnames: pc = iros_mapper.get_pointcloud(fname) print fname, bestpose = segment_map.align_points(pc, bestpose)[0] print bestpose hits, new_points = segment_map.mrol.getfinest().segment_ovl(bestpose,pc.points) inliers, outliers = freespace_ol.segment_ovl(None, new_points) segment_map.add_points(pointcloud.PointCloud(outliers), None) if freespace2: if len(inliers) > 0: tmp_freespace_ol = segment_map.generate_freespace2(inliers,pose=bestpose) else: # nothing to segment? import pdb;pdb.set_trace() tmp_freespace_ol = occupiedlist.OccupiedList(1)