/// <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_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;
}
}
}
//*************************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.
}
private unsafe void init(MatrixFixed M, float zero_out_tol)
{
m_ = M.Rows;
n_ = M.Columns;
U_ = new MatrixFixed(m_, n_);
W_ = new DiagMatrix(n_);
Winverse_ = new DiagMatrix(n_);
V_ = new MatrixFixed(n_, n_);
//assert(m_ > 0);
//assert(n_ > 0);
int n = M.Rows;
int p = M.Columns;
int mm = Netlib.min(n+1,p);
// Copy source matrix into fortran storage
// SVD is slow, don't worry about the cost of this transpose.
Vector X = Vector.fortran_copy(M);
// Make workspace vectors
Vector work = new Vector(n);
work.Fill(0);
Vector uspace = new Vector(n*p);
uspace.Fill(0);
Vector vspace = new Vector(p*p);
vspace.Fill(0);
Vector wspace = new Vector(mm);
wspace.Fill(0); // complex fortran routine actually _wants_ complex W!
Vector espace = new Vector(p);
espace.Fill(0);
// Call Linpack SVD
int info = 0;
int job = 21;
fixed (float* data = X.Datablock())
{
fixed (float* data2 = wspace.Datablock())
{
fixed (float* data3 = espace.Datablock())
{
fixed (float* data4 = uspace.Datablock())
{
fixed (float* data5 = vspace.Datablock())
{
fixed (float* data6 = work.Datablock())
{
Netlib.dsvdc_(data, &n, &n, &p,
data2,
data3,
data4, &n,
data5, &p,
data6,
&job, &info);
}
}
}
}
}
}
// Error return?
if (info != 0)
{
// If info is non-zero, it contains the number of singular values
// for this the SVD algorithm failed to converge. The condition is
// not bogus. Even if the returned singular values are sensible,
// the singular vectors can be utterly wrong.
// It is possible the failure was due to NaNs or infinities in the
// matrix. Check for that now.
M.assert_finite();
// If we get here it might be because
// 1. The scalar type has such
// extreme precision that too few iterations were performed to
// converge to within machine precision (that is the svdc criterion).
// One solution to that is to increase the maximum number of
// iterations in the netlib code.
//
// 2. The LINPACK dsvdc_ code expects correct IEEE rounding behaviour,
// which some platforms (notably x86 processors)
// have trouble doing. For example, gcc can output
// code in -O2 and static-linked code that causes this problem.
// One solution to this is to persuade gcc to output slightly different code
// by adding and -fPIC option to the command line for v3p\netlib\dsvdc.c. If
// that doesn't work try adding -ffloat-store, which should fix the problem
// at the expense of being significantly slower for big problems. Note that
// if this is the cause, vxl/vnl/tests/test_svd should have failed.
//
// You may be able to diagnose the problem here by printing a warning message.
Debug.WriteLine("__FILE__ : suspicious return value (" + Convert.ToString(info) + ") from SVDC" +
"__FILE__ : M is " + Convert.ToString(M.Rows) + "x" + Convert.ToString(M.Columns));
valid_ = false;
}
else
valid_ = true;
// Copy fortran outputs into our storage
int ctr = 0;
for (int j = 0; j < p; ++j)
for (int i = 0; i < n; ++i)
{
U_[i, j] = uspace[ctr];
ctr++;
}
for (int j = 0; j < mm; ++j)
W_[j, j] = Math.Abs(wspace[j]); // we get rid of complexness here.
for (int j = mm; j < n_; ++j)
W_[j, j] = 0;
ctr = 0;
for (int j = 0; j < p; ++j)
for (int i = 0; i < p; ++i)
{
V_[i, j] = vspace[ctr];
ctr++;
}
//if (test_heavily)
{
// Test that recomposed matrix == M
//float recomposition_residual = Math.Abs((Recompose() - M).FrobeniusNorm());
//float n2 = Math.Abs(M.FrobeniusNorm());
//float thresh = m_ * (float)(eps) * n2;
//if (recomposition_residual > thresh)
{
//std::cerr << "VNL::SVD<T>::SVD<T>() -- Warning, recomposition_residual = "
//<< recomposition_residual << std::endl
//<< "FrobeniusNorm(M) = " << n << std::endl
//<< "eps*FrobeniusNorm(M) = " << thresh << std::endl
//<< "Press return to continue\n";
//char x;
//std::cin.get(&x, 1, '\n');
}
}
if (zero_out_tol >= 0)
// Zero out small sv's and update rank count.
ZeroOutAbsolute((float)(+zero_out_tol));
else
// negative tolerance implies relative to max elt.
ZeroOutRelative((float)(-zero_out_tol));
}
/// <summary>
/// Form the covariance matrix Q of the process noise associated with x_v .
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
// Fill noise covariance matrix Pnn: this is the covariance of
// the noise vector (V)
// (Omega)
// that gets added to the state.
// Form of this could change later, but for now assume that
// V and Omega are independent, and that each of their components is
// independent...
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6, 6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6);
dxnew_by_dn.Fill(0.0f);
// The translation part is just I \Delta t
MatrixFixed Temp33A = new MatrixFixed(3, 3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// qnew = q x \Omega \Deltat
// dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega
// Get the first part
Vector qRXYZ = xv.Extract(4, 3);
Quaternion qold = new Quaternion();
qold.SetRXYZ(qRXYZ);
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_dOmega
Vector Omega = new Vector(3);
Omega.Fill(SD_alpha_component_filter);
MatrixFixed Temp43A = new MatrixFixed(4, 3);
dqomegadt_by_domega(new Vector3D(Omega), delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose());
}
private unsafe void init(MatrixFixed M, float zero_out_tol)
{
m_ = M.Rows;
n_ = M.Columns;
U_ = new MatrixFixed(m_, n_);
W_ = new DiagMatrix(n_);
Winverse_ = new DiagMatrix(n_);
V_ = new MatrixFixed(n_, n_);
//assert(m_ > 0);
//assert(n_ > 0);
int n = M.Rows;
int p = M.Columns;
int mm = Netlib.min(n + 1, p);
// Copy source matrix into fortran storage
// SVD is slow, don't worry about the cost of this transpose.
Vector X = Vector.fortran_copy(M);
// Make workspace vectors
Vector work = new Vector(n);
work.Fill(0);
Vector uspace = new Vector(n * p);
uspace.Fill(0);
Vector vspace = new Vector(p * p);
vspace.Fill(0);
Vector wspace = new Vector(mm);
wspace.Fill(0); // complex fortran routine actually _wants_ complex W!
Vector espace = new Vector(p);
espace.Fill(0);
// Call Linpack SVD
int info = 0;
int job = 21;
fixed(float *data = X.Datablock())
{
fixed(float *data2 = wspace.Datablock())
{
fixed(float *data3 = espace.Datablock())
{
fixed(float *data4 = uspace.Datablock())
{
fixed(float *data5 = vspace.Datablock())
{
fixed(float *data6 = work.Datablock())
{
Netlib.dsvdc_(data, &n, &n, &p,
data2,
data3,
data4, &n,
data5, &p,
data6,
&job, &info);
}
}
}
}
}
}
// Error return?
if (info != 0)
{
// If info is non-zero, it contains the number of singular values
// for this the SVD algorithm failed to converge. The condition is
// not bogus. Even if the returned singular values are sensible,
// the singular vectors can be utterly wrong.
// It is possible the failure was due to NaNs or infinities in the
// matrix. Check for that now.
M.assert_finite();
// If we get here it might be because
// 1. The scalar type has such
// extreme precision that too few iterations were performed to
// converge to within machine precision (that is the svdc criterion).
// One solution to that is to increase the maximum number of
// iterations in the netlib code.
//
// 2. The LINPACK dsvdc_ code expects correct IEEE rounding behaviour,
// which some platforms (notably x86 processors)
// have trouble doing. For example, gcc can output
// code in -O2 and static-linked code that causes this problem.
// One solution to this is to persuade gcc to output slightly different code
// by adding and -fPIC option to the command line for v3p\netlib\dsvdc.c. If
// that doesn't work try adding -ffloat-store, which should fix the problem
// at the expense of being significantly slower for big problems. Note that
// if this is the cause, vxl/vnl/tests/test_svd should have failed.
//
// You may be able to diagnose the problem here by printing a warning message.
Debug.WriteLine("__FILE__ : suspicious return value (" + Convert.ToString(info) + ") from SVDC" +
"__FILE__ : M is " + Convert.ToString(M.Rows) + "x" + Convert.ToString(M.Columns));
valid_ = false;
}
else
{
valid_ = true;
}
// Copy fortran outputs into our storage
int ctr = 0;
for (int j = 0; j < p; ++j)
{
for (int i = 0; i < n; ++i)
{
U_[i, j] = uspace[ctr];
ctr++;
}
}
for (int j = 0; j < mm; ++j)
{
W_[j, j] = Math.Abs(wspace[j]); // we get rid of complexness here.
}
for (int j = mm; j < n_; ++j)
{
W_[j, j] = 0;
}
ctr = 0;
for (int j = 0; j < p; ++j)
{
for (int i = 0; i < p; ++i)
{
V_[i, j] = vspace[ctr];
ctr++;
}
}
//if (test_heavily)
{
// Test that recomposed matrix == M
//float recomposition_residual = Math.Abs((Recompose() - M).FrobeniusNorm());
//float n2 = Math.Abs(M.FrobeniusNorm());
//float thresh = m_ * (float)(eps) * n2;
//if (recomposition_residual > thresh)
{
//std::cerr << "VNL::SVD<T>::SVD<T>() -- Warning, recomposition_residual = "
//<< recomposition_residual << std::endl
//<< "FrobeniusNorm(M) = " << n << std::endl
//<< "eps*FrobeniusNorm(M) = " << thresh << std::endl
//<< "Press return to continue\n";
//char x;
//std::cin.get(&x, 1, '\n');
}
}
if (zero_out_tol >= 0)
{
// Zero out small sv's and update rank count.
ZeroOutAbsolute((float)(+zero_out_tol));
}
else
{
// negative tolerance implies relative to max elt.
ZeroOutRelative((float)(-zero_out_tol));
}
}
/// <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;
}
/// <summary>
/// Unpack and return unitary part Q.
/// </summary>
/// <returns></returns>
public MatrixFixed Q()
{
int m = qrdc_out_.Columns; // column-major storage
int n = qrdc_out_.Rows;
bool verbose = false;
if (Q_ == null)
{
Q_ = new MatrixFixed(m, m);
// extract Q.
if (verbose)
{
Debug.WriteLine("__FILE__: VNL::QR<T>Q()");
Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n));
Debug.WriteLine("qr0 = [" + qrdc_out_ + "];");
Debug.WriteLine("aux = [" + qraux_ + "];");
}
Q_.SetIdentity();
MatrixFixed Q = Q_;
Vector v = new Vector(m);
v.Fill(0);
Vector w = new Vector(m);
w.Fill(0);
// Golub and vanLoan, p199. backward accumulation of householder matrices
// Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]]
for (int k = n - 1; k >= 0; --k)
{
if (k >= m)
{
continue;
}
// Make housevec v, and accumulate norm at the same time.
v[k] = qraux_[k];
float sq = (v[k] * v[k]);
for (int j = k + 1; j < m; ++j)
{
v[j] = qrdc_out_[k, j];
sq += (v[j] * v[j]);
}
//if (verbose) MatlabPrint(std::cerr, v, "v");
// Premultiply emerging Q by house(v), noting that v[0..k-1] == 0.
// Q_new = (1 - (2/v'*v) v v')Q
// or Q -= (2/v'*v) v (v'Q)
if (sq > 0.0)
{
float scale = 2.0f / sq;
// w = (2/v'*v) v' Q
for (int i = k; i < m; ++i)
{
w[i] = 0.0f;
for (int j = k; j < m; ++j)
{
w[i] += scale * v[j] * Q[j, i];
}
//w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i];
}
//if (verbose) VNL::MatlabPrint(std::cerr, w, "w");
// Q -= v w
for (int i = k; i < m; ++i)
{
for (int j = k; j < m; ++j)
{
Q[i, j] -= (v[i]) * (w[j]);
}
}
}
}
}
return(Q_);
}
/// <summary>
/// Construct a diagonal matrix with diagonal elements equal to value.
/// </summary>
/// <param name="nn"></param>
/// <param name="value"></param>
public DiagMatrix(int nn, float value)
{
diagonal_ = new Vector(nn);
diagonal_.Fill(value);
}
/// <summary>
/// Read the initial state vector and covariance from the settings class.
/// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in
/// the class, these are passed by reference to be filled in by this function.
/// </summary>
/// <param name="settings"></param>
/// <param name="initial_xv"></param>
/// <param name="initial_Pxx"></param>
public void read_initial_state(Settings settings,
ref Vector initial_xv,
ref MatrixFixed initial_Pxx)
{
ArrayList values;
// Check that the motion model is correct
values = settings.get_entry("InitialState", "MotionModel");
if ((String)values[0] != motion_model_type)
{
Debug.WriteLine("Attempted to read an initial state with a motion model of type " +
motion_model_type + " where the initialisation data in the [InitialState] section" +
" reports the type " + settings.get_entry("InitialState", "MotionModel") + ".");
//throw Scene::InitialisationError(error.str());
}
// Make sure the vector and matrix are the correct sizes
initial_xv = new Vector(STATE_SIZE);
initial_Pxx = new MatrixFixed(STATE_SIZE, STATE_SIZE);
//initial_xv.Resize(STATE_SIZE);
//initial_Pxx.Resize(STATE_SIZE, STATE_SIZE);
initial_xv.Fill(0.0f);
initial_Pxx.Fill(0.0f);
values = settings.get_entry("InitialState", "xv");
String xv_stream = (String)values[0];
initial_xv.ReadASCII(xv_stream);
values = settings.get_entry("InitialState", "Pxx");
String Pxx_stream = (String)values[0];
initial_Pxx.ReadASCII(Pxx_stream);
}
/// <summary>
/// Read the initial state vector and covariance from a stream.
/// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in
/// the class, these are passed by reference to be filled in by this function.
/// </summary>
/// <param name="stream"></param>
/// <param name="initial_xv"></param>
/// <param name="initial_Pxx"></param>
public void read_initial_state(BinaryReader stream,
ref Vector initial_xv,
ref MatrixFixed initial_Pxx)
{
// Make sure the vector and matrix are the correct sizes
initial_xv.Resize(STATE_SIZE);
initial_Pxx.Resize(STATE_SIZE, STATE_SIZE);
initial_xv.Fill(0.0f);
initial_Pxx.Fill(0.0f);
for (int r = 0; r < STATE_SIZE; r++)
initial_xv[r] = stream.ReadSingle();
for (int r = 0; r < STATE_SIZE; r++)
for (int c = 0; c < STATE_SIZE; c++)
initial_Pxx[r, c] = stream.ReadSingle();
}
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);
}
/// <summary>
/// Construct a diagonal matrix with diagonal elements equal to value.
/// </summary>
/// <param name="nn"></param>
/// <param name="value"></param>
public DiagMatrix(int nn, float value)
{
diagonal_ = new Vector(nn);
diagonal_.Fill(value);
}
/// <summary>
/// Unpack and return unitary part Q.
/// </summary>
/// <returns></returns>
public MatrixFixed Q()
{
int m = qrdc_out_.Columns; // column-major storage
int n = qrdc_out_.Rows;
bool verbose = false;
if (Q_ == null)
{
Q_ = new MatrixFixed(m,m);
// extract Q.
if (verbose)
{
Debug.WriteLine("__FILE__: VNL::QR<T>Q()");
Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n));
Debug.WriteLine("qr0 = [" + qrdc_out_ + "];");
Debug.WriteLine("aux = [" + qraux_ + "];");
}
Q_.SetIdentity();
MatrixFixed Q = Q_;
Vector v = new Vector(m);
v.Fill(0);
Vector w = new Vector(m);
w.Fill(0);
// Golub and vanLoan, p199. backward accumulation of householder matrices
// Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]]
for (int k = n-1; k >= 0; --k)
{
if (k >= m) continue;
// Make housevec v, and accumulate norm at the same time.
v[k] = qraux_[k];
float sq = (v[k] * v[k]);
for (int j = k+1; j < m; ++j)
{
v[j] = qrdc_out_[k,j];
sq += (v[j] * v[j]);
}
//if (verbose) MatlabPrint(std::cerr, v, "v");
// Premultiply emerging Q by house(v), noting that v[0..k-1] == 0.
// Q_new = (1 - (2/v'*v) v v')Q
// or Q -= (2/v'*v) v (v'Q)
if (sq > 0.0)
{
float scale = 2.0f/sq;
// w = (2/v'*v) v' Q
for (int i = k; i < m; ++i)
{
w[i] = 0.0f;
for (int j = k; j < m; ++j)
w[i] += scale * v[j] * Q[j, i];
//w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i];
}
//if (verbose) VNL::MatlabPrint(std::cerr, w, "w");
// Q -= v w
for (int i = k; i < m; ++i)
for (int j = k; j < m; ++j)
Q[i,j] -= (v[i]) * (w[j]);
}
}
}
return Q_;
}
