Exemple #1
0
        /// <summary>
        /// show the position uncertainty distribution
        /// </summary>
        /// <param name="img">image data within which to draw</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        public void Show(Byte[] img, int width, int height,
                         bool clearBackground)
        {
            float min_x = 99999;
            float min_y = 99999;
            float max_x = -99999;
            float max_y = -99999;

            for (int sample = 0; sample < Poses.Count; sample++)
            {
                particlePose pose = Poses[sample].current_pose;
                if (pose.x < min_x)
                {
                    min_x = pose.x;
                }
                if (pose.y < min_y)
                {
                    min_y = pose.y;
                }
                if (pose.x > max_x)
                {
                    max_x = pose.x;
                }
                if (pose.y > max_y)
                {
                    max_y = pose.y;
                }
            }
            Show(img, width, height, min_x, min_y, max_x, max_y, clearBackground);
        }
Exemple #2
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);
        }
Exemple #3
0
        /// <summary>
        /// show an overhead view of the grid map as an image
        /// </summary>
        /// <param name="grid_index">index number of the grid to be shown</param>
        /// <param name="img">colour image data</param>
        /// <param name="width">width in pixels</param>
        /// <param name="height">height in pixels</param>
        /// <param name="show_all_occupied_cells">show all occupied pixels</param>
        /// <param name="map_width_mm">width of the larger map area within this will be displayed</param>
        /// <param name="map_height_mm">height of the larger map area within this will be displayed</param>
        /// <param name="map_centre_x_mm">centre x position of the larger map area within this will be displayed</param>
        /// <param name="map_centre_y_mm">centre x position of the larger map area within this will be displayed</param>
        /// <param name="clear_image">clear the image</param>
        /// <param name="show_localisation">whether to show grid cells probed during localisation</param>
        public void Show(
            int grid_index,
            byte[] img,
            int width,
            int height,
            bool show_all_occupied_cells,
            int map_width_mm,
            int map_height_mm,
            int map_centre_x_mm,
            int map_centre_y_mm,
            bool clear_image,
            bool show_localisation)
        {
            switch (grid_type)
            {
            case TYPE_SIMPLE:
            {
                occupancygridSimple grd = (occupancygridSimple)grid[grid_index];
                grd.Show(img, width, height, show_all_occupied_cells, map_width_mm, map_height_mm, map_centre_x_mm, map_centre_y_mm, clear_image, show_localisation);
                break;
            }

            case TYPE_MULTI_HYPOTHESIS:
            {
                // TODO: get the best pose
                particlePose pose = null;
                occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
                grd.Show(
                    occupancygridMultiHypothesis.VIEW_ABOVE,
                    img, width, height, pose,
                    true, true);
                break;
            }
            }
        }
Exemple #4
0
        /// <summary>
        /// returns a list containing the robots velocity at each point in the path
        /// </summary>
        /// <param name="starting_forward_velocity">the forward velocity to start with in mm/sec</param>
        /// <param name="starting_angular_velocity">the angular velocity to start with in radians/sec</param>
        /// <param name="time_per_index_sec">time taken between indexes along the path in seconds</param>
        /// <returns>list of forward and angular velocities</returns>
        public List <float> getVelocities(float starting_forward_velocity,
                                          float starting_angular_velocity,
                                          float time_per_index_sec)
        {
            List <float> result = new List <float>();

            float forward_velocity = starting_forward_velocity;
            float angular_velocity = starting_angular_velocity;

            for (int i = 1; i < path.Count; i++)
            {
                particlePose prev_pose = path[i - 1];
                particlePose pose      = path[i];

                float dx           = pose.x - prev_pose.x;
                float dy           = pose.y - prev_pose.y;
                float distance     = (float)Math.Sqrt((dx * dx) + (dy * dy));
                float acceleration = (2 * (distance - (forward_velocity * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
                acceleration    /= time_per_index_sec;
                forward_velocity = forward_velocity + (acceleration * time_per_index_sec);
                result.Add(forward_velocity);

                distance         = pose.pan - prev_pose.pan;
                acceleration     = (2 * (distance - (angular_velocity * time_per_index_sec))) / (time_per_index_sec * time_per_index_sec);
                acceleration    /= time_per_index_sec;
                angular_velocity = angular_velocity + (acceleration * time_per_index_sec);
                result.Add(angular_velocity);
            }
            return(result);
        }
Exemple #5
0
        /// <summary>
        /// probes the grid using the given 3D line and returns the distance to the nearest occupied grid cell
        /// </summary>
        /// <param name="grid_index">index number of the grid to be probed</param>
        /// <param name="x0_mm">start x coordinate</param>
        /// <param name="y0_mm">start y coordinate</param>
        /// <param name="z0_mm">start z coordinate</param>
        /// <param name="x1_mm">end x coordinate</param>
        /// <param name="y1_mm">end y coordinate</param>
        /// <param name="z1_mm">end z coordinate</param>
        /// <returns>range to the nearest occupied grid cell in millimetres</returns>
        public float ProbeRange(
            int grid_index,
            float x0_mm,
            float y0_mm,
            float z0_mm,
            float x1_mm,
            float y1_mm,
            float z1_mm)
        {
            float range_mm = -1;

            switch (grid_type)
            {
            case TYPE_SIMPLE:
            {
                range_mm = grid[grid_index].ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
                break;
            }

            case TYPE_MULTI_HYPOTHESIS:
            {
                occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_index];
                // TODO get the best pose estimate
                particlePose pose = null;
                range_mm = grd.ProbeRange(x0_mm, y0_mm, z0_mm, x1_mm, y1_mm, z1_mm);
                break;
            }
            }
            return(range_mm);
        }
Exemple #6
0
        /// <summary>
        /// insert a stereo ray into the grid
        /// </summary>
        /// <param name="ray">stereo ray</param>
        /// <param name="origin">pose from which this observation was made</param>
        /// <param name="sensormodel">sensor model for each grid level</param>
        /// <param name="left_camera_location">left stereo camera position and orientation</param>
        /// <param name="right_camera_location">right stereo camera position and orientation</param>
        /// <param name="localiseOnly">whether we are mapping or localising</param>
        /// <returns>localisation matching score</returns>
        public float Insert(
            evidenceRay ray,
            particlePose origin,
            stereoModel[] sensormodel,
            pos3D left_camera_location,
            pos3D right_camera_location,
            bool localiseOnly)
        {
            float matchingScore = 0;

            switch (grid_type)
            {
            case TYPE_MULTI_HYPOTHESIS:
            {
                for (int grid_level = 0; grid_level < grid.Length; grid_level++)
                {
                    rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
                    occupancygridMultiHypothesis grd  = (occupancygridMultiHypothesis)grid[grid_level];
                    matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
                }
                break;
            }
            }
            return(matchingScore);
        }
Exemple #7
0
        /// <summary>
        /// parse an xml node to extract camera calibration parameters
        /// </summary>
        /// <param name="xnod"></param>
        /// <param name="level"></param>
        public void LoadFromXml(XmlNode xnod,
                                int level)
        {
            XmlNode xnodWorking;

            if (xnod.Name == "Pose")
            {
                IFormatProvider format   = new System.Globalization.CultureInfo("en-GB");
                string[]        poseStr  = xnod.InnerText.Split(',');
                particlePose    new_pose = new particlePose(Convert.ToSingle(poseStr[0], format),
                                                            Convert.ToSingle(poseStr[1], format),
                                                            Convert.ToSingle(poseStr[2], format) * (float)Math.PI / 180.0f, null);
                Add(new_pose);
            }


            // call recursively on all children of the current node
            if ((xnod.HasChildNodes) &&
                ((xnod.Name == "RobotPath") ||
                 (xnod.Name == "Sentience")
                ))
            {
                xnodWorking = xnod.FirstChild;
                while (xnodWorking != null)
                {
                    LoadFromXml(xnodWorking, level + 1);
                    xnodWorking = xnodWorking.NextSibling;
                }
            }
        }
Exemple #8
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);
        }
Exemple #9
0
        /// <summary>
        /// add a new pose to the path
        /// </summary>
        /// <param name="pose">pose to be added</param>
        public void Add(particlePose pose)
        {
            pose.path = this;

            // set the parent of this pose
            pose.parent = current_pose;

            // update the list of previous path IDs
            if (branch_pose != null)
            {
                if (current_pose == branch_pose)
                {
                    pose.previous_paths = new List <particlePath>();
                    int min = branch_pose.previous_paths.Count - MAX_PATH_HISTORY;
                    if (min < 0)
                    {
                        min = 0;
                    }
                    for (int i = min; i < branch_pose.previous_paths.Count; i++)
                    {
                        pose.previous_paths.Add(branch_pose.previous_paths[i]);
                    }
                    pose.previous_paths.Add(this);
                }
                else
                {
                    pose.previous_paths = pose.parent.previous_paths;
                }
            }
            else
            {
                if (pose.parent == null)
                {
                    pose.previous_paths = new List <particlePath>();
                    pose.previous_paths.Add(this);
                }
                else
                {
                    pose.previous_paths = pose.parent.previous_paths;
                }
            }

            // set the current pose
            current_pose = pose;

            // add the pose to the path
            path.Add(pose);

            //total_score += pose.score;
            total_poses++;

            // ensure that the path does not exceed a maximum length
            if (path.Count > max_length)
            {
                // remove the oldest pose in the path
                path.RemoveAt(0);
            }
        }
Exemple #10
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;
        }
 /// <summary>
 /// constructor
 /// </summary>
 /// <param name="x">x coordinate of the cell within the grid</param>
 /// <param name="y">y coordinate of the cell within the grid</param>
 /// <param name="z">z coordinate of the cell within the grid</param>
 /// <param name="probability">probability assigned to this cell</param>
 /// <param name="pose">the pose from which this cell was observed</param>
 /// <param name="colour">the colour of this cell</param>
 public particleGridCell(
     int x, int y, int z, 
     float probability, 
     particlePose pose,
     byte[] colour)
 {
     this.x = (short)x;
     this.y = (short)y;
     this.z = (short)z;
     this.probabilityLogOdds = probabilities.LogOdds(probability); // store as log odds
     this.pose = pose;
     this.colour = colour;
     Enabled = true;
 }
 /// <summary>
 /// constructor
 /// </summary>
 /// <param name="x">x coordinate of the cell within the grid</param>
 /// <param name="y">y coordinate of the cell within the grid</param>
 /// <param name="z">z coordinate of the cell within the grid</param>
 /// <param name="probability">probability assigned to this cell</param>
 /// <param name="pose">the pose from which this cell was observed</param>
 /// <param name="colour">the colour of this cell</param>
 public particleGridCell(
     int x, int y, int z,
     float probability,
     particlePose pose,
     byte[] colour)
 {
     this.x = (short)x;
     this.y = (short)y;
     this.z = (short)z;
     this.probabilityLogOdds = probabilities.LogOdds(probability); // store as log odds
     this.pose   = pose;
     this.colour = colour;
     Enabled     = true;
 }
