private void ApplyNoise(double stddev)
{
pts1 = new List <PointF>();
pts2 = new List <PointF>();
pts3 = new List <PointF>();
Random rand = new Random(seed);
for (int i = 0; i < pointsCount; ++i)
{
var real = ptsReal[i];
var i1 = pts1Ref[i];
var i2 = pts2Ref[i];
var i3 = pts3Ref[i];
i1 = new PointF(i1.X + Noise(stddev, rand), i1.Y + Noise(stddev, rand));
pts1.Add(i1);
i2 = new PointF(i2.X + Noise(stddev, rand), i2.Y + Noise(stddev, rand));
pts2.Add(i2);
i3 = new PointF(i3.X + Noise(stddev, rand), i3.Y + Noise(stddev, rand));
pts3.Add(i3);
}
pts1_n = new List <PointF>(pts1);
pts2_n = new List <PointF>(pts2);
pts3_n = new List <PointF>(pts3);
FindTransformation.NormalizePoints2d(pts1_n, out N1);
FindTransformation.NormalizePoints2d(pts2_n, out N2);
FindTransformation.NormalizePoints2d(pts3_n, out N3);
}
private void ErrorOfRComputation_Click(object sender, RoutedEventArgs e)
{
Func <double> testFunc = () =>
{
var F = ComputeF();
var E = ComputeMatrix.E(F, K);
FindTransformation.DecomposeToRTAndTriangulate(pts1, pts2, K, E, out var R, out var t, out var pts3d);
return(FindRError(R));
};
public static void FindBestScale(Matrix R12, Matrix t12, Matrix R23, Matrix t23, Matrix K,
List <PointF> pts1, List <PointF> pts2, List <PointF> pts3, int minSampleSize,
out double scale, out double confidence, out List <int> inliers)
{
FindTransformation.TriangulateChieral(pts1, pts2, K, R12, t12, out var est3d_12);
FindTransformation.TriangulateChieral(pts2, pts3, K, R23, t23, out var est3d_23);
// Find best scale, so that both sets will be closest
RansacScaleEstimation ransacModel = new RansacScaleEstimation(est3d_12, est3d_23, R12, ComputeMatrix.Center(t12, R12));
int sampleSize = Math.Max(minSampleSize, (int)(0.05 * pts1.Count));
int minGoodPoints = (int)(0.4 * pts1.Count);
int maxIterations = 100;
double meanRefPointSize = GetMeanSize(est3d_12);
double threshold = meanRefPointSize * meanRefPointSize * 0.08;
var result = RANSAC.ProcessMostInliers(ransacModel, maxIterations, sampleSize, minGoodPoints, threshold, 1.0);
scale = (double)result.BestModel;
inliers = result.Inliers;
var backprojected23to12 = TransfromBack3dPoints(R12, t12, est3d_23, scale);
Image <Arthmetic, double> inliersOnly12 = new Image <Arthmetic, double>(result.Inliers.Count, 4);
Image <Arthmetic, double> inliersOnly23 = new Image <Arthmetic, double>(result.Inliers.Count, 4);
for (int i = 0; i < result.Inliers.Count; ++i)
{
int k = result.Inliers[i];
for (int j = 0; j < 4; ++j)
{
inliersOnly12[j, i] = est3d_12[j, k];
inliersOnly23[j, i] = backprojected23to12[j, k];
}
}
// Find errors
var distances = inliersOnly12.Sub(inliersOnly23);
double meanDistance = distances.Norm / distances.Cols;
double meanSize = GetMeanSize(inliersOnly12, inliersOnly23);
double relativeMeanDistance = meanDistance / meanSize;
double error = result.BestError;
double relativeError = error / (meanSize * meanSize);
Errors.TraingulationError(inliersOnly12, inliersOnly23, out double mean, out double median, out List <double> errs);
confidence = (double)inliers.Count / (double)pts1.Count;
}
public void ProcessImages(Mat left, Mat right, Feature2D detector, Feature2D descriptor, DistanceType distanceType, double takeBest)
{
var match = MatchImagePair.Match(left, right, detector, descriptor, distanceType, 20.0);
DrawFeatures(left, right, match, takeBest);
var lps = match.LeftPointsList.Take((int)(match.LeftPoints.Size * takeBest));
var rps = match.RightPointsList.Take((int)(match.RightPoints.Size * takeBest));
var F = ComputeMatrix.F(new VectorOfPointF(lps.ToArray()), new VectorOfPointF(rps.ToArray()));
var K = EstimateCameraFromImagePair.K(F, left.Width, right.Height);
var E = ComputeMatrix.E(F, K);
FindTransformation.DecomposeToRTAndTriangulate(lps.ToList(), rps.ToList(), K, E,
out Image <Arthmetic, double> R, out Image <Arthmetic, double> t, out Image <Arthmetic, double> X);
PrintMatricesInfo(E, K, R, t);
}
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();
}
}));
}
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);
}
public static OdometerFrame GetOdometerFrame3(
Mat left, Mat middle, Mat right, double lastScale, out double thisScale,
Feature2D detector, Feature2D descriptor, DistanceType distanceType, double maxDistance,
Image <Arthmetic, double> K, double takeBest = 1.0)
{
thisScale = lastScale;
var match12 = MatchImagePair.Match(left, middle, detector, descriptor, distanceType, maxDistance);
var match23 = MatchImagePair.Match(middle, right, detector, descriptor, distanceType, maxDistance);
var match13 = MatchImagePair.Match(left, right, detector, descriptor, distanceType, maxDistance);
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);
TripletMatch tmatch = new TripletMatch();
List <MDMatch> m12 = new List <MDMatch>();
List <MDMatch> m23 = new List <MDMatch>();
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], maxDistance))
{
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());
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 || F13 == null)
{
return(null);
}
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.DecomposeToRT(Es[1], out Image<Arthmetic, double> R23, out Image<Arthmetic, double> t23);
FindTransformation.DecomposeToRTAndTriangulate(tmatch.Left, tmatch.Right, K, Es[1],
out Image <Arthmetic, double> R13, out Image <Arthmetic, double> t13, out Image <Arthmetic, double> X13);
var Rs = new List <Image <Arthmetic, double> >
{
R12,
R13
};
var ts = new List <Image <Arthmetic, double> >
{
t12,
t13
};
var cc = ComputeCameraCenter3(K, Rs, ts, tmatch);
OdometerFrame odometerFrame = new OdometerFrame();
odometerFrame.Rotation = RotationConverter.MatrixToEulerXYZ(Rs[0]);
odometerFrame.RotationMatrix = Rs[0];
odometerFrame.MatK = K;
odometerFrame.Match = match12;
// Image<Arthmetic, double> C = ComputeCameraCenter(R, t, K, match);
// odometerFrame.Translation = R.Multiply(C);
// odometerFrame.Translation = R.T().Multiply(odometerFrame.Translation);
odometerFrame.Translation = ts[0].Mul(lastScale / ts[0].Norm);
odometerFrame.Center = lastScale * cc.C12;
thisScale = cc.Ratio3To2;
return(odometerFrame);
}
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);
}
