Пример #1
0
        /// <summary>
        /// generate scores for poses.  This is only for testing purposes.
        /// </summary>
        /// <param name="rob"></param>
        private void surveyPosesDummy(robot rob)
        {
            motionModel motion_model = rob.motion;

            // examine the pose list
            for (int sample = 0; sample < motion_model.survey_trial_poses; sample++)
            {
                particlePath path = (particlePath)motion_model.Poses[sample];
                particlePose pose = path.current_pose;

                float dx    = rob.x - pose.x;
                float dy    = rob.y - pose.y;
                float dist  = (float)Math.Sqrt((dx * dx) + (dy * dy));
                float score = 1.0f / (1 + dist);

                // update the score for this pose
                motion_model.updatePoseScore(path, score);
            }

            // indicate that the pose scores have been updated
            motion_model.PosesEvaluated = true;
        }
Пример #2
0
        //[Test()]
        public static void CreateSim()
        {
            int   dimension_cells          = 100;
            int   dimension_cells_vertical = 20;
            int   cellSize_mm           = 50;
            int   localisationRadius_mm = 2000;
            int   maxMappingRange_mm    = 2000;
            float vacancyWeighting      = 0.8f;

            int map_dim = 14;

            byte[] map =
            {
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 1, 1, 1, 1, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0,
                0, 0, 0, 0, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
            };
            dpslam sim = CreateSimulation(map_dim, map, 50);

            particlePose pose      = null;
            int          img_width = 640;

            byte[] img = new byte[img_width * img_width * 3];
            sim.Show(0, img, img_width, img_width, pose, true, true);
            Bitmap bmp = new Bitmap(img_width, img_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

            BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
            bmp.Save("dpslam_tests_CreateSimulation1.bmp", System.Drawing.Imaging.ImageFormat.Bmp);

            robot rob = new robot(1);

            rob.WheelBase_mm     = 90;
            rob.WheelDiameter_mm = 30;
            rob.x   = 0;
            rob.y   = 0;
            rob.pan = 90 * (float)Math.PI / 180.0f;
            rob.head.cameraFOVdegrees[0]    = 90;
            rob.head.cameraSensorSizeMm[0]  = 4.17f;
            rob.head.cameraFocalLengthMm[0] = 3.6f;
            rob.head.cameraImageWidth[0]    = 320;
            rob.head.cameraImageHeight[0]   = 240;

            rayModelLookup sensor_model = new rayModelLookup(10, 10);

            sensor_model.InitSurveyorSVS();

            rob.head.sensormodel[0] = sensor_model;

            float time_elapsed_sec      = 1;
            float forward_velocity      = 50;
            float angular_velocity_pan  = 0;
            float angular_velocity_tilt = 0;
            float angular_velocity_roll = 0;
            float min_x_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
            float min_y_mm = -((dimension_cells / 2) * cellSize_mm) / 3;
            float max_x_mm = -min_x_mm;
            float max_y_mm = -min_y_mm;

            for (float t = 0.0f; t < 4; t += time_elapsed_sec)
            {
                rob.updateFromVelocities(sim, forward_velocity, angular_velocity_pan, angular_velocity_tilt, angular_velocity_roll, time_elapsed_sec);
                Console.WriteLine("xy: " + rob.x.ToString() + " " + rob.y.ToString());

                rob.motion.Show(img, img_width, img_width, min_x_mm, min_y_mm, max_x_mm, max_y_mm, true, false, t == 0.0f);
            }
            rob.SaveGridImage("dpslam_tests_CreateSimulation2.bmp");
            BitmapArrayConversions.updatebitmap_unsafe(img, bmp);
            bmp.Save("dpslam_tests_CreateSimulation3.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
        }