예제 #1
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 []
예제 #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 []
예제 #3
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
예제 #4
0
def uniformposes(centrepose, spreadpose, positionres, orientationres):
    """ Produce uniform distribution of poses with positions separated by 
    positionres and orientations separated by orientationres, centered around 
    center pose and spanning +/- spreadpose """
    # TODO this needs to be done properly for 3D poses
    # see papers on generating uniform random 3D rotations
    cp = np.array(centrepose.get())
    sp = np.array(spreadpose.get())
    B = cp - sp 
    E = cp + sp + 0.0001 
    # this small addition is to fix when the spreadpose is 0 still want to 
    # generate some poses rather than none
    S = np.hstack((np.tile(positionres, 3), np.tile(orientationres, 3)))
    pose_count = np.prod((E-B)/S)
    assert pose_count < 1e5, 'Requested too many poses %d' % int(pose_count)

    mg = np.mgrid[B[0]:E[0]:S[0], B[1]:E[1]:S[1], B[2]:E[2]:S[2], B[3]:E[3]:S[3], B[4]:E[4]:S[4], B[5]:E[5]:S[5]]
    mg.shape = 6, -1
    return quantizer.uniquerows(mg.T, 3)
예제 #5
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
예제 #6
0
def refineposes(poses, deltaposition, deltaorientation):
    '''Produces a new set of poses that are around the supplied poses'''
    # TODO this needs to be done properly for 3D
    # TODO does not handle a single pose
   
    # each pose should spawn 3 new poses for each dimension
    # this definitely produces better results in 2D tested by testing on 2D mapping 
    # and looking at the voxel count of the final map
    #mg = np.mgrid[-1:2, -1:2, -1:2, -1:2, -1:2, -1:2].astype(float)
    # each pose should spawn 2 new poses for each dimension

    mg = np.mgrid[-1:1, -1:1, -1:1, -1:1, -1:1, -1:1].astype(float)
    mg += 0.5

    mg.shape = 6, -1
    mg = mg.T

    # TODO remove this nasty hack for 3DOF speed
    #mg[:, [2, 3, 4]] = 0
    #mg = quantizer.uniquerows(mg, 3)

    mg[:, 0:3] *= deltaposition
    mg[:, 3:6] *= deltaorientation
    #zerorows = np.zeros((6, mg.shape[1]))
    #mg = np.vstack((mg[0:3, :]*deltaposition, zerorows, mg[3:6, :]*deltaorientation))

    refinedposes = np.empty((poses.shape[0]*mg.shape[0], 6))
    row = 0
    for p in poses:
        ptile = np.tile(p, (mg.shape[0], 1))
        newposes = ptile + mg
        refinedposes[row:row+mg.shape[0], :] = newposes
        row += mg.shape[0]

    # TODO remove those outside of centrepose + spreadpose
    return quantizer.uniquerows(refinedposes, 3)