in modules/calib3d/src/fisheye.cpp [818:1052]
double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
OutputArray R, OutputArray T, int flags, TermCriteria criteria)
{
CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty());
CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total());
CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2);
CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2);
CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty());
CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty());
CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty());
CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty());
CV_Assert(((flags & CALIB_FIX_INTRINSIC) && !K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC));
//-------------------------------Initialization
const int threshold = 50;
const double thresh_cond = 1e6;
const int check_cond = 1;
int n_points = (int)objectPoints.getMat(0).total();
int n_images = (int)objectPoints.total();
double change = 1;
cv::internal::IntrinsicParams intrinsicLeft;
cv::internal::IntrinsicParams intrinsicRight;
cv::internal::IntrinsicParams intrinsicLeft_errors;
cv::internal::IntrinsicParams intrinsicRight_errors;
Matx33d _K1, _K2;
Vec4d _D1, _D2;
if (!K1.empty()) K1.getMat().convertTo(_K1, CV_64FC1);
if (!D1.empty()) D1.getMat().convertTo(_D1, CV_64FC1);
if (!K2.empty()) K2.getMat().convertTo(_K2, CV_64FC1);
if (!D2.empty()) D2.getMat().convertTo(_D2, CV_64FC1);
std::vector<Vec3d> rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images);
if (!(flags & CALIB_FIX_INTRINSIC))
{
calibrate(objectPoints, imagePoints1, imageSize, _K1, _D1, rvecs1, tvecs1, flags, TermCriteria(3, 20, 1e-6));
calibrate(objectPoints, imagePoints2, imageSize, _K2, _D2, rvecs2, tvecs2, flags, TermCriteria(3, 20, 1e-6));
}
intrinsicLeft.Init(Vec2d(_K1(0,0), _K1(1, 1)), Vec2d(_K1(0,2), _K1(1, 2)),
Vec4d(_D1[0], _D1[1], _D1[2], _D1[3]), _K1(0, 1) / _K1(0, 0));
intrinsicRight.Init(Vec2d(_K2(0,0), _K2(1, 1)), Vec2d(_K2(0,2), _K2(1, 2)),
Vec4d(_D2[0], _D2[1], _D2[2], _D2[3]), _K2(0, 1) / _K2(0, 0));
if ((flags & CALIB_FIX_INTRINSIC))
{
cv::internal::CalibrateExtrinsics(objectPoints, imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1);
cv::internal::CalibrateExtrinsics(objectPoints, imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2);
}
intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicLeft.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicLeft.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicLeft.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicLeft.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicLeft.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicLeft.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicLeft.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicLeft.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicRight.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicRight.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicRight.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicRight.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
intrinsicRight.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicRight.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicRight.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicRight.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicRight.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1;
intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;
std::vector<int> selectedParams;
std::vector<int> tmp(6 * (n_images + 1), 1);
selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end());
selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end());
selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end());
//Init values for rotation and translation between two views
cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3);
cv::Mat om_ref, R_ref, T_ref, R1, R2;
for (int image_idx = 0; image_idx < n_images; ++image_idx)
{
cv::Rodrigues(rvecs1[image_idx], R1);
cv::Rodrigues(rvecs2[image_idx], R2);
R_ref = R2 * R1.t();
T_ref = cv::Mat(tvecs2[image_idx]) - R_ref * cv::Mat(tvecs1[image_idx]);
cv::Rodrigues(R_ref, om_ref);
om_ref.reshape(3, 1).copyTo(om_list.col(image_idx));
T_ref.reshape(3, 1).copyTo(T_list.col(image_idx));
}
cv::Vec3d omcur = cv::internal::median3d(om_list);
cv::Vec3d Tcur = cv::internal::median3d(T_list);
cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1),
e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk;
cv::Mat J2_inv;
for(int iter = 0; ; ++iter)
{
if ((criteria.type == 1 && iter >= criteria.maxCount) ||
(criteria.type == 2 && change <= criteria.epsilon) ||
(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
break;
J.create(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1);
e.create(4 * n_points * n_images, 1, CV_64FC1);
Jkk.create(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
ekk.create(4 * n_points, 1, CV_64FC1);
cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT;
for (int image_idx = 0; image_idx < n_images; ++image_idx)
{
Jkk = cv::Mat::zeros(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1);
cv::Mat object = objectPoints.getMat(image_idx).clone();
cv::Mat imageLeft = imagePoints1.getMat(image_idx).clone();
cv::Mat imageRight = imagePoints2.getMat(image_idx).clone();
cv::Mat jacobians, projected;
//left camera jacobian
cv::Mat rvec = cv::Mat(rvecs1[image_idx]);
cv::Mat tvec = cv::Mat(tvecs1[image_idx]);
cv::internal::projectPoints(object, projected, rvec, tvec, intrinsicLeft, jacobians);
cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(0, 2 * n_points));
jacobians.colRange(8, 11).copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(0, 2 * n_points));
jacobians.colRange(11, 14).copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(0, 2 * n_points));
jacobians.colRange(0, 2).copyTo(Jkk.colRange(0, 2).rowRange(0, 2 * n_points));
jacobians.colRange(2, 4).copyTo(Jkk.colRange(2, 4).rowRange(0, 2 * n_points));
jacobians.colRange(4, 8).copyTo(Jkk.colRange(5, 9).rowRange(0, 2 * n_points));
jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points));
//right camera jacobian
cv::internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
rvec = cv::Mat(rvecs2[image_idx]);
tvec = cv::Mat(tvecs2[image_idx]);
cv::internal::projectPoints(object, projected, omr, Tr, intrinsicRight, jacobians);
cv::Mat(cv::Mat((imageRight - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(2 * n_points, 4 * n_points));
cv::Mat dxrdom = jacobians.colRange(8, 11) * domrdom + jacobians.colRange(11, 14) * dTrdom;
cv::Mat dxrdT = jacobians.colRange(8, 11) * domrdT + jacobians.colRange(11, 14)* dTrdT;
cv::Mat dxrdomckk = jacobians.colRange(8, 11) * domrdomckk + jacobians.colRange(11, 14) * dTrdomckk;
cv::Mat dxrdTckk = jacobians.colRange(8, 11) * domrdTckk + jacobians.colRange(11, 14) * dTrdTckk;
dxrdom.copyTo(Jkk.colRange(18, 21).rowRange(2 * n_points, 4 * n_points));
dxrdT.copyTo(Jkk.colRange(21, 24).rowRange(2 * n_points, 4 * n_points));
dxrdomckk.copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
dxrdTckk.copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(0, 2).copyTo(Jkk.colRange(9 + 0, 9 + 2).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(2, 4).copyTo(Jkk.colRange(9 + 2, 9 + 4).rowRange(2 * n_points, 4 * n_points));
jacobians.colRange(4, 8).copyTo(Jkk.colRange(9 + 5, 9 + 9).rowRange(2 * n_points, 4 * n_points));
jacobians.col(14).copyTo(Jkk.col(9 + 4).rowRange(2 * n_points, 4 * n_points));
//check goodness of sterepair
double abs_max = 0;
for (int i = 0; i < 4 * n_points; i++)
{
if (fabs(ekk.at<double>(i)) > abs_max)
{
abs_max = fabs(ekk.at<double>(i));
}
}
CV_Assert(abs_max < threshold); // bad stereo pair
Jkk.copyTo(J.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
ekk.copyTo(e.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points));
}
cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
//update all parameters
cv::subMatrix(J, J, selectedParams, std::vector<int>(J.rows, 1));
cv::Mat J2 = J.t() * J;
J2_inv = J2.inv();
int a = cv::countNonZero(intrinsicLeft.isEstimate);
int b = cv::countNonZero(intrinsicRight.isEstimate);
cv::Mat deltas = J2_inv * J.t() * e;
intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a);
intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b);
omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3));
Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6));
for (int image_idx = 0; image_idx < n_images; ++image_idx)
{
rvecs1[image_idx] = cv::Mat(cv::Mat(rvecs1[image_idx]) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6));
tvecs1[image_idx] = cv::Mat(cv::Mat(tvecs1[image_idx]) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6));
}
cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
change = cv::norm(newTom - oldTom) / cv::norm(newTom);
}
double rms = 0;
const Vec2d* ptr_e = e.ptr<Vec2d>();
for (size_t i = 0; i < e.total() / 2; i++)
{
rms += ptr_e[i][0] * ptr_e[i][0] + ptr_e[i][1] * ptr_e[i][1];
}
rms /= ((double)e.total() / 2.0);
rms = sqrt(rms);
_K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
0, intrinsicLeft.f[1], intrinsicLeft.c[1],
0, 0, 1);
_K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
0, intrinsicRight.f[1], intrinsicRight.c[1],
0, 0, 1);
Mat _R;
Rodrigues(omcur, _R);
if (K1.needed()) cv::Mat(_K1).convertTo(K1, K1.empty() ? CV_64FC1 : K1.type());
if (K2.needed()) cv::Mat(_K2).convertTo(K2, K2.empty() ? CV_64FC1 : K2.type());
if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(D1, D1.empty() ? CV_64FC1 : D1.type());
if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type());
if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type());
if (T.needed()) cv::Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type());
return rms;
}