Exemple #13
0
 /// <summary>
 /// show an above view of the robot using the best available pose
 /// </summary>
 /// <param name="img">image data within which to draw</param>
 /// <param name="width">width of the image</param>
 /// <param name="height">height of the image</param>
 /// <param name="min_x">bounding box top left x coordinate</param>
 /// <param name="min_y">bounding box top left y coordinate</param>
 /// <param name="max_x">bounding box bottom right x coordinate</param>
 /// <param name="max_y">bounding box bottom right y coordinate</param>
 /// <param name="clear_background">whether to clear the image before drawing</param>
 public void ShowBestPose(Byte[] img, int width, int height,
                          int min_x, int min_y, int max_x, int max_y,
                          bool clear_background, bool showFieldOfView)
 {
     if (best_path != null)
     {
         particlePose best_pose = best_path.current_pose;
         if (best_pose != null)
         {
             best_pose.Show(rob, img, width, height,
                            clear_background, min_x, min_y, max_x, max_y, 0, showFieldOfView);
         }
     }
 }
 /// <summary>
 /// return this path segment as a sequence of poses
 /// </summary>
 /// <returns>list of poses of type particlePose</returns>
 public List<particlePose> getPoses()
 {
     List<particlePose> result = new List<particlePose>();
     float xx = x;
     float yy = y;
     float curr_pan = pan;
     for (int i = 0; i < no_of_steps; i++)
     {
         particlePose pose = new particlePose(xx, yy, curr_pan, null);
         result.Add(pose);
         curr_pan += pan_per_step;
         xx += (distance_per_step_mm * (float)Math.Sin(curr_pan));
         yy += (distance_per_step_mm * (float)Math.Cos(curr_pan));
     }
     return (result);
 }
Exemple #15
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="x">x position in millimetres</param>
        /// <param name="y">y position in millimetres</param>
        /// <param name="pan">pan angle in radians</param>
        /// <param name="max_length">the maximum number of poses which may be stored within this path</param>
        /// <param name="time_step">time step</param>
        /// <param name="path_ID">ID number for the path</param>
        /// <param name="grid_dimension_cells">occupancy grid dimension</param>
        public particlePath(float x,
                            float y,
                            float pan,
                            int max_length,
                            UInt32 time_step,
                            UInt32 path_ID,
                            int grid_dimension_cells)
        {
            ID = path_ID;
            this.max_length = max_length;
            path            = new List <particlePose>();
            particlePose pose = new particlePose(x, y, pan, this);

            pose.time_step = time_step;
            Enabled        = true;
            Add(pose);
        }
        /// <summary>
        /// return this path segment as a sequence of poses
        /// </summary>
        /// <returns>list of poses of type particlePose</returns>
        public List <particlePose> getPoses()
        {
            List <particlePose> result = new List <particlePose>();
            float xx       = x;
            float yy       = y;
            float curr_pan = pan;

            for (int i = 0; i < no_of_steps; i++)
            {
                particlePose pose = new particlePose(xx, yy, curr_pan, null);
                result.Add(pose);
                curr_pan += pan_per_step;
                xx       += (distance_per_step_mm * (float)Math.Sin(curr_pan));
                yy       += (distance_per_step_mm * (float)Math.Cos(curr_pan));
            }
            return(result);
        }
Exemple #17
0
        /// <summary>
        /// show an above view of the robot using the best available pose
        /// </summary>
        /// <param name="img">image data within which to draw</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="clear_background">whether to clear the image before drawing</param>
        public void ShowBestPose(byte[] img, int width, int height,
                                 bool clear_background, bool showFieldOfView)
        {
            if (best_path != null)
            {
                particlePose best_pose = best_path.current_pose;
                if (best_pose != null)
                {
                    int min_x = (int)(best_pose.x - rob.BodyWidth_mm);
                    int min_y = (int)(best_pose.y - rob.BodyLength_mm);
                    int max_x = (int)(best_pose.x + rob.BodyWidth_mm);
                    int max_y = (int)(best_pose.y + rob.BodyLength_mm);

                    ShowBestPose(img, width, height, min_x, min_y, max_x, max_y, clear_background, showFieldOfView);
                }
            }
        }
Exemple #18
0
        /// <summary>
        /// update the given pose using the motion model
        /// </summary>
        /// <param name="path">path to add the new estimated pose to</param>
        /// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
        private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
        {
            // calculate noisy velocities
            float fwd_velocity = forward_velocity + sample_normal_distribution(
                (motion_noise[0] * Math.Abs(forward_velocity)) +
                (motion_noise[1] * Math.Abs(angular_velocity)));

            float ang_velocity = angular_velocity + sample_normal_distribution(
                (motion_noise[2] * Math.Abs(forward_velocity)) +
                (motion_noise[3] * Math.Abs(angular_velocity)));

            float v = sample_normal_distribution(
                (motion_noise[4] * Math.Abs(forward_velocity)) +
                (motion_noise[5] * Math.Abs(angular_velocity)));

            float fraction = 0;

            if (Math.Abs(ang_velocity) > 0.000001f)
            {
                fraction = fwd_velocity / ang_velocity;
            }
            float current_pan = path.current_pose.pan;

            // if scan matching is active use the current estimated pan angle
            if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
            {
                current_pan = rob.ScanMatchingPanAngleEstimate;
            }

            float pan2 = current_pan - (ang_velocity * time_elapsed_sec);

            float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
                          (fraction * (float)Math.Sin(pan2));
            float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
                          (fraction * (float)Math.Cos(pan2));
            float new_pan = pan2 + (v * time_elapsed_sec);

            particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);

            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
Exemple #19
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="parent">parent path from which this originates</param>
        /// <param name="path_ID">unique ID for this path</param>
        /// <param name="grid_dimension_cells">dimension of the grid</param>
        public particlePath(particlePath parent,
                            UInt32 path_ID,
                            int grid_dimension_cells)
        {
            ID = path_ID;
            parent.current_pose.no_of_children++;
            current_pose = parent.current_pose;
            branch_pose  = parent.current_pose;

            incrementPathChildren(parent, 1);

            this.max_length = parent.max_length;
            path            = new List <particlePose>();
            total_score     = parent.total_score;
            total_poses     = parent.total_poses;
            Enabled         = true;

            // map cache for this path
            //map_cache = new ArrayList[grid_dimension_cells][][];
        }
Exemple #20
0
        /// <summary>
        /// returns the path as an xml object
        /// </summary>
        /// <param name="doc"></param>
        /// <param name="parent"></param>
        /// <returns></returns>
        public XmlElement getXml(XmlDocument doc,
                                 XmlElement parent)
        {
            XmlElement nodePath = doc.CreateElement("RobotPath");

            parent.AppendChild(nodePath);

            xml.AddComment(doc, nodePath, "The path through which the robot has moved, as an X,Y coordinate");
            xml.AddComment(doc, nodePath, "in millimetres followed by the heading in degrees");

            IFormatProvider format = new System.Globalization.CultureInfo("en-GB");

            for (int i = 0; i < path.Count; i++)
            {
                particlePose pose = path[i];
                xml.AddTextElement(doc, nodePath, "Pose", Convert.ToString(pose.x, format) + "," +
                                   Convert.ToString(pose.y, format) + "," +
                                   Convert.ToString(pose.pan * 180 / Math.PI, format));
            }

            return(nodePath);
        }
