def free_space_ray_trace(self, pc, origin, resolution, voxel_offset=0):
# This is essentially independant of the VoxelMap; put elsewhere?
free_pts = []
res = resolution
v_off = np.ceil(voxel_offset * (res / self.get_resolution()))
free_pts_OL = occupiedlist.OccupiedList(res, maxvoxels=3e6)
count = 0
for point in pc.points:
ray_vox_hits, range2, dir_unit, all_ray_pts = self.ray_trace_pts(
origin, point, return_extras=True)
all_ray_pts = all_ray_pts.points[:-(v_off + 1)]
all_ray_vox = occupiedlist.pointstovoxels(all_ray_pts, res)
all_ray_vox_ol = occupiedlist.OccupiedList(1)
all_ray_vox_ol.addpoints(all_ray_vox)
ray_vox_hits_ol = occupiedlist.OccupiedList(1)
ray_vox_hits_ol.addpoints(ray_vox_hits.points)
free_ray_vox_ol = all_ray_vox_ol - ray_vox_hits_ol
free_pts_OL.addpoints(free_ray_vox_ol.getpoints() * res)
count += 1
#if np.remainder(count / len(pc) * 100,1) == 0:
# print count / len(pc) * 100 , '%'
return pointcloud.PointCloud(free_pts_OL.getpoints())
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 volumetricsample(P, res):
inds = occupiedlist.pointstovoxels(P, res)
#np.random.shuffle(inds)
# TODO accessing a private method should move this to occupiedlist, or make
# it part of the public api of occupiedlist?
ids = occupiedlist._pack(inds)
# This introduces a systematic error as inds_unique is just the first index encountered.
ids_unique, inds_unique = np.unique1d(ids, return_index=True)
return P[inds_unique]
def volumetricsample2(P, res):
'''Return a subset of points where one has been selected at random from
each cell.'''
np.random.shuffle(P)
mapvoxels = {}
# TODO use generators and numpy to optimize for speed?
for p in P:
pV = occupiedlist.pointstovoxels(p, res)
mapvoxels[(pV[0], pV[1], pV[2])] = (p[0], p[1], p[2])
return np.array(mapvoxels.values())
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 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 free_space_ray_trace(self,pc,origin,resolution,voxel_offset=0):
# This is essentially independant of the VoxelMap; put elsewhere?
free_pts = []
res = resolution
v_off = np.ceil(voxel_offset*(res/self.get_resolution()))
free_pts_OL = occupiedlist.OccupiedList(res,maxvoxels=3e6)
count = 0
for point in pc.points:
ray_vox_hits, range2, dir_unit, all_ray_pts = self.ray_trace_pts(origin,point,return_extras=True)
all_ray_pts = all_ray_pts.points[:-(v_off+1)]
all_ray_vox = occupiedlist.pointstovoxels(all_ray_pts, res)
all_ray_vox_ol = occupiedlist.OccupiedList(1)
all_ray_vox_ol.addpoints(all_ray_vox)
ray_vox_hits_ol = occupiedlist.OccupiedList(1)
ray_vox_hits_ol.addpoints(ray_vox_hits.points)
free_ray_vox_ol = all_ray_vox_ol - ray_vox_hits_ol
free_pts_OL.addpoints(free_ray_vox_ol.getpoints()*res)
count +=1
#if np.remainder(count / len(pc) * 100,1) == 0:
# print count / len(pc) * 100 , '%'
return pointcloud.PointCloud(free_pts_OL.getpoints())
