Example #1
0
        // Инициализация фильтра Кальмана
        public KalmanFilter()
        {
            m_timeLastMeasurement = clock();
            int dynam_params = 4; // x,y,dx,dy
            int measure_params = 2;
            m_pKalmanFilter = cvCreateKalman(dynam_params, measure_params);
            state = cvCreateMat( dynam_params, 1, CV_32FC1 );

            // Генератор случайных чисел
            cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI );
            cvRandSetRange( &rng, 0, 1, 0 );
            rng.disttype = CV_RAND_NORMAL;
            cvRand( &rng, state );

            process_noise = cvCreateMat( dynam_params, 1, CV_32FC1 ); // (w_k)
            measurement = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y  (z_k)
            measurement_noise = cvCreateMat( measure_params, 1, CV_32FC1 ); // two parameters for x,y (v_k)
            cvZero(measurement);

            // F matrix data
            // F is transition matrix.  It relates how the states interact
            // For single input fixed velocity the new value
            // depends on the previous value and velocity- hence 1 0 1 0
            // on top line. Новое значение скорости не зависит от предыдущего
            // значения координаты, и зависит только от предыдущей скорости- поэтому 0 1 0 1 on second row
            const float[] F = {
                1, 0, 1, 0,//x + dx
                0, 1, 0, 1,//y + dy
                0, 0, 1, 0,//dx = dx
                0, 0, 0, 1,//dy = dy
            };
            memcpy( m_pKalmanFilter->transition_matrix->data.fl, F, sizeof(F));
            cvSetIdentity( m_pKalmanFilter->measurement_matrix, cvRealScalar(1) ); // (H)
            cvSetIdentity( m_pKalmanFilter->process_noise_cov, cvRealScalar(1e-5) ); // (Q)
            cvSetIdentity( m_pKalmanFilter->measurement_noise_cov, cvRealScalar(1e-1) ); // (R)
            cvSetIdentity( m_pKalmanFilter->error_cov_post, cvRealScalar(1));

            // choose random initial state
            cvRand( &rng, m_pKalmanFilter->state_post );

            //InitializeCriticalSection(&mutexPrediction);
        }
Example #2
0
        private void Calibrate(IEnumerable <Image <Gray, byte> > images, int nx, int ny, int useUncalibrated, float _squareSize /* Chessboard square size in cm */)
        {
            int  displayCorners   = 1;
            int  showUndistorted  = 1;
            bool isVerticalStereo = false;//OpenCV can handle left-right
            //or up-down camera arrangements
            const int maxScale = 1;

            //FILE* f = fopen(imageList, "rt");
            int i, j, lr, nframes, n = nx * ny, N = 0;

            List <MCvPoint3D32f> objectPoints;

            var points = new List <MCvPoint2D64f> [2];

            List <int> npoints;

            List <char>[] active = new List <char> [2];

            var temp = new PointF[n];

            var imageSize = new System.Drawing.Size(0, 0);

            // ARRAY AND VECTOR STORAGE:
            Matrix <double> _M1 = new Matrix <double>(3, 3);
            Matrix <double> _M2 = new Matrix <double>(3, 3);
            Matrix <double> _D1 = new Matrix <double>(1, 5);
            Matrix <double> _D2 = new Matrix <double>(1, 5);
            Matrix <double> _R  = new Matrix <double>(3, 3);
            Matrix <double> _T  = new Matrix <double>(3, 1);
            Matrix <double> _E  = new Matrix <double>(3, 3);
            Matrix <double> _F  = new Matrix <double>(3, 3);
            Matrix <double> _Q  = new Matrix <double>(4, 4);

            double [,] M1 = _M1.Data;
            double [,] M2 = _M2.Data;
            double [,] D1 = _D1.Data;
            double [,] D2 = _D2.Data;

            double [,] R = _R.Data;
            double [,] T = _T.Data;
            double [,] E = _E.Data;
            double [,] F = _F.Data;

            double [,] Q = _Q.Data;

            for (i = 0; i < images.Count(); i++)
            {
                int count = 0, result = 0;

                lr = i % 2;

                List <MCvPoint2D64f> pts = points[lr];

                var img = images.ElementAt(i);

                imageSize = img.Size;

                //FIND CHESSBOARDS AND CORNERS THEREIN:
                for (int s = 1; s <= maxScale; s++)
                {
                    var timg = img;

                    if (s > 1)
                    {
                        timg = new Image <Gray, byte>(img.Width * s, img.Height * s);

                        img.Resize(timg.Width, timg.Height, Emgu.CV.CvEnum.INTER.CV_INTER_CUBIC);
                    }



                    temp = Emgu.CV.CameraCalibration.FindChessboardCorners(timg, new Size(nx, ny),
                                                                           Emgu.CV.CvEnum.CALIB_CB_TYPE.ADAPTIVE_THRESH | Emgu.CV.CvEnum.CALIB_CB_TYPE.NORMALIZE_IMAGE);

                    Emgu.CV.CameraCalibration.

                    result = temp.Count();


                    if (timg != img)
                    {
                        timg.Dispose();
                    }

                    if (result == 0 || s == maxScale)
                    {
                        for (j = 0; j < count; j++)
                        {
                            temp[j].X /= s;
                            temp[j].Y /= s;
                        }
                    }
                    if (result)
                    {
                        break;
                    }
                }
                if (displayCorners)
                {
                    printf("%s\n", buf);
                    IplImage *cimg = cvCreateImage(imageSize, 8, 3);
                    cvCvtColor(img, cimg, CV_GRAY2BGR);
                    cvDrawChessboardCorners(cimg, cvSize(nx, ny), &temp[0],
                                            count, result);
                    cvShowImage("corners", cimg);
                    cvReleaseImage(&cimg);
                    if (cvWaitKey(0) == 27) //Allow ESC to quit
                    {
                        exit(-1);
                    }
                }
                else
                {
                    putchar('.');
                }
                N = pts.size();
                pts.resize(N + n, cvPoint2D32f(0, 0));
                active[lr].push_back((uchar)result);
                //assert( result != 0 );
                if (result)
                {
                    //Calibration will suffer without subpixel interpolation
                    cvFindCornerSubPix(img, &temp[0], count,
                                       cvSize(11, 11), cvSize(-1, -1),
                                       cvTermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS,
                                                      30, 0.01));
                    copy(temp.begin(), temp.end(), pts.begin() + N);
                }
                cvReleaseImage(&img);
            }
            fclose(f);
            printf("\n");
