private void UdpateFrame(int n)
{
if (Frames == null || n >= Frames.Count - Step || n < 0)
{
isRunning = false;
nextFrameTimer.Stop();
return;
}
Dispatcher.BeginInvoke((Action)(() =>
{
currentFrame = n;
var frame = frames[n];
var frame2 = frames[n + Step];
var mat = CvInvoke.Imread(frame.ImageFile, Emgu.CV.CvEnum.ImreadModes.Color).ToImage <Bgr, byte>();
var mat2 = CvInvoke.Imread(frame2.ImageFile, Emgu.CV.CvEnum.ImreadModes.Color).ToImage <Bgr, byte>();
try
{
double maxDistance = 20.0;
OdometerFrame odometerFrame = FindTransformation.GetOdometerFrame(mat.Mat, mat2.Mat, Detector, Descriptor, DistanceType, maxDistance, K);
if (odometerFrame != null)
{
videoViewer.Source = new BitmapImage(new Uri(frame.ImageFile, UriKind.Absolute));
recursive = true;
frameProgression.Value = n;
recursive = false;
frameCurrentLabel.Content = n;
totalRotation = odometerFrame.RotationMatrix.Multiply(totalRotation);
var rotationEuler = RotationConverter.MatrixToEulerXYZ(totalRotation);
totalTranslation = totalTranslation + odometerFrame.Translation;
var refTranslation = frame2.Odometry.Translation.Sub(frames[0].Odometry.Translation);
var refRotation = frames[0].Odometry.RotationMatrix.T().Multiply(frame2.Odometry.RotationMatrix);
var refRotationEuler = RotationConverter.MatrixToEulerXYZ(refRotation);
var refTranslationDiff = frame2.Odometry.Translation.Sub(frame.Odometry.Translation);
var refRotationDiff = frame.Odometry.RotationMatrix.T().Multiply(frame2.Odometry.RotationMatrix);
var refRotationDiffEuler = RotationConverter.MatrixToEulerXYZ(refRotationDiff);
infoReference.Text = FormatInfo(refTranslation, refRotationEuler, "Ref Cumulative");
infoReferenceDiff.Text = FormatInfo(refTranslationDiff, refRotationDiffEuler, "Ref Diff");
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();
}
}));
}
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();
}
}));
}
public void ProcessImages(Mat left, Mat middle, Mat right, Feature2D detector, Feature2D descriptor, DistanceType distance)
{
double maxDistance = 20.0;
var match12 = MatchImagePair.Match(left, middle, detector, descriptor, distance, maxDistance);
var match23 = MatchImagePair.Match(middle, right, detector, descriptor, distance, maxDistance);
var match13 = MatchImagePair.Match(left, right, detector, descriptor, distance, maxDistance);
TripletMatch tmatch = new TripletMatch();
List <MDMatch> m12 = new List <MDMatch>();
List <MDMatch> m23 = new List <MDMatch>();
var left1 = match12.LeftPoints;
var right1 = match12.RightPoints;
var left2 = match23.LeftPoints;
var left2_X = MatchClosePoints.SortByX(match23.LeftPoints);
var right2 = match23.RightPoints;
var left3 = match13.LeftPoints;
var right3 = match13.RightPoints;
var right3_X = MatchClosePoints.SortByX(match13.LeftPoints);
for (int idx12 = 0; idx12 < left1.Size; ++idx12)
{
var p1 = left1[idx12];
var p2 = right1[idx12];
int idx23 = IndexOf_X(left2_X, p2);
if (idx23 != -1)
{
var p3 = right2[idx23];
int idx13 = IndexOf_X(right3_X, p1);
if (idx13 != -1)
{
if (AreEqual(left1[idx12], left3[idx13]))
{
tmatch.Left.Add(p1);
tmatch.Middle.Add(p2);
tmatch.Right.Add(p3);
m12.Add(match12.Matches[idx12]);
m23.Add(match23.Matches[idx23]);
}
}
}
}
match12.Matches = new VectorOfDMatch(m12.ToArray());
match23.Matches = new VectorOfDMatch(m23.ToArray());
MatchDrawer.DrawFeatures(left, right, match12, 1.0, bottomView);
MatchDrawer.DrawFeatures(left, right, match23, 1.0, upperView);
var F12 = ComputeMatrix.F(new VectorOfPointF(tmatch.Left.ToArray()), new VectorOfPointF(tmatch.Middle.ToArray()));
var F23 = ComputeMatrix.F(new VectorOfPointF(tmatch.Middle.ToArray()), new VectorOfPointF(tmatch.Right.ToArray()));
var F13 = ComputeMatrix.F(new VectorOfPointF(tmatch.Left.ToArray()), new VectorOfPointF(tmatch.Right.ToArray()));
if (F12 == null || F23 == null || F13 == null)
{
info.Text = "Too few matches";
return;
}
var Fs = new List <Image <Arthmetic, double> > {
F12, F23, F13
};
var K = EstimateCameraFromImageSequence.K(Fs, left.Width, right.Height);
var Es = new List <Image <Arthmetic, double> >
{
ComputeMatrix.E(F12, K),
ComputeMatrix.E(F23, K),
ComputeMatrix.E(F13, K)
};
FindTransformation.DecomposeToRTAndTriangulate(tmatch.Left, tmatch.Middle, K, Es[0],
out Image <Arthmetic, double> R12, out Image <Arthmetic, double> t12, out Image <Arthmetic, double> X12);
FindTransformation.DecomposeToRTAndTriangulate(tmatch.Middle, tmatch.Right, K, Es[1],
out Image <Arthmetic, double> R23, out Image <Arthmetic, double> t23, out Image <Arthmetic, double> X23);
FindTransformation.DecomposeToRTAndTriangulate(tmatch.Left, tmatch.Right, K, Es[2],
out Image <Arthmetic, double> R13, out Image <Arthmetic, double> t13, out Image <Arthmetic, double> X13);
var Rs = new List <Image <Arthmetic, double> >
{
RotationConverter.MatrixToEulerXYZ(R12),
RotationConverter.MatrixToEulerXYZ(R23),
RotationConverter.MatrixToEulerXYZ(R13)
};
var ts = new List <Image <Arthmetic, double> >
{
t12,
t23,
t13
};
PrintMatricesInfo(Es, K, Rs, ts);
}
private void DrawFeatures(Mat left, Mat right, MatchingResult match, double takeBest)
{
MatchDrawer.DrawFeatures(left, right, match, takeBest, macthedView);
MatchDrawer.DrawCricles(leftView, left, match.LeftKps);
MatchDrawer.DrawCricles(rightView, right, match.RightKps);
}
