Пример #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 show_uncertainty_ellipse,
            bool show_path_tree,
            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, show_uncertainty_ellipse, show_path_tree, clearBackground);
        }
Пример #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,
            dpslam 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);
        }
Пример #3
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);
        }
Пример #4
0
        /// <summary>
        /// remove the mapping particles associated with this path
        /// </summary>
        public bool Remove(dpslam 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);
        }
Пример #5
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.z, format) + "," +
                                   Convert.ToString(pose.pan * 180 / Math.PI, format) + "," +
                                   Convert.ToString(pose.tilt * 180 / Math.PI, format) + "," +
                                   Convert.ToString(pose.roll * 180 / Math.PI, format));
            }

            return(nodePath);
        }
Пример #6
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);
            }
        }
Пример #7
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="show_uncertainty_ellipse">whether to show the uncertainty ellipse</param>
        /// <param name="show_path_tree">whether to show the path tree</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 show_uncertainty_ellipse,
            bool show_path_tree,
            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;
                            }
                        }
                    }
                }
            }

            if (show_uncertainty_ellipse)
            {
                ShowUncertaintyEllipse(
                    img, width, height,
                    min_x_mm, min_y_mm,
                    max_x_mm, max_y_mm,
                    255, 0, 0);
            }

            // show the path
            if (show_path_tree)
            {
                ShowPath(img, width, height, 0, 255, 0, 0, min_x_mm, min_y_mm, max_x_mm, max_y_mm, false);
            }
        }
Пример #8
0
        /// <summary>
        /// distill all grid particles within this path
        /// </summary>
        /// <param name="grid"></param>
        public void Distill(dpslam grid)
        {
            particlePose pose = current_pose;

            while (pose != null)
            {
                pose.Distill(grid);
                pose = pose.parent;
            }
            map_cache = null;
            Enabled   = false;
        }
Пример #9
0
 /// <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;
 }
Пример #10
0
 /// <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;
 }
Пример #11
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);
         }
     }
 }
Пример #12
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);
                }
            }
        }
Пример #13
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_pan)));

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

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

            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, 0, new_pan, 0, 0, path);

            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
Пример #14
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 z,
            float pan,
            float tilt,
            float roll,
            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, z, pan, tilt, roll, this);

            pose.time_step = time_step;
            Enabled        = true;
            Add(pose);
        }
Пример #15
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_speed(
            particlePath path,
            float time_elapsed_sec)
        {
            float fwd_velocity = forward_velocity +
                                 sample_normal_distribution(
                speed_uncertainty_forward);
            float ang_velocity = angular_velocity_pan +
                                 sample_normal_distribution(
                speed_uncertainty_angular);

            float new_pan = path.current_pose.pan + (ang_velocity * time_elapsed_sec);
            float dist    = fwd_velocity * time_elapsed_sec;
            float new_y   = path.current_pose.y + (dist * (float)Math.Cos(new_pan));
            float new_x   = path.current_pose.x + (dist * (float)Math.Sin(new_pan));

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

            new_pose.time_step = time_step;
            path.Add(new_pose);
        }
Пример #16
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][][];
        }
Пример #17
0
        /// <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));
        }
Пример #18
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),
                        Convert.ToSingle(poseStr[3], format) * (float)Math.PI / 180.0f,
                        Convert.ToSingle(poseStr[4], format) * (float)Math.PI / 180.0f,
                        Convert.ToSingle(poseStr[5], 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;
                }
            }
        }
Пример #19
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;
                    }
            }

        }
Пример #20
0
        /// <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
				//Console.WriteLine("occupancy_probability = " + occupancy_probability.ToString());
				if (occupancy_probability > 1) occupancy_probability = 1;
                prob_log_odds = LogOdds[(int)(occupancy_probability * colour_probability * LOG_ODDS_LOOKUP_LEVELS)];
            }
            return (prob_log_odds);
        }
Пример #21
0
        /// <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);
        }
