Exemple #1
0
        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];
        }
Exemple #2
0
        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);
        }
Exemple #3
0
 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));
     };
Exemple #4
0
            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);
            }
Exemple #5
0
        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);
        }
Exemple #7
0
        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);
        }
Exemple #12
0
        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 };
        }
Exemple #13
0
        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);
        }
Exemple #14
0
        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);
        }
Exemple #16
0
        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;
        }