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