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()); }