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 double Cost(Image <Arthmetic, double> F, double fx, double fy, double px, double py, double w, double h) { Image <Arthmetic, double> E = ComputeMatrix.E(F, ComputeMatrix.K(fx, fy, px, py)); Svd svd = new Svd(E); double s1 = svd.S[0, 0]; double s2 = svd.S[1, 0]; double errS = s2 == 0 ? 1.0 : (s1 - s2) / s2; return(errS); }
private void EigenRatioForKnownK_Click(object sender, RoutedEventArgs e) { PlotFunctionForErrors(() => { var F = ComputeF(); var E = ComputeMatrix.E(F, K); var svd = new Svd(E); double error = svd.S[1, 0] / svd.S[0, 0]; return(error); }, "s2 / s1"); }
public static double Cost(List <Image <Arthmetic, double> > Fs, double fx, double fy, double px, double py, double w, double h) { double errS = 0; foreach (var F in Fs) { Image <Arthmetic, double> E = ComputeMatrix.E(F, ComputeMatrix.K(fx, fy, px, py)); Svd svd = new Svd(E); double s1 = svd.S[0, 0]; double s2 = svd.S[1, 0]; errS += s2 == 0 ? 1 : (s1 - s2) / s2; } return(errS); }
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); }