Ejemplo n.º 1
0
        private void RectifyImages(object sender, RoutedEventArgs e)
        {
            if(_camImageFirst.ImageSource == null || _camImageSec.ImageSource == null)
            {
                MessageBox.Show("Images must be set");
                return;
            }
            if(_camImageFirst.ImageSource.PixelWidth != _camImageSec.ImageSource.PixelWidth ||
                _camImageFirst.ImageSource.PixelHeight != _camImageSec.ImageSource.PixelHeight)
            {
                MessageBox.Show("Images must have same size");
                return;
            }
            if(CalibrationData.Data.IsCamLeftCalibrated == false ||
                CalibrationData.Data.IsCamRightCalibrated == false)
            {
                MessageBox.Show("Cameras must be calibrated");
                return;
            }

            ImageRectification_ZhangLoop rectifier = new ImageRectification_ZhangLoop();
            RectificationTransformation rectTransformation = new RectificationTransformation();
            rectTransformation.Rectifier = rectifier;

            ImageTransformer transformer = new ImageTransformer();
            transformer.Transformation = rectTransformation;
            transformer.UsedInterpolationMethod = ImageTransformer.InterpolationMethod.Quadratic;

            rectifier.ImageHeight = _camImageFirst.ImageSource.PixelHeight;
            rectifier.ImageWidth = _camImageFirst.ImageSource.PixelWidth;
            rectifier.EpiCrossLeft = CalibrationData.Data.EpipoleCrossLeft;
            rectifier.EpiCrossRight = CalibrationData.Data.EpipoleCrossRight;
            rectifier.EpipoleLeft = CalibrationData.Data.EpipoleLeft;
            rectifier.EpipoleRight = CalibrationData.Data.EpipoleRight;
            rectifier.IsEpiLeftInInfinity = CalibrationData.Data.EpiLeftInInfinity;
            rectifier.IsEpiRightInInfinity = CalibrationData.Data.EpiRightInInfinity;
            rectifier.FundamentalMatrix = CalibrationData.Data.Fundamental;
            rectifier.ComputeRectificationMatrices();

            ColorImage imgLeft = new ColorImage();
            imgLeft.FromBitmapSource(_camImageFirst.ImageSource);
            ColorImage imgRight = new ColorImage();
            imgRight.FromBitmapSource(_camImageSec.ImageSource);

            rectTransformation.WhichImage = RectificationTransformation.ImageIndex.Left;
            ColorImage rectLeft = transformer.TransfromImageBackwards(imgLeft, true);

            rectTransformation.WhichImage = RectificationTransformation.ImageIndex.Right;
            ColorImage rectRight = transformer.TransfromImageBackwards(imgRight, true);

            _camImageFirst.ImageSource = rectLeft.ToBitmapSource();
            _camImageSec.ImageSource = rectRight.ToBitmapSource();
        }
Ejemplo n.º 2
0
        public void Test_Rectification()
        {
            var Fi = new DenseMatrix(3); // Target F
            Fi[1, 2] = -1.0;
            Fi[2, 1] = 1.0;

            var K_l = new DenseMatrix(3, 3);
            K_l[0, 0] = 10.0; // fx
            K_l[1, 1] = 10.0; // fy
            K_l[0, 1] = 0.0; // s
            K_l[0, 2] = 300.0; // x0
            K_l[1, 2] = 250.0; // y0
            K_l[2, 2] = 1.0; // 1

            var K_r = new DenseMatrix(3, 3);
            K_r[0, 0] = 12.0; // fx
            K_r[1, 1] = 12.5; // fy
            K_r[0, 1] = 0.0; // s
            K_r[0, 2] = 300.0; // x0
            K_r[1, 2] = 200.0; // y0
            K_r[2, 2] = 1.0; // 1

            var R_l = DenseMatrix.CreateIdentity(3);
            var R_r = DenseMatrix.CreateIdentity(3);

            Vector<double> C_l = new DenseVector(3);
            C_l[0] = 50.0;
            C_l[1] = 50.0;
            C_l[2] = 0.0;

            Vector<double> C_r = new DenseVector(3);
            C_r[0] = 40.0;
            C_r[1] = 40.0;
            C_r[2] = 10.0;

            Matrix<double> Ext_l = new DenseMatrix(3, 4);
            Ext_l.SetSubMatrix(0, 0, R_l);
            Ext_l.SetColumn(3, -R_l * C_l);

            Matrix<double> Ext_r = new DenseMatrix(3, 4);
            Ext_r.SetSubMatrix(0, 0, R_r);
            Ext_r.SetColumn(3, -R_r * C_r);

            var CM_l = K_l * Ext_l;
            var CM_r = K_r * Ext_r;

            // Find e_R = P_R*C_L, e_L = P_L*C_R
            var epi_r = CM_r * new DenseVector(new double[] { C_l[0], C_l[1], C_l[2], 1.0 });
            var epi_l = CM_l * new DenseVector(new double[] { C_r[0], C_r[1], C_r[2], 1.0 });

            var ex_l = new DenseMatrix(3, 3);
            ex_l[0, 0] = 0.0;
            ex_l[1, 0] = epi_l[2];
            ex_l[2, 0] = -epi_l[1];
            ex_l[0, 1] = -epi_l[2];
            ex_l[1, 1] = 0.0;
            ex_l[2, 1] = epi_l[0];
            ex_l[0, 2] = epi_l[1];
            ex_l[1, 2] = -epi_l[0];
            ex_l[2, 2] = 0.0;

            var ex_r = new DenseMatrix(3, 3);
            ex_r[0, 0] = 0.0;
            ex_r[1, 0] = epi_r[2];
            ex_r[2, 0] = -epi_r[1];
            ex_r[0, 1] = -epi_r[2];
            ex_r[1, 1] = 0.0;
            ex_r[2, 1] = epi_r[0];
            ex_r[0, 2] = epi_r[1];
            ex_r[1, 2] = -epi_r[0];
            ex_r[2, 2] = 0.0;

            // F = [er]x * Pr * pseudoinv(Pl)
            var F = ex_r * (CM_r * CM_l.PseudoInverse());
            int rank = F.Rank();
            if(rank == 3)
            {
                // Need to ensure rank 2, so set smallest singular value to 0
                var svd = F.Svd();
                var E = svd.W;
                E[2, 2] = 0;
                var oldF = F;
                F = svd.U * E * svd.VT;
                var diff = F - oldF; // Difference should be very small if all is correct
            }

            // Scale F, so that F33 = 1
            F = F.Divide(F[2, 2]);

            ImageRectification_ZhangLoop rect = new ImageRectification_ZhangLoop();
            // Assume image of size 640x480
            rect.ImageHeight = 480;
            rect.ImageWidth = 640;
            rect.FundamentalMatrix = F;
            rect.EpiCrossLeft = ex_l;

            rect.ComputeRectificationMatrices();

            // Test H'^T * Fi * H should be very close to F
            var H_r = rect.RectificationRight;
            var H_l = rect.RectificationLeft;
            var eF = H_r.Transpose() * Fi * H_l;

            double err = (eF - F).FrobeniusNorm();
            Assert.IsTrue(err < 1e-6);
        }