Exemple #21
0
        /// <summary>
        /// show the position uncertainty distribution within the given region
        /// </summary>
        /// <param name="img">image data within which to draw</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="min_x">bounding box top left x coordinate</param>
        /// <param name="min_y">bounding box top left y coordinate</param>
        /// <param name="max_x">bounding box bottom right x coordinate</param>
        /// <param name="max_y">bounding box bottom right y coordinate</param>
        /// <param name="clear_background">whether to clear the image before drawing</param>
        public void Show(byte[] img, int width, int height,
                         float min_x_mm, float min_y_mm,
                         float max_x_mm, float max_y_mm,
                         bool clear_background)
        {
            if (clear_background)
            {
                // clear the image
                for (int i = 0; i < width * height * 3; i++)
                {
                    img[i] = (Byte)250;
                }
            }

            if ((max_x_mm > min_x_mm) && (max_y_mm > min_y_mm))
            {
                for (int sample = 0; sample < Poses.Count; sample++)
                {
                    particlePose pose = Poses[sample].current_pose;
                    int          x    = (int)((pose.x - min_x_mm) * (width - 1) / (max_x_mm - min_x_mm));
                    if ((x > 0) && (x < width))
                    {
                        int y = height - 1 - (int)((pose.y - min_y_mm) * (height - 1) / (max_y_mm - min_y_mm));
                        if ((y > 0) && (y < height))
                        {
                            int n = ((y * width) + x) * 3;
                            for (int col = 0; col < 3; col++)
                            {
                                img[n + col] = (Byte)0;
                            }
                        }
                    }
                }
            }

            // show the path
            ShowPath(img, width, height, 0, 255, 0, 0, min_x_mm, min_y_mm, max_x_mm, max_y_mm, false);
        }
        /// <summary>
        /// return the probability of occupancy for the entire cell
        /// </summary>
        /// <param name="pose">pose from which the observation was made</param>
        /// <returns>probability as log odds</returns>
        public float GetProbability(
            particlePose pose,
            int x, int y,
            float[] mean_colour,
            ref float mean_variance)
        {
            float probabilityLogOdds = 0;
            int   hits = 0;

            mean_variance = 0;

            mean_colour[0] = 0;
            mean_colour[1] = 0;
            mean_colour[2] = 0;

            for (int z = Hypothesis.Length - 1; z >= 0; z--)
            {
                float variance = 0;
                float probLO   = GetProbability(pose, x, y, z, true, temp_colour, ref variance);
                if (probLO != NO_OCCUPANCY_EVIDENCE)
                {
                    probabilityLogOdds += probLO;
                    mean_colour[0]     += temp_colour[0];
                    mean_colour[1]     += temp_colour[1];
                    mean_colour[2]     += temp_colour[2];
                    mean_variance      += variance;
                    hits++;
                }
            }
            if (hits > 0)
            {
                mean_variance  /= hits;
                mean_colour[0] /= hits;
                mean_colour[1] /= hits;
                mean_colour[2] /= hits;
            }
            return(probabilities.LogOddsToProbability(probabilityLogOdds));
        }
 /// <summary>
 /// save the occupancy grid data to file as a tile
 /// it is expected that multiple tiles will be saved per grid
 /// </summary>
 /// <param name="binfile">file to write to</param>
 /// <param name="pose">best available pose</param>
 /// <param name="centre_x">centre of the tile in grid cells</param>
 /// <param name="centre_y">centre of the tile in grid cells</param>
 /// <param name="width_cells">width of the tile in grid cells</param>
 public void SaveTile(BinaryWriter binfile,
                      particlePose pose, 
                      int centre_x, 
                      int centre_y, 
                      int width_cells)
 {
     // write the whole thing to disk in one go
     binfile.Write(SaveTile(pose, centre_x, centre_y, width_cells));
 }
 /// <summary>
 /// save the entire grid to file
 /// </summary>
 /// <param name="filename"></param>
 /// <param name="pose">best available pose</param>
 public void Save(string filename, 
                  particlePose pose)
 {
     FileStream fp = new FileStream(filename, FileMode.Create);
     BinaryWriter binfile = new BinaryWriter(fp);
     Save(binfile, pose);
     binfile.Close();
     fp.Close();
 }
        /// <summary>
        /// returns the probability of occupancy at this grid cell at the given vertical (z) coordinate
        /// warning: this could potentially suffer from path ID or time step rollover problems
        /// </summary>
        /// <param name="pose">the robot pose from which this cell was observed</param>
        /// <param name="x">x grid coordinate</param>
        /// <param name="y">y grid coordinate</param>
        /// <param name="z">z grid coordinate</param>
        /// <param name="returnLogOdds">return the probability value expressed as log odds</param>
        /// <param name="colour">average colour of this grid cell</param>
        /// <param name="mean_variance">average colour variance of this grid cell</param>
        /// <returns>probability value</returns>
        public unsafe float GetProbability(
		    particlePose pose, 
            int x, 
            int y, 
            int z, 
            bool returnLogOdds,
            float[] colour, 
            ref float mean_variance)
        {
            int col, p, i, hits = 0;
            float probabilityLogOdds = 0;
			byte level;
			List<particleGridCell> map_cache_observations;
			List<particlePath> previous_paths;
			particleGridCell h;
			particlePath path;
			uint time_step;
            mean_variance = 1;

			// pin some arrays to avoid range checking
			fixed (float* unsafe_colour = colour)
			{
			    fixed (float* unsafe_min_level = min_level)
			    {			
			        fixed (float* unsafe_max_level = max_level)
			        {
			            unsafe_colour[0] = 0;
						unsafe_colour[1] = 0;
						unsafe_colour[2] = 0;

			            // first retrieve any distilled occupancy value
			            if (distilled != null)
			                if (distilled[z] != null)
			                {
			                    probabilityLogOdds += distilled[z].probabilityLogOdds;
			                    
			                    unsafe_colour[0] += distilled[z].colour[0];
							    unsafe_colour[1] += distilled[z].colour[1];
							    unsafe_colour[2] += distilled[z].colour[2];
							
			                    hits++;
			                }

			            // and now get the data for any additional non-distilled particles
			            if ((Hypothesis[z] != null) && (pose != null))
			            {
                            previous_paths = pose.previous_paths;                            
			                if (previous_paths != null)
			                {
                                time_step = pose.time_step;

			                    // cycle through the previous paths for this pose            
			                    for (p = previous_paths.Count-1; p >= 0; p--)
			                    {
			                        path = previous_paths[p];
			                        if (path != null)
			                        {
			                            if (path.Enabled)
			                            {
			                                // do any hypotheses for this path exist at this location ?
			                                map_cache_observations = path.GetHypotheses(x, y, z);
			                                if (map_cache_observations != null)
			                                {
			                                    for (i = map_cache_observations.Count - 1; i >= 0; i--)
			                                    {
			                                        h = map_cache_observations[i];
			                                        if (h.Enabled)
			                                        {
			                                            // only use evidence older than the current time 
			                                            // step to avoid getting into a muddle
			                                            if (time_step != h.pose.time_step)
			                                            {
			                                                probabilityLogOdds += h.probabilityLogOdds;

	                                                        level = h.colour[0];
															level = h.colour[1];
															level = h.colour[2];
			                                                unsafe_colour[0] += level;
															unsafe_colour[1] += level;
															unsafe_colour[2] += level;
															
			                                                // update mean colour and variance
		                                                    if (hits > 0)
		                                                    {
		                                                        if (level < unsafe_min_level[0]) unsafe_min_level[0] = level;
		                                                        if (level < unsafe_min_level[1]) unsafe_min_level[1] = level;
		                                                        if (level < unsafe_min_level[2]) unsafe_min_level[2] = level;

		                                                        if (level > unsafe_max_level[0]) unsafe_max_level[0] = level;
																if (level > unsafe_max_level[1]) unsafe_max_level[1] = level;
																if (level > unsafe_max_level[2]) unsafe_max_level[2] = level;
		                                                    }
		                                                    else
		                                                    {
		                                                        unsafe_min_level[0] = level;
																unsafe_min_level[1] = level;
																unsafe_min_level[2] = level;
																
		                                                        unsafe_max_level[0] = level;
																unsafe_max_level[1] = level;
																unsafe_max_level[2] = level;
		                                                    }

			                                                hits++;
			                                            }
			                                        }
			                                    }
			                                }
			                            }
			                            else
			                            {
			                                // remove a dead path, because it has been distilled
			                                pose.previous_paths.RemoveAt(p);
			                            }
			                        }
			                    }


			                }
			            }
					}
				}
			}

            if (hits > 0)
            {
                // calculate mean colour variance
                mean_variance = 0;
                
                mean_variance += max_level[0] - min_level[0];
				mean_variance += max_level[1] - min_level[1];
				mean_variance += max_level[2] - min_level[2];
				
                //mean_variance /= (3 * 255.0f);
                mean_variance *= 0.001307189542483660130718954248366f;

                // calculate the average colour                
                colour[0] /= hits;
				colour[1] /= hits;
				colour[2] /= hits;

                // at the end we convert the total log odds value into a probability
                if (returnLogOdds)
                    return (probabilityLogOdds);
                else
                    return (probabilities.LogOddsToProbability(probabilityLogOdds));
            }
            else
                return (NO_OCCUPANCY_EVIDENCE);
        }
Exemple #26
0
        /// <summary>
        /// insert a stereo ray into the grid
        /// </summary>
        /// <param name="ray">stereo ray</param>
        /// <param name="origin">pose from which this observation was made</param>
        /// <param name="sensormodel">sensor model for each grid level</param>
        /// <param name="left_camera_location">left stereo camera position and orientation</param>
        /// <param name="right_camera_location">right stereo camera position and orientation</param>
        /// <param name="localiseOnly">whether we are mapping or localising</param>
        /// <returns>localisation matching score</returns>
        public float Insert(
            evidenceRay ray,
            particlePose origin,
		    stereoModel[] sensormodel,                            
            pos3D left_camera_location,
            pos3D right_camera_location,
            bool localiseOnly)
        {
            float matchingScore = 0;

            switch (grid_type)
            {
                case TYPE_MULTI_HYPOTHESIS:
                    {
                        for (int grid_level = 0; grid_level < grid.Length; grid_level++)
                        {
					        rayModelLookup sensormodel_lookup = sensormodel[grid_level].ray_model;
                            occupancygridMultiHypothesis grd = (occupancygridMultiHypothesis)grid[grid_level];
                            matchingScore += grid_weight[grid_level] * grd.Insert(ray, origin, sensormodel_lookup, left_camera_location, right_camera_location, localiseOnly);
                        }
                        break;
                    }
            }
            return (matchingScore);
        }
Exemple #27
0
 /// <summary>
 /// constructor
 /// </summary>
 /// <param name="x">x position in millimetres</param>
 /// <param name="y">y position in millimetres</param>
 /// <param name="pan">pan angle in radians</param>
 /// <param name="max_length">the maximum number of poses which may be stored within this path</param>
 /// <param name="time_step">time step</param>
 /// <param name="path_ID">ID number for the path</param>
 /// <param name="grid_dimension_cells">occupancy grid dimension</param>
 public particlePath(float x, 
                     float y, 
                     float pan,
                     int max_length, 
                     UInt32 time_step, 
                     UInt32 path_ID,
                     int grid_dimension_cells)
 {
     ID = path_ID;
     this.max_length = max_length;
     path = new List<particlePose>();
     particlePose pose = new particlePose(x, y, pan, this);
     pose.time_step = time_step;
     Enabled = true;
     Add(pose);
 }
Exemple #28
0
        /// <summary>
        /// show the path
        /// </summary>
        /// <param name="img">image data</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="r">red</param>
        /// <param name="g">green</param>
        /// <param name="b">blue</param>
        /// <param name="min_x_mm">minimum x coordinate of the window within which to show the path</param>
        /// <param name="min_y_mm">minimum y coordinate of the window within which to show the path</param>
        /// <param name="max_x_mm">maximum x coordinate of the window within which to show the path</param>
        /// <param name="max_y_mm">maximum y coordinate of the window within which to show the path</param>
        /// <param name="clearBackground">whether to clear the image before drawing the path</param>
        /// <param name="root_time_step">time step at which to begin drawing the path</param>
        public void Show(byte[] img,
                         int width,
                         int height,
                         int r,
                         int g,
                         int b,
                         int line_thickness,
                         float min_x_mm,
                         float min_y_mm,
                         float max_x_mm,
                         float max_y_mm,
                         bool clearBackground,
                         UInt32 root_time_step)
        {
            if (clearBackground)
            {
                for (int i = 0; i < width * height * 3; i++)
                {
                    img[i] = 255;
                }
            }

            int rr, gg, bb;
            int x = -1, y = -1;
            int prev_x = -1, prev_y = -1;

            if ((max_x_mm > min_x_mm) && (max_y_mm > min_y_mm))
            {
                particlePose pose = current_pose;
                if (pose != null)
                {
                    while (pose.parent != null)
                    {
                        if ((pose.time_step <= root_time_step) &&
                            (root_time_step != UInt32.MaxValue))
                        {
                            rr = 0;
                            gg = 255;
                            bb = 0;
                        }
                        else
                        {
                            rr = r;
                            gg = g;
                            bb = b;
                        }

                        x = (int)((pose.x - min_x_mm) * (width - 1) / (max_x_mm - min_x_mm));
                        if ((x > 0) && (x < width))
                        {
                            y = height - 1 - (int)((pose.y - min_y_mm) * (height - 1) / (max_y_mm - min_y_mm));
                            if ((y < 0) || (y >= height))
                            {
                                y = -1;
                            }
                        }
                        else
                        {
                            x = -1;
                        }
                        if ((x > -1) && (y > -1))
                        {
                            if (prev_x > -1)
                            {
                                drawing.drawLine(img, width, height, x, y, prev_x, prev_y, rr, gg, bb, line_thickness, false);
                            }

                            prev_x = x;
                            prev_y = y;
                        }

                        // move along, nothing to see...
                        pose = pose.parent;
                    }
                }
            }
        }
        /// <summary>
        /// updates the navigable space
        /// </summary>
        /// <param name="pose">best pose from which the occupancy value is calculated</param>
        /// <param name="x">x coordinate in cells</param>
        /// <param name="y">y coordinate in cells</param>
        private void updateNavigableSpace(particlePose pose, int x, int y)
        {
            if (x < 1) x = 1;
            if (y < 1) y = 1;

            float[] mean_colour = new float[3];
            float mean_variance = 0;
            float prob = cell[x][y].GetProbability(pose, x, y,
                                                   mean_colour, 
                                                   ref mean_variance);
            bool state = false;
            if (prob < 0.5f) state = true;
            navigable_space[x][y] = state;
            navigable_space[x - 1][y] = state;
            navigable_space[x - 1][y - 1] = state;
            navigable_space[x][y - 1] = state;
        }
