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