CvStereoCalibrate (CvLevMarq :: updateAlt ()) занимает бесконечное количество времени

У меня есть некоторые проблемы. Я пытаюсь откалибровать свои камеры. Я написал для этого какую-то функцию, но она потребовала некоторых странных данных, поэтому я пытаюсь проверить это на примере из книги O’Reilly OpenCV (есть пример 12-3 из главы 12: Проекция и 3D Vision).
Я подгоняю этот пример к моей программе, но программа зависает (если это хорошее слово). Я глубже проверил функции opencv и обнаружил, что в cvStereoCalibrate есть cvCalibrateCamera2, который содержит проблему: во время оптимизации часть строки:

bool proceed = solver.updateAlt( _param, _JtJ, _JtErr, _errNorm ); //solver is object of CvLevMarq

занимает очень много времени (бесконечность).

Почему так долго (много часов)? Или, может быть, я допустил какую-то ошибку, но я не знаю, где (я переписываю ее из книги, возможно, неправильно)

Вот мой код:

void Calibration::stereoCalibTest()
{
int displayCorners = 0;
int showUndistorted = 1;
bool isVerticalStereo = false;
const int maxScale = 1;
const float squareSize = m_squareSize;
int nframes, n = m_boardSize.width() * m_boardSize.height(), N = 0;
int nx = m_boardSize.width();
int ny = m_boardSize.height();

vector<CvPoint3D32f> objectPoints;
vector<CvPoint2D32f> points[2];
vector<int> npoints;
vector<CvPoint2D32f> temp(n);
vector<uchar> active[2];

CvSize imageSize = {0,0};

//ARRAY AND VECTOR STORAGE
double M1[3][3], M2[3][3], D1[5], D2[5];
double R[3][3], T[3], E[3][3], F[3][3];
CvMat _M1 = cvMat(3, 3, CV_64F, M1);
CvMat _M2 = cvMat(3, 3, CV_64F, M2);
CvMat _D1 = cvMat(1, 5, CV_64F, D1);
CvMat _D2 = cvMat(1, 5, CV_64F, D2);
CvMat _R = cvMat(3, 3, CV_64F, R);
CvMat _T = cvMat(3, 1, CV_64F, T);
CvMat _E = cvMat(3, 3, CV_64F, E);
CvMat _F = cvMat(3, 3, CV_64F, F);

//FOR LEFT CAMERA

for(int  i = 0; i < m_imageList1.size(); i++)
{
IplImage *im_gray = cvCreateImage(cvGetSize(m_imageList1[i]),IPL_DEPTH_8U,1);
cvCvtColor(m_imageList1[i],im_gray,CV_RGB2GRAY);

vector<CvPoint2D32f> &pts = points[0];
int count = 0, result = 0;
imageSize = cvGetSize(m_imageList1[i]);
result = cvFindChessboardCorners(im_gray, cvSize(nx, ny), &temp[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);

N = pts.size();
pts.resize(N + n, cvPoint2D32f(0,0));
active[0].push_back((uchar)result);

if(result)
{
cvFindCornerSubPix(im_gray, &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);
}

}

//FOR RIGHT CAMERA

for(int  i = 0; i < m_imageList2.size(); i++)
{
vector<CvPoint2D32f> &pts = points[1];
int count = 0, result = 0;
imageSize = cvGetSize(m_imageList2[i]);
IplImage *im_gray = cvCreateImage(imageSize,IPL_DEPTH_8U,1);
cvCvtColor(m_imageList2[i],im_gray,CV_RGB2GRAY);
result = cvFindChessboardCorners(im_gray, cvSize(nx, ny), &temp[0], &count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE);

N = pts.size();
pts.resize(N + n, cvPoint2D32f(0,0));
active[0].push_back((uchar)result);

if(result)
{
cvFindCornerSubPix(im_gray, &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);
}
}

// HARVEST CHESSBOARD 3D OBJECT POINT LIST:

nframes = active[0].size();
objectPoints.resize(nframes * n);
for(int i = 0; i < ny; i++)
for(int j = 0; j < nx; j++)
objectPoints[i * nx + j] = cvPoint3D32f(i*squareSize, j*squareSize, 0);

for(int 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
ui.teInfo->append("Calibrating from TEST FUNCTION");

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);

ui.teInfo->append("Done!");// 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(int 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;
}

ui.teInfo->append("average error: " +QString::number(avgErr/(nframes*n)));

double R1[3][3], R2[3][3], P1[3][4], P2[3][4];
CvMat _R1 = cvMat(3, 3, CV_64F, R1);
CvMat _R2 = cvMat(3, 3, CV_64F, R2);
CvMat _P1 = cvMat(3, 4, CV_64F, P1);
CvMat _P2 = cvMat(3, 4, CV_64F, P2);

cvStereoRectify(&_M1, &_M2, &_D1, &_D2, imageSize, &_R, &_T,
&_R1, &_R2, &_P1, &_P2, 0,0);

ui.teInfo->append("TEST: cameras calibrated");
bool isSaved = saveToFile(&_M1, &_M2, &_D1, &_D2, &_R1, &_R2, &_P1, &_P2);
if(isSaved)
ui.teInfo->append("TEST: saved to file");
else
ui.teInfo->append("TEST: cannot save to file");
}

занимает очень много времени