/// <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; }
//[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); }