/// <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="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 Wide_Camera_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_)
{
MAXIMUM_LENGTH_RATIO = MAXIMUM_LENGTH_RATIO_;
MAXIMUM_ANGLE_DIFFERENCE = MAXIMUM_ANGLE_DIFFERENCE_;
IMAGE_SEARCH_BOUNDARY = IMAGE_SEARCH_BOUNDARY_;
SD_IMAGE_SIMULATION = SD_IMAGE_SIMULATION_;
m_camera = cam;
if ((String)(motion_model.motion_model_dimensionality_type) != "THREED")
{
String os;
os = "Motion Model given to the Feature Measurement Model is of type " +
motion_model.motion_model_dimensionality_type +
", not type THREED.";
Debug.WriteLine(os);
//throw Scene::InitialisationError(os.str());
}
threed_motion_model = (ThreeD_Motion_Model)motion_model;
}
/// <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 override void func_hi_and_dhi_by_dxp_and_dhi_by_dyi(Vector yi, Vector xp)
{
// This function gives relative position of feature: also call this hR
// (vector from camera to feature in robot frame)
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp);
// Project this from 3D into the 2D image using our camera
Vector feature_3D_position = get_zeroedyiRES();
WideCamera cam = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera;
hiRES = cam.Project(feature_3D_position);
// And ask the camera what the Jacobian of this projection was
MatrixFixed dhid_by_dzeroedyi = cam.ProjectionJacobian();
// Form the required Jacobians
dhi_by_dxpRES = dhid_by_dzeroedyi * dzeroedyi_by_dxpRES;
dhi_by_dyiRES = dhid_by_dzeroedyi * dzeroedyi_by_dyiRES;
}
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_);
}
/// <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="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 Wide_Camera_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_)
{
MAXIMUM_LENGTH_RATIO = MAXIMUM_LENGTH_RATIO_;
MAXIMUM_ANGLE_DIFFERENCE = MAXIMUM_ANGLE_DIFFERENCE_;
IMAGE_SEARCH_BOUNDARY = IMAGE_SEARCH_BOUNDARY_;
SD_IMAGE_SIMULATION = SD_IMAGE_SIMULATION_;
m_camera = cam;
if ((String)(motion_model.motion_model_dimensionality_type) != "THREED")
{
String os;
os = "Motion Model given to the Feature Measurement Model is of type " +
motion_model.motion_model_dimensionality_type +
", not type THREED.";
Debug.WriteLine(os);
//throw Scene::InitialisationError(os.str());
}
threed_motion_model = (ThreeD_Motion_Model)motion_model;
}