/// <summary>
/// Project a point from an ideal (Euclidean) camera frame into image co-ordinates
/// with radial distortion.
///
/// This first transforms the input position vector vct{y} (relative to
/// the camera position) into an undistorted image location \vct{u}_c
/// (relative to an origin at the optical centre)
///
/// vct{u}_c = evct{u_c \ v_c} = evct{-F_{ku} y_x / y_z \ -F_{kv} y_y / y_z} =
/// emat{-F_{ku} & 0 \ 0 & -F_{kv}}\evct{y_x/y_z \ y_y/y_z}
///
/// Then radial distortion is applied to give the final image location h
/// (with the origin back in the normal location of of the top left of the image).
///
/// vct{h} = frac{vct{u}_c}{sqrt{1 + 2 k_1 |vct{u}_c|^2}} + evct{u_0 \ v_0}
/// </summary>
/// <param name="camera"></param>
/// <returns></returns>
public virtual Vector Project(Vector camera)
{
// Remember this position in case we are asked about the Jacobians of this
// transformation
m_last_camera = camera;
// First do perspective projection
// This turns the position vector into an undistorted image location (with the
// origin at the centre)
// Use -Fkuv to swap the image co-ordinates (0,0) at the top to
// camera co-ordinates (0,0) at the bottom
Vector imagepos_centred = new Vector(2);
imagepos_centred[0] = -m_Fku * camera[0] / camera[2];
imagepos_centred[1] = -m_Fkv * camera[1] / camera[2];
m_last_image_centred = imagepos_centred;
// 1 distortion coefficient model
float radius2 = imagepos_centred.SquaredMagnitude();
float factor = (float)Math.Sqrt(1 + 2 * m_Kd1 * radius2);
return imagepos_centred / factor + m_centre;
}
/// <summary>
/// Step the MonoSLAM application on by one frame. This should be called every time
/// a new frame is captured (and care should be taken to avoid skipping frames).
/// Before calling this function, Scene_Single::load_new_image() should be called
/// (e.g. using
/// <code>monoslaminterface.GetRobotNoConst()->load_new_image()</code>), since the
/// first parameter to GoOneStep() is currently ignored.
///
/// GoOneStep() performs the following processing steps:
/// - Kalman filter prediction step (by calling Kalman::predict_filter_fast(),
/// with a zero control vector)
/// - Select a set of features to make measurements from (set by
/// SetNumberOfFeaturesToSelect())
/// - Predict the locations and and make measurements of those features
/// - Kalman filter update step
/// - Delete any bad features (those that have repeatedly failed to be matched)
/// - If we are not currently initialising a enough new features, and the camera is
/// translating, and <code>currently_mapping_flag</code> is set, initialise a new
/// feature somewhere sensible
/// - Update the partially-initialised features
/// </summary>
/// <param name="img">camera image</param>
/// <param name="delta_t">The time between frames in seconds</param>
/// <param name="currently_mapping_flag">Set to be true if new features should be detected and added to the map.</param>
/// <returns></returns>
public bool GoOneStep(float delta_t, bool currently_mapping_flag)
{
if (delta_t > 0)
{
// update the integral image for use in feature detection
//img.updateIntegralImage();
// nullify image selection
robot.nullify_image_selection();
init_feature_search_region_defined_flag = false;
// Control vector of accelerations
Vector u = new Vector(3);
u.Fill(0.0f);
sim_or_rob.set_control(u, delta_t);
// Record the current position so that I can estimate velocity
// (We can guarantee that the state vector has position; we can't
// guarantee that it has velocity.)
Vector xv = scene.get_xv();
scene.get_motion_model().func_xp(xv);
Vector prev_xp_pos = (scene.get_motion_model().get_xpRES()).Extract(3);
// Prediction step
if (currently_mapping_flag)
kalman.predict_filter_fast(scene, u, delta_t);
// if features are not seen the first time try a few more times
int tries = 0;
number_of_visible_features = 0;
while (((tries == 0) || (scene.get_no_selected() < 2)) && (tries < 5))
{
number_of_visible_features = scene.auto_select_n_features(NUMBER_OF_FEATURES_TO_SELECT);
if (scene.get_no_selected() > 0)
{
//scene.predict_measurements(); // commented out in original code
number_of_matched_features = (uint)SceneLib.make_measurements(scene, sim_or_rob, rnd);
if (scene.get_successful_measurement_vector_size() != 0)
{
// this function is the slowest part of the algorithm
kalman.total_update_filter_slow(scene);
scene.normalise_state();
}
}
tries++;
}
if (currently_mapping_flag)
scene.delete_bad_features();
// Let's enforce symmetry of covariance matrix...
// Add to transpose and divide by 2
uint tot_state_size = scene.get_total_state_size();
MatrixFixed Pxx = new MatrixFixed(tot_state_size, tot_state_size);
scene.construct_total_covariance(ref Pxx);
MatrixFixed PxxT = Pxx.Transpose();
Pxx.Update(Pxx * 0.5f + PxxT * 0.5f);
scene.fill_covariances(Pxx);
// Look at camera speed estimate
// Get the current position and estimate the speed from it
xv = scene.get_xv();
scene.get_motion_model().func_xp(xv);
Vector xp_pos = scene.get_motion_model().get_xpRES().Extract(3);
velocity = (xp_pos - prev_xp_pos) / delta_t;
speed = (float)Math.Sqrt(velocity.SquaredMagnitude());
// This section of code is a workaround for a situation where
// the camera suddenly shoots off at high speed, which must be
// a bug perhaps in the state update. If the camera suddenly accelerates
// at a high rate then this just sets the state back to the previous one.
if (prev_speed == 0)
prev_speed = speed;
else
{
float speed_change = speed / prev_speed;
if ((speed > 1) && (speed_change > 1.2) && (prev_xv != null))
{
xv.Update(prev_xv);
}
}
prev_speed = speed;
if (prev_xv != null) // TODO: minor bug here with vector != operator
prev_xv.Update(xv);
else
prev_xv = new Vector(xv);
if (currently_mapping_flag)
{
if (speed > 0.2)
{
if ((number_of_visible_features < NUMBER_OF_FEATURES_TO_KEEP_VISIBLE) &&
(scene.get_feature_init_info_vector().Count < (uint)(MAX_FEATURES_TO_INIT_AT_ONCE)))
{
// if the number of features is low make more attempts
// to initialise new ones
tries = 1;
if (number_of_visible_features < 8) tries = 2;
if (number_of_visible_features < 5) tries = 3;
for (int i = 0; i < tries; i++) AutoInitialiseFeature(u, delta_t);
}
}
}
MatchPartiallyInitialisedFeatures();
recordCameraHistory();
}
return true;
}
