本文整理汇总了C#中SceneLibrary.MatrixFixed类的典型用法代码示例。如果您正苦于以下问题:C# MatrixFixed类的具体用法?C# MatrixFixed怎么用?C# MatrixFixed使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
MatrixFixed类属于SceneLibrary命名空间,在下文中一共展示了MatrixFixed类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C#代码示例。
示例1: MatrixFixed
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];
}
开发者ID:iManbot,项目名称:monoslam,代码行数:7,代码来源:MatrixFixed.cs
示例2: Motion_Model
/// <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);
}
开发者ID:iManbot,项目名称:monoslam,代码行数:38,代码来源:models_base.cs
示例3: Trace
/// <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;
}
开发者ID:kasertim,项目名称:sentience,代码行数:13,代码来源:SceneLib.cs
示例4: init
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);
}
开发者ID:kasertim,项目名称:sentience,代码行数:8,代码来源:widecamera.cs
示例5: Particle
//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);
}
开发者ID:iManbot,项目名称:monoslam,代码行数:19,代码来源:init_feature.cs
示例6: make_internal_measurement
// 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;
}
开发者ID:iManbot,项目名称:monoslam,代码行数:10,代码来源:Sim_Or_Rob.cs
示例7: predict_filter_slow
/// <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);
}
开发者ID:kasertim,项目名称:sentience,代码行数:59,代码来源:kalman.cs
示例8: Internal_Measurement
//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;
}
开发者ID:kasertim,项目名称:sentience,代码行数:17,代码来源:internal_measurement.cs
示例9: predict_internal_measurement
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;
}
开发者ID:kasertim,项目名称:sentience,代码行数:16,代码来源:internal_measurement.cs
示例10: SearchDatum
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;
}
开发者ID:kasertim,项目名称:sentience,代码行数:16,代码来源:ellipses.cs
示例11: init
/// <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;
}
开发者ID:iManbot,项目名称:monoslam,代码行数:56,代码来源:cholesky.cs
示例12: QR
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);
}
}
}
}
}
开发者ID:iManbot,项目名称:monoslam,代码行数:39,代码来源:QR.cs
示例13: set_dh_by_dy
// 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); }
开发者ID:iManbot,项目名称:monoslam,代码行数:3,代码来源:feature.cs
示例14: set_Pxy
// 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); }
开发者ID:iManbot,项目名称:monoslam,代码行数:3,代码来源:feature.cs
示例15: measure_feature
/// <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); }
开发者ID:kasertim,项目名称:sentience,代码行数:9,代码来源:Sim_Or_Rob.cs
示例16: convert_from_partially_to_fully_initialised
/// <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);
}
开发者ID:iManbot,项目名称:monoslam,代码行数:91,代码来源:feature.cs
示例17: set_Pyy
// Set the feature state covariance, \mat{P}_{yy}.
public void set_Pyy(MatrixFixed new_Pyy) { Pyy.Update(new_Pyy); }
开发者ID:iManbot,项目名称:monoslam,代码行数:2,代码来源:feature.cs
示例18: feature_constructor_bookeeping
/// <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);
}
开发者ID:iManbot,项目名称:monoslam,代码行数:25,代码来源:feature.cs
示例19: measure_feature
/// <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); }
开发者ID:iManbot,项目名称:monoslam,代码行数:9,代码来源:Sim_Or_Rob.cs
示例20: selection_score
public override float selection_score(MatrixFixed m)
{
// Always measureable for now
return 100000.0f;
}
开发者ID:iManbot,项目名称:monoslam,代码行数:5,代码来源:models_wideangle.cs
注:本文中的SceneLibrary.MatrixFixed类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论