/// <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]; }
// Function to find a region in an image guided by current motion prediction // which doesn't overlap existing features public static bool FindNonOverlappingRegion(Scene_Single scene, Vector local_u, float delta_t, Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation, uint camera_width, uint camera_height, uint BOXSIZE, ref int init_feature_search_ustart, ref int init_feature_search_vstart, ref int init_feature_search_ufinish, ref int init_feature_search_vfinish, uint FEATURE_INIT_STEPS_TO_PREDICT, float FEATURE_INIT_DEPTH_HYPOTHESIS, Random rnd) { ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model(); Vector local_xv = new Vector(scene.get_xv()); for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++) { scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t); local_xv.Update(scene.get_motion_model().get_fvRES()); } threed_motion_model.func_xp(local_xv); Vector local_xp = new Vector(threed_motion_model.get_xpRES()); threed_motion_model.func_r(local_xp); Vector3D rW = threed_motion_model.get_rRES(); threed_motion_model.func_q(local_xp); Quaternion qWR = threed_motion_model.get_qRES(); // yW = rW + RWR hR Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS); // Used Inverse + transpose because this was't compiling the normal way Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR); // Then project that point into the current camera position scene.get_motion_model().func_xp(scene.get_xv()); Fully_Initialised_Feature_Measurement_Model fully_init_fmm = (Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model); Vector yWVNL = yW.GetVNL3(); fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES()); // Now, this defines roughly how much we expect a feature initialised to move float suggested_u = fully_init_fmm.get_hiRES()[0]; float suggested_v = fully_init_fmm.get_hiRES()[1]; float predicted_motion_u = camera_width / 2.0f - suggested_u; float predicted_motion_v = camera_height / 2.0f - suggested_v; // So, the limits of a "safe" region within which we can initialise // features so that they end up staying within the screen // (Making the approximation that the whole screen moves like the // centre point) int safe_feature_search_ustart = (int)(-predicted_motion_u); int safe_feature_search_vstart = (int)(-predicted_motion_v); int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u); int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v); if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1)) { safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1); } if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1)) { safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1); } if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1)) { safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1); } if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1)) { safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1); } return(FindNonOverlappingRegionNoPredict(safe_feature_search_ustart, safe_feature_search_vstart, safe_feature_search_ufinish, safe_feature_search_vfinish, scene, ref init_feature_search_ustart, ref init_feature_search_vstart, ref init_feature_search_ufinish, ref init_feature_search_vfinish, rnd)); }
/// <summary> /// Function to find non-overlapping region over without prediction /// this is really the service function called by both the above /// </summary> /// <param name="safe_feature_search_ustart"></param> /// <param name="safe_feature_search_vstart"></param> /// <param name="safe_feature_search_ufinish"></param> /// <param name="safe_feature_search_vfinish"></param> /// <param name="scene"></param> /// <param name="init_feature_search_ustart"></param> /// <param name="init_feature_search_vstart"></param> /// <param name="init_feature_search_ufinish"></param> /// <param name="init_feature_search_vfinish"></param> /// <param name="rnd"></param> /// <returns></returns> public static bool FindNonOverlappingRegionNoPredict( int safe_feature_search_ustart, int safe_feature_search_vstart, int safe_feature_search_ufinish, int safe_feature_search_vfinish, Scene_Single scene, ref int init_feature_search_ustart, ref int init_feature_search_vstart, ref int init_feature_search_ufinish, ref int init_feature_search_vfinish, Random rnd) { int i, j; //if (Camera_Constants.DEBUGDUMP) cout << "FNOLRNP timer start: " << timerlocal << endl; int INIT_FEATURE_SEARCH_WIDTH = 80; int INIT_FEATURE_SEARCH_HEIGHT = 60; // Within this, choose a random region // Check that we've got some room for manouevre if ((safe_feature_search_ufinish - safe_feature_search_ustart > INIT_FEATURE_SEARCH_WIDTH) && (safe_feature_search_vfinish - safe_feature_search_vstart > INIT_FEATURE_SEARCH_HEIGHT)) { // Try a few times to get one that's not overlapping with any features // we know about int NUMBER_OF_RANDOM_INIT_FEATURE_SEARCH_REGION_TRIES = 5; int FEATURE_SEPARATION_MINIMUM = (int)Camera_Constants.BOXSIZE * 3; // Build vectors of feature positions so we only have to work them out once ArrayList u_array = new ArrayList(); ArrayList v_array = new ArrayList(); Feature it; for (j = 0; j < scene.get_feature_list().Count; j++) { it = (Feature)(scene.get_feature_list())[j]; //Vector z = it.get_z(); //u_array.Add(z[0]); //v_array.Add(z[1]); if (it.get_feature_measurement_model().fully_initialised_flag) { //Vector z = it.get_z(); //u_array.Add(z[0]); //v_array.Add(z[1]); Fully_Initialised_Feature_Measurement_Model fifmm = (Fully_Initialised_Feature_Measurement_Model)(it.get_feature_measurement_model()); fifmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(it.get_y(), scene.get_motion_model().get_xpRES()); // Check that this is not a feature behind the camera if (it.get_feature_measurement_model().feature_graphics_type == "THREED_POINT") { fifmm.func_zeroedyigraphics_and_Pzeroedyigraphics(it.get_y(), scene.get_xv(), scene.get_Pxx(), it.get_Pxy(), it.get_Pyy()); if (fifmm.get_zeroedyigraphicsRES()[2] > 0) { u_array.Add(fifmm.get_hiRES()[0]); v_array.Add(fifmm.get_hiRES()[1]); } } } } //if (Camera_Constants.DEBUGDUMP) cout << "FNOLRNP timer after functions: " << timerlocal << endl; bool feature_found = false; i = 0; while (i < NUMBER_OF_RANDOM_INIT_FEATURE_SEARCH_REGION_TRIES) { int u_offset = (int)((safe_feature_search_ufinish - safe_feature_search_ustart - INIT_FEATURE_SEARCH_WIDTH) * (rnd.Next(10000) / 10000.0f)); int v_offset = (int)((safe_feature_search_vfinish - safe_feature_search_vstart - INIT_FEATURE_SEARCH_HEIGHT) * (rnd.Next(10000) / 10000.0f)); init_feature_search_ustart = safe_feature_search_ustart + u_offset; init_feature_search_ufinish = init_feature_search_ustart + INIT_FEATURE_SEARCH_WIDTH; init_feature_search_vstart = safe_feature_search_vstart + v_offset; init_feature_search_vfinish = init_feature_search_vstart + INIT_FEATURE_SEARCH_HEIGHT; bool found_a_feature_in_region_flag = false; // These arrays will be the same size float uit, vit; for (j = 0; j < u_array.Count; j++) { uit = (float)u_array[j]; vit = (float)v_array[j]; if ((uit >= init_feature_search_ustart - FEATURE_SEPARATION_MINIMUM) && (uit < init_feature_search_ufinish + FEATURE_SEPARATION_MINIMUM) && (vit >= init_feature_search_vstart - FEATURE_SEPARATION_MINIMUM) && (vit < init_feature_search_vfinish + FEATURE_SEPARATION_MINIMUM)) { found_a_feature_in_region_flag = true; feature_found = true; break; } } if (!found_a_feature_in_region_flag) { break; } i++; } if (!feature_found) { return(false); } } else { return(false); } return(true); }