Exemple #1
0
        /// <summary>
        /// initialise with the given number of stereo cameras
        /// </summary>
        /// <param name="no_of_stereo_cameras">the number of stereo cameras on the robot (not the total number of cameras)</param>
        /// <param name="rays_per_stereo_camera">the number of rays which will be thrown from each stereo camera per time step</param>
        /// <param name="mapping_type">the type of mapping to be used</param>
        private void init(int no_of_stereo_cameras, 
                          int rays_per_stereo_camera,
                          int mapping_type)
        {
            this.no_of_stereo_cameras = no_of_stereo_cameras;
            this.mapping_type = mapping_type;

            // head and shoulders
            head = new stereoHead(no_of_stereo_cameras);

            // sensor model used for mapping
            inverseSensorModel = new stereoModel();

            // set the number of stereo features to be detected and inserted into the grid
            // on each time step.  This figure should be large enough to get reasonable
            // detail, but not so large that the mapping consumes a huge amount of 
            // processing resource
            inverseSensorModel.no_of_stereo_features = rays_per_stereo_camera;
            correspondence = new stereoCorrespondence[no_of_stereo_cameras];
			for (int i = 0; i < no_of_stereo_cameras; i++)
                correspondence[i] = new stereoCorrespondence(inverseSensorModel.no_of_stereo_features);

            if (mapping_type == MAPPING_DPSLAM)
            {
	            // add local occupancy grids
	            LocalGrid = new occupancygridMultiHypothesis[mapping_threads];
	            for (int i = 0; i < mapping_threads; i++) createLocalGrid(i);
	
	            // create a motion model for each possible grid
	            motion = new motionModel[mapping_threads];
	            for (int i = 0; i < mapping_threads; i++) motion[i] = new motionModel(this, LocalGrid[i], 100 * (i+1));
            }
            
            if (mapping_type == MAPPING_SIMPLE)
            {
            }

            // a list of places where the robot might work or make observations
            worksites = new kmlZone();

            // zero encoder positions
            prev_left_wheel_encoder = 0;
            prev_right_wheel_encoder = 0;
        }
Exemple #2
0
        /// <summary>
        /// load worksite locations from a KML file
        /// </summary>
        /// <param name="Kml"></param>
        /// <returns></returns>
        public bool LoadWorkSite(String Kml)
        {
            bool loaded = false;

            if (File.Exists(Kml))
            {
                worksites = new kmlZone();
                worksites.Load(Kml);
            }
            return (loaded);
        }