private Image <Arthmetic, double> ComputeF()
{
var F = ComputeMatrix.F(new VectorOfPointF(pts1_n.ToArray()), new VectorOfPointF(pts2_n.ToArray()));
// F is normalized - lets denormalize it
F = N2.T().Multiply(F).Multiply(N1);
return(F);
}
public static Image <Arthmetic, double> K(List <Mat> mats, Feature2D detector, Feature2D descriptor, DistanceType distanceType, double maxDistance)
{
List <Image <Arthmetic, double> > Fs = new List <Image <Arthmetic, double> >();
for (int i = 0; i < mats.Count - 1; i += 2)
{
var match = MatchImagePair.Match(mats[i], mats[i + 1], detector, descriptor, distanceType, maxDistance);
var F = ComputeMatrix.F(match.LeftPoints, match.RightPoints);
if (F == null)
{
continue;
}
Fs.Add(F);
}
return(K(Fs, mats[0].Width, mats[0].Height));
}
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);
}
public static bool FindTwoViewsMatrices(List <PointF> left, List <PointF> right, Matrix K, out Matrix F, out Matrix E, out Matrix R, out Matrix t, out Matrix X)
{
var lps_n = new List <PointF>(left);
var rps_n = new List <PointF>(right);
NormalizePoints2d(lps_n, out var NL);
NormalizePoints2d(rps_n, out var NR);
F = ComputeMatrix.F(new VectorOfPointF(lps_n.ToArray()), new VectorOfPointF(rps_n.ToArray()));
if (F == null)
{
E = R = t = X = null;
return(false);
}
F = NR.T().Multiply(F).Multiply(NL);
E = ComputeMatrix.E(F, K);
DecomposeToRTAndTriangulate(left, right, K, E, out R, out t, out X);
return(true);
}
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);
}
