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 setUp(self):
#np.random.seed(6)
self.mpts = 100000 #Adding a zero here changes the cython results dramatically
self.npts = 10000
self.high = 50
self.M = np.random.randint(0, self.high,
(self.mpts, 3)).astype(np.int16)
self.P = np.random.randint(0, self.high,
(self.npts, 3)).astype(np.int16)
self.testpose = (-2., 42., 80., 0.2, 0.1, 2.6)
self.testposeinv = poseutil.inverse(self.testpose)
self.Pxformed = poseutil.check_and_transform_points(
self.P, self.testpose)[0]
self.ol = occupiedlist.OccupiedList(1)
self.ol.addpoints(self.M)
self.Mkdt = spatial.cKDTree(self.M)
self.maparray = np.zeros((self.high, self.high, self.high),
dtype=np.bool)
#self.maparray = ndsparse.ndsparse((self.high, self.high, self.high))
self.maparray[self.M[:, 0], self.M[:, 1], self.M[:, 2]] = True
# set up packed arrays map
ids = occupiedlist._pack(self.M)
self.mapvoxels_int16 = dict.fromkeys(ids, 1)
self.mapset = set(ids)
#self.mapvoxels_int16 = collections.defaultdict(int)
#for ID in ids:
# self.mapvoxels_int16[ID.tostring()] += 1
# bloom map
self.bloom = occupiedlist.BloomFilter(self.mpts)
self.bloom.add_voxel_ids(occupiedlist._pack(self.M))
# dictionary of dictionaries
#D = dict.fromkeys(self.mpts[:,0], dict())
self.Pint = self.P.astype(int)
D = dict()
# first initialise
for a, b, c in self.M:
D.setdefault(a, dict())
D[a].setdefault(b, dict())
D[a][b][c] = 0
for a, b, c in self.M:
D[a][b][c] += 1
self.nestedDict = D
def setUp(self):
#np.random.seed(6)
self.mpts = 100000 #Adding a zero here changes the cython results dramatically
self.npts = 10000
self.high = 50
self.M = np.random.randint(0, self.high, (self.mpts, 3)).astype(np.int16)
self.P = np.random.randint(0, self.high, (self.npts, 3)).astype(np.int16)
self.testpose = (-2., 42., 80., 0.2, 0.1, 2.6)
self.testposeinv = poseutil.inverse(self.testpose)
self.Pxformed = poseutil.check_and_transform_points(self.P, self.testpose)[0]
self.ol = occupiedlist.OccupiedList(1)
self.ol.addpoints(self.M)
self.Mkdt = spatial.cKDTree(self.M)
self.maparray = np.zeros((self.high, self.high, self.high), dtype=np.bool)
#self.maparray = ndsparse.ndsparse((self.high, self.high, self.high))
self.maparray[self.M[:, 0], self.M[:, 1], self.M[:, 2]] = True
# set up packed arrays map
ids = occupiedlist._pack(self.M)
self.mapvoxels_int16 = dict.fromkeys(ids, 1)
self.mapset = set(ids)
#self.mapvoxels_int16 = collections.defaultdict(int)
#for ID in ids:
# self.mapvoxels_int16[ID.tostring()] += 1
# bloom map
self.bloom = occupiedlist.BloomFilter(self.mpts)
self.bloom.add_voxel_ids(occupiedlist._pack(self.M))
# dictionary of dictionaries
#D = dict.fromkeys(self.mpts[:,0], dict())
self.Pint = self.P.astype(int)
D = dict()
# first initialise
for a, b, c in self.M:
D.setdefault(a, dict())
D[a].setdefault(b, dict())
D[a][b][c] = 0
for a, b, c in self.M:
D[a][b][c] += 1
self.nestedDict = D
# print time_out
if pose != None:
_, new_pose, ros_time = msg_pose.next()
rot = np.array(new_pose.rvec) # *180./3.14159
rot_mat = rodrigues2mat(rot)
trans = np.array(new_pose.tvec)
T = np.eye(4)
T[0:3, 0:3] = rot_mat
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)
# print time_out
if pose != None:
_, new_pose, ros_time = msg_pose.next()
rot = np.array(new_pose.rvec) #*180./3.14159
rot_mat = rodrigues2mat(rot)
trans = np.array(new_pose.tvec)
T = np.eye(4)
T[0:3, 0:3] = rot_mat
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]])
