public static void ReprojectionError( Image <Arthmetic, double> estReal, List <PointF> img, Image <Arthmetic, double> K, Image <Arthmetic, double> R, Image <Arthmetic, double> t, out double mean, out double median, out List <double> errors) { var P = ComputeMatrix.Camera(K, R, t); var estImg = P.Multiply(estReal); errors = new List <double>(); for (int i = 0; i < img.Count; ++i) { var estPoint = new Image <Arthmetic, double>(new double[, , ] { { { estImg[0, i] / estImg[2, i] } }, { { estImg[1, i] / estImg[2, i] } }, }); var realPoint = new Image <Arthmetic, double>(new double[, , ] { { { img[i].X } }, { { img[i].Y } }, }); errors.Add(estPoint.Sub(realPoint).Norm); } mean = errors.Sum() / errors.Count; median = errors[errors.Count / 2]; }
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); }
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 Matrix TransfromBack3dPoints(Matrix R, Matrix t, Matrix pts23, double scale) { var backprojected = Utils.PutRTo4x4(R.T()).Multiply(pts23); var C = ComputeMatrix.Center(t, R); for (int i = 0; i < backprojected.Cols; ++i) { backprojected[0, i] = backprojected[0, i] * scale + C[0, 0]; backprojected[1, i] = backprojected[1, i] * scale + C[1, 0]; backprojected[2, i] = backprojected[2, i] * scale + C[2, 0]; } return(backprojected); }
public static int TriangulateChieral( List <PointF> left, List <PointF> right, Image <Arthmetic, double> K, Image <Arthmetic, double> R, Image <Arthmetic, double> t, out Image <Arthmetic, double> pts3d) { // init 3d point matrix pts3d = new Image <Arthmetic, double>(left.Count, 4); // init projection matrices var P1 = ComputeMatrix.Camera(K); var P2 = ComputeMatrix.Camera(K, R, t); // Transform points lists into matrices var img1 = new Image <Arthmetic, double>(left.Count, 2); var img2 = new Image <Arthmetic, double>(left.Count, 2); for (int i = 0; i < left.Count; ++i) { img1[0, i] = left[i].X; img1[1, i] = left[i].Y; img2[0, i] = right[i].X; img2[1, i] = right[i].Y; } CvInvoke.TriangulatePoints(P1, P2, img1, img2, pts3d); // Scale points, so that W = 1 for (int i = 0; i < left.Count; ++i) { pts3d[0, i] = pts3d[0, i] / pts3d[3, i]; pts3d[1, i] = pts3d[1, i] / pts3d[3, i]; pts3d[2, i] = pts3d[2, i] / pts3d[3, i]; pts3d[3, i] = pts3d[3, i] / pts3d[3, i]; } // compute points in front of camera (TODO: how does it work?) var AX1 = P1.Multiply(pts3d); var BX1 = P2.Multiply(pts3d); int num = 0; for (int i = 0; i < left.Count; i++) { if (AX1[2, i] * pts3d[3, i] > 0 && BX1[2, i] * pts3d[3, i] > 0) { num++; } } return(num); }
public static Image <Arthmetic, double> K(List <Image <Arthmetic, double> > Fs, double width, double height) { double fi = (width + height) / 2; var minimizer = new BfgsMinimizer(1e-6, 1e-6, 1e-6); var result = minimizer.FindMinimum( new ObjFunc() { Fs = Fs, Width = width, Height = height }, new DenseVector(new double[] { fi, fi, width / 2, height / 2 }) ); var p = result.MinimizingPoint; return(ComputeMatrix.K(p[0], p[1], p[2], p[3])); }
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 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 void DecomposeToRT(Image <Arthmetic, double> E, out Image <Arthmetic, double>[] Rs, out Image <Arthmetic, double>[] ts) { var svd = new Svd(E); Image <Arthmetic, double> W = new Image <Arthmetic, double>(new double[, , ] { { { 0 }, { -1 }, { 0 } }, { { 1 }, { 0 }, { 0 } }, { { 0 }, { 0 }, { 1 } }, }); var R1 = svd.U.Multiply(W.T()).Multiply(svd.VT); double det1 = CvInvoke.Determinant(R1); if (det1 < 0) { R1 = R1.Mul(-1); } var R2 = svd.U.Multiply(W).Multiply(svd.VT); double det2 = CvInvoke.Determinant(R2); if (det2 < 0) { R2 = R2.Mul(-1); } double ss = (svd.S[0, 0] + svd.S[1, 0]) / 2; Image <Arthmetic, double> Z = new Image <Arthmetic, double>(new double[, , ] { { { 0 }, { -ss }, { 0 } }, { { ss }, { 0 }, { 0 } }, { { 0 }, { 0 }, { 0 } }, }); var t1 = svd.U.Multiply(Z).Multiply(svd.U.T()); t1 = ComputeMatrix.CrossProductToVector(t1); var t2 = t1.Mul(-1); Rs = new Image <Arthmetic, double>[] { R1, R2 }; ts = new Image <Arthmetic, double>[] { t1, t2 }; }
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); }
private void ResetPoints() { P1 = ComputeMatrix.Camera(K); P2 = ComputeMatrix.Camera(K, R12, C2); P3 = ComputeMatrix.Camera(K, R13, C3); ptsReal = new List <Image <Arthmetic, double> >(); pts1Ref = new List <PointF>(); pts2Ref = new List <PointF>(); pts3Ref = new List <PointF>(); Random rand = new Random(564073); for (int i = 0; i < pointsCount; ++i) { var real = new Image <Arthmetic, double>(new double[, , ] { { { rand.Next(100, 200) } }, { { rand.Next(100, 200) } }, { { rand.Next(50, 100) } }, { { 1 } }, }); ptsReal.Add(real); var i1 = P1.Multiply(real).ToPointF(); pts1Ref.Add(i1); var i2 = P2.Multiply(real).ToPointF(); pts2Ref.Add(i2); var i3 = P3.Multiply(real).ToPointF(); pts3Ref.Add(i3); } double rangeLx = pts1Ref.Max((x) => x.X) - pts1Ref.Min((x) => x.X); double rangeLy = pts1Ref.Max((x) => x.Y) - pts1Ref.Min((x) => x.Y); double rangeRx = pts2Ref.Max((x) => x.X) - pts2Ref.Min((x) => x.X); double rangeRy = pts2Ref.Max((x) => x.Y) - pts2Ref.Min((x) => x.Y); pixelRange = 0.25 * (rangeLx + rangeLy + rangeRx + rangeRy); }
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 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; }