Beispiel #1
0
        public static void dlt(Matrix x, Matrix xp)
        {
            var n = x.Rows;

            if (n < 4)
            {
                throw new Exception();
            }
            Matrix A = null;

            for (int i = 0; i < x.Rows; i++)
            {
                var xip = xp[i, 0];
                var yip = xp[i, 1];
                var wip = xp[i, 2];
                var xi  = x.GetRow(i);

                var    test = -wip * xi;
                double te   = (-wip * xi)[0, 0];
                var    xit  = xi.Transpose();
                var    row1 = new Matrix(new float[, ] {
                    { 0, 0, 0 }
                });
                var row2 = (wip * xit);
                row1 = row1.ConcateHorizontal((-wip * xit)).ConcateHorizontal((yip * xit));
                row2 = row2.ConcateHorizontal(new Matrix(new float[, ] {
                    { 0, 0, 0 }
                })).ConcateHorizontal((-xip * xit));
                if (A == null)
                {
                    A = row1.ConcateVertical(row2);
                }
                else
                {
                    A.ConcateVertical(row1);
                    A.ConcateVertical(row2);
                }
            }

            var    svd = new SVD <float>(A);
            Matrix H   = svd.V.Reshape(3, 3);

            H = H / H[2, 2];
        }
        public static unsafe void ceresSolveAruco()
        {
            var                phc          = PinholeCamera.getTestCameraHuawei();
            string             dir          = @"C:\Users\jens\Desktop\calibratie\Huawei p9\aruco\stereo test\";
            List <CeresMarker> ceresmarkers = new List <CeresMarker>();
            List <CeresCamera> cerescameras = new List <CeresCamera>();
            var                files        = Directory.GetFiles(dir).ToList();


            //8 punten nodig
            var markerDictionary = Aruco.findArucoMarkers(files, Path.Combine(dir, "aruco_detected\\"), 1);
            var pairs            = findImagePairsMinMarkers(markerDictionary, 8);

            Matrix K = new Matrix(phc.Intrinsics.Mat);

            var W = new Matrix(new double[] {
                0.0D, -1.0D, 0.0D,
                1.0D, 0.0D, 0.0D,
                0.0D, 0.0D, 1.0D
            });


            var Wt = new Matrix(new double[] {
                0.0D, 1.0D, 0.0D,
                -1.0D, 0.0D, 0.0D,
                0.0D, 0.0D, 1.0D
            });
            var Z = new Matrix(new double[] {
                0.0D, 1.0D, 0.0D,
                -1.0D, 0.0D, 0.0D,
                0.0D, 0.0D, 0D
            });

            var diag = new Matrix(new double[] {
                1.0D, 0.0D, 0.0D,
                0.0D, 1.0D, 0.0D,
                0.0D, 0.0D, 0.0D
            });


            foreach (var stereoPair in pairs)
            {
                var            points_count = stereoPair.intersection.Count;
                VectorOfPointF punten1px, punten2px;
                {
                    int           i  = 0;
                    List <PointF> p1 = new List <PointF>();
                    List <PointF> p2 = new List <PointF>();
                    foreach (KeyValuePair <ArucoMarker, ArucoMarker> kvp in stereoPair.intersection)
                    {
                        p1.Add(kvp.Key.Corner1);
                        p2.Add(kvp.Value.Corner1);
                        i++;
                    }
                    punten1px = new VectorOfPointF(p1.ToArray());
                    punten2px = new VectorOfPointF(p2.ToArray());
                }


                Matrix F      = new Matrix(3, 3);
                CVI.FindFundamentalMat(punten1px, punten2px, F);



                Matrix essential = K.Transpose() * F * K;
                var    decomp = new SVD <double>(essential);
                var    U      = decomp.U;
                var    Vt     = decomp.Vt;

                var R1 = U * W * Vt;
                var R2 = U * W.Transpose() * Vt;
                var T1 = U.GetCol(2);
                var T2 = -1 * U.GetCol(2);

                Matrix[] Ps = new Matrix[4];

                for (int i = 0; i < 4; i++)
                {
                    Ps[i] = new Matrix(3, 4);
                }

                CVI.HConcat(R1, T1, Ps[0]);
                CVI.HConcat(R1, T2, Ps[1]);
                CVI.HConcat(R2, T1, Ps[2]);
                CVI.HConcat(R2, T2, Ps[3]);

                var KPs = new Matrix[4];
                KPs[0] = K * Ps[0];
                KPs[1] = K * Ps[1];
                KPs[2] = K * Ps[2];
                KPs[3] = K * Ps[3];


                var KP0 = K * new Matrix(new double [, ] {
                    { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }
                });

                for (int i = 0; i < 4; i++)
                {
                    Matrix <float>     output_hom = new Matrix <float>(4, punten1px.Size);
                    VectorOfPoint3D32F output_3d  = new VectorOfPoint3D32F();

                    CVI.TriangulatePoints(KP0, KPs[i], punten1px, punten2px, output_hom);
                    CVI.ConvertPointsFromHomogeneous(output_hom, output_3d);
                }

                Matrix S = U * diag * W * U.Transpose();

                Matrix R = U * W * decomp.Vt;
            }
        }