Пример #22
0
		/// <summary>
		/// probes the grid and returns a set of ranges
		/// </summary>
		/// <param name="pose">current best pose estimate</param>
		/// <param name="camera_x_mm">camera sensor x coordinate</param>
		/// <param name="camera_y_mm">camera sensor y coordinate</param>
		/// <param name="camera_z_mm">camera sensor z coordinate</param>
		/// <param name="camera_pan">camera pan in radians</param>
		/// <param name="camera_tilt">camera tilt in radians</param>
		/// <param name="camera_roll">camera roll in radians</param>
		/// <param name="camera_FOV_degrees">camera field of view in degrees</param>
		/// <param name="camera_resolution_width">camera resolution pixels horizontal</param>
		/// <param name="camera_resolution_height">camera resolution pixels vertical</param>
		/// <param name="step_size">sampling step size in pixels</param>
		/// <param name="max_range_mm">maximum range</param>
		/// <param name="use_ground_plane">whether to detect the ground plane, z=0</param>
		/// <param name="range_buffer">buffer storing returned ranges (-1 = range unknown)</param>
		public void ProbeView(
		    particlePose pose,
		    float camera_x_mm,
		    float camera_y_mm,
		    float camera_z_mm,
		    float camera_pan,
		    float camera_tilt,
		    float camera_roll,
		    float camera_FOV_degrees,
		    int camera_resolution_width,
		    int camera_resolution_height,
		    int step_size,
		    float max_range_mm,
		    bool use_ground_plane,
		    float[] range_buffer)
		{
			float camera_FOV_radians = camera_FOV_degrees * (float)Math.PI / 180.0f;
			float camera_FOV_radians2 = camera_FOV_radians * camera_resolution_height / camera_resolution_width;			
			int n = 0;
			for (int camy = 0; camy < camera_resolution_height; camy+=step_size)
			{
				float tilt = (camy * camera_FOV_radians2 / camera_resolution_height) - (camera_FOV_radians2*0.5f);
				float z = max_range_mm*(float)Math.Tan(tilt);
			    for (int camx = 0; camx < camera_resolution_width; camx+=step_size, n++)
			    {
				    float pan = (camx * camera_FOV_radians / camera_resolution_width) - (camera_FOV_radians*0.5f);
					float x = max_range_mm*(float)Math.Tan(pan);
					pos3D p = new pos3D(x,max_range_mm,z);
					pos3D p2 = p.rotate(camera_pan, camera_tilt, camera_roll);

					range_buffer[n] = ProbeRange(
		                pose,
	                    camera_x_mm,
					    camera_y_mm,
					    camera_z_mm,
		                camera_x_mm + p2.x,
		                camera_y_mm + p2.y,
		                camera_z_mm + p2.z,
					    use_ground_plane);
				}
			}
		}
Пример #23
0
        /// <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);
        }
Пример #24
0
        /// <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));
        }
Пример #25
0
        /// <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);
        }
Пример #26
0
        /// <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   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);
            }
        }
Пример #27
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;
                    }
                }
            }
        }
Пример #28
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][][];
        }
Пример #29
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 z, 
            float pan,
		    float tilt,
		    float roll,
            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, z, pan, tilt, roll, this);
            pose.time_step = time_step;
            Enabled = true;
            Add(pose);
        }
Пример #30
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);
            }
        }
Пример #31
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,
            dpslam 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);
        }
Пример #32
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),
                        Convert.ToSingle(poseStr[3], format) * (float)Math.PI/180.0f, 
                        Convert.ToSingle(poseStr[4], format) * (float)Math.PI/180.0f, 
                        Convert.ToSingle(poseStr[5], 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;
                }
            }
        }
Пример #33
0
        /// <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();
        }
Пример #34
0
        /// <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));
        }
Пример #35
0
        /// <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));
        }
Пример #36
0
        /// <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 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);
        }
Пример #37
0
        /// <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);
        }
Пример #38
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_pan)));

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

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

            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, 0, new_pan, 0, 0, path);
            new_pose.time_step = time_step;
            path.Add(new_pose);
        }		
Пример #39
0
        /// <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();
                    }
                }

            }
        }
Пример #40
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_speed(
		    particlePath path, 
		    float time_elapsed_sec)
        {
            float fwd_velocity = forward_velocity + 
				sample_normal_distribution(
                    speed_uncertainty_forward);
            float ang_velocity = angular_velocity_pan + 
				sample_normal_distribution(
                    speed_uncertainty_angular);

			float new_pan = path.current_pose.pan + (ang_velocity * time_elapsed_sec);
			float dist = fwd_velocity * time_elapsed_sec;
			float new_y = path.current_pose.y + (dist * (float)Math.Cos(new_pan));
			float new_x = path.current_pose.x + (dist * (float)Math.Sin(new_pan));

            particlePose new_pose = new particlePose(new_x, new_y, 0, new_pan, 0, 0, path);
            new_pose.time_step = time_step;
            path.Add(new_pose);
        }		
Пример #41
0
		/// <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,
		    bool use_ground_plane)
		{
			float range_mm = -1;
			int half_dimension_cells = dimension_cells/2;
			int half_dimension_cells_vertical = dimension_cells_vertical/2;
			int ground_plane_cells = half_dimension_cells_vertical - (int)(z / cellSize_mm);
			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_vertical + (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_vertical + (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.0f / (float)dist_cells;
			float fraction = 0;
			float[] colour = new float[3];
			float mean_variance = 0;
			float prob, peak_prob = 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))
						{
							if ((z_cell < ground_plane_cells) && (use_ground_plane))
							{
								range_mm = dist * cellSize_mm;
								//break;
							}
							else
							{
								occupancygridCellMultiHypothesis c = cell[x_cell][y_cell];
								if (c != null)
								{
								    prob = c.GetProbability(pose, x_cell, y_cell, z_cell, false, colour, ref mean_variance);
									if (prob > 0)
									{
										if (prob > peak_prob)
									    {
											peak_prob  = prob;
											range_mm = dist * cellSize_mm;										
										}
										if ((peak_prob > 0.5f) && (prob < peak_prob)) break;
								    }
								}
							}
						}
						else break;
					}
					else break;
				}
				else break;
			}
			return(range_mm);
		}
