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