private void UdpateFrame(int n)
{
if (Frames == null || n >= Frames.Count - Step || n < 0)
{
isRunning = false;
nextFrameTimer.Stop();
return;
}
Dispatcher.BeginInvoke((Action)(() =>
{
currentFrame = n;
try
{
var frame = frames[n];
var frame2 = frames[n + Step];
frame = Undistort(frame);
frame2 = Undistort(frame2);
var mat = frame.ToImage <Bgr, byte>();
var mat2 = frame2.ToImage <Bgr, byte>();
double maxDistance = MaxDistance(frame);
Func <int, int, MatchingResult> matcher = (i1, i2) =>
{
if (!features.TryGetValue(i1, out var features1))
{
MatchImagePair.FindFeatures(frames[i1], Detector, Descriptor, out MKeyPoint[] kps1, out Mat desc1);
features1 = new MatchingResult()
{
LeftKps = kps1,
LeftDescriptors = desc1
};
}
if (!features.TryGetValue(i2, out var features2))
{
MatchImagePair.FindFeatures(frames[i2], Detector, Descriptor, out MKeyPoint[] kps2, out Mat desc2);
features2 = new MatchingResult()
{
LeftKps = kps2,
LeftDescriptors = desc2
};
}
return(MatchImagePair.Match(features1.LeftKps, features1.LeftDescriptors, features2.LeftKps, features2.LeftDescriptors, DistanceType, maxDistance));
};
OdometerFrame odometerFrame = scaler.NextFrame(n, n + Step, matcher);
// OdometerFrame odometerFrame = FindTransformation.GetOdometerFrame(mat.Mat, mat2.Mat, Detector, Descriptor, DistanceType, maxDistance, K);
if (odometerFrame != null)
{
videoViewer.Source = ImageLoader.ImageSourceForBitmap(frame.Bitmap);
recursive = true;
frameProgression.Value = n;
recursive = false;
frameCurrentLabel.Content = n;
totalRotation = odometerFrame.RotationMatrix.Multiply(totalRotation);
var rotationEuler = RotationConverter.MatrixToEulerXYZ(totalRotation);
totalTranslation = totalTranslation + odometerFrame.Translation;
infoComputed.Text = FormatInfo(odometerFrame.Translation, odometerFrame.Rotation, "Comp Diff");
infoComputedCumulative.Text = FormatInfo(totalTranslation, rotationEuler, "Comp Cumulative");
infoK.Text = FormatInfoK(odometerFrame);
MatchDrawer.DrawFeatures(mat.Mat, mat2.Mat, odometerFrame.Match, TakeBest, matchedView);
}
}
catch (Exception e)
{
infoComputed.Text = "Error!";
}
if (isRunning)
{
nextFrameTimer.Start();
}
}));
}
