public MatrixFixed(MatrixFixed m)
{
init(m.Rows, m.Columns);
for (int i = 0; i < Rows; i++)
for (int j = 0; j < Columns; j++)
m_matrix[i, j] = m[i, j];
}
/// <summary>
/// Base-class constructor. This should be called in the constructor of any
/// derived class.
/// </summary>
/// <param name="position_state_size">The number of dimensions in the position state</param>
/// <param name="state_size">The total state size</param>
/// <param name="control_size">The control state size</param>
/// <param name="m_m_d_t">A string representing the motion model dimensionality type </param>
/// <param name="m_m_t">A unique string representing this particular motion model type</param>
public Motion_Model(uint position_state_size,
uint state_size,
uint control_size,
String m_m_d_t, String m_m_t)
{
POSITION_STATE_SIZE = position_state_size;
STATE_SIZE = state_size;
CONTROL_SIZE = control_size;
motion_model_dimensionality_type = m_m_d_t;
motion_model_type = m_m_t;
fvRES = new Vector(STATE_SIZE);
xpRES = new Vector(POSITION_STATE_SIZE);
fv_noisyRES = new Vector(STATE_SIZE);
xvredefRES = new Vector(STATE_SIZE);
xvnormRES = new Vector(STATE_SIZE);
zeroedxvRES = new Vector(STATE_SIZE);
xpredefRES = new Vector(POSITION_STATE_SIZE);
dfv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
QxRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
dxp_by_dxvRES = new MatrixFixed(POSITION_STATE_SIZE, STATE_SIZE);
dxvredef_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
dxvredef_by_dxpdefRES = new MatrixFixed(STATE_SIZE, POSITION_STATE_SIZE);
dxvnorm_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
dzeroedxv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
dxpredef_by_dxpRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
dxpredef_by_dxpdefRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
}
/// <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;
}
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>
/// 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);
}
//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 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;
}
public SearchDatum(MatrixFixed _PuInv, Vector _search_centre)
{
PuInv = _PuInv;
search_centre = _search_centre;
result_flag = false;
result_u = 0;
result_v = 0;
halfwidth = (uint)(SceneLib.NO_SIGMA /
Math.Sqrt(PuInv[0, 0] - PuInv[0, 1] * PuInv[0, 1] / PuInv[1, 1]));
halfheight = (uint)(SceneLib.NO_SIGMA /
Math.Sqrt(PuInv[1, 1] - PuInv[0, 1] * PuInv[0, 1] / PuInv[0, 0]));
if (halfwidth > 10) halfwidth = 10;
if (halfheight > 10) halfheight = 10;
}
/// <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 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);
}
}
}
}
}
// Set the Jacobian \partfracv{h}{y}
// between the feature measurement state and the feature state.
public void set_dh_by_dy(MatrixFixed new_dh_by_dy) { dh_by_dy.Update(new_dh_by_dy); }
// Set the covariance, \mat{P}_{xy}, between the feature state and
// the robot state.
public void set_Pxy(MatrixFixed new_Pxy) { Pxy.Update(new_Pxy); }
/// <summary>
/// Make a measurement of a feature.
/// </summary>
/// <param name="id">The identifier for this feature</param>
/// <param name="z">The measurement of the state, to be filled in by this function</param>
/// <param name="h">The expected measurement</param>
/// <param name="S">The measurement covariance.</param>
/// <returns></returns>
public virtual bool measure_feature(byte[] id, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }
/// <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);
}
// Set the feature state covariance, \mat{P}_{yy}.
public void set_Pyy(MatrixFixed new_Pyy) { Pyy.Update(new_Pyy); }
/// <summary>
/// Function which unites common stuff for constructors below
/// Constructor stuff which is common to more than one constructor
/// </summary>
protected void feature_constructor_bookeeping()
{
selected_flag = false; // Feature is unselected when first detected
scheduled_for_termination_flag = false;
attempted_measurements_of_feature = 0;
successful_measurements_of_feature = 0;
// Allocate matrices for storing predicted and actual measurements
h = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
z = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
prev_z = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
nu = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
vz = new Vector(2);
accn_z = new Vector(2);
dh_by_dxv = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.get_motion_model().STATE_SIZE);
dh_by_dy = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
R = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE);
S = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE);
}
/// <summary>
/// Make a measurement of a feature.
/// </summary>
/// <param name="id">The identifier for this feature</param>
/// <param name="z">The measurement of the state, to be filled in by this function</param>
/// <param name="h">The expected measurement</param>
/// <param name="S">The measurement covariance.</param>
/// <returns></returns>
public virtual bool measure_feature(classimage_mono id, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }
public override float selection_score(MatrixFixed m)
{
// Always measureable for now
return 100000.0f;
}
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());
}
public override float selection_score(MatrixFixed Si)
{
// Return the trace of the innovation covariance
return SceneLib.Trace(Si);
}
// In this case the graphics representation y_i^{graphics} and its
// covariance is the same as the feature state y_i and covariance.
public override void func_yigraphics_and_Pyiyigraphics(Vector yi, MatrixFixed Pyiyi)
{
// The graphics representation is the same as the state
yigraphicsRES.Update(yi);
PyiyigraphicsRES.Update(Pyiyi);
}
// Set the covariance of the measurement \mat{R}
// (for example, its uncertainty due to image resolution).
public void set_R(MatrixFixed new_R) { R.Update(new_R); }
public override void func_yigraphics_and_Pyiyigraphics(Vector yi, MatrixFixed Pyiyi)
{
yigraphicsRES.Update(yi);
PyiyigraphicsRES.Update(Pyiyi);
}
// Set the innovation covariance \mat{S}
// (for example, the overall uncertainty in image location).
public void set_S(MatrixFixed new_S) { S.Update(new_S); }
public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp)
{
Wide_Camera_Point_Feature_Measurement_Model wm = (Wide_Camera_Point_Feature_Measurement_Model)wide_model;
// Extract cartesian and quaternion components of xp
wm.threed_motion_model.func_r(xp);
wm.threed_motion_model.func_q(xp);
// Extract ri and hhati components of yi = ypi
func_ri(yi);
func_hhati(yi);
// ri part: transformation is just the same as in the normal point case
// zeroedri = RRW(rWi - rW) //commented out in original code
// ri - r
Vector3D yWiminusrW = new Vector3D(riRES - wm.threed_motion_model.get_rRES());
Quaternion qRW = wm.threed_motion_model.get_qRES().Inverse();
MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq();
// Rotation RRW
RotationMatrix RRW = qRW.RotationMatrix();
// RRW(rWi - rW)
Vector3D zeroedri = new Vector3D(RRW * yWiminusrW);
// Now calculate Jacobians
// dzeroedri_by_dri is RRW
// dzeroedri_by_dhhati = 0
MatrixFixed dzeroedri_by_dri = new MatrixFixed(RRW);
// dzeroedyi_by_dxp:
// dzeroedri_by_dr = -RRW
// dzeroedri_by_dq = d_dq(RRW (ri - r))
MatrixFixed dzeroedri_by_dr = RRW * -1.0f;
MatrixFixed dzeroedri_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW);
MatrixFixed dzeroedri_by_dq = dzeroedri_by_dqRW * dqRW_by_dq;
// Now for the hhati part (easier...)
// zeroedhhati = RRW hhati
Vector3D zeroedhhati = new Vector3D(RRW * hhatiRES);
// Jacobians
// dzeroedhhati_by_dr = 0
// dzeroedhhati_by_dq = d_dq(RRW hhati)
// dzeroedhhati_by_dhhati = RRW
// dzeroedhhati_by_dri = 0
MatrixFixed dzeroedhhati_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, hhatiRES);
MatrixFixed dzeroedhhati_by_dq = dzeroedhhati_by_dqRW * dqRW_by_dq;
MatrixFixed dzeroedhhati_by_dhhati = new MatrixFixed(RRW);
// And put it all together
zeroedyiRES.Update(zeroedri.GetVNL3(), 0);
zeroedyiRES.Update(zeroedhhati.GetVNL3(), 3);
//cout << "Line: zeroedri = " << zeroedri << "zeroedhhati = " << zeroedhhati;
dzeroedyi_by_dxpRES.Fill(0.0f);
dzeroedyi_by_dxpRES.Update(dzeroedri_by_dr, 0, 0);
dzeroedyi_by_dxpRES.Update(dzeroedri_by_dq, 0, 3);
dzeroedyi_by_dxpRES.Update(dzeroedhhati_by_dq, 3, 3);
dzeroedyi_by_dyiRES.Fill(0.0f);
dzeroedyi_by_dyiRES.Update(dzeroedri_by_dri, 0, 0);
dzeroedyi_by_dyiRES.Update(dzeroedhhati_by_dhhati, 3, 3);
}
/// <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(classimage_mono 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>
/// 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>
/// 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(classimage_mono 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;
}
}