/// <summary> /// Model for single camera making point measurements /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Fully_Init_Wide_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
/// <summary> /// Constructor /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="fully_init_f_m_m">The type of feature measurement model to convert to when the feature is fully initialised.</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, Feature_Measurement_Model fully_init_f_m_m, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m) { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public Fully_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, float WIDE_MAXIMUM_ANGLE_DIFFERENCE) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = WIDE_MAXIMUM_ANGLE_DIFFERENCE; //Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public Line_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, Feature_Measurement_Model fully_init_f_m_m) : base(2, 6, 6, motion_model, "CAMERA_WIDE_LINE_INIT", "THREED_SEMI_INFINITE_LINE", 1, fully_init_f_m_m) { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
/// <summary> /// Model for single camera making point measurements /// </summary> /// <param name="motion_model">The motion model describing the robot motion. This must be derived from ThreeD_Motion_Model</param> /// <param name="cam">The camera model to use</param> /// <param name="MAXIMUM_LENGTH_RATIO_"></param> /// <param name="MAXIMUM_ANGLE_DIFFERENCE_"></param> /// <param name="IMAGE_SEARCH_BOUNDARY_"></param> /// <param name="SD_IMAGE_SIMULATION_"></param> public Fully_Init_Wide_Point_Feature_Measurement_Model( Motion_Model motion_model, WideCamera cam, float MAXIMUM_LENGTH_RATIO_, float MAXIMUM_ANGLE_DIFFERENCE_, float IMAGE_SEARCH_BOUNDARY_, float SD_IMAGE_SIMULATION_) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public Fully_Init_Wide_Point_Feature_Measurement_Model(Motion_Model motion_model, float WIDE_MAXIMUM_ANGLE_DIFFERENCE) : base(2, 3, 3, motion_model, "CAMERA_WIDE_POINT", "THREED_POINT") { WideCamera cam = new WideCamera(); float MAXIMUM_LENGTH_RATIO_ = Camera_Constants.WIDE_MAXIMUM_LENGTH_RATIO; float MAXIMUM_ANGLE_DIFFERENCE_ = WIDE_MAXIMUM_ANGLE_DIFFERENCE; //Camera_Constants.WIDE_MAXIMUM_ANGLE_DIFFERENCE; float IMAGE_SEARCH_BOUNDARY_ = Camera_Constants.WIDE_IMAGE_SEARCH_BOUNDARY; float SD_IMAGE_SIMULATION_ = Camera_Constants.WIDE_SD_IMAGE_SIMULATION; wide_model = new Wide_Camera_Point_Feature_Measurement_Model(motion_model, cam, MAXIMUM_LENGTH_RATIO_, MAXIMUM_ANGLE_DIFFERENCE_, IMAGE_SEARCH_BOUNDARY_, SD_IMAGE_SIMULATION_); }
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp) { Wide_Camera_Point_Feature_Measurement_Model wm = (Wide_Camera_Point_Feature_Measurement_Model)wide_model; // Extract cartesian and quaternion components of xp wm.threed_motion_model.func_r(xp); wm.threed_motion_model.func_q(xp); // Extract ri and hhati components of yi = ypi func_ri(yi); func_hhati(yi); // ri part: transformation is just the same as in the normal point case // zeroedri = RRW(rWi - rW) //commented out in original code // ri - r Vector3D yWiminusrW = new Vector3D(riRES - wm.threed_motion_model.get_rRES()); Quaternion qRW = wm.threed_motion_model.get_qRES().Inverse(); MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq(); // Rotation RRW RotationMatrix RRW = qRW.RotationMatrix(); // RRW(rWi - rW) Vector3D zeroedri = new Vector3D(RRW * yWiminusrW); // Now calculate Jacobians // dzeroedri_by_dri is RRW // dzeroedri_by_dhhati = 0 MatrixFixed dzeroedri_by_dri = new MatrixFixed(RRW); // dzeroedyi_by_dxp: // dzeroedri_by_dr = -RRW // dzeroedri_by_dq = d_dq(RRW (ri - r)) MatrixFixed dzeroedri_by_dr = RRW * -1.0f; MatrixFixed dzeroedri_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW); MatrixFixed dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq; // Now for the hhati part (easier...) // zeroedhhati = RRW hhati Vector3D zeroedhhati = new Vector3D(RRW * hhatiRES); // Jacobians // dzeroedhhati_by_dr = 0 // dzeroedhhati_by_dq = d_dq(RRW hhati) // dzeroedhhati_by_dhhati = RRW // dzeroedhhati_by_dri = 0 MatrixFixed dzeroedhhati_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, hhatiRES); MatrixFixed dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq; MatrixFixed dzeroedhhati_by_dhhati = new MatrixFixed(RRW); // And put it all together zeroedyiRES.Update(zeroedri.GetVNL3(), 0); zeroedyiRES.Update(zeroedhhati.GetVNL3(), 3); //cout << "Line: zeroedri = " << zeroedri << "zeroedhhati = " << zeroedhhati; dzeroedyi_by_dxpRES.Fill(0.0f); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dr, 0, 0); dzeroedyi_by_dxpRES.Update(dzeroedri_by_dq, 0, 3); dzeroedyi_by_dxpRES.Update(dzeroedhhati_by_dq, 3, 3); dzeroedyi_by_dyiRES.Fill(0.0f); dzeroedyi_by_dyiRES.Update(dzeroedri_by_dri, 0, 0); dzeroedyi_by_dyiRES.Update(dzeroedhhati_by_dhhati, 3, 3); }