public override void func_hi_noisy(Vector yi_true, Vector xp_true) { func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yi_true, xp_true); hi_noisyRES.Put(0, SceneLib.SampleNormal(hiRES[0], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd)); hi_noisyRES.Put(1, SceneLib.SampleNormal(hiRES[1], ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).SD_IMAGE_SIMULATION, rnd)); }
/// <summary> /// Make a measurement of a feature. This function calls elliptical_search() to /// find the best match within three standard deviations of the predicted location. /// </summary> /// <param name="patch">The identifier for this feature (in this case an image patch)</param> /// <param name="z">The best image location match for the feature, to be filled in by this function</param> /// <param name="h">The expected image location</param> /// <param name="S">The expected location covariance, used to specify the search region.</param> /// <returns></returns> public override bool measure_feature(byte[] patch, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { Cholesky S_cholesky = new Cholesky(S); MatrixFixed Sinv = S_cholesky.Inverse(); uint u_found = 0, v_found = 0; if (SceneLib.elliptical_search(image, image_width, image_height, patch, patchwidth, patchwidth, h, Sinv, ref u_found, ref v_found, vz, Camera_Constants.BOXSIZE, outputimage, outputimage_width, outputimage_height, show_ellipses, calibrating, rnd) != true) { // Feature not successfully matched return(false); } z.Put(0, (float)u_found); z.Put(1, (float)v_found); return(true); }
/// <summary> /// Search a region for the best image patch, and set the current selection to /// this. This just calls find_best_patch_inside_region() to find a patch using the /// Shi and Tomasi criterion. /// </summary> /// <param name="ustart">The x-cordinate of the start of the region</param> /// <param name="vstart">The y-cordinate of the start of the region</param> /// <param name="ufinish">The x-cordinate of the end of the region</param> /// <param name="vfinish">The y-cordinate of the end of the region</param> /// <returns>The smallest eigenvalue of the best patch (high means good for correlation)</returns> public float set_image_selection_automatically(uint ustart, uint vstart, uint ufinish, uint vfinish) { float evbest = 0; SceneLib.find_best_patch_inside_region(image, ref uu, ref vv, ref evbest, Camera_Constants.BOXSIZE, ustart, vstart, ufinish, vfinish); location_selected_flag = true; //if (Camera_Constants.DEBUGDUMP) cout << "Found patch with score " << evbest << endl; return(evbest); }
/// <summary> /// Noisy process equation for simulation /// Simply perturb xv with Gaussian noise and send it through func_fv /// </summary> /// <param name="xv_true"></param> /// <param name="u_true"></param> /// <param name="delta_t"></param> public override void func_fv_noisy(Vector xv_true, Vector u_true, float delta_t) { Vector xv_noisy = xv_true; // Linear velocity for (int row = 7; row < 10; row++) { xv_noisy[row] = SceneLib.SampleNormal(xv_true[row], SD_A_component * delta_t, rnd); } // Angular velocity for (int row = 10; row < 13; row++) { xv_noisy[row] = SceneLib.SampleNormal(xv_true[row], SD_alpha_component * delta_t, rnd); } // Now send through normal process equaion func_fv_and_dfv_by_dxv(xv_noisy, u_true, delta_t); // And copy result fv_noisyRES.Update(fvRES); }
public override float selection_score(MatrixFixed Si) { // Return the trace of the innovation covariance return(SceneLib.Trace(Si)); }