/// <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; }