// Set the current innovation covariance S_i^{-1} for this
// particle. Called from
// Scene_Single::predict_partially_initialised_feature_measurements()
public void set_S(MatrixFixed Si)
{
Cholesky local_Si_cholesky = new Cholesky(Si);
m_SInv.Update(local_Si_cholesky.Inverse());
m_detS = SceneLib.Determinant(Si);
}
/// <summary>
/// Search for the image patch over all of the ellipses registered with the class.
/// Since correlation is expensive we locally cache the correlation results so that
/// we only do it once at each image location.
/// </summary>
public virtual void search()
{
// Set all positions to impossible correlation value
for (int x = 0; x < m_image.width; x++)
{
for (int y = 0; y < m_image.height; y++)
{
correlation_image[x, y] = -1.0f;
}
}
// Now, we loop over ellipses
foreach (SearchDatum i in m_searchdata)
{
// Limits of search
int urelstart = -(int)(i.halfwidth);
int urelfinish = (int)(i.halfwidth);
int vrelstart = -(int)(i.halfheight);
int vrelfinish = (int)i.halfheight;
int ucentre = (int)(i.search_centre[0]);
int vcentre = (int)(i.search_centre[1]);
// Check these limits aren't outside the image
if (ucentre + urelstart - (int)(m_boxsize - 1) / 2 < 0)
{
urelstart = (int)(m_boxsize - 1) / 2 - ucentre;
}
if (ucentre + urelfinish - (int)(m_boxsize - 1) / 2 >
(int)(m_image.width) - (int)(m_boxsize))
{
urelfinish = (int)(m_image.width) - (int)(m_boxsize) -
ucentre + (int)(m_boxsize - 1) / 2;
}
if (vcentre + vrelstart - (int)(m_boxsize - 1) / 2 < 0)
{
vrelstart = (int)(m_boxsize - 1) / 2 - vcentre;
}
if (vcentre + vrelfinish - (int)(m_boxsize - 1) / 2 >
(int)(m_image.height) - (int)(m_boxsize))
{
vrelfinish = (int)(m_image.height) - (int)(m_boxsize) -
vcentre + (int)(m_boxsize - 1) / 2;
}
// Search counters
int urel, vrel;
float corrmax = 1000000.0f;
float corr;
// For passing to and_correlate2_warning
float sdpatch = 0, sdimage = 0;
// Do the search
for (urel = urelstart; urel <= urelfinish; urel++)
{
for (vrel = vrelstart; vrel <= vrelfinish; vrel++)
{
if (i.inside_relative(urel, vrel))
{
//if (DEBUGDUMP) cout << "Searching at "
// << ucentre + urel << ", "
// << vcentre + vrel
// << endl;
// We are inside ellipse
// Has this place been searched before?
float corr_ptr = correlation_image[ucentre + urel, vcentre + vrel];
if (corr_ptr != -1.0)
{
corr = corr_ptr;
//if (DEBUGDUMP) cout << "Searched before, score " << corr << endl;
}
else
{
corr = SceneLib.correlate2_warning(0, 0, (int)m_boxsize, (int)m_boxsize,
ucentre + urel - (int)(m_boxsize - 1) / 2,
vcentre + vrel - (int)(m_boxsize - 1) / 2,
ref m_patch, ref m_image, ref sdpatch, ref sdimage);
if (sdimage < SceneLib.CORRELATION_SIGMA_THRESHOLD)
{
//if (DEBUGDUMP) cout << "Low image sigma sdimage " << sdimage << endl;
corr += SceneLib.LOW_SIGMA_PENALTY;
}
correlation_image[ucentre + urel, vcentre + vrel] = corr;
//if (DEBUGDUMP) cout << "New search, score " << corr << endl;
}
if (corr <= corrmax)
{
corrmax = corr;
i.result_u = (uint)(urel + ucentre);
i.result_v = (uint)(vrel + vcentre);
}
}
}
}
//Debug.WriteLine("Best match correlation: " + corrmax);
// Threshold correlation score: check if good enough
if (corrmax > SceneLib.CORRTHRESH2)
{
// cout << "Matching correlation not good enough." << endl;
i.result_flag = false;
}
else
{
i.result_flag = true;
}
//if (DEBUGDUMP) cout << "Search " << count++ << " correlation " << corrmax
//<< " location " << i.result_u << ", "
//<< i.result_v << endl;
}
}
