public void ReceivedOdometryUpdate(ROSBridgeMsg data)
    {
        //In WGS84
        OdometryMsg nav = (OdometryMsg)data;

        GeoPointWGS84 geoPoint = new GeoPointWGS84
        {
            latitude  = nav._pose._pose._position.GetY(),
            longitude = nav._pose._pose._position.GetX(),
            altitude  = nav._pose._pose._position.GetZ(),
        };
        Quaternion orientation = new Quaternion(
            x: nav._pose._pose._orientation.GetX(),
            z: nav._pose._pose._orientation.GetY(),
            y: nav._pose._pose._orientation.GetZ(),
            w: nav._pose._pose._orientation.GetW()
            );

        _odometryDataToConsume = new OdometryData
        {
            Position    = geoPoint,
            Orientation = orientation
        };
        _hasOdometryDataToConsume = true;
    }
        public void LocaliseAlongPath()
        {
            // systematic bias
            float bias_x_mm = -200;
            float bias_y_mm = 0;

            // number of stereo cameras on the robot's head
            int no_of_stereo_cameras = 2;

            // diameter of the robot's head
            float head_diameter_mm = 100;

            // number of stereo features per step during mapping
            int no_of_mapping_stereo_features = 300;

            // number of stereo features observed per localisation step
            int no_of_localisation_stereo_features = 100;

            string filename                  = "localise_along_path.dat";
            float  path_length_mm            = 20000;
            float  start_orientation         = 0;
            float  end_orientation           = 0; // 90 * (float)Math.PI / 180.0f;
            float  distance_between_poses_mm = 100;
            float  disparity                 = 15;

            string overall_map_filename = "overall_map.jpg";

            byte[] overall_map_img          = null;
            int    overall_img_width        = 640;
            int    overall_img_height       = 480;
            int    overall_map_dimension_mm = 0;
            int    overall_map_centre_x_mm  = 0;
            int    overall_map_centre_y_mm  = 0;

            string[] str = filename.Split('.');

            List <OdometryData> path = null;

            SavePath(
                filename,
                path_length_mm,
                start_orientation,
                end_orientation,
                distance_between_poses_mm,
                disparity,
                no_of_mapping_stereo_features,
                no_of_stereo_cameras,
                ref path);

            Assert.AreEqual(true, File.Exists(filename));
            Assert.AreEqual(true, File.Exists(str[0] + "_disparities_index.dat"));
            Assert.AreEqual(true, File.Exists(str[0] + "_disparities.dat"));

            int   no_of_grids           = 1;
            int   grid_type             = metagrid.TYPE_SIMPLE;
            int   dimension_mm          = 8000;
            int   dimension_vertical_mm = 2000;
            int   cellSize_mm           = 50;
            int   localisationRadius_mm = 8000;
            int   maxMappingRange_mm    = 10000;
            float vacancyWeighting      = 0.5f;

            metagridBuffer buffer =
                new metagridBuffer(
                    no_of_grids,
                    grid_type,
                    dimension_mm,
                    dimension_vertical_mm,
                    cellSize_mm,
                    localisationRadius_mm,
                    maxMappingRange_mm,
                    vacancyWeighting);

            buffer.LoadPath(
                filename,
                str[0] + "_disparities_index.dat",
                str[0] + "_disparities.dat",
                ref overall_map_dimension_mm,
                ref overall_map_centre_x_mm,
                ref overall_map_centre_y_mm);

            int img_width  = 640;
            int img_height = 640;

            byte[] img = new byte[img_width * img_height * 3];
            Bitmap bmp = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

            buffer.ShowPath(img, img_width, img_height, true, true);
            BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
            bmp.Save("localise_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);

            robotGeometry geom = new robotGeometry();

            geom.CreateStereoCameras(
                no_of_stereo_cameras, 120, 0,
                320, 240,
                65,
                head_diameter_mm,
                0);

            geom.CreateSensorModels(buffer);

            Random rnd               = new Random(0);
            pos3D  pose_offset       = null;
            bool   buffer_transition = false;

            float[][] stereo_features = new float[no_of_stereo_cameras][];
            byte[][,] stereo_features_colour = new byte[no_of_stereo_cameras][, ];
            float[][] stereo_features_uncertainties = new float[no_of_stereo_cameras][];
            for (int i = 0; i < no_of_stereo_cameras; i++)
            {
                stereo_features_uncertainties[i] = new float[no_of_localisation_stereo_features];
                for (int j = 0; j < no_of_localisation_stereo_features; j++)
                {
                    stereo_features_uncertainties[i][j] = 1;
                }
            }

            float average_offset_x_mm          = 0;
            float average_offset_y_mm          = 0;
            List <OdometryData> estimated_path = new List <OdometryData>();

            int no_of_localisation_failures = 0;

            for (int i = 0; i < path.Count - 1; i += 5)
            {
                string debug_mapping_filename = "localise_along_path_map_" + i.ToString() + ".jpg";

                OdometryData p0 = path[i];
                OdometryData p1 = path[i + 1];

                // create an intermediate pose
                for (int cam = 0; cam < no_of_stereo_cameras; cam++)
                {
                    geom.pose[cam].x   = p0.x + ((p1.x - p0.x) / 2) + bias_x_mm;
                    geom.pose[cam].y   = p0.y + ((p1.y - p0.y) / 2) + bias_y_mm;
                    geom.pose[cam].z   = 0;
                    geom.pose[cam].pan = p0.orientation + ((p1.orientation - p0.orientation) / 2);

                    // create stereo features
                    int ctr = 0;
                    stereo_features[cam]        = new float[no_of_localisation_stereo_features * 3];
                    stereo_features_colour[cam] = new byte[no_of_localisation_stereo_features, 3];
                    for (int f = 0; f < no_of_localisation_stereo_features; f += 5)
                    {
                        if (f < no_of_localisation_stereo_features / 2)
                        {
                            stereo_features[cam][ctr++] = 20;
                            stereo_features[cam][ctr++] = rnd.Next(239);
                        }
                        else
                        {
                            stereo_features[cam][ctr++] = geom.image_width[cam] - 20;
                            stereo_features[cam][ctr++] = rnd.Next(239);
                        }
                        stereo_features[cam][ctr++] = disparity;
                    }
                }

                float matching_score = buffer.Localise(
                    geom,
                    stereo_features,
                    stereo_features_colour,
                    stereo_features_uncertainties,
                    rnd,
                    ref pose_offset,
                    ref buffer_transition,
                    debug_mapping_filename,
                    bias_x_mm, bias_y_mm,
                    overall_map_filename,
                    ref overall_map_img,
                    overall_img_width,
                    overall_img_height,
                    overall_map_dimension_mm,
                    overall_map_centre_x_mm,
                    overall_map_centre_y_mm);

                if (matching_score != occupancygridBase.NO_OCCUPANCY_EVIDENCE)
                {
                    Console.WriteLine("pose_offset (mm): " + pose_offset.x.ToString() + ", " + pose_offset.y.ToString() + ", " + pose_offset.pan.ToString());
                    OdometryData estimated_pose = new OdometryData();
                    estimated_pose.x           = geom.pose[0].x + pose_offset.x;
                    estimated_pose.y           = geom.pose[0].y + pose_offset.y;
                    estimated_pose.orientation = geom.pose[0].pan + pose_offset.pan;
                    estimated_path.Add(estimated_pose);
                    average_offset_x_mm += pose_offset.x;
                    average_offset_y_mm += pose_offset.y;
                }
                else
                {
                    // fail!
                    no_of_localisation_failures++;
                    Console.WriteLine("Localisation failure");
                }
            }

            buffer.ShowPath(img, img_width, img_height, true, true);
            BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
            bmp.Save("localisations_along_path.jpg", System.Drawing.Imaging.ImageFormat.Jpeg);

            average_offset_x_mm /= estimated_path.Count;
            average_offset_y_mm /= estimated_path.Count;
            Console.WriteLine("Average offsets: " + average_offset_x_mm.ToString() + ", " + average_offset_y_mm.ToString());

            float diff_x_mm = Math.Abs(average_offset_x_mm - bias_x_mm);
            float diff_y_mm = Math.Abs(average_offset_y_mm - bias_y_mm);

            Assert.Less(diff_x_mm, cellSize_mm * 3 / 2, "x bias not detected");
            Assert.Less(diff_y_mm, cellSize_mm * 3 / 2, "y bias not detected");

            if (no_of_localisation_failures > 0)
            {
                Console.WriteLine("Localisation failures: " + no_of_localisation_failures.ToString());
            }
            else
            {
                Console.WriteLine("No localisation failures!");
            }
            Assert.Less(no_of_localisation_failures, 4, "Too many localisation failures");
        }
        /// <summary>
        /// saves path data to file
        /// </summary>
        /// <param name="filename">filename to save as</param>
        /// <param name="path_length_mm">length of the path</param>
        /// <param name="start_orientation">orientation at the start of the path</param>
        /// <param name="end_orientation">orientation at the end of the path</param>
        /// <param name="distance_between_poses_mm">distance between poses</param>
        public static void SavePath(
            string filename,
            float path_length_mm,
            float start_orientation,
            float end_orientation,
            float distance_between_poses_mm,
            float disparity,
            int no_of_stereo_features,
            int no_of_stereo_cameras,
            ref List <OdometryData> save_path)
        {
            string[] str = filename.Split('.');
            float    start_x_mm = 0;
            float    start_y_mm = 0;
            float    x_mm = start_x_mm, y_mm = start_y_mm;
            float    orientation = start_orientation;
            int      steps       = (int)(path_length_mm / distance_between_poses_mm);

            save_path = new List <OdometryData>();
            Random rnd = new Random(0);

            if (File.Exists(str[0] + "_disparities_index.dat"))
            {
                File.Delete(str[0] + "_disparities_index.dat");
            }
            if (File.Exists(str[0] + "_disparities.dat"))
            {
                File.Delete(str[0] + "_disparities.dat");
            }

            for (int i = 0; i < steps; i++)
            {
                for (int cam = 0; cam < no_of_stereo_cameras; cam++)
                {
                    OdometryData data = new OdometryData();
                    data.orientation = orientation;
                    data.x           = x_mm;
                    data.y           = y_mm;
                    save_path.Add(data);

                    List <StereoFeatureTest> features = new List <StereoFeatureTest>();
                    for (int f = 0; f < no_of_stereo_features; f++)
                    {
                        StereoFeatureTest feat;
                        if (f < no_of_stereo_features / 2)
                        {
                            feat = new StereoFeatureTest(20, rnd.Next(239), disparity);
                        }
                        else
                        {
                            feat = new StereoFeatureTest(300, rnd.Next(239), disparity);
                        }
                        feat.SetColour(0, 0, 0);
                        features.Add(feat);
                    }

                    ProcessPose(
                        str[0],
                        DateTime.Now,
                        x_mm, y_mm,
                        orientation,
                        0, 0, 0,
                        cam, features);
                }

                x_mm       += distance_between_poses_mm * (float)Math.Sin(orientation);
                y_mm       += distance_between_poses_mm * (float)Math.Cos(orientation);
                orientation = start_orientation + ((end_orientation - start_orientation) * i / steps);
            }

            FileStream fs = File.Open(filename, FileMode.Create);

            BinaryWriter bw = new BinaryWriter(fs);

            bw.Write(save_path.Count);
            for (int i = 0; i < save_path.Count; i++)
            {
                save_path[i].Write(bw);
            }

            bw.Close();
            fs.Close();

            // save images of the path
            if (filename.Contains("."))
            {
                int    img_width  = 640;
                int    img_height = 480;
                byte[] img        = new byte[img_width * img_height * 3];
                Bitmap bmp        = new Bitmap(img_width, img_height, System.Drawing.Imaging.PixelFormat.Format24bppRgb);


                filename = str[0] + "_positions.jpg";
                float tx = 0, ty = 0;
                float bx = 0, by = 0;
                ShowPath(
                    save_path, img, img_width, img_height,
                    0, 0, 0, true,
                    ref tx, ref ty,
                    ref bx, ref by);
                BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
                bmp.Save(filename, System.Drawing.Imaging.ImageFormat.Jpeg);
            }
        }
Esempio n. 4
0
        public void LocaliseAlongPath()
        {
            // systematic bias
            float bias_x_mm = -200;
            float bias_y_mm = 0;

            // number of stereo features per step during mapping
            int no_of_mapping_stereo_features = 300;

            // number of stereo features observed per localisation step
            int no_of_localisation_stereo_features = 50;

            string filename                  = "steersman_localise_along_path.dat";
            float  path_length_mm            = 10000;
            float  start_orientation         = 0;
            float  end_orientation           = 0; //90 * (float)Math.PI / 180.0f;
            float  distance_between_poses_mm = 100;
            float  disparity                 = 15;

            string steersman_filename               = "tests_steersman_LocaliseAlongPath.xml";
            int    body_width_mm                    = 465;
            int    body_length_mm                   = 380;
            int    body_height_mm                   = 1660;
            int    centre_of_rotation_x             = body_width_mm / 2;
            int    centre_of_rotation_y             = body_length_mm / 2;
            int    centre_of_rotation_z             = 10;
            int    head_centroid_x                  = body_width_mm / 2;
            int    head_centroid_y                  = 65;
            int    head_centroid_z                  = 1600;
            string sensormodels_filename            = "";
            int    no_of_stereo_cameras             = 2;
            float  baseline_mm                      = 120;
            float  dist_to_centre_of_tilt_mm        = 0;
            int    image_width                      = 320;
            int    image_height                     = 240;
            float  FOV_degrees                      = 65;
            float  head_diameter_mm                 = 160;
            float  default_head_orientation_degrees = 0;
            int    no_of_grid_levels                = 1;
            int    dimension_mm                     = 8000;
            int    dimension_vertical_mm            = 2000;
            int    cellSize_mm                      = 50;

            string[] str = filename.Split('.');

            List <OdometryData> path = null;

            tests_metagridbuffer.SavePath(
                filename,
                path_length_mm,
                start_orientation,
                end_orientation,
                distance_between_poses_mm,
                disparity,
                no_of_mapping_stereo_features,
                no_of_stereo_cameras,
                ref path);

            Assert.AreEqual(true, File.Exists(filename));
            Assert.AreEqual(true, File.Exists(str[0] + "_disparities_index.dat"));
            Assert.AreEqual(true, File.Exists(str[0] + "_disparities.dat"));

            steersman visual_guidance = null;

            if (File.Exists(steersman_filename))
            {
                visual_guidance = new steersman();
                visual_guidance.Load(steersman_filename);
            }
            else
            {
                visual_guidance = new steersman(
                    body_width_mm,
                    body_length_mm,
                    body_height_mm,
                    centre_of_rotation_x,
                    centre_of_rotation_y,
                    centre_of_rotation_z,
                    head_centroid_x,
                    head_centroid_y,
                    head_centroid_z,
                    sensormodels_filename,
                    no_of_stereo_cameras,
                    baseline_mm,
                    dist_to_centre_of_tilt_mm,
                    image_width,
                    image_height,
                    FOV_degrees,
                    head_diameter_mm,
                    default_head_orientation_degrees,
                    no_of_grid_levels,
                    dimension_mm,
                    dimension_vertical_mm,
                    cellSize_mm);

                visual_guidance.Save(steersman_filename);
            }

            visual_guidance.LoadPath(filename, str[0] + "_disparities_index.dat", str[0] + "_disparities.dat");

            visual_guidance.ShowLocalisations("steersman_localise_along_path.jpg", 640, 480);

            Random rnd               = new Random(0);
            pos3D  pose_offset       = null;
            bool   buffer_transition = false;

            float[][] stereo_features = new float[no_of_stereo_cameras][];
            byte[][,] stereo_features_colour = new byte[no_of_stereo_cameras][, ];
            float[][] stereo_features_uncertainties = new float[no_of_stereo_cameras][];
            for (int i = 0; i < no_of_stereo_cameras; i++)
            {
                stereo_features_uncertainties[i] = new float[no_of_localisation_stereo_features];
                for (int j = 0; j < no_of_localisation_stereo_features; j++)
                {
                    stereo_features_uncertainties[i][j] = 1;
                }
            }

            float average_offset_x_mm          = 0;
            float average_offset_y_mm          = 0;
            List <OdometryData> estimated_path = new List <OdometryData>();

            int no_of_localisation_failures = 0;

            DateTime start_time          = DateTime.Now;
            int      no_of_localisations = 0;

            for (int i = 0; i < path.Count - 1; i += 5, no_of_localisations++)
            {
                string debug_mapping_filename = "steersman_localise_along_path_map_" + i.ToString() + ".jpg";

                OdometryData p0 = path[i];
                OdometryData p1 = path[i + 1];

                float current_x_mm = p0.x + ((p1.x - p0.x) / 2) + bias_x_mm;
                float current_y_mm = p0.y + ((p1.y - p0.y) / 2) + bias_y_mm;
                float current_pan  = p0.orientation + ((p1.orientation - p0.orientation) / 2);

                // create an intermediate pose
                for (int cam = 0; cam < no_of_stereo_cameras; cam++)
                {
                    // set the current pose
                    visual_guidance.SetCurrentPosition(
                        cam,
                        current_x_mm,
                        current_y_mm,
                        0,
                        current_pan,
                        0, 0);

                    // create stereo features
                    int ctr = 0;
                    stereo_features[cam]        = new float[no_of_localisation_stereo_features * 3];
                    stereo_features_colour[cam] = new byte[no_of_localisation_stereo_features, 3];
                    for (int f = 0; f < no_of_localisation_stereo_features; f += 5)
                    {
                        if (f < no_of_localisation_stereo_features / 2)
                        {
                            stereo_features[cam][ctr++] = 20;
                            stereo_features[cam][ctr++] = rnd.Next(239);
                        }
                        else
                        {
                            stereo_features[cam][ctr++] = image_width - 20;
                            stereo_features[cam][ctr++] = rnd.Next(239);
                        }
                        stereo_features[cam][ctr++] = disparity;
                    }
                }

                float offset_x_mm = 0;
                float offset_y_mm = 0;
                float offset_z_mm = 0;
                float offset_pan  = 0;
                float offset_tilt = 0;
                float offset_roll = 0;

                bool valid_localisation = visual_guidance.Localise(
                    stereo_features,
                    stereo_features_colour,
                    stereo_features_uncertainties,
                    debug_mapping_filename,
                    bias_x_mm, bias_y_mm,
                    ref offset_x_mm,
                    ref offset_y_mm,
                    ref offset_z_mm,
                    ref offset_pan,
                    ref offset_tilt,
                    ref offset_roll);

                if (valid_localisation)
                {
                    Console.WriteLine("pose_offset (mm): " + offset_x_mm.ToString() + ", " + offset_y_mm.ToString() + ", " + offset_pan.ToString());
                    OdometryData estimated_pose = new OdometryData();
                    estimated_pose.x           = current_x_mm + offset_x_mm;
                    estimated_pose.y           = current_y_mm + offset_y_mm;
                    estimated_pose.orientation = current_pan + offset_pan;
                    estimated_path.Add(estimated_pose);
                    average_offset_x_mm += offset_x_mm;
                    average_offset_y_mm += offset_y_mm;
                }
                else
                {
                    // fail!
                    no_of_localisation_failures++;
                    Console.WriteLine("Localisation failure");
                }
            }

            TimeSpan diff = DateTime.Now.Subtract(start_time);
            float    time_per_localisation_sec = (float)diff.TotalSeconds / no_of_localisations;

            Console.WriteLine("Time per localisation: " + time_per_localisation_sec.ToString() + " sec");

            visual_guidance.ShowLocalisations("steersman_localisations_along_path.jpg", 640, 480);

            average_offset_x_mm /= estimated_path.Count;
            average_offset_y_mm /= estimated_path.Count;
            Console.WriteLine("Average offsets: " + average_offset_x_mm.ToString() + ", " + average_offset_y_mm.ToString());

            float diff_x_mm = Math.Abs(average_offset_x_mm - bias_x_mm);
            float diff_y_mm = Math.Abs(average_offset_y_mm - bias_y_mm);

            Assert.Less(diff_x_mm, cellSize_mm * 3 / 2, "x bias not detected");
            Assert.Less(diff_y_mm, cellSize_mm * 3 / 2, "y bias not detected");

            if (no_of_localisation_failures > 0)
            {
                Console.WriteLine("Localisation failures: " + no_of_localisation_failures.ToString());
            }
            else
            {
                Console.WriteLine("No localisation failures!");
            }
            Assert.Less(no_of_localisation_failures, 4, "Too many localisation failures");
        }