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