def TestVoxelize(X):
A = point_cloud.Voxelize(0.010, X)
B = point_cloud.Voxelize(0.005, X)
C = point_cloud.Voxelize(0.002, X)
if A.shape[0] > B.shape[0] or B.shape[0] > C.shape[0] or C.shape[0] > X.shape[0]:
print X.shape, A.shape, B.shape, C.shape
return False
if A.shape[1] != 3 or B.shape[1] != 3 or C.shape[1] != 3: return False
return True
def TestVoxelizeWithNormals(X):
N = point_cloud.ComputeNormals(X)
XX, NN = point_cloud.Voxelize(0.005, X, N)
norms = norm(NN, axis=1)
if XX.shape[0] > X.shape[0] or NN.shape[0] > N.shape[0]:
print X.shape, XX.shape, N.shape, NN.shape
return False
if XX.shape[0] != NN.shape[0] or XX.shape[1] != NN.shape[1]:
return False
if XX.shape[1] != 3 or NN.shape[1] != 3: return False
if (norms > 1.001).any() or (norms < 0.999).any():
print max(norms), min(norms)
return False
return True
def GetFullCloud(self, viewCenter, viewKeepout, workspace, add45DegViews=False,
computeNormals=False, voxelSize=None):
'''Gets a full point cloud of the scene (6 views) and also computes normals.'''
poses = self.GetFullViewPoses(viewCenter, viewKeepout, add45DegViews)
cloud = []; viewPoints = []
for pose in poses:
self.MoveSensorToPose(pose)
X = self.GetCloud(workspace)
cloud.append(X)
V = tile(pose[0:3, 3], (X.shape[0], 1))
viewPoints.append(V)
cloud = vstack(cloud)
viewPoints = vstack(viewPoints)
if computeNormals:
normals = point_cloud.ComputeNormals(cloud, viewPoints, kNeighbors=30, rNeighbors=-1)
if voxelSize: cloud, normals = point_cloud.Voxelize(voxelSize, cloud, normals)
return cloud, normals
if voxelSize: cloud = point_cloud.Voxelize(voxelSize, cloud)
return cloud
def GetDualCloud(self, viewCenter, viewKeepout, workspace):
'''Gets a point cloud combined form two views, 45 degrees above table and 90 degrees apart.
- Returns cloud: nx3 combined point cloud.
- Returns cloudTree: cKDTree object of cloud.
'''
poses = self.GetDualViewPoses(viewCenter, viewKeepout)
cloud = []
for pose in poses:
self.MoveSensorToPose(pose)
cloud.append(self.GetCloud(workspace))
cloud = vstack(cloud)
cloud = point_cloud.Voxelize(cloud, voxelSize=0.002)
return cloud, cKDTree(cloud)