Esempio n. 1
0
		public void GridCreation()
		{
		    int dimension_cells = 50;
		    int dimension_cells_vertical = 30;
		    int cellSize_mm = 50;
		    int localisationRadius_mm = 1000;
		    int maxMappingRange_mm = 5000;
		    float vacancyWeighting = 0;
		    
		    occupancygridMultiHypothesis grid = 
		        new occupancygridMultiHypothesis(dimension_cells,
		                                         dimension_cells_vertical,
		                                         cellSize_mm,
		                                         localisationRadius_mm,
		                                         maxMappingRange_mm,
		                                         vacancyWeighting);
		    
		    Assert.AreNotEqual(grid, null, "object occupancygridMultiHypothesis was not created");
		}
Esempio n. 2
0
        public motionModel(robot rob, 
                           occupancygridMultiHypothesis LocalGrid,
                           int random_seed)
        {
            // seed the random number generator
            rnd = new Random(random_seed); // MersenneTwister(random_seed);

            this.rob = rob;
            this.LocalGrid = LocalGrid;
            Poses = new List<particlePath>();
            ActivePoses = new List<particlePath>();
            motion_noise = new float[6];

            // some default noise values
            int i = 0;
            while (i < 2)
            {
                motion_noise[i] = default_motion_noise_1;
                i++;
            }
            while (i < 4)
            {
                motion_noise[i] = default_motion_noise_2;
                i++;
            }
            while (i < motion_noise.Length)
            {
                motion_noise[i] = default_motion_noise_3;
                i++;
            }

            // create some initial poses
            createNewPoses(rob.x, rob.y, rob.pan);
        }
Esempio n. 3
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="no_of_grids">The number of sub grids</param>
        /// <param name="grid_type">the type of sub grids</param>
        /// <param name="dimension_mm">dimension of the smallest sub grid</param>
        /// <param name="dimension_vertical_mm">vertical dimension of the smallest sub grid</param>
        /// <param name="cellSize_mm">cell size of the smallest sub grid</param>
        /// <param name="localisationRadius_mm">localisation radius within the smallest sub grid</param>
        /// <param name="maxMappingRange_mm">maximum mapping radius within the smallest sub grid</param>
        /// <param name="vacancyWeighting">vacancy model weighting, typically between 0.2 and 2</param>
        public metagrid(
            int no_of_grids,
            int grid_type,
		    int dimension_mm, 
            int dimension_vertical_mm, 
            int cellSize_mm, 
            int localisationRadius_mm, 
            int maxMappingRange_mm, 
            float vacancyWeighting)
        {
            int dimension_cells = dimension_mm / cellSize_mm;
            int dimension_cells_vertical = dimension_vertical_mm / cellSize_mm;
            if (vacancyWeighting < 0.2f) vacancyWeighting = 0.2f;
            this.grid_type = grid_type;
            grid = new occupancygridBase[no_of_grids];

            // values used to weight the matching score for each sub grid
            grid_weight = new float[no_of_grids];
            grid_weight[0] = 1;
            for (int i = 1; i < no_of_grids; i++)
                grid_weight[i] = grid_weight[i] * 0.5f;

            switch (grid_type)
            {
                case TYPE_SIMPLE:
                    {
                        for (int g = 0; g < no_of_grids; g++)
                            grid[g] = new occupancygridSimple(dimension_cells, dimension_cells_vertical, cellSize_mm * (g + 1), localisationRadius_mm * (g + 1), maxMappingRange_mm * (g + 1), vacancyWeighting);

                        break;
                    }
                case TYPE_MULTI_HYPOTHESIS:
                    {
                        for (int g = 0; g < no_of_grids; g++)
                            grid[g] = new occupancygridMultiHypothesis(dimension_cells, dimension_cells_vertical, cellSize_mm * (g + 1), localisationRadius_mm * (g + 1), maxMappingRange_mm * (g + 1), vacancyWeighting);

                        break;
                    }
            }
        }
Esempio n. 4
0
        /// <summary>
        /// remove all poses along the path until a branch point is located
        /// </summary>
        /// <param name="pose"></param>
        /// <returns></returns>
        private particlePose CollapseBranch(particlePose pose,
                                            occupancygridMultiHypothesis grid)
        {
            int children = 0;
            while ((pose != branch_pose) &&
                   (pose.path == this) &&
                   (children == 0))
            {
                if (pose != current_pose)
                    children = pose.no_of_children;

                if (children == 0)
                    if (pose.path == this)
                    {
                        pose.Remove(grid);
                        pose = pose.parent;
                    }
            }

            return (pose);
        }
