public model_vertex(Feature feat)
{
position = new Vector(3);
vertex_feature = feat;
position[0] = feat.get_y()[0];
position[1] = feat.get_y()[1];
position[2] = feat.get_y()[2];
}
private void reconstructFeaturePosition(Feature feat)
{
Vector fpos;
Vector position = new Vector(3);
int i, k, hits = 1;
fpos = feat.get_y();
for (k = 0; k < 3; k++)
{
position[k] = fpos[k];
}
for (i = 0; i < feat.triangles.Count; i++)
{
Feature f;
int j, index = 0;
model_triangle tri = (model_triangle)feat.triangles[i];
for (j = 0; j < 3; j++)
{
f = ((model_vertex)tri.vertices[j]).vertex_feature;
if (f == feat)
{
index = j;
}
}
for (j = 0; j < 3; j++)
{
if (j != index)
{
f = ((model_vertex)tri.vertices[j]).vertex_feature;
if (!((f.get_successful_measurement_flag()) && (f.get_feature_measurement_model().fully_initialised_flag)))
{
fpos = f.get_y();
for (k = 0; k < 3; k++)
{
position[k] += fpos[k] - tri.offsets[index, j, k];
}
hits++;
}
}
}
}
if (hits > 0)
{
for (k = 0; k < 3; k++)
{
position[k] /= hits;
}
fpos = feat.get_y();
for (k = 0; k < 3; k++)
{
fpos[k] = position[k];
}
}
}
/// <summary>
/// project a feature from 3D into 2D image coordinates
/// </summary>
/// <param name="feat"></param>
public void projectFeature(Feature feat, ref int screen_x, ref int screen_y)
{
Vector yi = feat.get_y();
Vector xp = scene.get_motion_model().get_xpRES();
Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
feat.get_fully_initialised_feature_measurement_model();
fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi, xp);
screen_x = (int)fully_init_fmm.get_hiRES()[0];
screen_y = (int)fully_init_fmm.get_hiRES()[1];
}
/// <summary>
/// update the line history
/// </summary>
/// <param name="img"></param>
public void update(classimage_mono img)
{
int i;
Vector position1 = feature1.get_z();
Vector position2 = feature2.get_z();
// store the feature positions
for (i = 0; i < 3; i++)
{
feature_position[curr_index, 0][i] = feature1.get_y()[i];
feature_position[curr_index, 1][i] = feature2.get_y()[i];
}
// distances between features in image coordinates
float dist_u = position1[0] - position2[0];
float dist_v = position1[1] - position2[1];
marked_for_deletion = false;
for (i = 0; i < no_of_points; i++)
{
int x = (int)(position1[0] + (i * dist_u / no_of_points));
if ((x > 0) && (x < img.width))
{
int y = (int)(position1[1] + (i * dist_v / no_of_points));
if ((y > 0) && (y < img.height))
{
pixel_intensity[i, curr_index] = img.image[x, y];
}
else
{
marked_for_deletion = true;
}
}
else
{
marked_for_deletion = true;
}
}
curr_index++;
if (curr_index >= history)
{
curr_index = 0;
}
if (hits < 254)
{
hits++;
}
}
/// <summary>
/// For a single feature, work out the predicted feature measurement and the
/// Jacobians with respect to the vehicle position and the feature position.
/// This calls the appropriate member functions in Feature to set the measurement
/// \vct{h}, the feature location Jacobian \partfracv{h}{y}, robot
/// position Jacobian \partfrac{\vct{h}}{\vct{x}_v}, measurement covariance
/// \mat{R} and innovation covariance \mat{S} respectively.
/// </summary>
/// <param name="sfp"></param>
public void predict_single_feature_measurements(Feature sfp)
{
motion_model.func_xp(xv);
Vector xpRES = motion_model.get_xpRES();
sfp.get_fully_initialised_feature_measurement_model().func_hi_and_dhi_by_dxp_and_dhi_by_dyi(sfp.get_y(),xpRES);
sfp.set_h(sfp.get_fully_initialised_feature_measurement_model().get_hiRES());
sfp.set_dh_by_dy(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dyiRES());
motion_model.func_dxp_by_dxv(xv);
sfp.set_dh_by_dxv(sfp.get_fully_initialised_feature_measurement_model().get_dhi_by_dxpRES() * motion_model.get_dxp_by_dxvRES());
sfp.get_fully_initialised_feature_measurement_model().func_Ri(sfp.get_h());
sfp.set_R(sfp.get_fully_initialised_feature_measurement_model().get_RiRES());
sfp.get_fully_initialised_feature_measurement_model().func_Si(Pxx, sfp.get_Pxy(), sfp.get_Pyy(),
sfp.get_dh_by_dxv(), sfp.get_dh_by_dy(), sfp.get_R());
sfp.set_S(sfp.get_fully_initialised_feature_measurement_model().get_SiRES());
}
