private void test_motion_model(bool closed_loop)
{
robot rob = new robot(1);
int min_x_mm = 0;
int min_y_mm = 0;
int max_x_mm = 1000;
int max_y_mm = 1000;
int step_size = (max_y_mm - min_y_mm) / 15;
int x = (max_x_mm - min_x_mm) / 2;
bool initial = true;
float pan = 0; // (float)Math.PI / 4;
for (int y = min_y_mm; y < max_y_mm; y += step_size)
{
if (closed_loop) surveyPosesDummy(rob);
rob.updateFromKnownPosition(null, x, y, pan);
rob.GetBestMotionModel().Show(img_rays, standard_width, standard_height,
min_x_mm, min_y_mm, max_x_mm, max_y_mm,
initial);
initial = false;
}
}
/*
private void test_head()
{
pos3D robotPosition = new pos3D(0, 0, 0);
pos3D robotOrientation = new pos3D(0, 0, 0);
beginStopWatch();
for (int i = 0; i < view.Length; i++)
{
for (int cam = 0; cam < 4; cam++)
{
//invent some stereo features
for (int f = 0; f < stereo_features.Length; f += 3)
{
stereo_features[f] = rnd.Next(robot_head.image_width - 1);
stereo_features[f + 1] = rnd.Next(robot_head.image_height - 1);
//stereo_features[f] = (robot_head.image_width*1/10) + (rnd.Next(robot_head.image_width/300) - 1);
//stereo_features[f + 1] = rnd.Next(robot_head.image_height - 1);
stereo_features[f + 2] = rnd.Next(robot_head.image_width * 35 / 100);
}
robot_head.setStereoFeatures(cam, stereo_features, stereo_uncertainties, stereo_features.Length);
}
view[i] = stereo_model.createViewpoint(robot_head, robotOrientation);
view[0].showAbove(img_rays, standard_width, standard_height, 2000, 255, 255, 255, true, 0, false);
robotPosition.y += 0;
robotPosition.pan += 0.0f;
}
long mappingTime = endStopWatch();
txtMappingTime.Text = Convert.ToString(mappingTime);
}
*/
/*
private void test_head2()
{
robot_head = new stereoHead(2);
robot_head.initDualCam();
pos3D robotPosition = new pos3D(0, 0, 0);
pos3D robotOrientation = new pos3D(0, 0, 0);
beginStopWatch();
for (int i = 0; i < view.Length; i++)
{
for (int cam = 0; cam < 2; cam++)
{
//invent some stereo features
for (int f = 0; f < stereo_features.Length; f += 3)
{
stereo_features[f] = rnd.Next(robot_head.image_width - 1);
stereo_features[f + 1] = robot_head.image_height / 2; // rnd.Next(robot_head.image_height - 1);
//stereo_features[f] = (robot_head.image_width*1/10) + (rnd.Next(robot_head.image_width/300) - 1);
//stereo_features[f + 1] = rnd.Next(robot_head.image_height - 1);
stereo_features[f + 2] = rnd.Next(robot_head.image_width * 35 / 100);
}
robot_head.setStereoFeatures(cam, stereo_features, stereo_uncertainties, stereo_features.Length);
}
view[i] = stereo_model.createViewpoint(robot_head, robotOrientation);
view[0].showFront(img_rays, standard_width, standard_height, 2000, 255, 255, 255, true);
robotPosition.y += 0;
robotPosition.pan += 0.0f;
}
long mappingTime = endStopWatch();
txtMappingTime.Text = Convert.ToString(mappingTime);
}
*/
/*
private void test_grid()
{
occupancygridClassic grd = new occupancygridClassic(128, 32);
pos3D robotPosition = new pos3D(0, 0, 0);
pos3D robotOrientation = new pos3D(0, 0, 0);
beginStopWatch();
//int i = 0;
//for (i = 0; i < 9; i++)
{
for (int cam = 0; cam < 4; cam++)
{
//invent some stereo features
for (int f = 0; f < stereo_features.Length; f += 3)
{
stereo_features[f] = rnd.Next(robot_head.image_width - 1);
stereo_features[f + 1] = rnd.Next(robot_head.image_height - 1);
stereo_features[f + 2] = rnd.Next(robot_head.image_width * 35 / 100);
}
robot_head.setStereoFeatures(cam, stereo_features, stereo_uncertainties, stereo_features.Length);
}
view[0] = stereo_model.createViewpoint(robot_head, robotOrientation);
pos3D centre = new pos3D(0, 0, 0);
grd.insert(view[0],true,centre);
//view[0].show(img_rays, standard_width, standard_height, 2000, 255, 255, 255, true);
robotPosition.y += 50;
robotPosition.pan += 0.0f;
}
long mappingTime = endStopWatch();
txtMappingTime.Text = Convert.ToString(mappingTime);
grd.show(img_rays, standard_width, standard_height);
}
*/
/// <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.GetBestMotionModel();
// 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;
}