Esempio n. 5
0
        /// <summary>
        /// remove the mapping particles associated with this path
        /// </summary>
        public bool Remove(occupancygridMultiHypothesis grid)
        {
            Enabled = false;

            particlePose pose = current_pose;
            if (current_pose != null)
            {
                // collapse this path down to the next branching point
                pose = CollapseBranch(pose, grid);
                if (pose != null)
                {
                    if (pose != current_pose)
                    {
                        if (pose.no_of_children > 0)
                        {
                            // reduce the number of children at the branch point
                            pose.no_of_children--;
                        }                        
                    }

                    // keep on truckin' down the line...
                    incrementPathChildren(pose.path, -1);
                    if (pose.path.total_children == 0)
                        pose.path.Remove(grid);
                }
            }

            return (!Enabled);
        }
Esempio n. 6
0
 /// <summary>
 /// distill all grid particles within this path
 /// </summary>
 /// <param name="grid"></param>
 public void Distill(occupancygridMultiHypothesis grid)
 {
     particlePose pose = current_pose;
     while (pose != null)
     {
         pose.Distill(grid);
         pose = pose.parent;
     }
     map_cache = null;
     Enabled = false;
 }
Esempio n. 7
0
 /// <summary>
 /// create a path planner for the given grid
 /// </summary>
 /// <param name="grid">occupancy grid</param>
 private void createPlanner(occupancygridMultiHypothesis grid)
 {
     if (planner == null)
         planner = new sentience.pathplanner.pathplanner(grid.navigable_space, (int)LocalGridCellSize_mm, grid.x, grid.y);
     else
         planner.navigable_space = grid.navigable_space;
 }
Esempio n. 8
0
        /// <summary>
        /// recreate the local grid
        /// </summary>
        /// <param name="index">index number of the occupancy grid being considered</param>
        private void createLocalGrid(int index)
        {
            // create the local grid
            LocalGrid[index] = new occupancygridMultiHypothesis(LocalGridDimension, LocalGridDimensionVertical, (int)LocalGridCellSize_mm, (int)LocalGridLocalisationRadius_mm, (int)LocalGridMappingRange_mm, LocalGridVacancyWeighting);

            //createPlanner(LocalGrid[index]);
        }
Esempio n. 9
0
 /// <summary>
 /// distills all grid particles associated with this pose
 /// </summary>
 /// <param name="grid">grid to be updated</param>
 public void Distill(occupancygridMultiHypothesis grid)
 {
     for (int i = observed_grid_cells.Count-1; i >= 0; i--)
     {
         particleGridCell hypothesis = observed_grid_cells[i];
         grid.Distill(hypothesis);
     }
 }
Esempio n. 10
0
        /// <summary>
        /// add an observation taken from this pose
        /// </summary>
        /// <param name="stereo_rays">list of ray objects in this observation</param>
        /// <param name="rob">robot object</param>
        /// <param name="LocalGrid">occupancy grid into which to insert the observation</param>
        /// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
        /// <returns>localisation matching score</returns>
        public float AddObservation(List<evidenceRay>[] stereo_rays,
                                    robot rob, 
                                    occupancygridMultiHypothesis LocalGrid,
                                    bool localiseOnly)
        {
            // clear the localisation score
            float localisation_score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;

            // get the positions of the head and cameras for this pose
            pos3D head_location = new pos3D(0,0,0);
            pos3D[] camera_centre_location = new pos3D[rob.head.no_of_stereo_cameras];
            pos3D[] left_camera_location = new pos3D[rob.head.no_of_stereo_cameras];
            pos3D[] right_camera_location = new pos3D[rob.head.no_of_stereo_cameras];
            calculateCameraPositions(rob, ref head_location,
                                     ref camera_centre_location,
                                     ref left_camera_location,
                                     ref right_camera_location);
            
            // itterate for each stereo camera
            int cam = stereo_rays.Length - 1;
            while (cam >= 0)
            {
                // itterate through each ray
                int r = stereo_rays[cam].Count - 1;
                while (r >= 0)
                {
                    // observed ray.  Note that this is in an egocentric
                    // coordinate frame relative to the head of the robot
                    evidenceRay ray = stereo_rays[cam][r];

                    // translate and rotate this ray appropriately for the pose
                    evidenceRay trial_ray = ray.trialPose(camera_centre_location[cam].pan, 
                                                          camera_centre_location[cam].x, 
                                                          camera_centre_location[cam].y);

                    // update the grid cells for this ray and update the
                    // localisation score accordingly
                    float score =
                        LocalGrid.Insert(trial_ray, this, 
                                         rob.head.sensormodel[cam],
                                         left_camera_location[cam], 
                                         right_camera_location[cam],
                                         localiseOnly);
                    if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                        if (localisation_score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                            localisation_score += score;
                        else
                            localisation_score = score;
                    r--;
                }
                cam--;
            }

            return (localisation_score);
        }