public void updateGraphs()
{
if (graph_colour_variance == null)
{
graph_colour_variance = new graph_points(0, max_position_error_mm,
0, max_colour_variance);
graph_no_of_particles = new graph_points(0, max_position_error_mm,
0, 200);
LoadGraphs();
}
graph_colour_variance.Update(position_error_mm, GetMeanColourVariance());
graph_no_of_particles.Update(position_error_mm, rob.GetBestMotionModel().survey_trial_poses);
}
/// <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();
}
}
/// <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;
}