Exemple #30
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);
        }
		/// <summary>
		/// probes the grid using the given 3D line and returns the distance to the nearest occupied grid cell
		/// </summary>
		/// <param name="pose">current best pose estimate</param>
		/// <param name="x0_mm">start x coordinate</param>
		/// <param name="y0_mm">start y coordinate</param>
		/// <param name="z0_mm">start z coordinate</param>
		/// <param name="x1_mm">end x coordinate</param>
		/// <param name="y1_mm">end y coordinate</param>
		/// <param name="z1_mm">end z coordinate</param>
		/// <returns>range to the nearest occupied grid cell in millimetres</returns>
		public float ProbeRange(
		    particlePose pose,
	        float x0_mm,
		    float y0_mm,
		    float z0_mm,
	        float x1_mm,
		    float y1_mm,
		    float z1_mm)
		{
			float range_mm = -1;
			int half_dimension_cells = dimension_cells/2;
			int x0_cell = half_dimension_cells + (int)((x0_mm - x) / cellSize_mm);
			int y0_cell = half_dimension_cells + (int)((y0_mm - y) / cellSize_mm);
			int z0_cell = half_dimension_cells + (int)((z0_mm - z) / cellSize_mm);
			int x1_cell = half_dimension_cells + (int)((x1_mm - x) / cellSize_mm);
			int y1_cell = half_dimension_cells + (int)((y1_mm - y) / cellSize_mm);
			int z1_cell = half_dimension_cells + (int)((z1_mm - z) / cellSize_mm);
			int dx = x1_cell - x0_cell;
			int dy = y1_cell - y0_cell;
			int dz = z1_cell - z0_cell;
			int dist_cells = (int)Math.Sqrt(dx*dx + dy*dy + dz*dz);
			int x_cell, y_cell, z_cell;
			float incr = 1 / (float)dist_cells;
			float fraction = 0;
			float[] colour = new float[3];
			float mean_variance = 0;
			for (int dist = 0; dist < dist_cells; dist++, fraction += incr)
			{
				x_cell = x0_cell + (int)(fraction * dx);
				if ((x_cell > -1) && (x_cell < dimension_cells))
				{
				    y_cell = y0_cell + (int)(fraction * dy);
					if ((y_cell > -1) && (y_cell < dimension_cells))
					{
				        z_cell = z0_cell + (int)(fraction * dz);
						if ((z_cell > -1) && (z_cell < dimension_cells_vertical))
						{
							occupancygridCellMultiHypothesis c = cell[x_cell][y_cell];
							if (c != null)
							{
							    if (c.GetProbability(pose, x_cell, y_cell, z_cell, false, colour, ref mean_variance) > 0.5f)
							    {
									range_mm = dist * cellSize_mm;
									break;
								}
							}
						}
						else break;
					}
					else break;
				}
				else break;
			}
			return(range_mm);
		}
        /// <summary>
        /// returns the average colour variance for the entire grid
        /// </summary>
        /// <param name="pose">best available robot pose hypothesis</param>
        /// <returns>colour variance value for the occupancy grid</returns>
        public float GetMeanColourVariance(particlePose pose)
        {
            float[] mean_colour = new float[3];
            float total_variance = 0;
            int hits = 0;

            for (int cell_y = 0; cell_y < dimension_cells; cell_y++)
            {
                for (int cell_x = 0; cell_x < dimension_cells; cell_x++)
                {
                    if (cell[cell_x][cell_y] != null)
                    {
                        float mean_variance = 0;
                        float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
                                                                         mean_colour, 
                                                                         ref mean_variance);
                        if (prob > 0.5)
                        {
                            total_variance += mean_variance;
                            hits++;
                        }
                    }
                }
            }
            if (hits > 0) total_variance /= hits;
            return (total_variance);
        }
        /// <summary>
        /// export the occupancy grid data to IFrIT basic particle file format for visualisation
        /// </summary>
        /// <param name="filename">name of the file to save as</param>
        /// <param name="pose">best available pose</param>
        /// <param name="centre_x">centre of the tile in grid cells</param>
        /// <param name="centre_y">centre of the tile in grid cells</param>
        /// <param name="width_cells">width of the tile in grid cells</param>
        public void ExportToIFrIT(string filename,
                                  particlePose pose,
                                  int centre_x, int centre_y,
                                  int width_cells)
        {
            float threshold = 0.5f;
            int half_width_cells = width_cells / 2;

            int tx = 99999;
            int ty = 99999;
            int tz = 99999;
            int bx = -99999;
            int by = -99999;
            int bz = -99999;

            // dummy variables needed by GetProbability
            float[] colour = new float[3];

            // another dummy variable needed by GetProbability but otherwise not used
            float mean_variance = 0;

            // get the bounding region within which there are actice grid cells
            int occupied_cells = 0;
            for (int x = centre_x - half_width_cells; x <= centre_x + half_width_cells; x++)
            {
                if ((x >= 0) && (x < dimension_cells))
                {
                    for (int y = centre_y - half_width_cells; y <= centre_y + half_width_cells; y++)
                    {
                        if ((y >= 0) && (y < dimension_cells))
                        {
                            if (cell[x][y] != null)
                            {
                                for (int z = 0; z < dimension_cells_vertical; z++)
                                {
                                    float prob = cell[x][y].GetProbability(pose, x, y,
                                                                           colour, ref mean_variance);

                                    if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                    {
                                        occupied_cells++;
                                        if (x < tx) tx = x;
                                        if (y < ty) ty = y;
                                        if (z < tz) tz = z;
                                        if (x > bx) bx = x;
                                        if (y > by) by = y;
                                        if (z > bz) bz = z;
                                    }
                                }
                            }
                        }
                    }
                }
            }
            bx++;
            by++;

            if (occupied_cells > 0)
            {
                // add bounding box information
                string bounding_box = Convert.ToString(0) + " " + 
                                      Convert.ToString(0) + " " +
                                      Convert.ToString(0) + " " +
                                      Convert.ToString(dimension_cells) + " " + 
                                      Convert.ToString(dimension_cells) + " " +
                                      Convert.ToString(dimension_cells_vertical) + " X Y Z";

                ArrayList particles = new ArrayList();

                for (int y = ty; y < by; y++)
                {
                    for (int x = tx; x < bx; x++)
                    {
                        if (cell[x][y] != null)
                        {
                            float prob = cell[x][y].GetProbability(pose, x, y,
                                                                   colour, ref mean_variance);

                            if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                            {
                                for (int z = 0; z < dimension_cells_vertical; z++)
                                {
                                    prob = cell[x][y].GetProbability(pose, x, y, z, false,
                                                                     colour, ref mean_variance);
                                    
                                    if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                    {
                                        if (prob > threshold)  // probably occupied space
                                        {
                                            // get the colour of the grid cell as a floating point value
                                            float colour_value = int.Parse(colours.GetHexFromRGB((int)colour[0], (int)colour[1], (int)colour[2]), NumberStyles.HexNumber);

                                            string particleStr = Convert.ToString(x) + " " +
                                                                 Convert.ToString(y) + " " +
                                                                 Convert.ToString(dimension_cells_vertical-1-z) + " " +
                                                                 Convert.ToString(prob) + " " +
                                                                 Convert.ToString(colour_value);
                                            particles.Add(particleStr);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }

                // write the text file
                if (particles.Count > 0)
                {
                    StreamWriter oWrite = null;
                    bool allowWrite = true;

                    try
                    {
                        oWrite = File.CreateText(filename);
                    }
                    catch
                    {
                        allowWrite = false;
                    }

                    if (allowWrite)
                    {
                        oWrite.WriteLine(Convert.ToString(particles.Count));
                        oWrite.WriteLine(bounding_box);
                        for (int p = 0; p < particles.Count; p++)
                            oWrite.WriteLine((string)particles[p]);
                        oWrite.Close();
                    }
                }

            }
        }
        /// <summary>
        /// returns the probability of occupancy at this grid cell at the given vertical (z) coordinate
        /// warning: this could potentially suffer from path ID or time step rollover problems
        /// </summary>
        /// <param name="pose">the robot pose from which this cell was observed</param>
        /// <param name="x">x grid coordinate</param>
        /// <param name="y">y grid coordinate</param>
        /// <param name="z">z grid coordinate</param>
        /// <param name="returnLogOdds">return the probability value expressed as log odds</param>
        /// <param name="colour">average colour of this grid cell</param>
        /// <param name="mean_variance">average colour variance of this grid cell</param>
        /// <returns>probability value</returns>
        public unsafe float GetProbability(
            particlePose pose,
            int x,
            int y,
            int z,
            bool returnLogOdds,
            float[] colour,
            ref float mean_variance)
        {
            int   col, p, i, hits = 0;
            float probabilityLogOdds = 0;
            byte  level;
            List <particleGridCell> map_cache_observations;
            List <particlePath>     previous_paths;
            particleGridCell        h;
            particlePath            path;
            uint time_step;

            mean_variance = 1;

            // pin some arrays to avoid range checking
            fixed(float *unsafe_colour = colour)
            {
                fixed(float *unsafe_min_level = min_level)
                {
                    fixed(float *unsafe_max_level = max_level)
                    {
                        unsafe_colour[0] = 0;
                        unsafe_colour[1] = 0;
                        unsafe_colour[2] = 0;

                        // first retrieve any distilled occupancy value
                        if (distilled != null)
                        {
                            if (distilled[z] != null)
                            {
                                probabilityLogOdds += distilled[z].probabilityLogOdds;

                                unsafe_colour[0] += distilled[z].colour[0];
                                unsafe_colour[1] += distilled[z].colour[1];
                                unsafe_colour[2] += distilled[z].colour[2];

                                hits++;
                            }
                        }

                        // and now get the data for any additional non-distilled particles
                        if ((Hypothesis[z] != null) && (pose != null))
                        {
                            previous_paths = pose.previous_paths;
                            if (previous_paths != null)
                            {
                                time_step = pose.time_step;

                                // cycle through the previous paths for this pose
                                for (p = previous_paths.Count - 1; p >= 0; p--)
                                {
                                    path = previous_paths[p];
                                    if (path != null)
                                    {
                                        if (path.Enabled)
                                        {
                                            // do any hypotheses for this path exist at this location ?
                                            map_cache_observations = path.GetHypotheses(x, y, z);
                                            if (map_cache_observations != null)
                                            {
                                                for (i = map_cache_observations.Count - 1; i >= 0; i--)
                                                {
                                                    h = map_cache_observations[i];
                                                    if (h.Enabled)
                                                    {
                                                        // only use evidence older than the current time
                                                        // step to avoid getting into a muddle
                                                        if (time_step != h.pose.time_step)
                                                        {
                                                            probabilityLogOdds += h.probabilityLogOdds;

                                                            level             = h.colour[0];
                                                            level             = h.colour[1];
                                                            level             = h.colour[2];
                                                            unsafe_colour[0] += level;
                                                            unsafe_colour[1] += level;
                                                            unsafe_colour[2] += level;

                                                            // update mean colour and variance
                                                            if (hits > 0)
                                                            {
                                                                if (level < unsafe_min_level[0])
                                                                {
                                                                    unsafe_min_level[0] = level;
                                                                }
                                                                if (level < unsafe_min_level[1])
                                                                {
                                                                    unsafe_min_level[1] = level;
                                                                }
                                                                if (level < unsafe_min_level[2])
                                                                {
                                                                    unsafe_min_level[2] = level;
                                                                }

                                                                if (level > unsafe_max_level[0])
                                                                {
                                                                    unsafe_max_level[0] = level;
                                                                }
                                                                if (level > unsafe_max_level[1])
                                                                {
                                                                    unsafe_max_level[1] = level;
                                                                }
                                                                if (level > unsafe_max_level[2])
                                                                {
                                                                    unsafe_max_level[2] = level;
                                                                }
                                                            }
                                                            else
                                                            {
                                                                unsafe_min_level[0] = level;
                                                                unsafe_min_level[1] = level;
                                                                unsafe_min_level[2] = level;

                                                                unsafe_max_level[0] = level;
                                                                unsafe_max_level[1] = level;
                                                                unsafe_max_level[2] = level;
                                                            }

                                                            hits++;
                                                        }
                                                    }
                                                }
                                            }
                                        }
                                        else
                                        {
                                            // remove a dead path, because it has been distilled
                                            pose.previous_paths.RemoveAt(p);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }

            if (hits > 0)
            {
                // calculate mean colour variance
                mean_variance = 0;

                mean_variance += max_level[0] - min_level[0];
                mean_variance += max_level[1] - min_level[1];
                mean_variance += max_level[2] - min_level[2];

                //mean_variance /= (3 * 255.0f);
                mean_variance *= 0.001307189542483660130718954248366f;

                // calculate the average colour
                colour[0] /= hits;
                colour[1] /= hits;
                colour[2] /= hits;

                // at the end we convert the total log odds value into a probability
                if (returnLogOdds)
                {
                    return(probabilityLogOdds);
                }
                else
                {
                    return(probabilities.LogOddsToProbability(probabilityLogOdds));
                }
            }
            else
            {
                return(NO_OCCUPANCY_EVIDENCE);
            }
        }
 /// <summary>
 /// save the entire grid to a single file
 /// </summary>
 /// <param name="binfile"></param>
 /// <param name="pose"></param>
 public void Save(BinaryWriter binfile, 
                  particlePose pose)
 {
     SaveTile(binfile, pose, dimension_cells / 2, dimension_cells / 2, dimension_cells);
 }
Exemple #36
0
        /// <summary>
        /// update the given pose using the motion model
        /// </summary>
        /// <param name="path">path to add the new estimated pose to</param>
        /// <param name="time_elapsed_sec">time elapsed since the last update in seconds</param>
        private void sample_motion_model_velocity(particlePath path, float time_elapsed_sec)
        {
            // calculate noisy velocities
            float fwd_velocity = forward_velocity + sample_normal_distribution(
                (motion_noise[0] * Math.Abs(forward_velocity)) +
                (motion_noise[1] * Math.Abs(angular_velocity)));

            float ang_velocity = angular_velocity + sample_normal_distribution(
                (motion_noise[2] * Math.Abs(forward_velocity)) +
                (motion_noise[3] * Math.Abs(angular_velocity)));

            float v = sample_normal_distribution(
                (motion_noise[4] * Math.Abs(forward_velocity)) +
                (motion_noise[5] * Math.Abs(angular_velocity)));

            float fraction = 0;
            if (Math.Abs(ang_velocity) > 0.000001f) fraction = fwd_velocity / ang_velocity;
            float current_pan = path.current_pose.pan;

            // if scan matching is active use the current estimated pan angle
            if (rob.ScanMatchingPanAngleEstimate != scanMatching.NOT_MATCHED)
                current_pan = rob.ScanMatchingPanAngleEstimate;

            float pan2 = current_pan - (ang_velocity * time_elapsed_sec);

            float new_y = path.current_pose.y + (fraction * (float)Math.Sin(current_pan)) -
                  (fraction * (float)Math.Sin(pan2));
            float new_x = path.current_pose.x - (fraction * (float)Math.Cos(current_pan)) +
                              (fraction * (float)Math.Cos(pan2));
            float new_pan = pan2 + (v * time_elapsed_sec);

            particlePose new_pose = new particlePose(new_x, new_y, new_pan, path);
            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
        /// <summary>
        /// returns the localisation probability
        /// </summary>
        /// <param name="x_cell">x grid coordinate</param>
        /// <param name="y_cell">y grid coordinate</param>
        /// <param name="origin">pose of the robot</param>
        /// <param name="sensormodel_probability">probability value from a specific point in the ray, taken from the sensor model</param>
        /// <param name="colour">colour of the localisation ray</param>
        /// <returns>log odds probability of there being a match between the ray and the grid</returns>
        private float matchingProbability(
            int x_cell, int y_cell, int z_cell,
            particlePose origin,
            float sensormodel_probability,
            byte[] colour)
        {
            float prob_log_odds = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
            float colour_variance = 0;

            // localise using this grid cell
            // first get the existing probability value at this cell
            float[] existing_colour = new float[3];

            float existing_probability =
                    cell[x_cell][y_cell].GetProbability(origin, x_cell, y_cell, z_cell,
                                                        false, existing_colour, ref colour_variance);

            if (existing_probability != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
            {
                // combine the occupancy probabilities
                float occupancy_probability = 
                    ((sensormodel_probability * existing_probability) +
                    ((1.0f - sensormodel_probability) * (1.0f - existing_probability)));

                // get the colour difference between the map and the observation
                float colour_difference = getColourDifference(colour, existing_colour);

                // turn the colour difference into a probability
                float colour_probability = 1.0f  - colour_difference;

                // localisation matching probability, expressed as log odds
                prob_log_odds = LogOdds[(int)(occupancy_probability * colour_probability * LOG_ODDS_LOOKUP_LEVELS)];
            }
            return (prob_log_odds);
        }
Exemple #38
0
        /// <summary>
        /// parse an xml node to extract camera calibration parameters
        /// </summary>
        /// <param name="xnod"></param>
        /// <param name="level"></param>
        public void LoadFromXml(XmlNode xnod, 
                                int level)
        {
            XmlNode xnodWorking;

            if (xnod.Name == "Pose")
            {
                IFormatProvider format = new System.Globalization.CultureInfo("en-GB");
                string[] poseStr = xnod.InnerText.Split(',');
                particlePose new_pose = new particlePose(Convert.ToSingle(poseStr[0], format),
                                                         Convert.ToSingle(poseStr[1], format),
                                                         Convert.ToSingle(poseStr[2], format)*(float)Math.PI/180.0f, null);
                Add(new_pose);
            }


            // call recursively on all children of the current node
            if ((xnod.HasChildNodes) &&
                ((xnod.Name == "RobotPath") ||
                 (xnod.Name == "Sentience")
                 ))
            {
                xnodWorking = xnod.FirstChild;
                while (xnodWorking != null)
                {
                    LoadFromXml(xnodWorking, level + 1);
                    xnodWorking = xnodWorking.NextSibling;
                }
            }
        }
        /// <summary>
        /// inserts the given ray into the grid
        /// There are three components to the sensor model used here:
        /// two areas determining the probably vacant area and one for 
        /// the probably occupied space
        /// </summary>
        /// <param name="ray">ray object to be inserted into the grid</param>
        /// <param name="origin">the pose of the robot</param>
        /// <param name="sensormodel">the sensor model to be used</param>
        /// <param name="leftcam_x">x position of the left camera in millimetres</param>
        /// <param name="leftcam_y">y position of the left camera in millimetres</param>
        /// <param name="rightcam_x">x position of the right camera in millimetres</param>
        /// <param name="rightcam_y">y position of the right camera in millimetres</param>
        /// <param name="localiseOnly">if true does not add any mapping particles (pure localisation)</param>
        /// <returns>matching probability, expressed as log odds</returns>
        public float Insert(evidenceRay ray, 
                            particlePose origin,
                            rayModelLookup sensormodel_lookup,
                            pos3D left_camera_location, 
                            pos3D right_camera_location,
                            bool localiseOnly)
        {
            // some constants to aid readability
            const int OCCUPIED_SENSORMODEL = 0;
            const int VACANT_SENSORMODEL_LEFT_CAMERA = 1;
            const int VACANT_SENSORMODEL_RIGHT_CAMERA = 2;

            const int X_AXIS = 0;
            const int Y_AXIS = 1;

            // which disparity index in the lookup table to use
            // we multiply by 2 because the lookup is in half pixel steps
            int sensormodel_index = (int)Math.Round(ray.disparity * 2);
            
            // the initial models are blank, so just default to the one disparity pixel model
            bool small_disparity_value = false;
            if (sensormodel_index < 2)
            {
                sensormodel_index = 2;
                small_disparity_value = true;
            }

            // beyond a certain disparity the ray model for occupied space
            // is always only going to be only a single grid cell
            if (sensormodel_index >= sensormodel_lookup.probability.GetLength(0))
                sensormodel_index = sensormodel_lookup.probability.GetLength(0) - 1;

            float xdist_mm=0, ydist_mm=0, zdist_mm=0, xx_mm=0, yy_mm=0, zz_mm=0;
            float occupied_dx = 0, occupied_dy = 0, occupied_dz = 0;
            float intersect_x = 0, intersect_y = 0, intersect_z = 0;
            float centre_prob = 0, prob = 0, prob_localisation = 0; // probability values at the centre axis and outside
            float matchingScore = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;  // total localisation matching score
            int rayWidth = 0;         // widest point in the ray in cells
            int widest_point;         // widest point index
            int step_size = 1;
            particleGridCell hypothesis;

            // ray width at the fattest point in cells
            rayWidth = (int)Math.Round(ray.width / (cellSize_mm * 2));

            // calculate the centre position of the grid in millimetres
            int half_grid_width_mm = dimension_cells * cellSize_mm / 2;
            //int half_grid_width_vertical_mm = dimension_cells_vertical * cellSize_mm / 2;
            int grid_centre_x_mm = (int)(x - half_grid_width_mm);
            int grid_centre_y_mm = (int)(y - half_grid_width_mm);
            int grid_centre_z_mm = (int)z;

            int max_dimension_cells = dimension_cells - rayWidth;

            // in turbo mode only use a single vacancy ray
            int max_modelcomponent = VACANT_SENSORMODEL_RIGHT_CAMERA;
            if (TurboMode) max_modelcomponent = VACANT_SENSORMODEL_LEFT_CAMERA;

			float[][] sensormodel_lookup_probability = sensormodel_lookup.probability;
			
            // consider each of the three parts of the sensor model
            for (int modelcomponent = OCCUPIED_SENSORMODEL; modelcomponent <= max_modelcomponent; modelcomponent++)
            {
                // the range from the cameras from which insertion of data begins
                // for vacancy rays this will be zero, but will be non-zero for the occupancy area
                int starting_range_cells = 0;

                switch (modelcomponent)
                {
                    case OCCUPIED_SENSORMODEL:
                        {
                            // distance between the beginning and end of the probably
                            // occupied area
                            occupied_dx = ray.vertices[1].x - ray.vertices[0].x;
                            occupied_dy = ray.vertices[1].y - ray.vertices[0].y;
                            occupied_dz = ray.vertices[1].z - ray.vertices[0].z;
                            intersect_x = ray.vertices[0].x + (occupied_dx * ray.fattestPoint);
                            intersect_y = ray.vertices[0].y + (occupied_dy * ray.fattestPoint);
                            intersect_z = ray.vertices[0].z + (occupied_dz * ray.fattestPoint);

                            xdist_mm = occupied_dx;
                            ydist_mm = occupied_dy;
                            zdist_mm = occupied_dz;

                            // begin insertion at the beginning of the 
                            // probably occupied area
                            xx_mm = ray.vertices[0].x;
                            yy_mm = ray.vertices[0].y;
                            zz_mm = ray.vertices[0].z;
                            break;
                        }
                    case VACANT_SENSORMODEL_LEFT_CAMERA:
                        {
                            // distance between the left camera and the left side of
                            // the probably occupied area of the sensor model                            
                            xdist_mm = intersect_x - left_camera_location.x;
                            ydist_mm = intersect_y - left_camera_location.y;
                            zdist_mm = intersect_z - left_camera_location.z;

                            // begin insertion from the left camera position
                            xx_mm = left_camera_location.x;
                            yy_mm = left_camera_location.y;
                            zz_mm = left_camera_location.z;
                            step_size = 2;
                            break;
                        }
                    case VACANT_SENSORMODEL_RIGHT_CAMERA:
                        {
                            // distance between the right camera and the right side of
                            // the probably occupied area of the sensor model
                            xdist_mm = intersect_x - right_camera_location.x;
                            ydist_mm = intersect_y - right_camera_location.y;
                            zdist_mm = intersect_z - right_camera_location.z;

                            // begin insertion from the right camera position
                            xx_mm = right_camera_location.x;
                            yy_mm = right_camera_location.y;
                            zz_mm = right_camera_location.z;
                            step_size = 2;
                            break;
                        }
                }

                // which is the longest axis ?
                int longest_axis = X_AXIS;
                float longest = Math.Abs(xdist_mm);
                if (Math.Abs(ydist_mm) > longest)
                {
                    // y has the longest length
                    longest = Math.Abs(ydist_mm);
                    longest_axis = Y_AXIS;
                }

                // ensure that the vacancy model does not overlap
                // the probably occupied area
                // This is crude and could potentially leave a gap
                if (modelcomponent != OCCUPIED_SENSORMODEL)
                    longest -= ray.width;

                int steps = (int)(longest / cellSize_mm);
                if (steps < 1) steps = 1;

                // calculate the range from the cameras to the start of the ray in grid cells
                if (modelcomponent == OCCUPIED_SENSORMODEL)
                {
                    if (longest_axis == Y_AXIS)
                        starting_range_cells = (int)Math.Abs((ray.vertices[0].y - ray.observedFrom.y) / cellSize_mm);
                    else
                        starting_range_cells = (int)Math.Abs((ray.vertices[0].x - ray.observedFrom.x) / cellSize_mm);
                }

                // what is the widest point of the ray in cells
                if (modelcomponent == OCCUPIED_SENSORMODEL)
                    widest_point = (int)(ray.fattestPoint * steps / ray.length);
                else
                    widest_point = steps;

                // calculate increment values in millimetres
                float x_incr_mm = xdist_mm / steps;
                float y_incr_mm = ydist_mm / steps;
                float z_incr_mm = zdist_mm / steps;

                // step through the ray, one grid cell at a time
                int grid_step = 0;
                while (grid_step < steps)
                {
                    // is this position inside the maximum mapping range
                    bool withinMappingRange = true;
                    if (grid_step + starting_range_cells > max_mapping_range_cells)
                    {
                        withinMappingRange = false;
                        if ((grid_step==0) && (modelcomponent == OCCUPIED_SENSORMODEL))
                        {
                            grid_step = steps;
                            modelcomponent = 9999;
                        }
                    }

                    // calculate the width of the ray in cells at this point
                    // using a diamond shape ray model
                    int ray_wdth = 0;
                    if (rayWidth > 0)
                    {
                        if (grid_step < widest_point)
                            ray_wdth = grid_step * rayWidth / widest_point;
                        else
                        {
                            if (!small_disparity_value)
                                // most disparity values tail off to some point in the distance
                                ray_wdth = (steps - grid_step + widest_point) * rayWidth / (steps - widest_point);
                            else
                                // for very small disparity values the ray model has an infinite tail
                                // and maintains its width after the widest point
                                ray_wdth = rayWidth; 
                        }
                    }

                    // localisation rays are wider, to enable a more effective matching score
                    // which is not too narrowly focussed and brittle
                    int ray_wdth_localisation = ray_wdth + 1; //localisation_search_cells;

                    xx_mm += x_incr_mm*step_size;
                    yy_mm += y_incr_mm*step_size;
                    zz_mm += z_incr_mm*step_size;
                    // convert the x millimetre position into a grid cell position
                    int x_cell = (int)Math.Round((xx_mm - grid_centre_x_mm) / (float)cellSize_mm);
                    if ((x_cell > ray_wdth_localisation) && (x_cell < dimension_cells - ray_wdth_localisation))
                    {
                        // convert the y millimetre position into a grid cell position
                        int y_cell = (int)Math.Round((yy_mm - grid_centre_y_mm) / (float)cellSize_mm);
                        if ((y_cell > ray_wdth_localisation) && (y_cell < dimension_cells - ray_wdth_localisation))
                        {
                            // convert the z millimetre position into a grid cell position
                            int z_cell = (int)Math.Round((zz_mm - grid_centre_z_mm) / (float)cellSize_mm);
                            if ((z_cell >= 0) && (z_cell < dimension_cells_vertical))
                            {

                                int x_cell2 = x_cell;
                                int y_cell2 = y_cell;

                                // get the probability at this point 
                                // for the central axis of the ray using the inverse sensor model
                                if (modelcomponent == OCCUPIED_SENSORMODEL)
                                    centre_prob = 0.5f + (sensormodel_lookup_probability[sensormodel_index][grid_step] * 0.5f);
                                else
                                    // calculate the probability from the vacancy model
                                    centre_prob = vacancyFunction(grid_step / (float)steps, steps);


                                // width of the localisation ray
                                for (int width = -ray_wdth_localisation; width <= ray_wdth_localisation; width++)
                                {
                                    // is the width currently inside the mapping area of the ray ?
                                    bool isInsideMappingRayWidth = false;
                                    if ((width >= -ray_wdth) && (width <= ray_wdth))
                                        isInsideMappingRayWidth = true;

                                    // adjust the x or y cell position depending upon the 
                                    // deviation from the main axis of the ray
                                    if (longest_axis == Y_AXIS)
                                        x_cell2 = x_cell + width;
                                    else
                                        y_cell2 = y_cell + width;

                                    // probability at the central axis
                                    prob = centre_prob;
                                    prob_localisation = centre_prob;

                                    // probabilities are symmetrical about the axis of the ray
                                    // this multiplier implements a gaussian distribution around the centre
                                    if (width != 0) // don't bother if this is the centre of the ray
                                    {
                                        // the probability used for wide localisation rays
                                        prob_localisation *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth_localisation];

                                        // the probability used for narrower mapping rays
                                        if (isInsideMappingRayWidth)
                                            prob *= gaussianLookup[Math.Abs(width) * 9 / ray_wdth];
                                    }

                                    if ((cell[x_cell2][y_cell2] != null) && (withinMappingRange))
                                    {
                                        // only localise using occupancy, not vacancy
                                        if (modelcomponent == OCCUPIED_SENSORMODEL)
                                        {
                                            // update the matching score, by combining the probability
                                            // of the grid cell with the probability from the localisation ray
                                            float score = occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE;
                                            if (longest_axis == X_AXIS)
                                                score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);

                                            if (longest_axis == Y_AXIS)
                                                score = matchingProbability(x_cell2, y_cell2, z_cell, origin, prob_localisation, ray.colour);

                                            if (score != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                            {
                                                if (matchingScore != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                                    matchingScore += score;
                                                else
                                                    matchingScore = score;
                                            }
                                        }
                                    }

                                    if ((isInsideMappingRayWidth) && 
                                        (withinMappingRange) &&
                                        (!localiseOnly))
                                    {
                                        // add a new hypothesis to this grid coordinate
                                        // note that this is also added to the original pose
                                        hypothesis = new particleGridCell(x_cell2, y_cell2, z_cell, 
                                                                          prob, origin,
                                                                          ray.colour);
                                        if (origin.AddHypothesis(hypothesis, max_mapping_range_cells, dimension_cells, dimension_cells_vertical))
                                        {
                                            // generate a grid cell if necessary
                                            if (cell[x_cell2][y_cell2] == null)
                                                cell[x_cell2][y_cell2] = new occupancygridCellMultiHypothesis(dimension_cells_vertical);

                                            cell[x_cell2][y_cell2].AddHypothesis(hypothesis);                                            
                                            total_valid_hypotheses++;
                                        }
                                    }
                                }
                            }
                            else grid_step = steps;  // its the end of the ray, break out of the loop
                        }
                        else grid_step = steps;  // its the end of the ray, break out of the loop
                    }
                    else grid_step = steps;  // time to bail out chaps!
                    grid_step += step_size;
                }
            }

            return (matchingScore);
        }
Exemple #40
0
        /// <summary>
        /// add a new pose to the path
        /// </summary>
        /// <param name="pose">pose to be added</param>
        public void Add(particlePose pose)
        {
            pose.path = this;

            // set the parent of this pose
            pose.parent = current_pose;

            // update the list of previous path IDs
            if (branch_pose != null)
            {
                if (current_pose == branch_pose)
                {
                    pose.previous_paths = new List<particlePath>();
                    int min = branch_pose.previous_paths.Count - MAX_PATH_HISTORY;
                    if (min < 0) min = 0;
                    for (int i = min; i < branch_pose.previous_paths.Count; i++)
                        pose.previous_paths.Add(branch_pose.previous_paths[i]);
                    pose.previous_paths.Add(this);
                }
                else pose.previous_paths = pose.parent.previous_paths;
            }
            else
            {
                if (pose.parent == null)
                {
                    pose.previous_paths = new List<particlePath>();
                    pose.previous_paths.Add(this);
                }
                else pose.previous_paths = pose.parent.previous_paths;
            }

            // set the current pose
            current_pose = pose;

            // add the pose to the path
            path.Add(pose);

            //total_score += pose.score;
            total_poses++;

            // ensure that the path does not exceed a maximum length
            if (path.Count > max_length)
            {
                // remove the oldest pose in the path
                path.RemoveAt(0);
            }
        }
        /// <summary>
        /// display the occupancy grid
        /// </summary>
        /// <param name="view_type">the angle from which to view the grid</param>
        /// <param name="img">image within which to insert data</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="pose">the best available robot pose hypothesis</param>
        /// <param name="colour">show grid cell colour, or just occupancy values</param>
        /// <param name="scalegrid">show a grid overlay which gives some idea of scale</param>
        public void Show(int view_type, 
                         byte[] img, 
                         int width, 
                         int height, 
                         particlePose pose, 
                         bool colour, 
                         bool scalegrid)
        {
            switch(view_type)
            {
                case VIEW_ABOVE:
                    {
                        if (!colour)
                            Show(img, width, height, pose);
                        else
                            ShowColour(img, width, height, pose);

                        if (scalegrid)
                        {
                            int r = 200;
                            int g = 200;
                            int b = 255;

                            // draw a grid to give an indication of scale
                            // where the grid resolution is one metre
                            float dimension_mm = cellSize_mm * dimension_cells;

                            for (float x = 0; x < dimension_mm; x += 1000)
                            {
                                int xx = (int)(x * width / dimension_mm);
                                drawing.drawLine(img, width, height, xx, 0, xx, height - 1, r, g, b, 0, false);
                            }
                            for (float y = 0; y < dimension_mm; y += 1000)
                            {
                                int yy = (int)(y * height / dimension_mm);
                                drawing.drawLine(img, width, height, 0, yy, width - 1, yy, r, g, b, 0, false);
                            }
                        }

                        break;
                    }
                case VIEW_LEFT_SIDE:
                    {
                        ShowSide(img, width, height, pose, true);
                        break;
                    }
                case VIEW_RIGHT_SIDE:
                    {
                        ShowSide(img, width, height, pose, false);
                        break;
                    }
                case VIEW_NAVIGABLE_SPACE:
                    {
                        ShowNavigable(img, width, height);
                        break;
                    }
            }

        }
Exemple #42
0
        /// <summary>
        /// constructor
        /// </summary>
        /// <param name="parent">parent path from which this originates</param>
        /// <param name="path_ID">unique ID for this path</param>
        /// <param name="grid_dimension_cells">dimension of the grid</param>
        public particlePath(particlePath parent, 
                            UInt32 path_ID, 
                            int grid_dimension_cells)
        {
            ID = path_ID;
            parent.current_pose.no_of_children++;
            current_pose = parent.current_pose;
            branch_pose = parent.current_pose;

            incrementPathChildren(parent, 1);

            this.max_length = parent.max_length;
            path = new List<particlePose>();
            total_score = parent.total_score;
            total_poses = parent.total_poses;
            Enabled = true;

            // map cache for this path
            //map_cache = new ArrayList[grid_dimension_cells][][];
        }
        /// <summary>
        /// show an occupancy grid from one side
        /// </summary>
        /// <param name="img">image in which to insert the data</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="pose">best available robot pose hypothesis</param>
        /// <param name="leftSide">view from the left side or the right</param>
        private void ShowSide(byte[] img, 
                              int width, 
                              int height, 
                              particlePose pose, 
                              bool leftSide)
        {
            float[] mean_colour = new float[3];

            // clear the image
            for (int i = 0; i < width * height * 3; i++)
                img[i] = (byte)255;

            // show the left or right half of the grid
            int start_x = 0;
            int end_x = dimension_cells / 2;
            if (!leftSide)
            {
                start_x = dimension_cells / 2;
                end_x = dimension_cells;
            }

            for (int cell_x = start_x; cell_x < end_x; cell_x++)
            {
                for (int cell_y = 0; cell_y < dimension_cells; cell_y++)
                {
                    if (cell[cell_x][cell_y] != null)
                    {
                        // what's the probability of there being a vertical structure here?
                        float mean_variance = 0;
                        float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
                                                                       mean_colour, ref mean_variance);

                        if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                        {
                            if (prob > 0.45f)
                            {
                                // show this vertical grid section
                                occupancygridCellMultiHypothesis c = cell[cell_x][cell_y];
                                for (int cell_z = 0; cell_z < dimension_cells_vertical; cell_z++)
                                {
                                    mean_variance = 0;
                                    prob = c.GetProbability(pose, cell_x, cell_y, cell_z, false, mean_colour, ref mean_variance);

                                    if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                    {
                                        if ((prob > 0.0f) && (mean_variance < 0.5f))
                                        {
                                            int multiplier = 2;
                                            int x = cell_y * width / dimension_cells;
                                            int y = (cell_z * height * multiplier / dimension_cells_vertical);
                                            if ((y >= 0) && (y < height - multiplier-2))
                                            {
                                                for (int yy = y; yy <= y + multiplier+2; yy++)
                                                {
                                                    int n = ((yy * width) + x) * 3;
                                                    for (int col = 0; col < 3; col++)
                                                        img[n + 2 - col] = (byte)mean_colour[col];
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
Exemple #44
0
        /// <summary>
        /// plan a route to a given location
        /// </summary>
        /// <param name="destination_waypoint"></param>
        public void PlanRoute(String destination_waypoint)
        {
            // clear the planned path
            planned_path = null;

            // retrieve the waypoint from the list of work sites
            kmlPlacemarkPoint waypoint = worksites.GetPoint(destination_waypoint);
            if (waypoint != null)
            {
                // get the destination waypoint position in millimetres
                // (the KML format stores positions in degrees)
                float destination_x = 0, destination_y = 0;
                waypoint.GetPositionMillimetres(ref destination_x, ref destination_y);

                // the best performing local grid
                occupancygridMultiHypothesis grid = LocalGrid[best_grid_index];

                // create a planner
                createPlanner(grid);

                // set variables, in the unlikely case that the centre position
                // of the grid has been changed since the planner was initialised
                planner.init(grid.navigable_space, grid.x, grid.y);

                // update the planner, in order to assign safety scores to the
                // navigable space.  The efficiency of this could be improved
                // by updating only those areas of the map which have changed
                planner.Update(0, 0, LocalGridDimension - 1, LocalGridDimension - 1);

                // create the plan
                List<float> new_plan = planner.CreatePlan(x, y, destination_x, destination_y);
                if (new_plan.Count > 0)
                {
                    // convert the plan into a set of poses
                    planned_path = new particlePath(new_plan.Count / 2);
                    float prev_xx = 0, prev_yy = 0;
                    for (int i = 0; i < new_plan.Count; i += 2)
                    {
                        float xx = (float)new_plan[i];
                        float yy = (float)new_plan[i + 1];
                        if (i > 0)
                        {
                            float dx = xx - prev_xx;
                            float dy = yy - prev_yy;
                            float dist = (float)Math.Sqrt((dx * dx) + (dy * dy));
                            if (dist > 0)
                            {
                                // TODO: check this pan angle
                                float pan = (float)Math.Sin(dx / dist);
                                if (dy < 0) pan = (2 * (float)Math.PI) - pan;

                                // create a pose and add it to the planned path
                                particlePose new_pose = new particlePose(prev_xx, prev_yy, pan, planned_path);
                                planned_path.Add(new_pose);
                            }
                        }
                        prev_xx = xx;
                        prev_yy = yy;
                    }
                }
            }
        }
        /// <summary>
        /// show the grid map as an image
        /// </summary>
        /// <param name="img">bitmap image</param>
        /// <param name="width">width in pixels</param>
        /// <param name="height">height in pixels</param>
        /// <param name="pose">best pose hypothesis from which the map is observed</param>
        private void Show(byte[] img, 
                          int width, 
                          int height, 
                          particlePose pose)
        {
            float[] mean_colour = new float[3];

            for (int y = 0; y < height; y++)
            {
                // get the y cell coordinate within the grid
                int cell_y = y * (dimension_cells-1) / height;

                for (int x = 0; x < width; x++)
                {
                    // get the x cell coordinate within the grid
                    int cell_x = x * (dimension_cells - 1) / width;

                    int n = ((y * width) + x) * 3;

                    if (cell[cell_x][cell_y] == null)
                    {
                        // terra incognita
                        for (int c = 0; c < 3; c++)
                            img[n + c] = (byte)255; 
                    }
                    else
                    {
                        // get the probability for this vertical column
                        float mean_variance = 0;
                        float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
                                                                         mean_colour, ref mean_variance);

                        if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                        {
                            for (int c = 0; c < 3; c++)
                                if (prob > 0.5f)
                                {
                                    if (prob > 0.7f)
                                        img[n + c] = (byte)0;  // occupied
                                    else
                                        img[n + c] = (byte)100;  // occupied
                                }
                                else
                                {
                                    if (prob < 0.3f)
                                        img[n + c] = (byte)230;  // vacant
                                    else
                                        img[n + c] = (byte)200;  // vacant
                                }
                        }
                        else
                        {
                            for (int c = 0; c < 3; c++)
                                img[n + c] = (byte)255; // terra incognita
                        }
                    }
                }
            }
        }
        /// <summary>
        /// return the probability of occupancy for the entire cell
        /// </summary>
        /// <param name="pose">pose from which the observation was made</param>
        /// <returns>probability as log odds</returns>
        public float GetProbability(
		    particlePose pose, 
		    int x, int y,
            float[] mean_colour, 
		    ref float mean_variance)
        {
            float probabilityLogOdds = 0;
            int hits = 0;
            mean_variance = 0;

            mean_colour[0] = 0;
			mean_colour[1] = 0;
			mean_colour[2] = 0;

            for (int z = Hypothesis.Length - 1; z >= 0; z--)
            {
                float variance = 0;
                float probLO = GetProbability(pose, x, y, z, true, temp_colour, ref variance);
                if (probLO != NO_OCCUPANCY_EVIDENCE)
                {
                    probabilityLogOdds += probLO;                    
                    mean_colour[0] += temp_colour[0];
					mean_colour[1] += temp_colour[1];
					mean_colour[2] += temp_colour[2];
                    mean_variance += variance;
                    hits++;
                }
            }
            if (hits > 0)
            {
                mean_variance /= hits;
                mean_colour[0] /= hits;
				mean_colour[1] /= hits;
				mean_colour[2] /= hits;
            }
            return (probabilities.LogOddsToProbability(probabilityLogOdds));
        }
        /// <summary>
        /// show a colour occupancy grid
        /// </summary>
        /// <param name="img">image in which to insert the data</param>
        /// <param name="width">width of the image</param>
        /// <param name="height">height of the image</param>
        /// <param name="pose">best available robot pose hypothesis</param>
        private void ShowColour(byte[] img, 
                                int width, 
                                int height, 
                                particlePose pose)
        {
            float[] mean_colour = new float[3];

            for (int y = 0; y < height; y++)
            {
                int cell_y = y * (dimension_cells - 1) / height;
                for (int x = 0; x < width; x++)
                {
                    int cell_x = x * (dimension_cells - 1) / width;

                    int n = ((y * width) + x) * 3;

                    if (cell[cell_x][cell_y] == null)
                    {
                        for (int c = 0; c < 3; c++)
                            img[n + c] = (byte)255; // terra incognita
                    }
                    else
                    {
                        float mean_variance = 0;
                        float prob = cell[cell_x][cell_y].GetProbability(pose, cell_x, cell_y,
                                                                         mean_colour, ref mean_variance);

                        if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                        {
                            if ((mean_variance < 0.7f) || (prob < 0.5f))
                            {
                                for (int c = 0; c < 3; c++)
                                    if (prob > 0.6f)
                                    {
                                        img[n + 2 - c] = (byte)mean_colour[c];  // occupied
                                    }
                                    else
                                    {
                                        if (prob < 0.3f)
                                            img[n + c] = (byte)200;
                                        else
                                            img[n + c] = (byte)255;
                                    }
                            }
                            else
                            {
                                for (int c = 0; c < 3; c++)
                                    img[n + c] = (byte)255;
                            }
                        }
                        else
                        {
                            for (int c = 0; c < 3; c++)
                                img[n + c] = (byte)255; // terra incognita
                        }
                    }
                }
            }
        }
        /// <summary>
        /// save the occupancy grid data to file as a tile
        /// it is expected that multiple tiles will be saved per grid
        /// This returns a byte array, which may subsequently be 
        /// compressed as a zip file for extra storage efficiency
        /// </summary>
        /// <param name="pose">best available pose</param>
        /// <param name="centre_x">centre of the tile in grid cells</param>
        /// <param name="centre_y">centre of the tile in grid cells</param>
        /// <param name="width_cells">width of the tile in grid cells</param>
        /// <returns>byte array containing the data</returns>
        public byte[] SaveTile(particlePose pose,
                               int centre_x, 
                               int centre_y,
                               int width_cells)
        {
            ArrayList data = new ArrayList();

            int half_width_cells = width_cells / 2;

            int tx = centre_x + half_width_cells;
            int ty = centre_y + half_width_cells;
            int bx = centre_x - half_width_cells;
            int by = centre_y - half_width_cells;

            // get the bounding region within which there are actice grid cells
            for (int x = centre_x - half_width_cells; x <= centre_x + half_width_cells; x++)
            {
                if ((x >= 0) && (x < dimension_cells))
                {
                    for (int y = centre_y - half_width_cells; y <= centre_y + half_width_cells; y++)
                    {
                        if ((y >= 0) && (y < dimension_cells))
                        {
                            if (cell[x][y] != null)
                            {
                                if (x < tx) tx = x;
                                if (y < ty) ty = y;
                                if (x > bx) bx = x;
                                if (y > by) by = y;
                            }
                        }
                    }
                }
            }
            bx++;
            by++;

            // write the bounding box dimensions to file
            byte[] intbytes = BitConverter.GetBytes(tx);
            for (int i = 0; i < intbytes.Length; i++)
                data.Add(intbytes[i]);
            intbytes = BitConverter.GetBytes(ty);
            for (int i = 0; i < intbytes.Length; i++)
                data.Add(intbytes[i]);
            intbytes = BitConverter.GetBytes(bx);
            for (int i = 0; i < intbytes.Length; i++)
                data.Add(intbytes[i]);
            intbytes = BitConverter.GetBytes(by);
            for (int i = 0; i < intbytes.Length; i++)
                data.Add(intbytes[i]);

            // create a binary index
            int w1 = bx - tx;
            int w2 = by - ty;
            bool[] binary_index = new bool[w1 * w2];

            int n = 0;
            int occupied_cells = 0;
            for (int y = ty; y < by; y++)
            {
                for (int x = tx; x < bx; x++)
                {
                    if (cell[x][y] != null)
                    {
                        occupied_cells++;
                        binary_index[n] = true;
                    }
                    n++;
                }
            }

            // convert the binary index to a byte array for later storage
            byte[] indexbytes = ArrayConversions.ToByteArray(binary_index);
            for (int i = 0; i < indexbytes.Length; i++)
                data.Add(indexbytes[i]);            

            // dummy variables needed by GetProbability
            float[] colour = new float[3];

            if (occupied_cells > 0)
            {
                float[] occupancy = new float[occupied_cells * dimension_cells_vertical];
                byte[] colourData = new byte[occupied_cells * dimension_cells_vertical * 3];
                float mean_variance = 0;

                n = 0;
                for (int y = ty; y < by; y++)
                {
                    for (int x = tx; x < bx; x++)
                    {
                        if (cell[x][y] != null)
                        {
                            for (int z = 0; z < dimension_cells_vertical; z++)
                            {
                                float prob = cell[x][y].GetProbability(pose,x, y, z, 
                                                                       true, colour, ref mean_variance);
                                int index = (n * dimension_cells_vertical) + z;
                                occupancy[index] = prob;
                                if (prob != occupancygridCellMultiHypothesis.NO_OCCUPANCY_EVIDENCE)
                                    for (int col = 0; col < 3; col++)
                                        colourData[(index * 3) + col] = (byte)colour[col];
                            }
                            n++;
                        }
                    }
                }

                // store the occupancy and colour data as byte arrays
                byte[] occupancybytes = ArrayConversions.ToByteArray(occupancy);
                for (int i = 0; i < occupancybytes.Length; i++)
                    data.Add(occupancybytes[i]);
                for (int i = 0; i < colourData.Length; i++)
                    data.Add(colourData[i]);            
            }
            byte[] result = (byte[])data.ToArray(typeof(byte));
            return(result);
        }
 /// <summary>
 /// save the entire grid as a byte array, suitable for subsequent compression
 /// </summary>
 /// <param name="pose"></param>
 /// <returns></returns>
 public byte[] Save(particlePose pose)
 {
     return(SaveTile(pose, dimension_cells / 2, dimension_cells / 2, dimension_cells));
 }
Exemple #50
0
        /// <summary>
        /// turns a list of path segments into a list of individual poses
        /// </summary>
        private void updatePath()
        {
            particlePose prev_pose = null;

            path = new particlePath(999999999);

            min_x = 9999;
            min_y = 9999;
            max_x = -9999;
            max_y = -9999;
            for (int s = 0; s < pathSegments.Count; s++)
            {
                simulationPathSegment segment = (simulationPathSegment)pathSegments[s];

                // get the last pose
                if (s > 0)
                {
                    prev_pose = (particlePose)path.path[path.path.Count - 1];
                }

                // update the list of poses
                List <particlePose> poses = segment.getPoses();

                if (prev_pose != null)
                {
                    // is the last pose position the same as the first in this segment?
                    // if so, remove the last pose added to the path
                    particlePose firstPose = (particlePose)poses[0];
                    if (((int)firstPose.x == (int)prev_pose.x) &&
                        ((int)firstPose.y == (int)prev_pose.y) &&
                        (Math.Abs(firstPose.pan - prev_pose.pan) < 0.01f))
                    {
                        path.path.RemoveAt(path.path.Count - 1);
                    }
                }

                for (int i = 0; i < poses.Count; i++)
                {
                    particlePose pose = (particlePose)poses[i];
                    if (pose.x < min_x)
                    {
                        min_x = pose.x;
                    }
                    if (pose.y < min_y)
                    {
                        min_y = pose.y;
                    }
                    if (pose.x > max_x)
                    {
                        max_x = pose.x;
                    }
                    if (pose.y > max_y)
                    {
                        max_y = pose.y;
                    }
                    path.Add(pose);
                }
            }

            // update the path velocities
            velocities = path.getVelocities(0, 0, time_per_index_sec);
        }
 /// <summary>
 /// export the occupancy grid data to IFrIT basic particle file format for visualisation
 /// </summary>
 /// <param name="filename">name of the file to save as</param>
 /// <param name="pose">best available pose</param>
 public void ExportToIFrIT(string filename, 
                           particlePose pose)
 {
     ExportToIFrIT(filename, pose, dimension_cells / 2, dimension_cells / 2, dimension_cells);
 }