/// <summary>
/// test the motion model in open or closed loop mode
/// </summary>
/// <param name="closed_loop">use closed loop mode</param>
private void test_motion_model(bool closed_loop)
{
robot rob = new robot(1);
motionModel motion_model = rob.GetBestMotionModel();
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);
motion_model.Show(img_rays, standard_width, standard_height,
min_x_mm, min_y_mm, max_x_mm, max_y_mm,
initial);
initial = false;
}
}
/// <summary>
/// load a robot design file
/// </summary>
/// <param name="filename"></param>
public void LoadRobot(String filename)
{
if (rob.Load(filename))
{
motionModel motion_model = rob.GetBestMotionModel();
txtName.Text = rob.Name;
txtTotalMass.Text = Convert.ToString(rob.TotalMass_kg);
txtNoOfThreads.Text = Convert.ToString(rob.GetMappingThreads());
txtBodyWidth.Text = Convert.ToString(rob.BodyWidth_mm);
txtBodyLength.Text = Convert.ToString(rob.BodyLength_mm);
txtBodyHeight.Text = Convert.ToString(rob.BodyHeight_mm);
cmbBodyShape.SelectedIndex = Convert.ToInt32(rob.BodyShape);
cmbPropulsion.SelectedIndex = rob.propulsionType;
txtWheelBase.Text = Convert.ToString(rob.WheelBase_mm);
txtWheelBaseForward.Text = Convert.ToString(rob.WheelBaseForward_mm);
txtWheelDiameter.Text = Convert.ToString(rob.WheelDiameter_mm);
cmbWheelFeedback.SelectedIndex = rob.WheelPositionFeedbackType;
txtGearRatio.Text = Convert.ToString(rob.GearRatio);
txtCountsPerRev.Text = Convert.ToString(rob.CountsPerRev);
txtMotorNoLoadSpeedRPM.Text = Convert.ToString(rob.MotorNoLoadSpeedRPM);
txtMotorTorque.Text = Convert.ToString(rob.MotorTorqueKgMm);
txtCameraBaseline.Text = Convert.ToString(rob.head.calibration[0].baseline);
txtCameraFOV.Text = Convert.ToString(rob.head.calibration[0].leftcam.camera_FOV_degrees);
txtRollAngle.Text = Convert.ToString(rob.head.calibration[0].positionOrientation.roll * 180.0f / (float)Math.PI);
cmbHeadType.SelectedIndex = rob.HeadType;
txtHeadSize.Text = Convert.ToString(rob.HeadSize_mm);
cmbHeadShape.SelectedIndex = rob.HeadShape;
txtHeadHeightFromGround.Text = Convert.ToString(rob.head.z);
txtHeadPositionLeft.Text = Convert.ToString(rob.head.x);
txtHeadPositionForward.Text = Convert.ToString(rob.head.y);
txtNoOfCameras.Text = Convert.ToString(rob.head.no_of_stereo_cameras);
cmbCameraOrientation.SelectedIndex = rob.CameraOrientation;
txtGridLevels.Text = Convert.ToString(rob.LocalGridLevels);
txtGridWidth.Text = Convert.ToString(rob.LocalGridDimension * rob.LocalGridCellSize_mm);
txtGridHeight.Text = Convert.ToString(rob.LocalGridDimensionVertical * rob.LocalGridCellSize_mm);
txtGridCellDimension.Text = Convert.ToString(rob.LocalGridCellSize_mm);
txtGridInterval.Text = Convert.ToString(rob.LocalGridInterval_mm);
txtMappingRange.Text = Convert.ToString(rob.LocalGridMappingRange_mm);
txtLocalGridLocalisationRadius.Text = Convert.ToString(rob.LocalGridLocalisationRadius_mm);
txtTrialPoses.Text = Convert.ToString(motion_model.survey_trial_poses);
txtCullingThreshold.Text = Convert.ToString(motion_model.cull_threshold);
chkEnableScanMatching.Checked = rob.EnableScanMatching;
updateSensorModelStatus();
}
}
public void SampleNormalDistribution()
{
int image_width = 640;
robot rob = new robot(1);
motionModel mm = new motionModel(rob, rob.LocalGrid, 1);
byte[] img_rays = new byte[image_width * image_width * 3];
Bitmap bmp = new Bitmap(image_width, image_width, System.Drawing.Imaging.PixelFormat.Format24bppRgb);
int[] hist = new int[20];
int max = 1;
int max_index = 0;
for (int sample = 0; sample < 2000; sample++)
{
float v = mm.sample_normal_distribution(20);
int index = (hist.Length / 2) + ((int)v / (hist.Length / 2));
if ((index > -1) && (index < hist.Length))
{
hist[index]++;
if (hist[index] > max)
{
max = hist[index];
max_index = index;
}
}
}
max += 5;
for (int x = 0; x < image_width; x++)
{
int index = x * hist.Length / image_width;
drawing.drawLine(img_rays, image_width, image_width, x, image_width - 1, x, image_width - 1 - (hist[index] * image_width / max), 255, 255, 255, 0, false);
}
BitmapArrayConversions.updatebitmap_unsafe(img_rays, bmp);
bmp.Save("motionmodel_tests_SampleNormalDistribution.bmp", System.Drawing.Imaging.ImageFormat.Bmp);
Assert.IsTrue(max_index == hist.Length / 2, "Peak of normal distribution is offset");
}
/// <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;
}