def calcray(self, ray):
ol = self.getfinest()
hits = ol.segment_ovl(None, ray)[0]
if len(hits) > 0:
vox_hits = occupiedlist.pointstovoxels(hits,ol.resolution,ol.lattice)
return quantizer.uniquerows(vox_hits, 0)
else:
return []
def calcray(self, ray):
ol = self.getfinest()
hits = ol.segment_ovl(None, ray)[0]
if len(hits) > 0:
vox_hits = occupiedlist.pointstovoxels(hits, ol.resolution,
ol.lattice)
return quantizer.uniquerows(vox_hits, 0)
else:
return []
def generate_freespace2(self,
pc=None,
pose=None,
resolution=None,
minR=0.5,
maxR=5):
'''
Uses existing ray-trace method to get
around the current limitations of above implementation.
Generates an occuplied list consisting of the observed free space at
the specified resolution. This is useful for detecting object removal
from a map.
'''
if resolution == None:
res = self.get_resolution()
else:
res = resolution
assert res >= self.get_resolution()
free_space_ol = occupiedlist.OccupiedList(res, maxvoxels=2e6)
if pc == None and pose == None:
# generate from map using pose list
if self._sensor_pts is None:
sensor_model_ol = _build_Kinect_sensor_model(res,
minR=minR,
maxR=maxR)
self._sensor_pts = pointcloud.PointCloud(
sensor_model_ol.getpoints())
print "Sensor model points:", len(self._sensor_pts)
base_ol = self.mrol.getfinest()
for pose in self.trajectory:
print pose
inliers, outliers = base_ol.segment_ovl(
pose, self._sensor_pts.points)
inlier_pc = pointcloud.PointCloud(inliers)
free_pts = self.free_space_ray_trace(inliers,
pose.getTuple()[:3],
self.get_resolution())
free_space_ol.addpoints(free_pts.points)
else:
assert xyzs != None and pose != None, "Both xyzs and pose must be specified"
# resampling to align with grid. bad hack, but makes ray_cast easier.
voxs = occupiedlist.pointstovoxels(xyzs, res)
voxs = quantizer.uniquerows(voxs, 0)
X = occupiedlist.voxelstopoints(voxs, res)
X_pc = pointcloud.PointCloud(X)
free_pts = self.free_space_ray_trace(X_pc,
pose.getTuple()[:3],
self.get_resolution())
free_space_ol.addpoints(free_pts.points)
return free_space_ol
def uniformposes(centrepose, spreadpose, positionres, orientationres):
""" Produce uniform distribution of poses with positions separated by
positionres and orientations separated by orientationres, centered around
center pose and spanning +/- spreadpose """
# TODO this needs to be done properly for 3D poses
# see papers on generating uniform random 3D rotations
cp = np.array(centrepose.get())
sp = np.array(spreadpose.get())
B = cp - sp
E = cp + sp + 0.0001
# this small addition is to fix when the spreadpose is 0 still want to
# generate some poses rather than none
S = np.hstack((np.tile(positionres, 3), np.tile(orientationres, 3)))
pose_count = np.prod((E-B)/S)
assert pose_count < 1e5, 'Requested too many poses %d' % int(pose_count)
mg = np.mgrid[B[0]:E[0]:S[0], B[1]:E[1]:S[1], B[2]:E[2]:S[2], B[3]:E[3]:S[3], B[4]:E[4]:S[4], B[5]:E[5]:S[5]]
mg.shape = 6, -1
return quantizer.uniquerows(mg.T, 3)
def generate_freespace2(self,pc=None, pose=None, resolution=None, minR=0.5, maxR=5):
'''
Uses existing ray-trace method to get
around the current limitations of above implementation.
Generates an occuplied list consisting of the observed free space at
the specified resolution. This is useful for detecting object removal
from a map.
'''
if resolution == None:
res = self.get_resolution()
else:
res = resolution
assert res >= self.get_resolution()
free_space_ol = occupiedlist.OccupiedList(res, maxvoxels=2e6)
if pc==None and pose==None:
# generate from map using pose list
if self._sensor_pts is None:
sensor_model_ol = _build_Kinect_sensor_model(res, minR=minR, maxR=maxR)
self._sensor_pts = pointcloud.PointCloud(sensor_model_ol.getpoints())
print "Sensor model points:", len(self._sensor_pts)
base_ol = self.mrol.getfinest()
for pose in self.trajectory:
print pose
inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts.points)
inlier_pc = pointcloud.PointCloud(inliers)
free_pts = self.free_space_ray_trace(inliers,pose.getTuple()[:3],self.get_resolution())
free_space_ol.addpoints(free_pts.points)
else:
assert xyzs != None and pose != None, "Both xyzs and pose must be specified"
# resampling to align with grid. bad hack, but makes ray_cast easier.
voxs = occupiedlist.pointstovoxels(xyzs, res)
voxs = quantizer.uniquerows(voxs, 0)
X = occupiedlist.voxelstopoints(voxs, res)
X_pc = pointcloud.PointCloud(X)
free_pts = self.free_space_ray_trace(X_pc,pose.getTuple()[:3],self.get_resolution())
free_space_ol.addpoints(free_pts.points)
return free_space_ol
def refineposes(poses, deltaposition, deltaorientation):
'''Produces a new set of poses that are around the supplied poses'''
# TODO this needs to be done properly for 3D
# TODO does not handle a single pose
# each pose should spawn 3 new poses for each dimension
# this definitely produces better results in 2D tested by testing on 2D mapping
# and looking at the voxel count of the final map
#mg = np.mgrid[-1:2, -1:2, -1:2, -1:2, -1:2, -1:2].astype(float)
# each pose should spawn 2 new poses for each dimension
mg = np.mgrid[-1:1, -1:1, -1:1, -1:1, -1:1, -1:1].astype(float)
mg += 0.5
mg.shape = 6, -1
mg = mg.T
# TODO remove this nasty hack for 3DOF speed
#mg[:, [2, 3, 4]] = 0
#mg = quantizer.uniquerows(mg, 3)
mg[:, 0:3] *= deltaposition
mg[:, 3:6] *= deltaorientation
#zerorows = np.zeros((6, mg.shape[1]))
#mg = np.vstack((mg[0:3, :]*deltaposition, zerorows, mg[3:6, :]*deltaorientation))
refinedposes = np.empty((poses.shape[0]*mg.shape[0], 6))
row = 0
for p in poses:
ptile = np.tile(p, (mg.shape[0], 1))
newposes = ptile + mg
refinedposes[row:row+mg.shape[0], :] = newposes
row += mg.shape[0]
# TODO remove those outside of centrepose + spreadpose
return quantizer.uniquerows(refinedposes, 3)
