/// <summary>
/// Unpack and return R.
/// </summary>
/// <returns></returns>
public MatrixFixed R()
{
if (R_ == null)
{
int m = qrdc_out_.Columns; // column-major storage
int n = qrdc_out_.Rows;
R_ = new MatrixFixed(m, n);
MatrixFixed R = R_;
for (int i = 0; i < m; ++i)
{
for (int j = 0; j < n; ++j)
{
if (i > j)
{
R[i, j] = 0.0f;
}
else
{
R[i, j] = qrdc_out_[j, i];
}
}
}
}
return(R_);
}
/// <summary>
/// Make a measurement of a feature. This function calls elliptical_search() to
/// find the best match within three standard deviations of the predicted location.
/// </summary>
/// <param name="patch">The identifier for this feature (in this case an image patch)</param>
/// <param name="z">The best image location match for the feature, to be filled in by this function</param>
/// <param name="h">The expected image location</param>
/// <param name="S">The expected location covariance, used to specify the search region.</param>
/// <returns></returns>
public override bool measure_feature(byte[] patch,
int patchwidth,
ref Vector z,
Vector vz,
Vector h,
MatrixFixed S,
Random rnd)
{
Cholesky S_cholesky = new Cholesky(S);
MatrixFixed Sinv = S_cholesky.Inverse();
uint u_found = 0, v_found = 0;
if (SceneLib.elliptical_search(image, image_width, image_height,
patch, patchwidth, patchwidth,
h, Sinv,
ref u_found,
ref v_found,
vz,
Camera_Constants.BOXSIZE,
outputimage,
outputimage_width, outputimage_height,
show_ellipses,
calibrating,
rnd) != true)
{
// Feature not successfully matched
return(false);
}
z.Put(0, (float)u_found);
z.Put(1, (float)v_found);
return(true);
}
/// <summary>
/// Compute inverse.\ Not efficient.
/// </summary>
/// <returns></returns>
public unsafe MatrixFixed Inverse()
{
int n = A_.Columns;
MatrixFixed I = new MatrixFixed(A_);
float[] det = new float[2];
int job = 01;
fixed(float *data = I.Datablock())
{
fixed(float *data2 = det)
{
Netlib.dpodi_(data, &n, &n, data2, &job);
}
}
// Copy lower triangle into upper
for (int i = 0; i < n; ++i)
{
for (int j = i + 1; j < n; ++j)
{
I[i, j] = I[j, i];
}
}
return(I);
}
// Set the current innovation covariance S_i^{-1} for this
// particle. Called from
// Scene_Single::predict_partially_initialised_feature_measurements()
public void set_S(MatrixFixed Si)
{
Cholesky local_Si_cholesky = new Cholesky(Si);
m_SInv.Update(local_Si_cholesky.Inverse());
m_detS = SceneLib.Determinant(Si);
}
/// <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>
/// Convert a vector into a 1-by-size matrix (i.e. a row vector).
/// </summary>
/// <returns></returns>
public MatrixFixed AsRow()
{
MatrixFixed ret = new MatrixFixed(1, data.Length);
ret.SetRow(0, this);
return(ret);
}
/// <summary>
/// Convert a vector into a 1-by-size matrix (i.e. a column vector).
/// </summary>
/// <returns></returns>
public MatrixFixed AsColumn()
{
MatrixFixed ret = new MatrixFixed(data.Length, 1);
ret.SetColumn(0, this);
return(ret);
}
/// <summary>
/// Calculate the Jacobian \partfrac{\vct{y}}{\vct{h}} for the
/// Unproject() operation, for the most recent point that was projected.
///
/// This is calculated in two stages: the Jacobian between undistorted and
/// distorted image co-ordinates
///
/// \partfrac{\vct{u}}{\vct{h}} = \emat{
/// \frac{ 2 u_c^2 k_1 }{ f^{\frac{3}{2}} } +
/// \frac{ 1 }{ f^{\frac{1}{2}} } &
/// \frac{ 2 u_c v_c k_1 }{ f^{\frac{3}{2}} } \\
/// \frac{ 2 v_c u_c k_1 }{ f^{\frac{3}{2}} } &
/// \frac{ 2 v_c^2 k_1 }{ f^{\frac{3}{2}} } +
/// \frac{ 1 }{ f^{\frac{1}{2}} } } =
/// \frac{2 k_1}{f^\frac{3}{2}} \vct{u}_c \vct{u}_c^T +
/// \emat{\frac{1}{\sqrt{f}} & 0 \\
/// 0 & \frac{1}{\sqrt{f}}}
///
/// where f = 1 - 2 k_1 |\vct{u}_c|^2 and the Jacobian for the unprojection
/// operation between image rays and (undistorted, centred) image co-ordinates
///
/// \partfrac{ \vct{y} }{ \vct{u} } = \emat{
/// -\frac{ 1 }{ f_{ku} } & 0 \\
/// 0 & -\frac{ 1}{ f_{kv}} \\
/// 0 & 0 }
///
/// The final Jacobian is then given by combining these two
///
/// \partfrac{\vct{y}}{\vct{h}} =
/// \partfrac{\vct{y}}{\vct{u}} \partfrac{\vct{u}}{\vct{h}}
/// </summary>
/// <returns></returns>
public virtual MatrixFixed UnprojectionJacobian()
{
float[] a = { -1 / m_Fku, 0.0f,
0.0f, -1 / m_Fkv,
0.0f, 0.0f };
MatrixFixed dy_by_du = new MatrixFixed(3, 2, a);
// Generate the outer product matrix first
MatrixFixed du_by_dh = m_last_image_centred.AsColumn() * m_last_image_centred.AsRow();
// this matrix is not yet du_by_dh, it is just
// [ uc*uc uc*vc ]
// [ vc*uc vc*vc ]
// The trace of this matrix gives the magnitude of the vector
float radius2 = du_by_dh[0, 0] + du_by_dh[1, 1];
// Calculate various constants to save typing
float distor = 1 - 2 * m_Kd1 * radius2;
float distor1_2 = (float)Math.Sqrt(distor);
float distor3_2 = distor1_2 * distor;
// Now form the proper du_by_dh by manipulating the outer product matrix
du_by_dh *= 2 * m_Kd1 / distor3_2;
du_by_dh[0, 0] += (1 / distor1_2);
du_by_dh[1, 1] += (1 / distor1_2);
return(dy_by_du * du_by_dh);
}
/// <summary>
/// Solve the matrix equation M X = B, returning X.
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public MatrixFixed Solve(MatrixFixed B)
{
MatrixFixed x; // solution matrix
if (U_.Rows < U_.Columns)
{
// augment y with extra rows of
MatrixFixed yy = new MatrixFixed(U_.Rows, B.Columns); // zeros, so that it matches
yy.Fill(0);
yy.Update(B); // cols of u.transpose. ???
x = U_.ConjugateTranspose() * yy;
}
else
{
x = U_.ConjugateTranspose() * B;
}
int i, j;
for (i = 0; i < x.Rows; i++)
{ // multiply with diagonal 1/W
float weight = W_[i, i];
if (weight != 0) //vnl_numeric_traits<T>::zero)
{
weight = 1.0f / weight;
}
for (j = 0; j < x.Columns; j++)
{
x[i, j] *= weight;
}
}
x = V_ * x; // premultiply with v.
return(x);
}
/// <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);
}
private void init()
{
m_centre = new Vector(2);
m_C = new MatrixFixed(3, 3);
m_Cinv = new MatrixFixed(3, 3);
m_last_camera = new Vector(3);
m_last_image_centred = new Vector(2);
}
//friend class FeatureInitInfo;
//friend class Scene_Single;
/// <summary>
/// Constructor. This is protected since it is only called
/// from FeatureInitInfo::add_particle()
/// </summary>
/// <param name="l">The value(s) for the free parameters \lambda represented by this particle.</param>
/// <param name="p">The initial probability for this particle</param>
/// <param name="MEASUREMENT_SIZE">The number of parameters representing a measurement of a feature</param>
public Particle(Vector l, float p, uint MEASUREMENT_SIZE)
{
lambda = new Vector(l);
probability = p;
m_z = new Vector(MEASUREMENT_SIZE);
m_h = new Vector(MEASUREMENT_SIZE);
m_SInv = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
}
// Default NULL version
public virtual bool make_internal_measurement(Internal_Measurement_Model m,
Vector v,
Vector v2,
MatrixFixed mt)
{
Debug.WriteLine("WARNING: make_internal_measurement not implemented.");
return(false);
}
/// <summary>
/// update the position of the feature, but don't add it to the state
/// </summary>
/// <param name="lambda"></param>
public void update_feature_position(Vector lambda)
{
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());
}
/// <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;
}
}
public void update_filter_internal_measurement_slow(Scene_Single scene, uint i)
{
// Size of measurement vector
uint size = ((Internal_Measurement)(scene.internal_measurement_vector[(int)i])).get_internal_measurement_model().MEASUREMENT_SIZE;
uint size2 = scene.get_total_state_size(); // Size of state vector
Vector x = new Vector(size2);
MatrixFixed P = new MatrixFixed(size2, size2);
scene.construct_total_state_and_covariance(ref x, ref P);
// cout << "x:" << x;
// cout << "P:" << P;
// 1. Form nu and dh_by_dx
Vector nu_tot = new Vector(size);
MatrixFixed dh_by_dx_tot = new MatrixFixed(size, size2);
MatrixFixed R_tot = new MatrixFixed(size, size);
scene.construct_total_internal_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot, i);
// 2. Calculate S(k+1)
MatrixFixed S = new MatrixFixed(size, size);
//MatrixFixed Tempss2 = new MatrixFixed(size, size2);
//MatrixFixed Temps2s = new MatrixFixed(size2, size);
MatrixFixed dh_by_dx_totT = dh_by_dx_tot.Transpose();
S.Update(dh_by_dx_tot * P * dh_by_dx_totT);
S += R_tot;
// cout << "R_tot:" << R_tot;
// cout << "S:" << S;
// cout << "dh_by_dx_tot:" << dh_by_dx_tot;
// cout << "dh_by_dx_totT:" << dh_by_dx_totT;
// 3. Calculate W(k+1)
Cholesky S_cholesky = new Cholesky(S);
MatrixFixed W = P * dh_by_dx_totT * S_cholesky.Inverse();
// cout << "W:" << W;
// 4. Calculate x(k+1|k+1)
x += W * nu_tot;
// 5. Calculate P(k+1|k+1)
P -= W * S * W.Transpose();
scene.fill_state_and_covariance(x, P);
// cout << "x after update:" << x;
// cout << "P after update:" << P;
}
/// <summary>
/// Predict the image location \vct{h}_i = (x,y) for
/// partially-initialised feature with state \vct{y}_i , given the current
/// camera location \vct{x}_p and the depth parameter \lambda .
/// </summary>
/// <param name="yi"></param>
/// <param name="xp"></param>
/// <param name="lambda"></param>
public override void func_hpi_and_dhpi_by_dxp_and_dhpi_by_dyi(Vector yi,
Vector xp,
Vector lambda)
{
// 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);
// Parameters of vector hLR from camera to feature in robot frame
// hLR = zeroedri + lambda * zeroedhhati
// Calculate the vector from the camera to the feature given the current
// lambda
Vector hLR = zeroedyiRES.Extract(3, 0) + zeroedyiRES.Extract(3, 3) * lambda[0];
// Project this into the image
hpiRES = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.Project(hLR);
// What is the Jacobian of this projection?
MatrixFixed dhpi_by_dhLRi = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera.ProjectionJacobian();
// Calculate the required result Jacobians
// Now how the vector to the feature depends on the parameterised line
// (this is a function of lambda)
float[] b = new float[18];
b[0] = 1.0f;
b[1] = 0.0f;
b[2] = 0.0f;
b[3] = lambda[0];
b[4] = 0.0f;
b[5] = 0.0f;
b[6] = 0.0f;
b[7] = 1.0f;
b[8] = 0.0f;
b[9] = 0.0f;
b[10] = lambda[0];
b[11] = 0.0f;
b[12] = 0.0f;
b[13] = 0.0f;
b[14] = 1.0f;
b[15] = 0.0f;
b[16] = 0.0f;
b[17] = lambda[0];
MatrixFixed dhLRi_by_dzeroedyi = new MatrixFixed(3, 6, b);
dhpi_by_dxpRES = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dxpRES;
dhpi_by_dyiRES = dhpi_by_dhLRi * dhLRi_by_dzeroedyi * dzeroedyi_by_dyiRES;
/*
* if (Camera_Constants.DEBUGDUMP) Debug.WriteLine("func_hpi: yi = " + yi + "," +
* "xp = " + xp + "," +
* "lambda = " + lambda + "," +
* "hpiRES = " + hpiRES);
*/
}
/// <summary>
/// Calculate trace of a matrix
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public static float Trace(MatrixFixed M)
{
float sum = 0;
int N = (M.Rows < M.Columns ? M.Rows : M.Columns);
for (int i = 0; i < N; ++i)
{
sum += M[i, i];
}
return(sum);
}
/// <summary>
/// Calculate inverse of transpose.
/// </summary>
/// <returns></returns>
public MatrixFixed TransposeInverse()
{
MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows, Winverse_.Columns);
Winverse.Fill(0);
for (int i = 0; i < rank_; i++)
{
Winverse[i, i] = Winverse_[i, i];
}
return(U_ * Winverse * V_.ConjugateTranspose());
}
/// <summary>
/// Calculate pseudo-inverse.
/// </summary>
/// <param name="rank"></param>
/// <returns></returns>
public MatrixFixed PseudoInverse(int rank)
{
MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows, Winverse_.Columns);
Winverse.Fill(0);
for (int i = 0; i < rank; i++)
{
Winverse[i, i] = Winverse_[i, i];
}
return(V_ * Winverse * U_.ConjugateTranspose());
}
/// <summary>
/// Recompose SVD to U*W*V'.
/// </summary>
/// <returns></returns>
public MatrixFixed Recompose()
{
MatrixFixed W = new MatrixFixed(W_.Rows, W_.Columns);
W.Fill(0);
for (int i = 0; i < rank_; i++)
{
W[i, i] = W_[i, i];
}
return(U_ * W * V_.ConjugateTranspose());
}
/// <summary>
/// Returns the nxn outer product of two nd-vectors, or [v1]^T*[v2].\ O(n).
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static MatrixFixed OuterProduct(Vector v1, Vector v2)
{
MatrixFixed outp = new MatrixFixed(v1.size(), v2.size());
for (int i = 0; i < outp.Rows; i++) // v1.column() * v2.row()
{
for (int j = 0; j < outp.Columns; j++)
{
outp[i, j] = v1[i] * v2[j];
}
}
return(outp);
}
//friend class Scene_Single;
//private Random rnd = new Random();
//typedef Vector<Particle> as ParticleVector;
/// <summary>
/// Constructor. The class is intialised with no particles, so these must be added later using add_particle()
/// </summary>
/// <param name="sc">The Scene class.</param>
/// <param name="f">The feature for which this class represents additional information.</param>
public FeatureInitInfo(Scene_Single sc, Feature f)
{
scene = sc;
fp = f;
PARTICLE_DIMENSION = f.get_partially_initialised_feature_measurement_model().FREE_PARAMETER_SIZE;
number_of_match_attempts = 0;
nonzerovariance = false;
mean = new Vector(PARTICLE_DIMENSION);
covariance = new MatrixFixed(PARTICLE_DIMENSION, PARTICLE_DIMENSION);
//if (Scene_Single::STATUSDUMP) cout << "PARTICLE_DIMENSION = "
//<< PARTICLE_DIMENSION << endl;
}
//friend class Scene_Single;
//friend class Kalman;
public Internal_Measurement(Internal_Measurement_Model i_m_m)
{
internal_measurement_model = i_m_m;
hv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);
zv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);
nuv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);
dhv_by_dxv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.get_motion_model().STATE_SIZE);
Rv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE);
Sv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE);
// if (DEBUGDUMP) cout << "Adding Internal Measurement type "
// << internal_measurement_model.internal_type << endl;
}
public unsafe QR(MatrixFixed M)
{
qrdc_out_ = new MatrixFixed(M.Columns, M.Rows);
qraux_ = new Vector(M.Columns);
jpvt_ = new int[M.Rows];
Q_ = null;
R_ = null;
// Fill transposed O/P matrix
int c = M.Columns;
int r = M.Rows;
for (int i = 0; i < r; ++i)
{
for (int j = 0; j < c; ++j)
{
qrdc_out_[j, i] = M[i, j];
}
}
int do_pivot = 0; // Enable[!=0]/disable[==0] pivoting.
for (int i = 0; i < jpvt_.Length; i++)
{
jpvt_[i] = 0;
}
Vector work = new Vector(M.Rows);
fixed(float *data = qrdc_out_.Datablock())
{
fixed(float *data2 = qraux_.Datablock())
{
fixed(int *data3 = jpvt_)
{
fixed(float *data4 = work.Datablock())
{
Netlib.dqrdc_(data, // On output, UT is R, below diag is mangled Q
&r, &r, &c,
data2, // Further information required to demangle Q
data3,
data4,
&do_pivot);
}
}
}
}
}
/// <summary>
/// Cholesky decomposition.
/// Make cholesky decomposition of M optionally computing
/// the reciprocal condition number. If mode is estimate_condition, the
/// condition number and an approximate nullspace are estimated, at a cost
/// of a factor of (1 + 18/n). Here's a table of 1 + 18/n:
///<pre>
/// n: 3 5 10 50 100 500 1000
/// slowdown: 7.0f 4.6 2.8 1.4 1.18 1.04 1.02
/// </summary>
/// <param name="M"></param>
/// <param name="mode"></param>
public unsafe void init(MatrixFixed M, Operation mode)
{
A_ = new MatrixFixed(M);
int n = M.Columns;
//assert(n == (int)(M.Rows()));
num_dims_rank_def_ = -1;
int num_dims_rank_def_temp = num_dims_rank_def_;
// BJT: This warning is pointless - it often doesn't detect non symmetry and
// if you know what you're doing you don't want to be slowed down
// by a cerr
/*
* if (Math.Abs(M[0,n-1] - M[n-1,0]) > 1e-8)
* {
* Debug.WriteLine("cholesky: WARNING: unsymmetric: " + M);
* }
*/
if (mode != Operation.estimate_condition)
{
// Quick factorization
fixed(float *data = A_.Datablock())
{
Netlib.dpofa_(data, &n, &n, &num_dims_rank_def_temp);
}
//if ((mode == Operation.verbose) && (num_dims_rank_def_temp != 0))
// Debug.WriteLine("cholesky:: " + Convert.ToString(num_dims_rank_def_temp) + " dimensions of non-posdeffness");
}
else
{
Vector nullvector = new Vector(n);
float rcond_temp = rcond_;
fixed(float *data = A_.Datablock())
{
fixed(float *data2 = nullvector.Datablock())
{
Netlib.dpoco_(data, &n, &n, &rcond_temp, data2, &num_dims_rank_def_temp);
}
}
rcond_ = rcond_temp;
}
num_dims_rank_def_ = num_dims_rank_def_temp;
}
public void predict_internal_measurement(Vector xv, MatrixFixed Pxx)
{
internal_measurement_model.func_hv_and_dhv_by_dxv(xv);
hv.Update(internal_measurement_model.get_hvRES());
dhv_by_dxv.Update(internal_measurement_model.get_dhv_by_dxvRES());
internal_measurement_model.func_Rv(hv);
Rv.Update(internal_measurement_model.get_RvRES());
internal_measurement_model.func_Sv(Pxx, dhv_by_dxv, Rv);
Sv.Update(internal_measurement_model.get_SvRES());
//if (DEBUGDUMP) cout << "Internal measurement prediction: hv " << endl
//<< hv << endl;
}
/// <summary>
/// Return upper-triangular factor.
/// </summary>
/// <returns></returns>
private MatrixFixed UpperTriangle()
{
int n = A_.Columns;
MatrixFixed U = new MatrixFixed(n, n);
// Zap lower triangle and transpose
for (int i = 0; i < n; ++i)
{
U[i, i] = A_[i, i];
for (int j = i + 1; j < n; ++j)
{
U[i, j] = A_[j, i];
U[j, i] = 0;
}
}
return(U);
}
/// <summary>
/// Return lower-triangular factor.
/// </summary>
/// <returns></returns>
public MatrixFixed LowerTriangle()
{
int n = A_.Columns;
MatrixFixed L = new MatrixFixed(n, n);
// Zap upper triangle and transpose
for (int i = 0; i < n; ++i)
{
L[i, i] = A_[i, i];
for (int j = i + 1; j < n; ++j)
{
L[j, i] = A_[j, i];
L[i, j] = 0;
}
}
return(L);
}
public override void func_zeroedyigraphics_and_Pzeroedyigraphics(Vector yi, Vector xv,
MatrixFixed Pxx, MatrixFixed Pxyi, MatrixFixed Pyiyi)
{
((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_xp(xv);
// In this case (where the feature state is the same as the graphics
// state) zeroedyigraphics is the same as zeroedyi
func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_xpRES());
zeroedyigraphicsRES.Update(zeroedyiRES);
MatrixFixed dzeroedyigraphics_by_dxv = dzeroedyi_by_dxpRES * ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_dxp_by_dxvRES();
PzeroedyigraphicsRES.Update(dzeroedyigraphics_by_dxv * Pxx * dzeroedyigraphics_by_dxv.Transpose() +
dzeroedyi_by_dyiRES * Pxyi.Transpose() * dzeroedyigraphics_by_dxv.Transpose() +
dzeroedyigraphics_by_dxv * Pxyi * dzeroedyi_by_dyiRES.Transpose() +
dzeroedyi_by_dyiRES * Pyiyi * dzeroedyi_by_dyiRES.Transpose());
}
