/// <summary>
/// Make the first measurement of a feature.
/// </summary>
/// <param name="z">The location of the feature to measure</param>
/// <param name="id">The Identifier to fill with the feature measurements</param>
/// <param name="f_m_m">The feature measurement model to use for this partially-initialised feature</param>
/// <returns></returns>
public virtual bool make_initial_measurement_of_feature(Vector z,
ref classimage_mono id,
Partially_Initialised_Feature_Measurement_Model f_m_m,
Vector patch_colour)
{
return(false);
}
/// <summary>
/// Constructor for known features. The different number of
/// arguments differentiates it from the constructor for partially-initialised
/// features
/// </summary>
/// <param name="id">reference to the feature identifier</param>
/// <param name="?"></param>
public Feature(byte[] id, uint lab, uint list_pos,
Scene_Single scene, Vector y_known,
Vector xp_o,
Feature_Measurement_Model f_m_m, uint k_f_l)
{
feature_measurement_model = f_m_m;
feature_constructor_bookeeping();
identifier = id;
label = lab;
position_in_list = list_pos; // Position of new feature in list
// Save the vehicle position where this feature was acquired
xp_orig = new Vector(xp_o);
// Straighforward initialisation of state and covariances
y = y_known;
Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pxy.Fill(0.0f);
Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pyy.Fill(0.0f);
int i = 0;
MatrixFixed newPyjyi_to_store;
foreach (Feature it in scene.get_feature_list_noconst())
{
if (i < position_in_list)
{
newPyjyi_to_store = new MatrixFixed(
it.get_feature_measurement_model().FEATURE_STATE_SIZE,
feature_measurement_model.FEATURE_STATE_SIZE);
//add to the list
matrix_block_list.Add(newPyjyi_to_store);
}
i++;
}
known_feature_label = (int)k_f_l;
if (feature_measurement_model.fully_initialised_flag)
{
partially_initialised_feature_measurement_model = null;
fully_initialised_feature_measurement_model =
(Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
else
{
fully_initialised_feature_measurement_model = null;
partially_initialised_feature_measurement_model =
(Partially_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
}
/// <summary>
/// Convert a partially-initialised feature to a fully-initialised feature,
/// given information about the free parameters \vct{\lambda}.
/// The new state \vct{y}_{fi} is given by calling
/// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda().
/// where the various Jacobians are returned by calls to
/// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices
/// \mat{P}_{kl} are already known and stored in the class, except for
/// \mat{P}_{\vct{\lambda}}, which is passed to the function.
/// </summary>
/// <param name="lambda">The mean value for \vct{\lambda}</param>
/// <param name="Plambda">The covariance for \vct{\lambda}</param>
/// <param name="scene">The SLAM map</param>
public void convert_from_partially_to_fully_initialised(
Vector lambda, MatrixFixed Plambda, Scene_Single scene)
{
// We'll do all the work here in feature.cc though probably this only
// works with scene_single...
// We calculate new state yfi(ypi, lambda)
// New feature covariance
// Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT +
// dyfi_by_dlambda Plambda dyfi_by_dlambdaT
// And we change cross covariances as follows:
// Pxyfi = Pxypi dyfi_by_dypiT
// Pyjyfi = Pyjypi dyfi_by_dypiT for j < i (since we only store top-right
// Pyfiyj = dyfi_by_dypi Pypiyj for j > i part of covariance matrix)
partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda);
MatrixFixed dyfi_by_dypiT = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose();
MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose();
// Replace y
y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES());
// Replace Pxy
Pxy = Pxy * dyfi_by_dypiT;
// Replace Pyy
MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() *
Pyy * dyfi_by_dypiT;
MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() *
Plambda * dyfi_by_dlambdaT;
Pyy = Pypiypi_1 + Pypiypi_2;
// Pyjyi elements for j < i (covariance with features before i in list)
uint i = position_in_list;
MatrixFixed m_it;
int j;
for (j = 0; j < position_in_list; j++)
{
m_it = (MatrixFixed)matrix_block_list[j];
matrix_block_list[j] = m_it * dyfi_by_dypiT;
}
Feature it;
int idx = scene.feature_list.IndexOf(this);
for (j = idx + 1; j < scene.feature_list.Count; j++)
{
it = (Feature)(scene.feature_list[j]);
it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i];
it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE);
}
// Change the total state size in scene, here with a negative increment
uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
scene.increment_total_state_size((int)size1 - (int)size2);
// Change fmm for this model to fully-initialised one
feature_measurement_model =
partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model;
partially_initialised_feature_measurement_model = null;
fully_initialised_feature_measurement_model =
(Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
//assert(fully_initialised_feature_measurement_model != NULL);
// Need to reallocate any other matrices
// Assume that measurement size doesn't change
dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
}
/// <summary>
/// Constructor for partially-initialised features.
/// </summary>
/// <param name="id">reference to the feature</param>
/// <param name="lab"></param>
/// <param name="list_pos"></param>
/// <param name="scene"></param>
/// <param name="h"></param>
/// <param name="p_i_f_m_m"></param>
public Feature(byte[] id, uint lab, uint list_pos,
Scene_Single scene, Vector h,
Partially_Initialised_Feature_Measurement_Model p_i_f_m_m,
Vector feature_colour)
{
feature_measurement_model = p_i_f_m_m;
partially_initialised_feature_measurement_model = p_i_f_m_m;
fully_initialised_feature_measurement_model = null;
// Stuff below substituted from Feature_common
// Feature_common(id, lab, list_pos, scene, h);
feature_constructor_bookeeping();
identifier = id;
label = lab;
position_in_list = list_pos; // Position of new feature in list
position_in_total_state_vector = 0; // This should be set properly
colour = feature_colour;
//when feature is added
// Save the vehicle position where this feature was acquired
scene.get_motion_model().func_xp(scene.get_xv());
//xp_orig = scene.get_motion_model().get_xpRES();
xp_orig = new Vector(scene.get_motion_model().get_xpRES());
// Call model functions to calculate feature state, measurement noise
// and associated Jacobians. Results are stored in RES matrices
// First calculate "position state" and Jacobian
scene.get_motion_model().func_xp(scene.get_xv());
scene.get_motion_model().func_dxp_by_dxv(scene.get_xv());
// Now ask the model to initialise the state vector and calculate Jacobians
// so that I can go and calculate the covariance matrices
partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES());
// State y
//y = partially_initialised_feature_measurement_model.get_ypiRES();
y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES());
// Temp_FS1 will store dypi_by_dxv
MatrixFixed Temp_FS1 =
partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() *
scene.get_motion_model().get_dxp_by_dxvRES();
// Pxy
Pxy = scene.get_Pxx() * Temp_FS1.Transpose();
// Pyy
Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose()
+ partially_initialised_feature_measurement_model.get_dypi_by_dhiRES()
* partially_initialised_feature_measurement_model.get_RiRES()
* partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose();
// Covariances of this feature with others
int j = 0;
foreach (Feature it in scene.get_feature_list_noconst())
{
if (j < position_in_list)
{
// new Pypiyj = dypi_by_dxv . Pxyj
// Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old)
MatrixFixed m = it.get_Pxy();
MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose();
//add to the list
matrix_block_list.Add(newPyjypi_to_store);
}
j++;
}
known_feature_label = -1;
}
/// <summary>
/// Make the initial measurement of the currently-selected feature. This fills in
/// the location and the identifier (a copy of the image patch) for the current
/// feature. The feature location is set using set_image_selection_automatically()
/// or manually by the user using set_image_selection(). This function just calls
/// partially_initialise_point_feature() to fill in the measurement and identifier.
/// </summary>
/// <param name="z">The measurement vector to be filled in.</param>
/// <param name="id_ptr">The identifier to be filled in.</param>
/// <param name="m"></param>
/// <returns>true on success, or <code>false</code> on failure (e.g. no patch is currently selected).</returns>
public override bool make_initial_measurement_of_feature(Vector z, ref classimage_mono patch, Partially_Initialised_Feature_Measurement_Model m, Vector patch_colour)
{
patch = partially_initialise_point_feature(z);
for (int c = 0; c < 3; c++)
{
patch_colour[c] = image_colour.image[uu, vv, c];
}
if (patch != null)
{
return(true);
}
else
{
return(false);
}
}
// Function to find a region in an image guided by current motion prediction
// which doesn't overlap existing features
public static bool FindNonOverlappingRegion(Scene_Single scene,
Vector local_u,
float delta_t,
Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation,
uint camera_width,
uint camera_height,
uint BOXSIZE,
ref int init_feature_search_ustart,
ref int init_feature_search_vstart,
ref int init_feature_search_ufinish,
ref int init_feature_search_vfinish,
uint FEATURE_INIT_STEPS_TO_PREDICT,
float FEATURE_INIT_DEPTH_HYPOTHESIS,
Random rnd)
{
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector local_xv = new Vector(scene.get_xv());
for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++)
{
scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t);
local_xv.Update(scene.get_motion_model().get_fvRES());
}
threed_motion_model.func_xp(local_xv);
Vector local_xp = new Vector(threed_motion_model.get_xpRES());
threed_motion_model.func_r(local_xp);
Vector3D rW = threed_motion_model.get_rRES();
threed_motion_model.func_q(local_xp);
Quaternion qWR = threed_motion_model.get_qRES();
// yW = rW + RWR hR
Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS);
// Used Inverse + transpose because this was't compiling the normal way
Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR);
// Then project that point into the current camera position
scene.get_motion_model().func_xp(scene.get_xv());
Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
(Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model);
Vector yWVNL = yW.GetVNL3();
fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES());
// Now, this defines roughly how much we expect a feature initialised to move
float suggested_u = fully_init_fmm.get_hiRES()[0];
float suggested_v = fully_init_fmm.get_hiRES()[1];
float predicted_motion_u = camera_width / 2.0f - suggested_u;
float predicted_motion_v = camera_height / 2.0f - suggested_v;
// So, the limits of a "safe" region within which we can initialise
// features so that they end up staying within the screen
// (Making the approximation that the whole screen moves like the
// centre point)
int safe_feature_search_ustart = (int)(-predicted_motion_u);
int safe_feature_search_vstart = (int)(-predicted_motion_v);
int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u);
int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v);
if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1);
}
if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1);
}
if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1);
}
if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1);
}
return(FindNonOverlappingRegionNoPredict(safe_feature_search_ustart,
safe_feature_search_vstart,
safe_feature_search_ufinish,
safe_feature_search_vfinish,
scene,
ref init_feature_search_ustart,
ref init_feature_search_vstart,
ref init_feature_search_ufinish,
ref init_feature_search_vfinish, rnd));
}
public void SetUp3DDisplays()
{
// Set display virtual camera parameters to match those of the real camera
// being used in MonoSLAM
Partially_Initialised_Feature_Measurement_Model default_pifmm =
monoslaminterface.GetDefaultFeatureTypeForInitialisation();
if (default_pifmm != null)
{
Line_Init_Wide_Point_Feature_Measurement_Model default_wide_pifmm =
(Line_Init_Wide_Point_Feature_Measurement_Model)default_pifmm;
Graphics_Fku = default_wide_pifmm.get_camera().Fku();
Graphics_Fkv = default_wide_pifmm.get_camera().Fkv();
Graphics_U0 = default_wide_pifmm.get_camera().U0();
Graphics_V0 = default_wide_pifmm.get_camera().V0();
Graphics_Kd1 = default_wide_pifmm.get_camera().Kd1();
// First display for external 3D view
/*
* threedtool = new ThreeDToolGlowWidget(this,
* controlpanel1->Width() + controlpanel2->Width(), 0,
* monoslaminterface->GetCameraWidth(),
* monoslaminterface->GetCameraHeight());
* threedtool->DrawEvent.Attach(this, &MonoSLAMGlow::Draw3D);
* threedtool->ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ProcessHits);
* threedtool->SetCameraParameters(Graphics_Fku, Graphics_Fkv,
* Graphics_U0, Graphics_V0);
*/
// Set start position for GL camera in 3D tool
// This is x, y, z position
Vector rthreed = new Vector(0.0f, 0.2f, -1.5f);
// This is camera orientation in my normal coordinate system
// (z forward, y up, x left)
Quaternion qWRthreed = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qROthreed = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWOthreed = qWRthreed.Multiply(qROthreed);
//threedtool.SetCameraPositionOrientation(rthreed, qWOthreed);
// Second 3D display for images and augmented reality
/*
* image_threedtool = new ThreeDToolGlowWidget(this,
* controlpanel1->Width() + controlpanel2->Width(), threedtool->Height(),
* monoslaminterface->GetCameraWidth(), monoslaminterface->GetCameraHeight());
* image_threedtool.DrawEvent.Attach(this, &MonoSLAMGlow::ImageDraw3D);
* image_threedtool.ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ImageProcessHits);
* image_threedtool.SetCameraParameters(Graphics_Fku, Graphics_Fkv, Graphics_U0, Graphics_V0);
*/
// Set up initial state of virtual camera for image display to match
// state vector
Scene_Single scene = monoslaminterface.GetScene();
if (scene != null)
{
Vector v = scene.get_xv();
if (v != null)
{
scene.get_motion_model().func_xp(v);
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector3D r = threed_motion_model.get_rRES();
Quaternion qWR = threed_motion_model.get_qRES();
// q is qWR between world frame and Scene robot frame
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWO = qWR.Multiply(qRO);
//image_threedtool.SetCameraPositionOrientation(r, qWO);
}
else
{
Debug.WriteLine("Scene xp not defined");
}
}
else
{
Debug.WriteLine("No scene object defined");
}
}
else
{
Debug.WriteLine("No partially initialised feature measurement model found");
}
}
