// Инициализация фильтра Кальмана
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);
}
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];
public unsafe static extern CvMat *cvEncodeImage([MarshalAs(UnmanagedType.LPStr)] string ext, CvMat *image, int *parameters = null);
public unsafe static extern void cvReleaseMat(ref CvMat *mat);