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)
