Exemplo n.º 1
0
    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())
Exemplo n.º 2
0
 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 []
Exemplo n.º 3
0
 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 []
Exemplo n.º 4
0
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]
Exemplo n.º 5
0
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())
Exemplo n.º 6
0
    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
Exemplo n.º 7
0
    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
Exemplo n.º 8
0
    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())