/// <summary>
/// Convert a non-homogeneous vector into a homogeneous vector. Returns a
/// new vector which is the input vector augmented by one more element
/// of value 1.0.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Vector Unproject(Vector v)
{
int n = v.Size();
Vector ret = new Vector(n + 1);
for (int i = 0; i < n; i++)
{
ret[i] = v[i];
}
ret[n] = 1;
return(ret);
}
/// <summary>
/// Project a homogeneous vector down to a non-homogeneous one. Returns a
/// new vector containing all the elements of the input vector, apart from the
/// last one, divided by the last one.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Vector Project(Vector v)
{
int newsize = v.Size() - 1;
float factor = v[newsize];
Vector ret = new Vector(newsize);
for (int i = 0; i < newsize; i++)
{
ret[i] = v[i] / factor;
}
return(ret);
}
public void fill_states(Vector V)
{
//assert (V.Size() == total_state_size);
uint y_position = 0;
xv.Update(V.Extract((int)motion_model.STATE_SIZE, (int)y_position));
y_position += motion_model.STATE_SIZE;
uint y_feature_no = 0;
Feature it;
for (int i=0;i<feature_list.Count;i++)
{
if (y_position < V.Size())
{
it = (Feature)feature_list[i];
it.set_y(V.Extract((int)it.get_feature_measurement_model().FEATURE_STATE_SIZE, (int)y_position));
y_feature_no++;
y_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
}
//assert (y_feature_no <= feature_list.size() && y_position <= total_state_size);
}
/// <summary>
/// Convert a non-homogeneous vector into a homogeneous vector. Returns a
/// new vector which is the input vector augmented by one more element
/// of value 1.0.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Vector Unproject(Vector v)
{
int n = v.Size();
Vector ret = new Vector(n+1);
for (int i = 0; i < n; i++)
ret[i] = v[i];
ret[n] = 1;
return ret;
}
/// <summary>
/// Project a homogeneous vector down to a non-homogeneous one. Returns a
/// new vector containing all the elements of the input vector, apart from the
/// last one, divided by the last one.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Vector Project(Vector v)
{
int newsize = v.Size()-1;
float factor = v[newsize];
Vector ret = new Vector(newsize);
for (int i = 0; i < newsize; i++)
ret[i] = v[i] / factor;
return ret;
}