// HARVEST CHESSBOARD 3D OBJECT POINT LIST:
            nframes = active[0].size();//Number of good chessboads found
            objectPoints.resize(nframes * n);
            for (i = 0; i < ny; i++)
            {
                for (j = 0; j < nx; j++)
                {
                    objectPoints[i * nx + j] = cvPoint3D32f(i * squareSize, j * squareSize, 0);
                }
            }
            for (i = 1; i < nframes; i++)
            {
                copy(objectPoints.begin(), objectPoints.begin() + n,
                     objectPoints.begin() + i * n);
            }
            npoints.resize(nframes, n);
            N = nframes * n;
            CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0]);
            CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]);
            CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]);
            CvMat _npoints      = cvMat(1, npoints.size(), CV_32S, &npoints[0]);

            cvSetIdentity(&_M1);
            cvSetIdentity(&_M2);
            cvZero(&_D1);
            cvZero(&_D2);

// CALIBRATE THE STEREO CAMERAS
            printf("Running stereo calibration ...");
            fflush(stdout);
            cvStereoCalibrate(&_objectPoints, &_imagePoints1,
                              &_imagePoints2, &_npoints,
                              &_M1, &_D1, &_M2, &_D2,
                              imageSize, &_R, &_T, &_E, &_F,
                              cvTermCriteria(CV_TERMCRIT_ITER +
                                             CV_TERMCRIT_EPS, 100, 1e-5),
                              CV_CALIB_FIX_ASPECT_RATIO +
                              CV_CALIB_ZERO_TANGENT_DIST +
                              CV_CALIB_SAME_FOCAL_LENGTH);
            printf(" done\n");
// CALIBRATION QUALITY CHECK
// because the output fundamental matrix implicitly
// includes all the output information,
// we can check the quality of calibration using the
// epipolar geometry constraint: m2^t*F*m1=0
            vector <CvPoint3D32f> lines[2];

            points[0].resize(N);
            points[1].resize(N);
            _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0]);
            _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0]);
            lines[0].resize(N);
            lines[1].resize(N);
            CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]);
            CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]);

//Always work in undistorted space
            cvUndistortPoints(&_imagePoints1, &_imagePoints1,
                              &_M1, &_D1, 0, &_M1);
            cvUndistortPoints(&_imagePoints2, &_imagePoints2,
                              &_M2, &_D2, 0, &_M2);
            cvComputeCorrespondEpilines(&_imagePoints1, 1, &_F, &_L1);
            cvComputeCorrespondEpilines(&_imagePoints2, 2, &_F, &_L2);
            double avgErr = 0;

            for (i = 0; i < N; i++)
            {
                double err = fabs(points[0][i].x * lines[1][i].x +
                                  points[0][i].y * lines[1][i].y + lines[1][i].z)
                             + fabs(points[1][i].x * lines[0][i].x +
                                    points[1][i].y * lines[0][i].y + lines[0][i].z);
                avgErr += err;
            }
            printf("avg err = %g\n", avgErr / (nframes * n));
//COMPUTE AND DISPLAY RECTIFICATION
            if (showUndistorted)
            {
                CvMat *mx1 = cvCreateMat(imageSize.height,
                                         imageSize.width, CV_32F);
                CvMat *my1 = cvCreateMat(imageSize.height,
                                         imageSize.width, CV_32F);
                CvMat *mx2 = cvCreateMat(imageSize.height,

                                         imageSize.width, CV_32F);
                CvMat *my2 = cvCreateMat(imageSize.height,
                                         imageSize.width, CV_32F);
                CvMat *img1r = cvCreateMat(imageSize.height,
                                           imageSize.width, CV_8U);
                CvMat *img2r = cvCreateMat(imageSize.height,
                                           imageSize.width, CV_8U);
                CvMat *disp = cvCreateMat(imageSize.height,
                                          imageSize.width, CV_16S);
                CvMat *vdisp = cvCreateMat(imageSize.height,
                                           imageSize.width, CV_8U);
                CvMat *pair;
                double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
Example #3
0
 public unsafe static extern CvMat *cvEncodeImage([MarshalAs(UnmanagedType.LPStr)] string ext, CvMat *image, int *parameters = null);
Example #4
0
 public unsafe static extern void cvReleaseMat(ref CvMat *mat);