/// <summary>
///
/// </summary>
/// <param name="storage">Memory storage used for pyramid and derivative calculation.
/// By using storage instead images if using sequential image input (e.g. frame by frame video) </param>
/// <param name="prevFeatures">Previous features.</param>
/// <param name="currFeatures">Current features.</param>
/// <param name="status">Feature status.</param>
/// <param name="windowSize">Aperture size.</param>
/// <param name="iterations">Maximal number of iterations. If <paramref name="minFeatureShift"/> is reached then number of iterations will be lower.</param>
/// <param name="minFeatureShift">Minimal feature shift in horizontal or vertical direction.</param>
/// <param name="minEigenValue">Minimal eigen value.
/// Eigen values could be interpreted as lengths of ellipse's axes.
/// If zero ellipse turn into line therefore there is no corner.</param>
/// <param name="initialEstimate">Initial estimate shifts input features by specified amount. Default is zero.</param>
/// <param name="storagePyrLevel">Used pyramid level for the input storage. Default is zero.</param>
public static void EstimateFlow(PyrLKStorage <TColor> storage,
PointF[] prevFeatures, out PointF[] currFeatures,
out KLTFeatureStatus[] status,
int windowSize = 15, int iterations = 30, float minFeatureShift = 0.1f, float minEigenValue = 0.01f,
PointF[] initialEstimate = null, int storagePyrLevel = 0)
{
LKOpticalFlow <TColor> .storage = storage;
LKOpticalFlow <TColor> .storagePyrLevel = storagePyrLevel;
initialEstimate = initialEstimate ?? new PointF[prevFeatures.Length];
PointF[] _currFeatures = new PointF[prevFeatures.Length];
KLTFeatureStatus[] _status = new KLTFeatureStatus[prevFeatures.Length];
float[] _error = new float[prevFeatures.Length];
Parallel.For(0, prevFeatures.Length, (int featureIdx) =>
//for (int featureIdx = 0; featureIdx < prevFeatures.Length; featureIdx++)
{
PointF currFeature;
KLTFeatureStatus featureStatus;
EstimateFeatureFlow(storage.PrevImgPyr[storagePyrLevel], storage.CurrImgPyr[storagePyrLevel],
prevFeatures[featureIdx], out currFeature, out featureStatus, windowSize, iterations, minFeatureShift, minEigenValue, initialEstimate[featureIdx]);
lock (_currFeatures)
lock (_error)
lock (_status)
{
_currFeatures[featureIdx] = currFeature;
_status[featureIdx] = featureStatus;
}
});
currFeatures = _currFeatures;
status = _status;
}
/// <summary>
/// Estimates LK optical flow.
/// </summary>
/// <param name="storage">Used storage. Number of pyramid levels is specified within storage. Use storage to gain performance in video* by 2x! </param>
/// <param name="prevFeatures">Previous features.</param>
/// <param name="currFeatures">Current features.</param>
/// <param name="status">Feature status.</param>
/// <param name="windowSize">Aperture size.</param>
/// <param name="iterations">Maximal number of iterations. If <paramref name="minFeatureShift"/> is reached then number of iterations will be lower.</param>
/// <param name="minFeatureShift">Minimal feature shift in horizontal or vertical direction.</param>
/// <param name="minEigenValue">Minimal eigen value.
/// Eigen values could be interpreted as lengths of ellipse's axes.
/// If zero ellipse turn into line therefore there is no corner.</param>
public static void EstimateFlow(PyrLKStorage <TColor> storage,
PointF[] prevFeatures, out PointF[] currFeatures,
out KLTFeatureStatus[] status,
int windowSize = 15, int iterations = 30, float minFeatureShift = 0.1f, float minEigenValue = 0.001f)
{
var initialEstimate = new PointF[prevFeatures.Length];
currFeatures = new PointF[prevFeatures.Length];
status = new KLTFeatureStatus[prevFeatures.Length];
var scaledPrevFeatures = prevFeatures.Apply(x => x.DownScale(storage.PyrLevels));
var usedIndicies = Enumerable.Range(0, prevFeatures.Length).ToArray();
for (int pyrLevel = storage.PyrLevels; pyrLevel >= 0; pyrLevel--)
{
PointF[] levelCurrFeatures;
KLTFeatureStatus[] levelStatus;
LKOpticalFlow <TColor> .EstimateFlow(storage,
scaledPrevFeatures.GetAt(usedIndicies), out levelCurrFeatures, out levelStatus,
windowSize, iterations, minFeatureShift, minEigenValue, initialEstimate, pyrLevel);
//update data
currFeatures.SetAt(usedIndicies, levelCurrFeatures);
status.SetAt(usedIndicies, levelStatus);
if (pyrLevel != 0)
{
scaledPrevFeatures.ApplyInPlace(usedIndicies, x => x.UpScale());
currFeatures.ApplyInPlace(usedIndicies, x => x.UpScale());
initialEstimate = Sub(currFeatures, scaledPrevFeatures);
usedIndicies = getValidFeatureIndicies(status);
}
}
}
private static void EstimateFeatureFlow(TColor[,] prevImg, TColor[,] currImg,
PointF prevFeature,
out PointF currFeature, out KLTFeatureStatus featureStatus,
int windowSize, int iterations, float minFeatureShift, float minEigenValue, PointF initialEstimate = default(PointF))
{
minEigenValue = System.Math.Max(1E-4f, minEigenValue);
currFeature = new PointF(prevFeature.X + initialEstimate.X, prevFeature.Y + initialEstimate.Y); //move searching window (used by pyrOpticalFlow)
var prevFeatureArea = getFeatureArea(prevFeature, windowSize);
if (isInsideImage(prevFeatureArea, prevImg.Size()) == false)
{
featureStatus = KLTFeatureStatus.OutOfBounds;
return;
}
var prevPatch = prevImg.GetRectSubPix(prevFeatureArea);
var AtA = calcHessian(prevFeatureArea);
//check min eigen-value
if (hasValidEigenvalues(AtA, windowSize, minEigenValue) == false)
{
featureStatus = KLTFeatureStatus.SmallEigenValue;
return;
}
double det = AtA[0, 0] * AtA[1, 1] - AtA[0, 1] * AtA[0, 1];
TColor[,] currPatch;
for (int iter = 0; iter < iterations; iter++)
{
var currFeatureArea = getFeatureArea(currFeature, windowSize);
if (isInsideImage(currFeatureArea, prevImg.Size()) == false) // see if it moved outside of the image
{
featureStatus = KLTFeatureStatus.OutOfBounds;
return;
}
currPatch = currImg.GetRectSubPix(currFeatureArea);
var Atb = calcE(prevPatch, currPatch, currFeatureArea);
//solve for D
var dx = (AtA[1, 1] * Atb[0] - AtA[0, 1] * Atb[1]) / det;
var dy = (AtA[0, 0] * Atb[1] - AtA[0, 1] * Atb[0]) / det;
currFeature.X += (float)dx;
currFeature.Y += (float)dy;
// see if it has moved more than possible if it is really tracking a target
// this happens in regions with little texture
if (System.Math.Abs(currFeature.X - prevFeature.X) > windowSize ||
System.Math.Abs(currFeature.Y - prevFeature.Y) > windowSize)
{
featureStatus = KLTFeatureStatus.Drifted;
return;
}
if (System.Math.Abs(dx) < minFeatureShift && System.Math.Abs(dy) < minFeatureShift)
{
break;
}
}
featureStatus = KLTFeatureStatus.Success;
return;
}