Пример #42
0
        public void Show(
		    string filename,
            int width, 
            int height, 
            particlePose pose)
		{
		    byte[] img = new byte[width * height * 3];
			Show(img, width, height, pose);
			Bitmap bmp = new Bitmap(width, height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
			BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
			if (filename.EndsWith("bmp")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Bmp);
			if (filename.EndsWith("jpg")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
			if (filename.EndsWith("gif")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Gif);
			if (filename.EndsWith("png")) bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Png);
		}
Пример #43
0
        /// <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;
        }
Пример #44
0
        /// <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
                        }
                    }
                }
            }
        }
Пример #45
0
        /// <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
			float ray_model_interval_pixels = sensormodel_lookup.ray_model_interval_pixels;
            int sensormodel_index = (int)Math.Round(ray.disparity / ray_model_interval_pixels);
            
            // 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_lookup == null) Console.WriteLine("Sensor models are missing");
            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 = ray_model_interval_pixels + (sensormodel_lookup_probability[sensormodel_index][grid_step] * ray_model_interval_pixels);
                                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);
        }
Пример #46
0
        /// <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
                        }
                    }
                }
            }
        }
Пример #47
0
        /// <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];
                                                }
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
Пример #48
0
        public void AveragePose(
            ref float x,
            ref float y,
            ref float z,
            ref float pan,
            ref float tilt,
            ref float roll,
            ref float deviation,
            ref float deviation_forward,
            ref float deviation_perp,
            ref float deviation_vertical)
        {
            x                  = 0;
            y                  = 0;
            z                  = 0;
            pan                = 0;
            tilt               = 0;
            roll               = 0;
            deviation          = 0;
            deviation_forward  = 0;
            deviation_perp     = 0;
            deviation_vertical = 0;
            for (int p = 0; p < Poses.Count; p++)
            {
                particlePose pose = Poses[p].current_pose;
                x += pose.x;
                y += pose.y;
                z += pose.z;
                float pan2 = pose.pan;
                if (pan2 < -Math.PI)
                {
                    pan2 = -(2 * (float)Math.PI) - pan2;
                }
                if (pan2 > Math.PI)
                {
                    pan2 = (2 * (float)Math.PI) - pan2;
                }
                pan += pan2;
                float tilt2 = pose.tilt;
                if (tilt2 < -Math.PI)
                {
                    tilt2 = -(2 * (float)Math.PI) - tilt2;
                }
                if (tilt2 > Math.PI)
                {
                    tilt2 = (2 * (float)Math.PI) - tilt2;
                }
                tilt += tilt2;
                roll += pose.roll;
            }
            if (Poses.Count > 0)
            {
                x    /= Poses.Count;
                y    /= Poses.Count;
                z    /= Poses.Count;
                pan  /= Poses.Count;
                tilt /= Poses.Count;
                roll /= Poses.Count;

                float axis_x0 = x + (10 * (float)Math.Sin(pan));
                float axis_y0 = y + (10 * (float)Math.Cos(pan));
                float axis_x1 = x + (10 * (float)Math.Sin(pan + (Math.PI / 2)));
                float axis_y1 = y + (10 * (float)Math.Cos(pan + (Math.PI / 2)));

                for (int p = 0; p < Poses.Count; p++)
                {
                    particlePose pose = Poses[p].current_pose;
                    float        dx   = x - pose.x;
                    float        dy   = y - pose.y;
                    float        dz   = z - pose.z;
                    float        dev  = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
                    deviation += dev;
                    float d = Math.Abs(geometry.circleDistanceFromLine(x, y, axis_x0, axis_y0, pose.x, pose.y, 0));
                    deviation_forward += d;
                    d = Math.Abs(geometry.circleDistanceFromLine(x, y, axis_x1, axis_y1, pose.x, pose.y, 0));
                    deviation_perp     += d;
                    deviation_vertical += Math.Abs(dz);
                }
                deviation          /= Poses.Count;
                deviation          *= 4;
                deviation_forward  /= Poses.Count;
                deviation_forward  *= 4;
                deviation_perp     /= Poses.Count;
                deviation_perp     *= 4;
                deviation_vertical /= Poses.Count;
                deviation_vertical *= 4;
            }
        }