public static List <MatchPair> FindCorrespondences12to23(MatchingResult match12, MatchingResult match23)
{
Dictionary <int, int> kpIndexToMatchIndexOld = new Dictionary <int, int>(match12.Matches.Size);
for (int i = 0; i < match12.Matches.Size; ++i)
{
kpIndexToMatchIndexOld[match12.Matches[i].TrainIdx] = i;
}
List <MatchPair> correspondences = new List <MatchPair>(match12.Matches.Size / 2);
for (int i = 0; i < match23.Matches.Size; ++i)
{
int kpIndex = match23.Matches[i].QueryIdx;
if (kpIndexToMatchIndexOld.TryGetValue(kpIndex, out int oldIndex))
{
var oldMatch = match12.Matches[oldIndex];
var newMatch = match23.Matches[i];
correspondences.Add(new MatchPair()
{
Match12 = oldMatch,
Match23 = newMatch,
Kp1 = match12.LeftKps[oldMatch.QueryIdx],
Kp2 = match23.LeftKps[newMatch.QueryIdx],
Kp3 = match23.RightKps[newMatch.TrainIdx]
});
}
}
return(correspondences);
}
public static void DrawFeatures(Mat left, Mat right, MatchingResult match, double takeBest, ImageViewer macthedView)
{
Mat matchesImage = new Mat();
VectorOfVectorOfDMatch matches2 = new VectorOfVectorOfDMatch();
VectorOfKeyPoint vectorOfKp2 = new VectorOfKeyPoint(match.LeftKps);
VectorOfKeyPoint vectorOfKp1 = new VectorOfKeyPoint(match.RightKps);
matches2.Push(new VectorOfDMatch(match.Matches.ToArray().OrderBy((x) => x.Distance).Take((int)(match.Matches.Size * takeBest)).ToArray()));
// Features2DToolbox.DrawMatches(left, vectorOfKp1, right, vectorOfKp2, matches2, matchesImage, new Bgr(Color.Red).MCvScalar, new Bgr(Color.Blue).MCvScalar);
Features2DToolbox.DrawMatches(right, vectorOfKp1, left, vectorOfKp2, matches2, matchesImage, new Bgr(Color.Red).MCvScalar, new Bgr(Color.Blue).MCvScalar);
macthedView.Source = ImageLoader.ImageSourceForBitmap(matchesImage.Bitmap);
}
public static List <MatchPair> FindCorrespondences12to23to31(MatchingResult match12, MatchingResult match23, MatchingResult match31)
{
var c12to23 = FindCorrespondences12to23(match12, match23);
var crossCorrespondences = new List <MatchPair>(c12to23.Count);
Dictionary <int, int> kpIndex3to1 = new Dictionary <int, int>(match31.Matches.Size);
for (int i = 0; i < match31.Matches.Size; ++i)
{
kpIndex3to1[match31.Matches[i].QueryIdx] = match31.Matches[i].TrainIdx;
}
foreach (var c in c12to23)
{
// Accept correspondences only if we have corresponding match from 3 to 1
int expectedKp1 = c.Match12.QueryIdx;
int expectedKp3 = c.Match23.TrainIdx;
if (kpIndex3to1.TryGetValue(expectedKp3, out int kp1) && kp1 == expectedKp1)
{
crossCorrespondences.Add(c);
}
}
return(crossCorrespondences);
}
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 OdometerFrame NextFrame(int left, int right, Func <int, int, MatchingResult> matcher)
{
MatchingResult match23 = matcher(left, right);
if (match23.Matches.Size < MinimumCorrespondencesNeeded)
{
// Track of points is lost, at least temporarly. Let's put handling lost-track case ou of scope for now.
lastGoodMatch = null;
isContinuous = false;
return(null);
}
OdometerFrame frame = new OdometerFrame()
{
MatK = K,
Match = match23,
Rotation = new Image <Arthmetic, double>(1, 3),
RotationMatrix = RotationConverter.EulerXYZToMatrix(new Image <Arthmetic, double>(1, 3)),
Translation = new Image <Arthmetic, double>(1, 3)
};
// 1) Determine if transformation between next frames has high enough baseline to be accurate
// 1a) For now lets determine it by finding if lone rotation is good enough
var H = FindTransformation.EstimateHomography(match23.LeftPointsList, match23.RightPointsList, K);
if (FindTransformation.IsPureRotation(H, RotationTreshold1, RotationTreshold2))
{
// 1c) If not then transformation is described only by rotation
// 1b) Find rotation and rotate all points in current set
isContinuous = false;
frame.Rotation = RotationConverter.MatrixToEulerXYZ(H);
frame.RotationMatrix = RotationConverter.EulerXYZToMatrix(frame.Rotation);
if (last3dPoints != null && R12 != null)
{
last3dPoints = Utils.PutRTo4x4(frame.RotationMatrix).Multiply(last3dPoints);
R12 = frame.RotationMatrix.Multiply(R12);
}
else
{
R12 = frame.RotationMatrix;
}
// 1c) Skip frame and wait for next one (but save matches)
return(frame);
}
// 2) We have legit frames
if (!FindTransformation.FindTwoViewsMatrices(match23.LeftPointsList, match23.RightPointsList, K,
out var F23, out var E23, out var R23, out var t23, out var X23))
{
// 3a) Or not
isContinuous = false;
return(null);
}
frame.Rotation = RotationConverter.MatrixToEulerXYZ(R23);
frame.RotationMatrix = R23;
frame.Translation = t23;
// 3) Find same points between old frame and current one
if (lastGoodMatch == null)
{
last3dPoints = X23;
isContinuous = true;
}
else
{
#region NonContinousCase
//if (!isContinuous) // This doesn't work well. Lets put it out of scope and just reset scale
// {
// Find correspondences between last right and new left
//var match12 = lastGoodMatch;
//var match34 = match23;
//var match23_ = matcher(lastGoodRightImage, left); // TODO: make use of already found feature points
//var correspondences23to34 = Correspondences.FindCorrespondences12to23(match23_, match34);
//// Now extend each correspondence to 4 points - find if point on 2 is matched to some point on 1
//var correspondences13to34 = new List<Correspondences.MatchPair>();
//foreach(var c in correspondences23to34)
//{
// var m23 = c.Match12;
// for (int i = 0; i < match12.Matches.Size; ++i)
// {
// if(match12.Matches[i].TrainIdx == m23.QueryIdx)
// {
// correspondences13to34.Add(new Correspondences.MatchPair()
// {
// Kp1 = match12.LeftKps[match12.Matches[i].QueryIdx],
// Kp2 = c.Kp2,
// Kp3 = c.Kp3
// });
// }
// }
//}
//if (correspondences13to34.Count >= MinimumCorrespondencesNeeded)
//{
// var t13 = R12.Multiply(c12).Mul(-1);
// FindBestScale(R12, t13, R23, t23, K, correspondences13to34, MinimumCorrespondencesNeeded, out double scale, out double confidence, out List<int> inliers);
// t23 = t23.Mul(scale);
// frame.Translation = t23;
// FindTransformation.TriangulateChieral(match23.LeftPointsList, match23.RightPointsList, K, R23, t23, out last3dPoints);
// isContinuous = true;
//}
//else
//{
// isContinuous = false;
//}
// }
#endregion
if (isContinuous)
{
var correspondences = Correspondences.FindCorrespondences12to23(lastGoodMatch, match23);
if (correspondences.Count >= MinimumCorrespondencesNeeded)
{
// Normalize to |t| = 1
t12 = t12.Mul(1.0 / t12.Norm);
t23 = t23.Mul(1.0 / t23.Norm);
FindBestScale(R12, t12, R23, t23, K, correspondences, MinimumCorrespondencesNeeded, out double scale, out double confidence, out List <int> inliers);
t23 = t23.Mul(scale);
frame.Translation = t23;
FindTransformation.TriangulateChieral(match23.LeftPointsList, match23.RightPointsList, K, R23, t23, out last3dPoints);
}
else
{
isContinuous = false;
}
}
}
lastGoodMatch = match23;
lastGoodLeftImage = left;
lastGoodRightImage = right;
R12 = R23;
t12 = t23;
c12 = R23.T().Multiply(t23).Mul(-1);
return(frame);
}
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);
}