Added tests for stereoCalibrate
This commit is contained in:
Ilya Krylov
parent
c2341fd446
commit
50b291995a
@ -806,10 +806,10 @@ public:
|
||||
|
||||
//! performs stereo calibaration
|
||||
static double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
|
||||
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
|
||||
InputOutputArrayOfArrays rvecs1, InputOutputArrayOfArrays tvecs1,
|
||||
InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
|
||||
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
|
||||
OutputArray R, OutputArray T, int flags,
|
||||
TermCriteria criteria = TermCriteria(3, 100, 1e-10));
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -764,10 +764,8 @@ double cv::Fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArray
|
||||
/// cv::Fisheye::stereoCalibrate
|
||||
|
||||
double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
|
||||
InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2,
|
||||
InputOutputArray rvecs1, InputOutputArrayOfArrays tvecs1,
|
||||
InputOutputArrayOfArrays rvecs2, InputOutputArrayOfArrays tvecs2,
|
||||
TermCriteria criteria)
|
||||
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());
|
||||
@ -780,14 +778,13 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty());
|
||||
CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty());
|
||||
|
||||
CV_Assert((!rvecs1.empty() && rvecs1.channels() == 3) || rvecs1.empty());
|
||||
CV_Assert((!tvecs1.empty() && tvecs1.channels() == 3) || tvecs1.empty());
|
||||
CV_Assert((!rvecs2.empty() && rvecs2.channels() == 3) || rvecs2.empty());
|
||||
CV_Assert((!tvecs2.empty() && tvecs2.channels() == 3) || tvecs2.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;
|
||||
|
||||
size_t n_points = objectPoints.getMat(0).total();
|
||||
size_t n_images = objectPoints.total();
|
||||
@ -800,38 +797,52 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
cv::internal::IntrinsicParams intrinsicLeft_errors;
|
||||
cv::internal::IntrinsicParams intrinsicRight_errors;
|
||||
|
||||
Matx33d _K;
|
||||
Vec4d _D;
|
||||
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);
|
||||
|
||||
K1.getMat().convertTo(_K, CV_64FC1);
|
||||
D1.getMat().convertTo(_D, CV_64FC1);
|
||||
intrinsicLeft.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
|
||||
Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
|
||||
std::vector<Vec3d> rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images);
|
||||
|
||||
K2.getMat().convertTo(_K, CV_64FC1);
|
||||
D2.getMat().convertTo(_D, CV_64FC1);
|
||||
intrinsicRight.Init(Vec2d(_K(0,0), _K(1, 1)), Vec2d(_K(0,2), _K(1, 2)),
|
||||
Vec4d(_D[0], _D[1], _D[2], _D[3]), _K(0, 1) / _K(0, 0));
|
||||
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.isEstimate[0] = 1;
|
||||
intrinsicLeft.isEstimate[1] = 1;
|
||||
intrinsicLeft.isEstimate[2] = 1;
|
||||
intrinsicLeft.isEstimate[3] = 1;
|
||||
intrinsicLeft.isEstimate[4] = 0;
|
||||
intrinsicLeft.isEstimate[5] = 1;
|
||||
intrinsicLeft.isEstimate[6] = 1;
|
||||
intrinsicLeft.isEstimate[7] = 1;
|
||||
intrinsicLeft.isEstimate[8] = 1;
|
||||
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.isEstimate[0] = 1;
|
||||
intrinsicRight.isEstimate[1] = 1;
|
||||
intrinsicRight.isEstimate[2] = 1;
|
||||
intrinsicRight.isEstimate[3] = 1;
|
||||
intrinsicRight.isEstimate[4] = 0;
|
||||
intrinsicRight.isEstimate[5] = 1;
|
||||
intrinsicRight.isEstimate[6] = 1;
|
||||
intrinsicRight.isEstimate[7] = 1;
|
||||
intrinsicRight.isEstimate[8] = 1;
|
||||
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))
|
||||
{
|
||||
internal::CalibrateExtrinsics(objectPoints, imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1);
|
||||
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;
|
||||
@ -847,10 +858,10 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
cv::Mat om_ref, R_ref, T_ref, R1, R2;
|
||||
for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
|
||||
{
|
||||
cv::Rodrigues(rvecs1.getMat(image_idx), R1);
|
||||
cv::Rodrigues(rvecs2.getMat(image_idx), R2);
|
||||
cv::Rodrigues(rvecs1[image_idx], R1);
|
||||
cv::Rodrigues(rvecs2[image_idx], R2);
|
||||
R_ref = R2 * R1.t();
|
||||
T_ref = tvecs2.getMat(image_idx).reshape(1, 3) - R_ref * tvecs1.getMat(image_idx).reshape(1, 3);
|
||||
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));
|
||||
@ -861,6 +872,7 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
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) ||
|
||||
@ -885,8 +897,8 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
cv::Mat jacobians, projected;
|
||||
|
||||
//left camera jacobian
|
||||
cv::Mat rvec = rvecs1.getMat(image_idx).clone();
|
||||
cv::Mat tvec = tvecs1.getMat(image_idx).clone();
|
||||
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));
|
||||
@ -898,8 +910,8 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
|
||||
//right camera jacobian
|
||||
internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT);
|
||||
rvec = rvecs2.getMat(image_idx).clone();
|
||||
tvec = tvecs2.getMat(image_idx).clone();
|
||||
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));
|
||||
@ -952,103 +964,43 @@ double cv::Fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayO
|
||||
Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6));
|
||||
for (size_t image_idx = 0; image_idx < n_images; ++image_idx)
|
||||
{
|
||||
rvecs1.getMat(image_idx) = rvecs1.getMat(image_idx) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6).reshape(3);
|
||||
tvecs1.getMat(image_idx) = tvecs1.getMat(image_idx) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6).reshape(3);
|
||||
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);
|
||||
}
|
||||
|
||||
//estimate uncertainties
|
||||
cv::Mat sigma_x;
|
||||
cv::meanStdDev(e, cv::noArray(), sigma_x);
|
||||
sigma_x *= sqrt((double)e.total() / (e.total() - 1));
|
||||
cv::sqrt(J2_inv, J2_inv);
|
||||
cv::Mat errors = 3 * J2_inv.diag() * sigma_x;
|
||||
int a1 = cv::countNonZero(intrinsicLeft_errors.isEstimate);
|
||||
int b1 = cv::countNonZero(intrinsicRight_errors.isEstimate);
|
||||
intrinsicLeft_errors = errors.rowRange(0, a1);
|
||||
intrinsicRight_errors = errors.rowRange(a1, a1 + b1);
|
||||
cv::Vec3d om_error = cv::Vec3d(errors.rowRange(a1 + b1, a1 + b1 + 3));
|
||||
cv::Vec3d T_error = cv::Vec3d(errors.rowRange(a1 + b1 + 3, a1 + b1 + 6));
|
||||
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];
|
||||
}
|
||||
|
||||
std::cout << std::setprecision(15) << "left f = " << intrinsicLeft.f << std::endl;
|
||||
std::cout << std::setprecision(15) << "left c = " << intrinsicLeft.c << std::endl;
|
||||
std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft.alpha << std::endl;
|
||||
std::cout << std::setprecision(15) << "left k = " << intrinsicLeft.k << std::endl;
|
||||
rms /= (e.total() / 2);
|
||||
rms = sqrt(rms);
|
||||
|
||||
std::cout << std::setprecision(15) << "right f = " << intrinsicRight.f << std::endl;
|
||||
std::cout << std::setprecision(15) << "right c = " << intrinsicRight.c << std::endl;
|
||||
std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight.alpha << std::endl;
|
||||
std::cout << std::setprecision(15) << "right k = " << intrinsicRight.k << std::endl;
|
||||
|
||||
std::cout << omcur << std::endl;
|
||||
std::cout << Tcur << std::endl;
|
||||
std::cout << "====================================================================================" << std::endl;
|
||||
std::cout.flush();
|
||||
|
||||
std::cout << std::setprecision(15) << "left f = " << intrinsicLeft_errors.f << std::endl;
|
||||
std::cout << std::setprecision(15) << "left c = " << intrinsicLeft_errors.c << std::endl;
|
||||
std::cout << std::setprecision(15) << "left alpha = " << intrinsicLeft_errors.alpha << std::endl;
|
||||
std::cout << std::setprecision(15) << "left k = " << intrinsicLeft_errors.k << std::endl;
|
||||
|
||||
std::cout << std::setprecision(15) << "right f = " << intrinsicRight_errors.f << std::endl;
|
||||
std::cout << std::setprecision(15) << "right c = " << intrinsicRight_errors.c << std::endl;
|
||||
std::cout << std::setprecision(15) << "right alpha = " << intrinsicRight_errors.alpha << std::endl;
|
||||
std::cout << std::setprecision(15) << "right k = " << intrinsicRight_errors.k << std::endl;
|
||||
|
||||
std::cout << om_error << std::endl;
|
||||
std::cout << T_error << std::endl;
|
||||
std::cout << "====================================================================================" << std::endl;
|
||||
std::cout.flush();
|
||||
|
||||
CV_Assert(cv::norm(intrinsicLeft.f - cv::Vec2d(561.195925927249, 562.849402029712)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicLeft.c - cv::Vec2d(621.282400272412, 380.555455380889)) < 1e-12);
|
||||
CV_Assert(intrinsicLeft.alpha == 0);
|
||||
CV_Assert(cv::norm(intrinsicLeft.k - cv::Vec4d(-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicRight.f - cv::Vec2d(560.395452535348, 561.90171021422)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicRight.c - cv::Vec2d(678.971652040359, 380.401340535339)) < 1e-12);
|
||||
CV_Assert(intrinsicRight.alpha == 0);
|
||||
CV_Assert(cv::norm(intrinsicRight.k - cv::Vec4d(-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222)) < 1e-12);
|
||||
CV_Assert(cv::norm(omcur - cv::Vec3d(-0.00605728469659871, 0.006287139337868821, -0.06960627514977027)) < 1e-12);
|
||||
CV_Assert(cv::norm(Tcur - cv::Vec3d(-0.09940272472412097, 0.002708121392654134, 0.001293302924726987)) < 1e-12);
|
||||
|
||||
CV_Assert(cv::norm(intrinsicLeft_errors.f - cv::Vec2d(0.71024293066476, 0.717596249442966)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicLeft_errors.c - cv::Vec2d(0.782164491020839, 0.538718002947604)) < 1e-12);
|
||||
CV_Assert(intrinsicLeft_errors.alpha == 0);
|
||||
CV_Assert(cv::norm(intrinsicLeft_errors.k - cv::Vec4d(0.00525819017878291, 0.0179451746982225, 0.0236417266063274, 0.0104757238170252)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicRight_errors.f - cv::Vec2d(0.748707568264116, 0.745355483082726)) < 1e-12);
|
||||
CV_Assert(cv::norm(intrinsicRight_errors.c - cv::Vec2d(0.788236834082615, 0.538854504490304)) < 1e-12);
|
||||
CV_Assert(intrinsicRight_errors.alpha == 0);
|
||||
CV_Assert(cv::norm(intrinsicRight_errors.k - cv::Vec4d(0.00534743998208779, 0.0175804116710864, 0.0226549382734192, 0.00979255348533809)) < 1e-12);
|
||||
CV_Assert(cv::norm(om_error - cv::Vec3d(0.0005903298904975326, 0.001048251127879415, 0.0001775640833531587)) < 1e-12);
|
||||
CV_Assert(cv::norm(T_error - cv::Vec3d(6.691282702437657e-05, 5.566841336891827e-05, 0.0001954901454589594)) < 1e-12);
|
||||
|
||||
|
||||
Matx33d _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
|
||||
_K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
|
||||
0, intrinsicLeft.f[1], intrinsicLeft.c[1],
|
||||
0, 0, 1);
|
||||
|
||||
Matx33d _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
|
||||
_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(K2, 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()) Tcur.convertTo(R, R.empty() ? CV_64FC1 : R.type());
|
||||
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());
|
||||
|
||||
// if (rvecs1.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type());
|
||||
// if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type());
|
||||
|
||||
|
||||
return 0;
|
||||
return rms;
|
||||
}
|
||||
|
||||
namespace cv{ namespace {
|
||||
|
||||
@ -368,6 +368,241 @@ TEST_F(FisheyeTest, rectify)
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(FisheyeTest, stereoCalibrate)
|
||||
{
|
||||
const int n_images = 34;
|
||||
|
||||
const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
|
||||
|
||||
std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
|
||||
std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
|
||||
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
|
||||
|
||||
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_left.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
|
||||
fs_left.release();
|
||||
|
||||
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_right.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
|
||||
fs_right.release();
|
||||
|
||||
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_object.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
|
||||
fs_object.release();
|
||||
|
||||
std::ofstream fs;
|
||||
|
||||
for (size_t i = 0; i < leftPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "left");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < leftPoints[i].size(); j++)
|
||||
{
|
||||
double x = leftPoints[i][j].x;
|
||||
double y = leftPoints[i][j].y;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < rightPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "right");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < rightPoints[i].size(); j++)
|
||||
{
|
||||
double x = rightPoints[i][j].x;
|
||||
double y = rightPoints[i][j].y;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < objectPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "object");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < objectPoints[i].size(); j++)
|
||||
{
|
||||
double x = objectPoints[i][j].x;
|
||||
double y = objectPoints[i][j].y;
|
||||
double z = objectPoints[i][j].z;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::setprecision(15) << "; " << z;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
cv::Matx33d K1, K2, R;
|
||||
cv::Vec3d T;
|
||||
cv::Vec4d D1, D2;
|
||||
|
||||
int flag = 0;
|
||||
flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
|
||||
flag |= cv::Fisheye::CALIB_CHECK_COND;
|
||||
flag |= cv::Fisheye::CALIB_FIX_SKEW;
|
||||
// flag |= cv::Fisheye::CALIB_FIX_INTRINSIC;
|
||||
|
||||
cv::Fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
|
||||
K1, D1, K2, D2, imageSize, R, T, flag,
|
||||
cv::TermCriteria(3, 12, 0));
|
||||
|
||||
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
|
||||
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
|
||||
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
|
||||
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
|
||||
cv::Matx33d K1_correct (561.195925927249, 0, 621.282400272412,
|
||||
0, 562.849402029712, 380.555455380889,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Matx33d K2_correct (560.395452535348, 0, 678.971652040359,
|
||||
0, 561.90171021422, 380.401340535339,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Vec4d D1_correct (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
|
||||
cv::Vec4d D2_correct (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
|
||||
|
||||
EXPECT_MAT_NEAR(R, R_correct, 1e-10);
|
||||
EXPECT_MAT_NEAR(T, T_correct, 1e-10);
|
||||
|
||||
EXPECT_MAT_NEAR(K1, K1_correct, 1e-10);
|
||||
EXPECT_MAT_NEAR(K2, K2_correct, 1e-10);
|
||||
|
||||
EXPECT_MAT_NEAR(D1, D1_correct, 1e-10);
|
||||
EXPECT_MAT_NEAR(D2, D2_correct, 1e-10);
|
||||
|
||||
}
|
||||
|
||||
TEST_F(FisheyeTest, stereoCalibrateFixIntrinsic)
|
||||
{
|
||||
const int n_images = 34;
|
||||
|
||||
const std::string folder =combine(datasets_repository_path, "calib-3_stereo_from_JY");
|
||||
|
||||
std::vector<std::vector<cv::Point2d> > leftPoints(n_images);
|
||||
std::vector<std::vector<cv::Point2d> > rightPoints(n_images);
|
||||
std::vector<std::vector<cv::Point3d> > objectPoints(n_images);
|
||||
|
||||
cv::FileStorage fs_left(combine(folder, "left.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_left.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_left[cv::format("image_%d", i )] >> leftPoints[i];
|
||||
fs_left.release();
|
||||
|
||||
cv::FileStorage fs_right(combine(folder, "right.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_right.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_right[cv::format("image_%d", i )] >> rightPoints[i];
|
||||
fs_right.release();
|
||||
|
||||
cv::FileStorage fs_object(combine(folder, "object.xml"), cv::FileStorage::READ);
|
||||
CV_Assert(fs_object.isOpened());
|
||||
for(int i = 0; i < n_images; ++i)
|
||||
fs_object[cv::format("image_%d", i )] >> objectPoints[i];
|
||||
fs_object.release();
|
||||
|
||||
|
||||
std::ofstream fs;
|
||||
|
||||
for (size_t i = 0; i < leftPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "left");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < leftPoints[i].size(); j++)
|
||||
{
|
||||
double x = leftPoints[i][j].x;
|
||||
double y = leftPoints[i][j].y;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < rightPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "right");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < rightPoints[i].size(); j++)
|
||||
{
|
||||
double x = rightPoints[i][j].x;
|
||||
double y = rightPoints[i][j].y;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < objectPoints.size(); i++)
|
||||
{
|
||||
std::string ss = combine(folder, "object");
|
||||
ss = combine_format(ss, "%d", i);
|
||||
fs.open(ss.c_str());
|
||||
CV_Assert(fs.is_open());
|
||||
for (size_t j = 0; j < objectPoints[i].size(); j++)
|
||||
{
|
||||
double x = objectPoints[i][j].x;
|
||||
double y = objectPoints[i][j].y;
|
||||
double z = objectPoints[i][j].z;
|
||||
fs << std::setprecision(15) << x << "; " << y;
|
||||
fs << std::setprecision(15) << "; " << z;
|
||||
fs << std::endl;
|
||||
}
|
||||
fs.close();
|
||||
}
|
||||
|
||||
cv::Matx33d R;
|
||||
cv::Vec3d T;
|
||||
|
||||
int flag = 0;
|
||||
flag |= cv::Fisheye::CALIB_RECOMPUTE_EXTRINSIC;
|
||||
flag |= cv::Fisheye::CALIB_CHECK_COND;
|
||||
flag |= cv::Fisheye::CALIB_FIX_SKEW;
|
||||
flag |= cv::Fisheye::CALIB_FIX_INTRINSIC;
|
||||
|
||||
cv::Matx33d K1 (561.195925927249, 0, 621.282400272412,
|
||||
0, 562.849402029712, 380.555455380889,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Matx33d K2 (560.395452535348, 0, 678.971652040359,
|
||||
0, 561.90171021422, 380.401340535339,
|
||||
0, 0, 1);
|
||||
|
||||
cv::Vec4d D1 (-7.44253716539556e-05, -0.00702662033932424, 0.00737569823650885, -0.00342230256441771);
|
||||
cv::Vec4d D2 (-0.0130785435677431, 0.0284434505383497, -0.0360333869900506, 0.0144724062347222);
|
||||
|
||||
cv::Fisheye::stereoCalibrate(objectPoints, leftPoints, rightPoints,
|
||||
K1, D1, K2, D2, imageSize, R, T, flag,
|
||||
cv::TermCriteria(3, 12, 0));
|
||||
|
||||
cv::Matx33d R_correct( 0.9975587205950972, 0.06953016383322372, 0.006492709911733523,
|
||||
-0.06956823121068059, 0.9975601387249519, 0.005833595226966235,
|
||||
-0.006071257768382089, -0.006271040135405457, 0.9999619062167968);
|
||||
cv::Vec3d T_correct(-0.099402724724121, 0.00270812139265413, 0.00129330292472699);
|
||||
|
||||
|
||||
EXPECT_MAT_NEAR(R, R_correct, 1e-10);
|
||||
EXPECT_MAT_NEAR(T, T_correct, 1e-10);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
/// FisheyeTest::
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user