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())