def segment_ovl(self, pose, xyzs): ''' returns the inliers and outliers of the intersection of a point cloud with this occupied voxel list.''' xyzs = poseutil.check_and_transform_points(xyzs, pose)[0] overlap_inds = np.array(self.calccollisions(None, xyzs, returntotal=False)) inliers = xyzs[overlap_inds] outliers = xyzs[np.logical_not(overlap_inds)] return inliers, outliers
def segment_ovl(self, pose, xyzs): """ returns the inliers and outliers of the intersection of a point cloud with this occupied voxel list.""" xyzs = poseutil.check_and_transform_points(xyzs, pose)[0] overlap_inds = np.array(self.calccollisions(None, xyzs, returntotal=False)) inliers = xyzs[overlap_inds] outliers = xyzs[np.logical_not(overlap_inds)] return inliers, outliers
def remove_points(self, xyzs, pose): '''Remove points from the map. In practice, the idea is to decrement the probability of the voxel in the map having been observed. Returns the voxels at the finest resolution that are now empty. ''' xyzs = poseutil.check_and_transform_points(xyzs, pose)[0] return self.mrol.removepoints(xyzs)
def remove_points(self, xyzs, pose): '''Remove points from the map. In practice, the idea is to decrement the probability of the voxel in the map having been observed. Returns the voxels at the finest resolution that are now empty. ''' xyzs = poseutil.check_and_transform_points(xyzs,pose)[0] return self.mrol.removepoints(xyzs)
def calccollisions(self, pose, xyzs, returntotal=True, query=False, out=None): '''The intersection of points with this occupied voxel list. If query then return the density for each point after transformation by the given pose, rather than just a count of hits. out is an output array for the result similar to numpy function.''' transformedpts, pose = poseutil.check_and_transform_points(xyzs,pose) #if type(pose) == type(poseutil.Pose3D()): # if pose == poseutil.Pose3D(): # transformedpts = xyzs # else: # transformedpts = pose.transformPoints(xyzs) #else: # if pose == None or (np.array(pose) == np.array((0, 0, 0, 0, 0, 0))).all() or (np.array(pose) == np.array((0.0, 0.0, 0.0, 0.0, 0.0, 0.0))).all(): # transformedpts = xyzs # else: # poseobj = poseutil.Pose3D() # poseobj.set(pose) # transformedpts = poseobj.transformPoints(xyzs) # TODO could speed this up by combining the transformPoints and division by # resolution into one operation by generating a matrix that represents the # transformation divided by the resolution and multiplying points through # by that # could also combine in the hash operation as well # in place operations and integer arithmetic might help transformedvoxels = pointstovoxels(transformedpts, self.resolution) ids = _pack(transformedvoxels) if query: if out is None: return [self._voxels[ID] for ID in ids] else: # TODO implement this raise NotImplemented #return sum(self._voxels[ID] for ID in ids) if self.use_bloom: present = self.bloomfilter.contains(ids) if returntotal: return np.sum(present) else: return present else: # no range weighting TODO decide whether it is needed # Think the range weighting has been fixed by volumetric sampling if returntotal: return sum(ID in self._voxels for ID in ids) else: return [ID in self._voxels for ID in ids]
def calccollisions(self, pose, xyzs, returntotal=True, query=False, out=None): """The intersection of points with this occupied voxel list. If query then return the density for each point after transformation by the given pose, rather than just a count of hits. out is an output array for the result similar to numpy function.""" transformedpts, pose = poseutil.check_and_transform_points(xyzs, pose) # if type(pose) == type(poseutil.Pose3D()): # if pose == poseutil.Pose3D(): # transformedpts = xyzs # else: # transformedpts = pose.transformPoints(xyzs) # else: # if pose == None or (np.array(pose) == np.array((0, 0, 0, 0, 0, 0))).all() or (np.array(pose) == np.array((0.0, 0.0, 0.0, 0.0, 0.0, 0.0))).all(): # transformedpts = xyzs # else: # poseobj = poseutil.Pose3D() # poseobj.set(pose) # transformedpts = poseobj.transformPoints(xyzs) # TODO could speed this up by combining the transformPoints and division by # resolution into one operation by generating a matrix that represents the # transformation divided by the resolution and multiplying points through # by that # could also combine in the hash operation as well # in place operations and integer arithmetic might help transformedvoxels = pointstovoxels(transformedpts, self.resolution) ids = _pack(transformedvoxels) if query: if out is None: return [self._voxels[ID] for ID in ids] else: # TODO implement this raise NotImplemented # return sum(self._voxels[ID] for ID in ids) if self.use_bloom: present = self.bloomfilter.contains(ids) if returntotal: return np.sum(present) else: return present else: # no range weighting TODO decide whether it is needed # Think the range weighting has been fixed by volumetric sampling if returntotal: return sum(ID in self._voxels for ID in ids) else: return [ID in self._voxels for ID in ids]
def raytrace(self, pose, length, axis=0): ''' perform a ray-trace from the pose and return all map voxels at the base resolution along the ray. It traces from any point in the map frame (e.g. the sensor's current position) along a ray oriented as per the pose's angular components. ''' res = self.getresolution()/2. #import pdb;pdb.set_trace() numcells = np.ceil(length/res)+2 # + 1 for 0th point + 1 for end point with poor aligned voxels ray = np.zeros([numcells,3]) ray[:,axis] = np.hstack([0,np.cumsum(np.ones(numcells-1))*res]) ray = poseutil.check_and_transform_points(ray, pose)[0] return self.calcray(ray)
def raytrace(self, pose, length, axis=0): ''' perform a ray-trace from the pose and return all map voxels at the base resolution along the ray. It traces from any point in the map frame (e.g. the sensor's current position) along a ray oriented as per the pose's angular components. ''' res = self.getresolution() / 2. #import pdb;pdb.set_trace() numcells = np.ceil( length / res ) + 2 # + 1 for 0th point + 1 for end point with poor aligned voxels ray = np.zeros([numcells, 3]) ray[:, axis] = np.hstack([0, np.cumsum(np.ones(numcells - 1)) * res]) ray = poseutil.check_and_transform_points(ray, pose)[0] return self.calcray(ray)
def add_points(self, xyzs, pose, timestamp=None): '''Adds the pointcloud xyzs to the map, transforming them by the given pose if required. Returns the overlap of the new points against the existing map.''' xyzs = xyzs.points xyzs, return_pose = poseutil.check_and_transform_points(xyzs, pose) self.trajectory.append(return_pose) if self._rv is not None: finest = self.mrol.getfinest() # determine unique new voxels convert to points and add to the map ol = occupiedlist.OccupiedList(finest.resolution) ol.addpoints(xyzs) ol = ol - finest self._rv.addmappts(ol.getpoints()) #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2])) self._rv.setrobotpose(return_pose.getMatrix()) self._rv.addtrajectorypoint(return_pose.getTuple()[0:3]) if self._rv_changes: inliers, outliers = self.mrol.getfinest().segment_ovl(None,xyzs) self._rv.setrightpts(inliers) self._rv.setleftpts(outliers) self.mrol.addpoints(xyzs, timestamp=timestamp) return self.get_overlap(return_pose.getTuple(), xyzs)
def add_points(self, xyzs, pose, timestamp=None): '''Adds the pointcloud xyzs to the map, transforming them by the given pose if required. Returns the overlap of the new points against the existing map.''' xyzs = xyzs.points xyzs, return_pose = poseutil.check_and_transform_points(xyzs, pose) self.trajectory.append(return_pose) if self._rv is not None: finest = self.mrol.getfinest() # determine unique new voxels convert to points and add to the map ol = occupiedlist.OccupiedList(finest.resolution) ol.addpoints(xyzs) ol = ol - finest self._rv.addmappts(ol.getpoints()) #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2])) self._rv.setrobotpose(return_pose.getMatrix()) self._rv.addtrajectorypoint(return_pose.getTuple()[0:3]) if self._rv_changes: inliers, outliers = self.mrol.getfinest().segment_ovl( None, xyzs) self._rv.setrightpts(inliers) self._rv.setleftpts(outliers) self.mrol.addpoints(xyzs, timestamp=timestamp) return self.get_overlap(return_pose.getTuple(), xyzs)