Beispiel #1
0
        public void TestMatricesFromNoisedPoints()
        {
            List <Image <Arthmetic, double> > ptsReal = new List <Image <Arthmetic, double> >();
            List <PointF> pts1    = new List <PointF>();
            List <PointF> pts2    = new List <PointF>();
            List <PointF> pts3    = new List <PointF>();
            List <PointF> pts1Ref = new List <PointF>();
            List <PointF> pts2Ref = new List <PointF>();
            List <PointF> pts3Ref = new List <PointF>();

            Random rand        = new Random(1003);
            double stddev      = 3;
            int    pointsCount = 100;

            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);
                i1 = new PointF(i1.X + Noise(stddev, rand), i1.Y + Noise(stddev, rand));
                pts1.Add(i1);

                var i2 = P2.Multiply(real).ToPointF();
                pts2Ref.Add(i2);
                i2 = new PointF(i2.X + Noise(stddev, rand), i2.Y + Noise(stddev, rand));
                pts2.Add(i2);

                var i3 = P3.Multiply(real).ToPointF();
                pts3Ref.Add(i3);
                i3 = new PointF(i3.X + Noise(stddev, rand), i3.Y + Noise(stddev, rand));
                pts3.Add(i3);
            }

            double rangeLx = pts1.Max((x) => x.X) - pts1.Min((x) => x.X);
            double rangeLy = pts1.Max((x) => x.Y) - pts1.Min((x) => x.Y);
            double rangeRx = pts2.Max((x) => x.X) - pts2.Min((x) => x.X);
            double rangeRy = pts2.Max((x) => x.Y) - pts2.Min((x) => x.Y);

            var pts1_n = new List <PointF>(pts1);
            var pts2_n = new List <PointF>(pts2);
            var pts3_n = new List <PointF>(pts3);

            FindTransformation.NormalizePoints2d(pts1_n, out Image <Arthmetic, double> N1);
            FindTransformation.NormalizePoints2d(pts2_n, out Image <Arthmetic, double> N2);
            FindTransformation.NormalizePoints2d(pts3_n, out Image <Arthmetic, double> N3);

            var F   = ComputeMatrix.F(new VectorOfPointF(pts1_n.ToArray()), new VectorOfPointF(pts2_n.ToArray()));
            var F23 = ComputeMatrix.F(new VectorOfPointF(pts2_n.ToArray()), new VectorOfPointF(pts3_n.ToArray()));

            // F is normalized - lets denormalize it
            F   = N2.T().Multiply(F).Multiply(N1);
            F23 = N3.T().Multiply(F23).Multiply(N2);

            var E   = ComputeMatrix.E(F, K);
            var E23 = ComputeMatrix.E(F23, K);

            var svd   = new Svd(E);
            var svd23 = new Svd(E23);

            FindTransformation.DecomposeToRTAndTriangulate(pts1, pts2, K, E, out var RR, out var TT, out Image <Arthmetic, double> estReal_);
            FindTransformation.DecomposeToRTAndTriangulate(pts2, pts3, K, E23, out var RR23, out var TT23, out Image <Arthmetic, double> estReal23_);

            var rr0 = RotationConverter.MatrixToEulerXYZ(R12);
            var rr1 = RotationConverter.MatrixToEulerXYZ(RR);

            double idealScale  = T23.Norm / T12.Norm;
            double idealScale2 = T12.Norm / T23.Norm;

            var tt0    = T12.Mul(1 / T12.Norm);
            var tt1    = TT.Mul(1 / TT.Norm).Mul(T12[0, 0] * TT[0, 0] < 0 ? -1 : 1);
            var tt1_23 = TT23.Mul(1 / TT23.Norm).Mul(T23[0, 0] * TT23[0, 0] < 0 ? -1 : 1);

            FindTransformation.TriangulateChieral(pts1, pts2, K, RR, tt1, out var estReal12);
            FindTransformation.TriangulateChieral(pts2, pts3, K, RR23, tt1_23, out var estReal23);

            RansacScaleEstimation ransacModel = new RansacScaleEstimation(estReal12, estReal23, RR, ComputeMatrix.Center(tt1, RR));

            int    sampleSize       = (int)(0.1 * pointsCount);
            int    minGoodPoints    = (int)(0.2 * pointsCount);
            int    maxIterations    = 1000;
            double meanRefPointSize = ScaleBy3dPointsMatch.GetMeanSize(estReal12);
            double threshold        = meanRefPointSize * meanRefPointSize * 0.1;
            var    result           = RANSAC.ProcessMostInliers(ransacModel, maxIterations, sampleSize, minGoodPoints, threshold, 1.0);
            double scale            = (double)result.BestModel;

            var estRealRef23To12 = ScaleBy3dPointsMatch.TransfromBack3dPoints(RR, tt1, estReal23, scale);

            double simpleScaleMean = 0.0;

            for (int i = 0; i < pointsCount; ++i)
            {
                var p12 = new Image <Arthmetic, double>(new double[, , ] {
                    { { estReal12[1, i] } },
                    { { estReal12[2, i] } },
                    { { estReal12[3, i] } },
                });
                var p23 = new Image <Arthmetic, double>(new double[, , ] {
                    { { estReal23[0, i] } },
                    { { estReal23[1, i] } },
                    { { estReal23[2, i] } },
                });

                double n1          = p12.Norm;
                double n2          = p23.Norm;
                double simpleScale = n2 / n1;
                simpleScaleMean += simpleScale;
            }

            simpleScaleMean /= pointsCount;
            double simpleScaleMean2 = 1 / simpleScaleMean;

            // TODO: compute below only on inliers
            Image <Arthmetic, double> inliersOnly12    = new Image <Arthmetic, double>(result.Inliers.Count, 4);
            Image <Arthmetic, double> inliersOnly12Ref = 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]    = estReal12[j, k];
                    inliersOnly12Ref[j, i] = ptsReal[k][j, 0];
                    inliersOnly23[j, i]    = estRealRef23To12[j, k];
                }
            }


            var ptsRealM = Utils.Matrixify(ptsReal);

            Errors.TraingulationError(ptsRealM, estReal12, out double mean1x, out double median1x, out List <double> errors1x);
            Errors.TraingulationError(ptsRealM, estRealRef23To12, out double mean1z, out double median1z, out List <double> errors1z);

            Errors.TraingulationError(inliersOnly12Ref, inliersOnly12, out double mean_in1, out double median_in1, out List <double> errors_in1);
            Errors.TraingulationError(inliersOnly12Ref, inliersOnly23, out double mean_in2, out double median_in2, out List <double> errors_in2);
            Errors.TraingulationError(inliersOnly12, inliersOnly23, out double mean_in3, out double median_in3, out List <double> errors_in3);

            var ptsReal23M = ForwardProject3dPoints(ptsRealM, R12, C2);

            Errors.TraingulationError(ptsReal23M, estReal23, out double mean1h, out double median1h, out List <double> errors1h);

            var estC2     = ComputeMatrix.Center(tt1, RR);
            var ptsEst23M = ForwardProject3dPoints(estReal12, RR, estC2);

            Errors.TraingulationError(ptsEst23M, estReal23, out double mean1t, out double median1t, out List <double> errors1t);

            int dummy = 0;

            //Errors.TraingulationError(ptsReal, estReal, out double mean1, out double median1, out List<double> errors1);
            //Errors.ReprojectionError(estReal, pts2, K, RR, tt1, out double mean_r1a, out double median_r1a, out List<double> _1);
            //Errors.ReprojectionError(estReal, pts2Ref, K, RR, tt1, out double mean_r1b, out double median_r1b, out List<double> _2);
            //Errors.ReprojectionError(estReal, pts2Ref, K, R12, tt0, out double mean_r1c, out double median_r1c, out List<double> _3);
            //Errors.ReprojectionError(Errors.Matrixify(ptsReal), pts2Ref, K, RR, tt1, out double mean_r1e, out double median_r1e, out List<double> _5);

            var H1   = FindTransformation.EstimateHomography(pts1, pts2, K);
            var H2   = FindTransformation.EstimateHomography(pts1Ref, pts2Ref, K);
            var hrr1 = RotationConverter.MatrixToEulerXYZ(H1);
            var hrr2 = RotationConverter.MatrixToEulerXYZ(H2);
            //var zeroT = new Image<Arthmetic, double>(1, 3);

            //var H3 = RotationConverter.EulerXYZToMatrix(hrr1);
            //var hrr3 = RotationConverter.MatrixToEulerXYZ(H1);

            //var svdH = new Svd(H1);

            bool isRotation = FindTransformation.IsPureRotation(H1);

            int dummy2 = 0;

            //Errors.ReprojectionError2d(pts1Ref, pts2Ref, K, H2, out double mean_h2, out double median_h2, out var err_h2);
            //Errors.ReprojectionError2d(pts1, pts2, K, H1, out double mean_h1, out double median_h1, out var err_h1);
            //Errors.ReprojectionError2d(pts1, pts2, K, H3, out double mean_h3, out double median_h3, out var err_h3);

            //Errors.ReprojectionError2dWithT(pts1, pts2, K, H1, zeroT, out double scale1, out double mean_h1a, out double median_h1a, out var err_h1a);
            //Errors.ReprojectionError2dWithT(pts1, pts2, K, H3, zeroT, out double scale1x, out double mean_h1ax, out double median_h1ax, out var err_h1ax);
            ////  Errors.ReprojectionError2dWithT(pts1, pts2, K, R12, tt0, out double scale2, out double mean_h1b, out double median_h1b, out var err_h1b);
            //Errors.ReprojectionError2dWithT(pts1, pts2, K, RR, tt1, out double scale3, out double mean_h1c, out double median_h1c, out var err_h1c);
            //Errors.ReprojectionError2dWithT(pts1, pts2, K, R12, tt1, out double scale5, out double mean_h1c1, out double median_h1c1, out var err_h1c1);
            //Errors.ReprojectionError2dWithT(pts1Ref, pts2Ref, K, R12, tt0, out double scale6, out double mean_h1c2, out double median_h1c2, out var err_h1c2);
            //Errors.ReprojectionError2dWithT(pts1, pts2, K, H1, tt1, out double scale4, out double mean_h1d, out double median_h1d, out var err_h1d);

            //var KK = EstimateCameraFromImagePair.K(F, 600, 500);
            //var EE = ComputeMatrix.E(F, KK);
            //var svd2 = new Svd(EE);

            //FindTransformation.DecomposeToRTAndTriangulate(pts1, pts2, KK, EE, out var RR2, out var TT2, out Image<Arthmetic, double> estReal2);
            //var tt2 = TT2.Mul(1 / TT2.Norm);
            //var rr2 = RotationConverter.MatrixToEulerXYZ(RR2);

            //Errors.TraingulationError(ptsReal, estReal2, out double mean2, out double median2, out List<double> errors2);
            //Errors.ReprojectionError(estReal2, pts2, KK, RR2, tt2, out double mean_r2a, out double median_r2a, out List<double> _1x);
            //Errors.ReprojectionError(estReal2, pts2Ref, KK, RR2, tt2, out double mean_r2b, out double median_r2b, out List<double> _2x);
            //Errors.ReprojectionError(estReal2, pts2Ref, KK, R12, tt0, out double mean_r2c, out double median_r2c, out List<double> _3x);
            //Errors.ReprojectionError(Errors.Matrixify(ptsReal), pts2Ref, KK, RR2, tt2, out double mean_r2e, out double median_r2e, out List<double> _5x);
        }