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