/// <summary>
/// Simple overall prediction
/// </summary>
/// <param name="scene"></param>
/// <param name="u"></param>
/// <param name="delta_t"></param>
public void predict_filter_slow(Scene_Single scene, Vector u, float delta_t)
{
Debug.WriteLine("*** SLOW PREDICTION ***");
// What we need to do for the prediction:
// Calculate f and grad_f_x
// Calculate Q
// Form x(k+1|k) and P(k+1|k)
int size = (int)scene.get_total_state_size();
// First form original total state and covariance
Vector x = new Vector(size);
MatrixFixed P = new MatrixFixed(size, size);
scene.construct_total_state_and_covariance(ref x, ref P);
// Make model calculations: store results in RES matrices
Vector xv = scene.get_xv();
//Vector xv = new Vector(scene.get_xv());
scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t);
scene.get_motion_model().func_Q(scene.get_xv(), u, delta_t);
// Find new state f
Vector f = new Vector(size);
// Feature elements of f are the same as x
f.Update(x);
f.Update(scene.get_motion_model().get_fvRES(), 0);
// Find new P
// Since most elements of df_by_dx are zero...
MatrixFixed df_by_dx = new MatrixFixed(size, size);
df_by_dx.Fill(0.0f);
// Fill the rest of the elements of df_by_dx: 1 on diagonal for features
for (int i = (int)scene.get_motion_model().STATE_SIZE; i < df_by_dx.Rows; i++)
{
df_by_dx[i, i] = 1.0f;
}
df_by_dx.Update(scene.get_motion_model().get_dfv_by_dxvRES(), 0, 0);
// Calculate the process noise
MatrixFixed Q = new MatrixFixed(size, size);
Q.Fill(0.0f);
Q.Update(scene.get_motion_model().get_QxRES(), 0, 0);
P.Update(df_by_dx * P * df_by_dx.Transpose());
P += Q;
scene.fill_state_and_covariance(f, P);
}
/// <summary>
/// Simple overall prediction
/// </summary>
/// <param name="scene"></param>
/// <param name="u"></param>
/// <param name="delta_t"></param>
public void predict_filter_slow (Scene_Single scene, Vector u, float delta_t)
{
Debug.WriteLine("*** SLOW PREDICTION ***");
// What we need to do for the prediction:
// Calculate f and grad_f_x
// Calculate Q
// Form x(k+1|k) and P(k+1|k)
int size = (int)scene.get_total_state_size();
// First form original total state and covariance
Vector x = new Vector(size);
MatrixFixed P = new MatrixFixed(size, size);
scene.construct_total_state_and_covariance(ref x, ref P);
// Make model calculations: store results in RES matrices
Vector xv = scene.get_xv();
//Vector xv = new Vector(scene.get_xv());
scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t);
scene.get_motion_model().func_Q(scene.get_xv(), u, delta_t);
// Find new state f
Vector f = new Vector(size);
// Feature elements of f are the same as x
f.Update(x);
f.Update(scene.get_motion_model().get_fvRES(), 0);
// Find new P
// Since most elements of df_by_dx are zero...
MatrixFixed df_by_dx = new MatrixFixed(size, size);
df_by_dx.Fill(0.0f);
// Fill the rest of the elements of df_by_dx: 1 on diagonal for features
for (int i = (int)scene.get_motion_model().STATE_SIZE; i < df_by_dx.Rows; i++)
df_by_dx[i,i] = 1.0f;
df_by_dx.Update(scene.get_motion_model().get_dfv_by_dxvRES(), 0, 0);
// Calculate the process noise
MatrixFixed Q = new MatrixFixed(size, size);
Q.Fill(0.0f);
Q.Update(scene.get_motion_model().get_QxRES(), 0, 0);
P.Update(df_by_dx * P * df_by_dx.Transpose());
P += Q;
scene.fill_state_and_covariance(f, P);
}
/// <summary>
/// Fast version of predict filter
/// </summary>
/// <param name="scene"></param>
/// <param name="u">Control vector of accelerations</param>
/// <param name="delta_t">time step in seconds</param>
public void predict_filter_fast(Scene_Single scene, Vector u, float delta_t)
{
Vector xv = scene.get_xv();
// Make model calculations: results are stored in RES matrices
scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t);
scene.get_motion_model().func_Q(xv, u, delta_t);
Vector fvRES = scene.get_motion_model().get_fvRES();
xv.Update(fvRES);
Motion_Model mm = scene.get_motion_model();
MatrixFixed Pxx = scene.get_Pxx();
MatrixFixed dfv_by_dxvRES = mm.get_dfv_by_dxvRES();
MatrixFixed dfv_by_dxvRES_transposed = dfv_by_dxvRES.Transpose();
MatrixFixed QxRES = mm.get_QxRES();
Pxx.Update((dfv_by_dxvRES * Pxx * dfv_by_dxvRES_transposed) + QxRES);
// Change the covariances between vehicle state and feature states
foreach (Feature it in scene.feature_list)
{
MatrixFixed Pxy = it.get_Pxy();
Pxy.Update(dfv_by_dxvRES * Pxy);
}
}
/// <summary>
/// Solve the matrix-vector system M x = y, returning x.
/// </summary>
/// <param name="y"></param>
/// <returns></returns>
public Vector Solve(Vector y)
{
// fsm sanity check :
if (y.size() != U_.Rows)
{
Debug.WriteLine("__FILE__ : size of rhs is incompatible with no. of rows in U_ " +
"y =" + Convert.ToString(y) + "\n" +
"m_=" + Convert.ToString(m_) + "\n" +
"n_=" + Convert.ToString(n_) + "\n" +
"U_=\n" + Convert.ToString(U_) + "V_=\n" + Convert.ToString(V_) +
"W_=\n" + W_);
}
Vector x = new Vector(V_.Rows); // Solution matrix.
if (U_.Rows < U_.Columns)
{ // Augment y with extra rows of
Vector yy = new Vector(U_.Rows); // zeros, so that it matches
yy.Fill(0);
if (yy.size() < y.size())
{ // fsm
Debug.WriteLine("yy=" + Convert.ToString(yy));
Debug.WriteLine("y =" + Convert.ToString(y));
// the update() call on the next line will abort...
}
yy.Update(y); // cols of u.transpose.
x = U_.ConjugateTranspose() * yy;
}
else
{
x = U_.ConjugateTranspose() * y;
}
for (int i = 0; i < x.size(); i++)
{ // multiply with diagonal 1/W
float weight = W_[i, i], zero_ = 0.0f;
if (weight != zero_)
{
x[i] /= weight;
}
else
{
x[i] = zero_;
}
}
return(V_ * x); // premultiply with v.
}
public void construct_total_internal_measurement_stuff(
Vector nu_tot, MatrixFixed dh_by_dx_tot,
MatrixFixed R_tot, uint total_state_size)
{
//uint size = internal_measurement_model.MEASUREMENT_SIZE;
//assert (nu_tot.Size() == size &&
//dh_by_dx_tot.Rows() == size &&
//dh_by_dx_tot.Cols() == total_state_size &&
//R_tot.Rows() == size && R_tot.Cols() == size);
//assert(successful_internal_measurement_flag);
nu_tot.Fill(0.0f);
dh_by_dx_tot.Fill(0.0f);
R_tot.Fill(0.0f);
nu_tot.Update(nuv, 0);
dh_by_dx_tot.Update(dhv_by_dxv, 0, 0);
R_tot.Update(Rv, 0, 0);
}
// FIXME. this should implement the above, not the other way round.
/*
* public void Solve(float[] y, float[] x)
* {
* Solve(new Vector(y, m_)).CopyOut(x);
* }
*/
/// <summary>
/// Solve the matrix-vector system M x = y.
/// Assume that the singular values W have been preinverted by the caller.
/// </summary>
/// <param name="y"></param>
/// <param name="x_out"></param>
public void SolvePreinverted(Vector y, Vector x_out)
{
Vector x; // solution matrix
if (U_.Rows < U_.Columns)
{ // augment y with extra rows of
//std::cout << "svd<T>::solve_preinverted() -- Augmenting y\n";
Vector yy = new Vector(U_.Rows); // zeros, so that it match
yy.Fill(0);
yy.Update(y); // cols of u.transpose. ??
x = U_.ConjugateTranspose() * yy;
}
else
{
x = U_.ConjugateTranspose() * y;
}
for (int i = 0; i < x.size(); i++) // multiply with diagonal W, assumed inverted
{
x[i] *= W_[i, i];
}
x_out = V_ * x; // premultiply with v.
}
public void construct_total_internal_measurement_stuff(
Vector nu_tot, MatrixFixed dh_by_dx_tot,
MatrixFixed R_tot, uint total_state_size)
{
//uint size = internal_measurement_model.MEASUREMENT_SIZE;
//assert (nu_tot.Size() == size &&
//dh_by_dx_tot.Rows() == size &&
//dh_by_dx_tot.Cols() == total_state_size &&
//R_tot.Rows() == size && R_tot.Cols() == size);
//assert(successful_internal_measurement_flag);
nu_tot.Fill(0.0f);
dh_by_dx_tot.Fill(0.0f);
R_tot.Fill(0.0f);
nu_tot.Update(nuv, 0);
dh_by_dx_tot.Update(dhv_by_dxv, 0, 0);
R_tot.Update(Rv, 0, 0);
}
public void construct_total_measurement_stuff(Vector nu_tot,
MatrixFixed dh_by_dx_tot,
MatrixFixed R_tot)
{
uint size = successful_measurement_vector_size;
//assert (nu_tot.Size() == size &&
//dh_by_dx_tot.Rows() == size &&
//dh_by_dx_tot.Cols() == total_state_size &&
//R_tot.Rows() == size && R_tot.Cols() == size);
nu_tot.Fill(0.0f);
dh_by_dx_tot.Fill(0.0f);
R_tot.Fill(0.0f);
uint vector_position = 0;
Feature it;
for (int i=0;i<selected_feature_list.Count;i++)
{
it = (Feature)selected_feature_list[i];
if (it.get_successful_measurement_flag())
{
nu_tot.Update(it.get_nu(), (int)vector_position);
dh_by_dx_tot.Update(it.get_dh_by_dxv(), (int)vector_position, 0);
dh_by_dx_tot.Update(it.get_dh_by_dy(), (int)vector_position,
(int)it.get_position_in_total_state_vector());
R_tot.Update(it.get_R(), (int)vector_position, (int)vector_position);
vector_position += it.get_feature_measurement_model().MEASUREMENT_SIZE;
}
}
}
//******************************Access functions*******************************
/// <summary>
/// Fills a vector with the state \vct{y}_i of the currently-marked feature.
/// </summary>
/// <param name="y_to_fill">The vector to fill with the state.</param>
/// <returns>false if no feature is currently marked (or the marked label is unknown); true otherwise.</returns>
public bool get_marked_feature_state(Vector y_to_fill)
{
if (marked_feature_label == -1)
{
Debug.WriteLine("No feature marked.");
return false;
}
Feature it = null;
bool found = false;
for (int i=0;i<feature_list.Count;i++)
{
it = (Feature)feature_list[i];
if ((int)it.get_label() == marked_feature_label) { found = true; break; }
}
if (!found)
{
Debug.WriteLine("Error: marked feature not found in list.");
return false;
}
else
{
y_to_fill.Update(it.get_y());
return true;
}
}
/// <summary>
/// Create the overall state vector by concatenating the robot state $x_v$ and all the feature states y_i
/// </summary>
/// <param name="V">The vector to fill with the state</param>
public void construct_total_state(ref Vector V)
{
//assert(V.Size() == total_state_size);
int y_position = 0;
V.Update(xv, y_position);
y_position += (int)motion_model.STATE_SIZE;
uint y_feature_no = 0;
Feature it;
for (int i=0;i<feature_list.Count;i++)
{
it = (Feature)feature_list[i];
V.Update(it.get_y(), y_position);
y_feature_no++;
y_position += (int)it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
//assert (y_feature_no == feature_list.size() && y_position == total_state_size);
}
//*************************Some important operations***************************
public void zero_axes()
{
uint size = get_total_state_size();
Vector x = new Vector(size);
x.Fill(0.0f);
MatrixFixed dxnew_by_dxold = new MatrixFixed(size, size);
dxnew_by_dxold.Fill(0.0f);
// We form the new state and Jacobian of the new state w.r.t. the old state
uint state_position = 0;
// Robot part
motion_model.func_zeroedxv_and_dzeroedxv_by_dxv(xv);
x.Update(motion_model.get_zeroedxvRES(), 0);
dxnew_by_dxold.Update(motion_model.get_dzeroedxv_by_dxvRES(), 0, 0);
state_position += motion_model.STATE_SIZE;
// Each feature in turn
uint feature_no = 0;
// Copy these to be on the safe side
motion_model.func_xp(xv);
Vector local_xp = new Vector(motion_model.get_xpRES());
//Vector local_xp = motion_model.get_xpRES();
motion_model.func_dxp_by_dxv(xv);
//MatrixFixed local_dxp_by_dxv = motion_model.get_dxp_by_dxvRES();
MatrixFixed local_dxp_by_dxv = new MatrixFixed(motion_model.get_dxp_by_dxvRES());
Feature it;
MatrixFixed Temp_FS1;
for (int i=0; i < feature_list.Count; i++)
{
it = (Feature)feature_list[i];
it.get_feature_measurement_model().func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(it.get_y(), local_xp);
x.Update(it.get_feature_measurement_model().get_zeroedyiRES(), (int)state_position);
// Calculate dzeroedyi_by_dxv in Temp_FS1
Temp_FS1 = it.get_feature_measurement_model().get_dzeroedyi_by_dxpRES() * local_dxp_by_dxv;
dxnew_by_dxold.Update(Temp_FS1, (int)state_position, 0);
dxnew_by_dxold.Update(it.get_feature_measurement_model().get_dzeroedyi_by_dyiRES(),
(int)state_position, (int)state_position);
// We also want to redefine xp_orig for this feature: the robot
// position at which is was initialised into the map (used by
// functions for checking visibility)
motion_model.func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef(
it.get_xp_orig(), local_xp);
it.set_xp_orig(it.get_feature_measurement_model().get_motion_model().get_xpredefRES());
feature_no++;
state_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
// Check we've counted properly
//assert (feature_no == feature_list.size() &&
// state_position == total_state_size);
// Do a Jacobian transform to get the new covariance
MatrixFixed P = new MatrixFixed(size, size);
construct_total_covariance(ref P);
P = dxnew_by_dxold * P * dxnew_by_dxold.Transpose();
// Finally load the scene data structures with the new state and covariance
fill_states(x);
fill_covariances(P);
}
// FIXME. this should implement the above, not the other way round.
/*
public void Solve(float[] y, float[] x)
{
Solve(new Vector(y, m_)).CopyOut(x);
}
*/
/// <summary>
/// Solve the matrix-vector system M x = y.
/// Assume that the singular values W have been preinverted by the caller.
/// </summary>
/// <param name="y"></param>
/// <param name="x_out"></param>
public void SolvePreinverted(Vector y, Vector x_out)
{
Vector x; // solution matrix
if (U_.Rows < U_.Columns)
{ // augment y with extra rows of
//std::cout << "svd<T>::solve_preinverted() -- Augmenting y\n";
Vector yy = new Vector(U_.Rows); // zeros, so that it match
yy.Fill(0);
yy.Update(y); // cols of u.transpose. ??
x = U_.ConjugateTranspose() * yy;
}
else
x = U_.ConjugateTranspose() * y;
for (int i = 0; i < x.size(); i++) // multiply with diagonal W, assumed inverted
x[i] *= W_[i, i];
x_out = V_ * x; // premultiply with v.
}
/// <summary>
/// Solve the matrix-vector system M x = y, returning x.
/// </summary>
/// <param name="y"></param>
/// <returns></returns>
public Vector Solve(Vector y)
{
// fsm sanity check :
if (y.size() != U_.Rows)
{
Debug.WriteLine("__FILE__ : size of rhs is incompatible with no. of rows in U_ " +
"y =" + Convert.ToString(y) + "\n" +
"m_=" + Convert.ToString(m_) + "\n" +
"n_=" + Convert.ToString(n_) + "\n" +
"U_=\n" + Convert.ToString(U_) + "V_=\n" + Convert.ToString(V_) +
"W_=\n" + W_);
}
Vector x = new Vector(V_.Rows); // Solution matrix.
if (U_.Rows < U_.Columns)
{ // Augment y with extra rows of
Vector yy = new Vector(U_.Rows); // zeros, so that it matches
yy.Fill(0);
if (yy.size() < y.size())
{ // fsm
Debug.WriteLine("yy=" + Convert.ToString(yy));
Debug.WriteLine("y =" + Convert.ToString(y));
// the update() call on the next line will abort...
}
yy.Update(y); // cols of u.transpose.
x = U_.ConjugateTranspose() * yy;
}
else
x = U_.ConjugateTranspose() * y;
for (int i = 0; i < x.size(); i++)
{ // multiply with diagonal 1/W
float weight = W_[i, i], zero_ = 0.0f;
if (weight != zero_)
x[i] /= weight;
else
x[i] = zero_;
}
return V_ * x; // premultiply with v.
}
/// <summary>
/// Create a state vector x_v from its component parts. Puts the matrices r, q, v, omega into their right places.
/// </summary>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
/// <param name="xv"></param>
public void compose_xv(ref Vector3D r, ref Quaternion q, ref Vector3D v, ref Vector3D omega, ref Vector xv)
{
xv.Update(r.GetVNL3(), 0);
xv.Update(q.GetRXYZ(), 3);
xv.Update(v.GetVNL3(), 7);
xv.Update(omega.GetVNL3(), 10);
}
// 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);
}
