diff --git a/doc/opencv.bib b/doc/opencv.bib index 5c8898f120..0fd3aa5262 100644 --- a/doc/opencv.bib +++ b/doc/opencv.bib @@ -860,6 +860,22 @@ number = {8}, publisher = {IOP Publishing Ltd} } +@article{Lourakis2009_sba, + author = {Lourakis, Manolis I. A. and Argyros, Antonis A.}, + title = {SBA: A Software Package for Generic Sparse Bundle Adjustment}, + year = {2009}, + month = mar, + journal = {ACM Transactions on Mathematical Software}, + volume = {36}, + number = {1}, + articleno = {2}, + pages = {2:1--2:30}, + numpages = {30}, + publisher = {Association for Computing Machinery}, + doi = {10.1145/1486525.1486527}, + url = {https://scispace.com/pdf/sba-a-software-package-for-generic-sparse-bundle-adjustment-1d4hp0z31z.pdf}, + month_numeric = {3} +} @article{LowIlie2003, author = {Kok-Lim Low, Adrian Ilie}, year = {2003}, @@ -1353,6 +1369,20 @@ publisher = {Taylor \& Francis}, url = {https://www.olivier-augereau.com/docs/2004JGraphToolsTelea.pdf} } +@incollection{Triggs2000_bundle_adjustment, + author = {Triggs, Bill and McLauchlan, Philip F. and Hartley, Richard I. and Fitzgibbon, Andrew W.}, + title = {Bundle Adjustment---A Modern Synthesis}, + booktitle = {Vision Algorithms: Theory and Practice}, + year = {2000}, + pages = {298--372}, + publisher = {Springer}, + series = {Lecture Notes in Computer Science}, + volume = {1883}, + editor = {Triggs, Bill and Zisserman, Andrew and Szeliski, Richard}, + doi = {10.1007/3-540-44480-7_21}, + isbn = {978-3-540-67973-8}, + url = {https://www.cs.jhu.edu/~misha/ReadingSeminar/Papers/Triggs00.pdf} +} @article{Tsai89, author = {R. Y. Tsai and R. K. Lenz}, journal = {IEEE Transactions on Robotics and Automation}, diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index fecc3ae254..7bd8421054 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/modules/calib3d/include/opencv2/calib3d.hpp @@ -627,7 +627,8 @@ enum { CALIB_NINTRINSIC = 18, // for stereo rectification CALIB_ZERO_DISPARITY = 0x00400, CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise - CALIB_USE_EXTRINSIC_GUESS = (1 << 22) //!< for stereoCalibrate + CALIB_USE_EXTRINSIC_GUESS = (1 << 22), //!< for stereoCalibrate + CALIB_DISABLE_SCHUR_COMPLEMENT = (1 << 23) //!< disable Schur complement (use Bouguet calibration engine) }; //! the algorithm for finding fundamental matrix @@ -1659,6 +1660,7 @@ fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set center ( imageSize is used), and focal distances are computed in a least-squares fashion. Note, that if intrinsic parameters are known, there is no need to use this function just to estimate extrinsic parameters. Use @ref solvePnP instead. +- @ref CALIB_DISABLE_SCHUR_COMPLEMENT Disable Schur complement and use the Bouguet calibration engine (@cite Zhang2000, @cite BouguetMCT). - @ref CALIB_FIX_PRINCIPAL_POINT The principal point is not changed during the global optimization. It stays at the center or at a different location specified when @ref CALIB_USE_INTRINSIC_GUESS is set too. @@ -1693,7 +1695,9 @@ supplied distCoeffs matrix is used. Otherwise, it is set to 0. @return the overall RMS re-projection error. The function estimates the intrinsic camera parameters and extrinsic parameters for each of the -views. The algorithm is based on @cite Zhang2000 and @cite BouguetMCT . The coordinates of 3D object +views. By default, the optimization follows a sparse bundle adjustment formulation with Schur +complement; see @cite Triggs2000_bundle_adjustment and @cite Lourakis2009_sba for background. Use +@ref CALIB_DISABLE_SCHUR_COMPLEMENT to switch to the Bouguet calibration engine. The coordinates of 3D object points and their corresponding 2D projections in each view must be specified. That may be achieved by using an object with known geometry and easily detectable feature points. Such an object is called a calibration rig or calibration pattern, and OpenCV has built-in support for a chessboard as @@ -1716,6 +1720,10 @@ The algorithm performs the following steps: the projected (using the current estimates for camera parameters and the poses) object points objectPoints. See @ref projectPoints for details. +- In practice, robust acquisition is essential for stable results: use multiple board poses with + significant tilt, avoid collecting all views at a single working distance, span the expected + working-distance range (a larger board with larger squares can help for longer distances). + @note If you use a non-square (i.e. non-N-by-N) grid and @ref findChessboardCorners for calibration, and @ref calibrateCamera returns bad values (zero distortion coefficients, \f$c_x\f$ and @@ -1801,8 +1809,8 @@ less precise and less stable in some rare cases. @return the overall RMS re-projection error. The function estimates the intrinsic camera parameters and extrinsic parameters for each of the -views. The algorithm is based on @cite Zhang2000, @cite BouguetMCT and @cite strobl2011iccv. See -#calibrateCamera for other detailed explanations. +views. The object-releasing extension follows @cite strobl2011iccv and uses the same optimization +core as #calibrateCamera. See #calibrateCamera for other detailed explanations. @sa calibrateCamera, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort */ diff --git a/modules/calib3d/perf/perf_calibratecamera.cpp b/modules/calib3d/perf/perf_calibratecamera.cpp new file mode 100644 index 0000000000..94d3ba9cb2 --- /dev/null +++ b/modules/calib3d/perf/perf_calibratecamera.cpp @@ -0,0 +1,100 @@ +// This file is part of OpenCV project. +// It is subject to the license terms in the LICENSE file found in the top-level directory +// of this distribution and at http://opencv.org/license.html +#include "perf_precomp.hpp" + +#include "opencv2/core/utils/filesystem.hpp" + +namespace opencv_test { + +static std::vector loadBulkImages(size_t max_images) +{ + const std::string data_dir = findDataDirectory("perf/calib3d/bulk_n500", false); + std::vector image_paths; + cv::utils::fs::glob(data_dir, "*.png", image_paths, false, false); + if (image_paths.empty()) + cv::utils::fs::glob(data_dir, "*.jpg", image_paths, false, false); + if (image_paths.empty()) + throw SkipTestException("No images found in perf/calib3d/bulk_n500"); + + std::sort(image_paths.begin(), image_paths.end()); + if (image_paths.size() > max_images) + image_paths.resize(max_images); + return image_paths; +} + +static std::vector buildObjectPoints(const Size& pattern_size, float square_size) +{ + std::vector object_points; + object_points.reserve(pattern_size.area()); + for (int y = 0; y < pattern_size.height; ++y) + for (int x = 0; x < pattern_size.width; ++x) + object_points.push_back(Point3f(x * square_size, y * square_size, 0.f)); + return object_points; +} + +PERF_TEST(CalibrateCamera, DISABLED_BulkImages_N500) +{ + applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_SIZE_HD); + + const Size pattern_size(6, 8); + const size_t max_images = 500; + + std::vector image_paths = loadBulkImages(max_images); + std::vector object_pattern = buildObjectPoints(pattern_size, 1.0f); + + std::vector > image_points; + std::vector > object_points; + image_points.reserve(image_paths.size()); + object_points.reserve(image_paths.size()); + + Size image_size; + for (const auto& path : image_paths) + { + Mat gray = imread(path, IMREAD_GRAYSCALE); + ASSERT_FALSE(gray.empty()) << "Can't read image: " << path; + if (image_size.empty()) + image_size = gray.size(); + else + ASSERT_EQ(gray.size(), image_size) << "Mismatched image size: " << path; + + std::vector corners; + bool found = findChessboardCorners( + gray, pattern_size, corners, + CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); + ASSERT_TRUE(found) << "Chessboard not found: " << path; + + cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), + TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1)); + + image_points.push_back(corners); + object_points.push_back(object_pattern); + } + + ASSERT_FALSE(image_points.empty()); + + Mat camera_matrix = Mat::eye(3, 3, CV_64F); + Mat dist_coeffs = Mat::zeros(8, 1, CV_64F); + std::vector rvecs; + std::vector tvecs; + double rms = 0.0; + + declare.in(image_points, object_points); + declare.out(camera_matrix, dist_coeffs); + declare.iterations(1); + + TEST_CYCLE() + { + camera_matrix = Mat::eye(3, 3, CV_64F); + dist_coeffs = Mat::zeros(8, 1, CV_64F); + rvecs.clear(); + tvecs.clear(); + rms = calibrateCamera(object_points, image_points, image_size, + camera_matrix, dist_coeffs, rvecs, tvecs, 0); + } + + SANITY_CHECK_NOTHING(); + EXPECT_GT(rms, 0.0); +} + +} // namespace opencv_test diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 09ecf0953c..0c60a9b12c 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -45,17 +45,420 @@ #include "distortion_model.hpp" #include "opencv2/core/core_c.h" #include "opencv2/calib3d/calib3d_c.h" -#include #include /* - This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet - that is (in a large extent) based on the paper: - Z. Zhang. "A flexible new technique for camera calibration". - IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. - The 1st initial port was done by Valery Mosyagin. + This program implements Levenberg-Marquardt optimization using Schur complement to calibrate cameras. + It implements many speedups over the original implemntation. + The techniques used here are primarily based upon the following papers: + [1] "Bundle Adjustment -- A Modern Synthesis", Triggs et al., 1999 + [2] "SBA: A Software Package for Generic Sparse Bundle Adjustment", Lourakis and Argyros, 2000 */ +namespace { + +class SchurLMSolver +{ +public: + enum State { STARTED, CALC_J, CHECK_ERR, DONE }; + + SchurLMSolver() + : currentError(0) + , state(STARTED) + , NINTRINSIC(18) + , nimages(0) + , n_global(0) + , lambdaLg10(-3) + , iters(0) + , prevErrNorm(DBL_MAX) + , solveMethod(cv::DECOMP_CHOLESKY) + { + } + + void init(int _nimages, int _NINTRINSIC, int nObjPoints, + cv::TermCriteria _criteria) + { + nimages = _nimages; + NINTRINSIC = _NINTRINSIC; + n_global = NINTRINSIC + nObjPoints * 3; + criteria = _criteria; + + U.create(n_global, n_global, CV_64F); + ea.create(n_global, 1, CV_64F); + deltaGlobal.create(n_global, 1, CV_64F); + + V.resize(nimages); + V_inv.resize(nimages); + W.resize(nimages); + eb.resize(nimages); + deltaLocal.resize(nimages); + + for (int i = 0; i < nimages; i++) + { + V[i].create(6, 6, CV_64F); + V_inv[i].create(6, 6, CV_64F); + W[i].create(n_global, 6, CV_64F); + eb[i].create(6, 1, CV_64F); + deltaLocal[i].create(6, 1, CV_64F); + } + + state = STARTED; + iters = 0; + lambdaLg10 = -3; + prevErrNorm = DBL_MAX; + solveMethod = cv::DECOMP_CHOLESKY; + } + + void reset() + { + U.setTo(0); + ea.setTo(0); + currentError = 0; + for (int i = 0; i < nimages; i++) + { + V[i].setTo(0); + W[i].setTo(0); + eb[i].setTo(0); + } + } + + void step(const cv::Mat& mask) + { + const double LOG10 = std::log(10.0); + double lambda = std::exp(lambdaLg10 * LOG10); + + // Invert V[i] blocks with regularization + for (int i = 0; i < nimages; i++) + { + cv::Mat V_reg = V[i].clone(); + V_reg.diag() *= (1.0 + lambda); + if (solveMethod == cv::DECOMP_QR) + { + cv::Mat I = cv::Mat::eye(6, 6, CV_64F); + cv::solve(V_reg, I, V_inv[i], solveMethod); + } + else + { + cv::invert(V_reg, V_inv[i], solveMethod); + } + } + + // Form Schur complement S and reduced RHS e + cv::Mat S = U.clone(); + S.diag() *= (1.0 + lambda); + cv::Mat e = ea.clone(); + + for (int i = 0; i < nimages; i++) + { + cv::Mat WtVinv = W[i] * V_inv[i]; + S -= WtVinv * W[i].t(); + e -= WtVinv * eb[i]; + } + + // Extract active submatrix using mask + std::vector mask_global(n_global); + const uchar* mask_ptr = mask.ptr(); + std::copy(mask_ptr, mask_ptr + NINTRINSIC, mask_global.begin()); + if (n_global > NINTRINSIC) + { + // Object points parameters follow intrinsic parameters in the global update vector + std::copy(mask_ptr + NINTRINSIC + nimages * 6, + mask_ptr + NINTRINSIC + nimages * 6 + (n_global - NINTRINSIC), + mask_global.begin() + NINTRINSIC); + } + + int nactive = 0; + for (int i = 0; i < n_global; i++) + if (mask_global[i]) nactive++; + + if (nactive == 0) + { + deltaGlobal.setTo(0); + for (int i = 0; i < nimages; i++) + deltaLocal[i].setTo(0); + return; + } + + cv::Mat S_sub(nactive, nactive, CV_64F); + cv::Mat e_sub(nactive, 1, CV_64F); + + int ii = 0; + for (int i = 0; i < n_global; i++) + { + if (!mask_global[i]) continue; + e_sub.at(ii) = e.at(i); + int jj = 0; + for (int j = 0; j < n_global; j++) + { + if (!mask_global[j]) continue; + S_sub.at(ii, jj) = S.at(i, j); + jj++; + } + ii++; + } + + // Solve the reduced system + cv::Mat delta_sub; + cv::solve(S_sub, e_sub, delta_sub, solveMethod); + + // Distribute the solution into the full update vector + deltaGlobal.setTo(0); + int j = 0; + for (int i = 0; i < n_global; i++) + { + if (mask_global[i]) + deltaGlobal.at(i) = delta_sub.at(j++); + } + + // Back-substitute to find the update for extrinsic parameters + for (int i = 0; i < nimages; i++) + { + deltaLocal[i] = V_inv[i] * (eb[i] - W[i].t() * deltaGlobal); + } + } + + bool iterate(double errNorm, bool& needsJacobian) + { + needsJacobian = false; + + if (state == DONE) + { + return false; + } + + if (state == STARTED) + { + state = CALC_J; + needsJacobian = true; + return true; + } + + if (state == CALC_J) + { + prevErrNorm = errNorm; + state = CHECK_ERR; + return true; + } + + // state == CHECK_ERR + if (errNorm > prevErrNorm) + { + // Reject step + if (++lambdaLg10 <= 16) + { + // Re-solve with higher lambda, DON'T recompute Jacobian + return true; + } + // Lambda exhausted + } + + // Accept step + lambdaLg10 = std::max(lambdaLg10 - 1, -16); + prevErrNorm = errNorm; + + if (++iters >= criteria.maxCount) + { + state = DONE; + return false; + } + + state = CALC_J; + needsJacobian = true; + return true; + } + + cv::Mat getGlobalUpdate() const { return deltaGlobal; } + cv::Mat getLocalUpdate(int i) const { return deltaLocal[i]; } + void setSolveMethod(int method) { solveMethod = method; } + + cv::Mat U; + std::vector V; + std::vector W; + cv::Mat ea; + std::vector eb; + double currentError; + + State state; + int NINTRINSIC; + int nimages; + int n_global; + int lambdaLg10; + +private: + int iters; + double prevErrNorm; + cv::TermCriteria criteria; + int solveMethod; + + std::vector V_inv; + cv::Mat deltaGlobal; + std::vector deltaLocal; +}; + +class JAccumulator : public cv::ParallelLoopBody +{ +public: + JAccumulator(SchurLMSolver& _solver, + const cv::Mat& _matM, const cv::Mat& _m, const cv::Mat& _npoints, + const cv::Mat& _param, int _flags, double _aspectRatio, + int _NINTRINSIC, bool _releaseObject, int _maxPoints, + cv::Mutex& _globalMutex) + : solver(_solver), matM(_matM), m(_m), npoints(_npoints), + param(_param), flags(_flags), aspectRatio(_aspectRatio), + NINTRINSIC(_NINTRINSIC), releaseObject(_releaseObject), + maxPoints(_maxPoints), globalMutex(_globalMutex) {} + + void operator()(const cv::Range& range) const CV_OVERRIDE + { + int nimages_total = npoints.checkVector(1, CV_32S); + int so = NINTRINSIC + nimages_total * 6; + + double fx = param.at(0), fy = param.at(1); + double cx = param.at(2), cy = param.at(3); + cv::Matx33d intrin(fx, 0, cx, 0, fy, cy, 0, 0, 1); + cv::Mat dist = param.rowRange(4, 4 + 14); + + cv::Mat JiBuf(maxPoints * 2, NINTRINSIC, CV_64F); + cv::Mat JeBuf(maxPoints * 2, 6, CV_64F); + cv::Mat JoBuf; + if (releaseObject) + JoBuf.create(maxPoints * 2, maxPoints * 3, CV_64F); + cv::Mat errBuf(maxPoints * 2, 1, CV_64F); + + int pos = 0; + for (int i = 0; i < range.start; i++) + pos += npoints.at(i); + + double localErr = 0; + + for (int i = range.start; i < range.end; i++) + { + int ni = npoints.at(i); + int si = NINTRINSIC + i * 6; + + cv::Mat _ri = param.rowRange(si, si + 3); + cv::Mat _ti = param.rowRange(si + 3, si + 6); + + cv::Mat _Mi = matM.colRange(pos, pos + ni); + if (releaseObject) + { + _Mi = param.rowRange(so, so + ni * 3); + _Mi = _Mi.reshape(3, 1); + } + cv::Mat _mi = m.colRange(pos, pos + ni); + + // Buffers for derivatives + cv::Mat Ji = JiBuf.rowRange(0, ni * 2); + Ji.setTo(0); + cv::Mat Je = JeBuf.rowRange(0, ni * 2); + Je.setTo(0); + cv::Mat Jo; + if (releaseObject) + { + Jo = JoBuf.rowRange(0, ni * 2).colRange(0, ni * 3); + Jo.setTo(0); + } + cv::Mat err = errBuf.rowRange(0, ni * 2); + cv::Mat _mp = err.reshape(2, 1); + + cv::Mat _dpdr, _dpdt, _dpdf, _dpdc, _dpdk, _dpdo; + + // Allocate contiguous buffers for Jacobians + _dpdr.create(ni * 2, 3, CV_64F); + _dpdt.create(ni * 2, 3, CV_64F); + _dpdk.create(ni * 2, NINTRINSIC - 4, CV_64F); + + if (!(flags & cv::CALIB_FIX_FOCAL_LENGTH)) + _dpdf.create(ni * 2, 2, CV_64F); + if (!(flags & cv::CALIB_FIX_PRINCIPAL_POINT)) + _dpdc.create(ni * 2, 2, CV_64F); + if (releaseObject) + _dpdo.create(ni * 2, ni * 3, CV_64F); + + cv::projectPoints(_Mi, _ri, _ti, intrin, dist, _mp, + _dpdr, _dpdt, + _dpdf.empty() ? cv::noArray() : _dpdf, + _dpdc.empty() ? cv::noArray() : _dpdc, + _dpdk, + _dpdo.empty() ? cv::noArray() : _dpdo, + (flags & cv::CALIB_FIX_ASPECT_RATIO) ? aspectRatio : 0.); + + // Copy Jacobians from projectPoints buffers to solver blocks + _dpdr.copyTo(Je.colRange(0, 3)); + _dpdt.copyTo(Je.colRange(3, 6)); + + if (!_dpdf.empty()) + { + if (flags & cv::CALIB_FIX_ASPECT_RATIO) + { + cv::Mat dpdf_fx = _dpdf.col(0); + cv::Mat dpdf_fy = _dpdf.col(1); + cv::scaleAdd(dpdf_fx, aspectRatio, dpdf_fy, dpdf_fy); + } + _dpdf.copyTo(Ji.colRange(0, 2)); + } + if (!_dpdc.empty()) + _dpdc.copyTo(Ji.colRange(2, 4)); + + _dpdk.copyTo(Ji.colRange(4, NINTRINSIC)); + + if (!_dpdo.empty()) + _dpdo.copyTo(Jo); + + cv::subtract(_mp, _mi, _mp); + localErr += cv::norm(err, cv::NORM_L2SQR); + + // Accumulate V and eb blocks (per-image) + solver.V[i] = Je.t() * Je; + solver.eb[i] = Je.t() * err; + cv::Mat JitJe = Ji.t() * Je; + JitJe.copyTo(solver.W[i].rowRange(0, NINTRINSIC)); + + if (releaseObject) + { + cv::Mat JotJe = Jo.t() * Je; + JotJe.copyTo(solver.W[i].rowRange(NINTRINSIC, NINTRINSIC + Jo.cols)); + } + + // Accumulate U and ea blocks (shared parameters) + { + cv::AutoLock lock(globalMutex); + solver.U(cv::Rect(0, 0, NINTRINSIC, NINTRINSIC)) += Ji.t() * Ji; + solver.ea.rowRange(0, NINTRINSIC) += Ji.t() * err; + + if (releaseObject) + { + cv::Mat JitJo = Ji.t() * Jo; + solver.U(cv::Rect(NINTRINSIC, 0, Jo.cols, NINTRINSIC)) += JitJo; + solver.U(cv::Rect(0, NINTRINSIC, NINTRINSIC, Jo.cols)) += JitJo.t(); + solver.U(cv::Rect(NINTRINSIC, NINTRINSIC, Jo.cols, Jo.cols)) += Jo.t() * Jo; + solver.ea.rowRange(NINTRINSIC, NINTRINSIC + Jo.cols) += Jo.t() * err; + } + + solver.currentError += localErr; + } + + pos += ni; + localErr = 0; + } + } + +private: + SchurLMSolver& solver; + const cv::Mat& matM; + const cv::Mat& m; + const cv::Mat& npoints; + const cv::Mat& param; + int flags; + double aspectRatio; + int NINTRINSIC; + bool releaseObject; + int maxPoints; + cv::Mutex& globalMutex; +}; + +} // anonymous namespace + namespace cv { static void initIntrinsicParams2D( const Mat& objectPoints, @@ -146,7 +549,6 @@ static void subMatrix(const Mat& src, Mat& dst, CV_Assert(src.type() == CV_64F && dst.type() == CV_64F); int m = (int)rows.size(), n = (int)cols.size(); int i1 = 0, j1 = 0; - const uchar* colsdata = cols.empty() ? 0 : &cols[0]; for(int i = 0; i < m; i++) { if(rows[i]) @@ -154,20 +556,31 @@ static void subMatrix(const Mat& src, Mat& dst, const double* srcptr = src.ptr(i); double* dstptr = dst.ptr(i1++); - for(int j = j1 = 0; j < n; j++) + if(n > 0) { - if(colsdata[j]) - dstptr[j1++] = srcptr[j]; + const uchar* colsdata = cols.data(); + for(int j = j1 = 0; j < n; j++) + { + if(colsdata[j]) + dstptr[j1++] = srcptr[j]; + } } } } } -static double calibrateCameraInternal( const Mat& objectPoints, - const Mat& imagePoints, const Mat& npoints, - Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs, - Mat rvecs, Mat tvecs, Mat newObjPoints, Mat stdDevs, - Mat perViewErr, int flags, const TermCriteria& termCrit ) +/* + This is straight-forward port v3 of Matlab calibration engine by Jean-Yves Bouguet + that is (in a large extent) based on the paper: + Z. Zhang. "A flexible new technique for camera calibration". + IEEE Transactions on Pattern Analysis and Machine Intelligence, 22(11):1330-1334, 2000. + The 1st initial port was done by Valery Mosyagin. +*/ +static double calibrateCameraInternalBouguet( const Mat& objectPoints, + const Mat& imagePoints, const Mat& npoints, + Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs, + Mat rvecs, Mat tvecs, Mat newObjPoints, Mat stdDevs, + Mat perViewErr, int flags, const TermCriteria& termCrit ) { int NINTRINSIC = CALIB_NINTRINSIC; double reprojErr = 0; @@ -621,6 +1034,677 @@ static double calibrateCameraInternal( const Mat& objectPoints, return std::sqrt(reprojErr/total); } +static double calibrateCameraInternalSchur( const Mat& objectPoints, + const Mat& imagePoints, const Mat& npoints, + Size imageSize, int iFixedPoint, Mat& cameraMatrix, Mat& distCoeffs, + Mat rvecs, Mat tvecs, Mat newObjPoints, Mat stdDevs, + Mat perViewErr, int flags, const TermCriteria& termCrit ) +{ + int NINTRINSIC = CALIB_NINTRINSIC; + double reprojErr = 0; + + double aspectRatio = 0.; + int nimages = npoints.checkVector(1, CV_32S); + CV_Assert(nimages >= 1); + int ndistCoeffs = (int)distCoeffs.total(); + bool releaseObject = iFixedPoint > 0 && iFixedPoint < npoints.at(0) - 1; + + // 0. check the parameters & allocate buffers + if (imageSize.width <= 0 || imageSize.height <= 0) + { + CV_Error(cv::Error::StsOutOfRange, "image width and height must be positive"); + } + + if (flags & CALIB_TILTED_MODEL) + { + // when the tilted sensor model is used the distortion coefficients matrix must have 14 parameters + if (ndistCoeffs != 14) + { + CV_Error(cv::Error::StsBadArg, "The tilted sensor model must have 14 parameters"); + } + }else + { + // when the thin prism model is used the distortion coefficients matrix must have 12 parameters + if (flags & CALIB_THIN_PRISM_MODEL) + { + if (ndistCoeffs != 12) + { + CV_Error(cv::Error::StsBadArg, "Thin prism model must have 12 parameters"); + } + } + } + + if (!rvecs.empty()) + { + int cn = rvecs.channels(); + CV_Assert(rvecs.depth() == CV_32F || rvecs.depth() == CV_64F); + CV_Assert((rvecs.rows == nimages && (rvecs.cols * cn == 3)) || + (rvecs.rows == 1 && rvecs.cols == nimages && cn == 3)); + } + + if (!tvecs.empty()) + { + int cn = tvecs.channels(); + CV_Assert(tvecs.depth() == CV_32F || tvecs.depth() == CV_64F); + CV_Assert((tvecs.rows == nimages && tvecs.cols * cn == 3) || + (tvecs.rows == 1 && tvecs.cols == nimages && cn == 3)); + } + + CV_Assert(cameraMatrix.type() == CV_32F || cameraMatrix.type() == CV_64F); + CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3); + + CV_Assert(distCoeffs.type() == CV_32F || distCoeffs.type() == CV_64F); + CV_Assert(distCoeffs.rows == 1 || distCoeffs.cols == 1); + CV_Assert(ndistCoeffs == 4 || ndistCoeffs == 5 || ndistCoeffs == 8 || + ndistCoeffs == 12 || ndistCoeffs == 14); + + int total = 0, maxPoints = 0; + for(int i = 0; i < nimages; i++ ) + { + int ni = npoints.at(i); + if( ni < 4 ) + { + CV_Error_( cv::Error::StsOutOfRange, ("The number of points in the view #%d is < 4", i)); + } + maxPoints = MAX( maxPoints, ni ); + total += ni; + } + + if( !newObjPoints.empty() ) + { + int cn = newObjPoints.channels(); + CV_Assert(newObjPoints.depth() == CV_32F || newObjPoints.depth() == CV_64F); + CV_Assert((newObjPoints.rows == maxPoints && newObjPoints.cols*cn == 3) || + (newObjPoints.rows == 1 && newObjPoints.cols == maxPoints && cn == 3)); + } + + if( !stdDevs.empty() ) + { + int cn = stdDevs.channels(); + CV_Assert(stdDevs.depth() == CV_32F || stdDevs.depth() == CV_64F); + int nstddev = nimages*6 + NINTRINSIC + (releaseObject ? maxPoints*3 : 0); + + CV_Assert((stdDevs.rows == nstddev && stdDevs.cols*cn == 1) || + (stdDevs.rows == 1 && stdDevs.cols == nstddev && cn == 1)); + } + + Mat matM( 1, total, CV_64FC3 ); + Mat _m( 1, total, CV_64FC2 ); + Mat allErrors(1, total, CV_64FC2); + + if (objectPoints.channels() == 3) + { + objectPoints.convertTo(matM, CV_64F); + }else + { + convertPointsToHomogeneous(objectPoints, matM); + matM.convertTo(matM, CV_64F); + } + + if (imagePoints.channels() == 2) + { + imagePoints.convertTo(_m, CV_64F); + }else + { + convertPointsFromHomogeneous(imagePoints, _m); + _m.convertTo(_m, CV_64F); + } + + int nparams = NINTRINSIC + nimages*6; + if( releaseObject ) + nparams += maxPoints * 3; + + std::vector k(14, 0.0); + Mat _k( distCoeffs.rows, distCoeffs.cols, CV_64F, k.data()); + if( distCoeffs.total() < 8 ) + { + if( distCoeffs.total() < 5 ) + flags |= CALIB_FIX_K3; + flags |= CALIB_FIX_K4 | CALIB_FIX_K5 | CALIB_FIX_K6; + } + const double minValidAspectRatio = 0.01; + const double maxValidAspectRatio = 100.0; + + Matx33d A; + cameraMatrix.convertTo(A, CV_64F); + + // 1. initialize intrinsic parameters & LM solver + if( flags & CALIB_USE_INTRINSIC_GUESS ) + { + if( A(0, 0) <= 0 || A(1, 1) <= 0 ) + CV_Error( cv::Error::StsOutOfRange, "Focal length (fx and fy) must be positive" ); + if( A(0, 2) < 0 || A(0, 2) >= imageSize.width || + A(1, 2) < 0 || A(1, 2) >= imageSize.height ) + CV_Error( cv::Error::StsOutOfRange, "Principal point must be within the image" ); + if( fabs(A(0, 1)) > 1e-5 ) + CV_Error( cv::Error::StsOutOfRange, "Non-zero skew is not supported by the function" ); + if( fabs(A(1, 0)) > 1e-5 || fabs(A(2, 0)) > 1e-5 || + fabs(A(2, 1)) > 1e-5 || fabs(A(2,2)-1) > 1e-5 ) + CV_Error( cv::Error::StsOutOfRange, + "The intrinsic matrix must have [fx 0 cx; 0 fy cy; 0 0 1] shape" ); + A(0, 1) = A(1, 0) = A(2, 0) = A(2, 1) = 0.; + A(2, 2) = 1.; + + if (flags & CALIB_FIX_ASPECT_RATIO) + { + aspectRatio = A(0, 0) / A(1, 1); + if (aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio) + { + CV_Error(cv::Error::StsOutOfRange, + "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect"); + } + } + distCoeffs.convertTo(_k, CV_64F); + }else + { + Scalar mean, sdv; + meanStdDev(matM, mean, sdv); + if (fabs(mean[2]) > 1e-5 || fabs(sdv[2]) > 1e-5) + { + CV_Error(cv::Error::StsBadArg, + "For non-planar calibration rigs the initial intrinsic matrix must be specified"); + } + for (int i = 0; i < total; i++) + { + matM.at(i).z = 0.; + } + + if (flags & CALIB_FIX_ASPECT_RATIO) + { + aspectRatio = A(0, 0); + aspectRatio /= A(1, 1); + if (aspectRatio < minValidAspectRatio || aspectRatio > maxValidAspectRatio) + { + CV_Error(cv::Error::StsOutOfRange, + "The specified aspect ratio (= cameraMatrix[0][0] / cameraMatrix[1][1]) is incorrect"); + } + } + initIntrinsicParams2D(matM, _m, npoints, imageSize, A, aspectRatio); + } + + SchurLMSolver solver; + solver.init(nimages, NINTRINSIC, releaseObject ? maxPoints : 0, termCrit); + if (flags & CALIB_USE_LU) + solver.setSolveMethod(cv::DECOMP_LU); + else if (flags & CALIB_USE_QR) + solver.setSolveMethod(cv::DECOMP_QR); + + Mat_ param_m(nparams, 1); + std::vector mask_vec(nparams, 1); + + param_m(0) = A(0, 0); param_m(1) = A(1, 1); param_m(2) = A(0, 2); param_m(3) = A(1, 2); + for (int i = 0; i < 14; i++) + param_m(4 + i) = k[i]; + + if (flags & CALIB_FIX_ASPECT_RATIO) + mask_vec[0] = 0; + if (flags & CALIB_FIX_FOCAL_LENGTH) + mask_vec[0] = mask_vec[1] = 0; + if (flags & CALIB_FIX_PRINCIPAL_POINT) + mask_vec[2] = mask_vec[3] = 0; + if (flags & CALIB_ZERO_TANGENT_DIST) + { + param_m(6) = param_m(7) = 0; + mask_vec[6] = mask_vec[7] = 0; + } + if (!(flags & CALIB_RATIONAL_MODEL)) + flags |= CALIB_FIX_K4 + CALIB_FIX_K5 + CALIB_FIX_K6; + if (!(flags & CALIB_THIN_PRISM_MODEL)) + flags |= CALIB_FIX_S1_S2_S3_S4; + if (!(flags & CALIB_TILTED_MODEL)) + flags |= CALIB_FIX_TAUX_TAUY; + + mask_vec[4] = !(flags & CALIB_FIX_K1); + mask_vec[5] = !(flags & CALIB_FIX_K2); + if (flags & CALIB_FIX_TANGENT_DIST) + mask_vec[6] = mask_vec[7] = 0; + mask_vec[8] = !(flags & CALIB_FIX_K3); + mask_vec[9] = !(flags & CALIB_FIX_K4); + mask_vec[10] = !(flags & CALIB_FIX_K5); + mask_vec[11] = !(flags & CALIB_FIX_K6); + + if (flags & CALIB_FIX_S1_S2_S3_S4) + { + mask_vec[12] = 0; + mask_vec[13] = 0; + mask_vec[14] = 0; + mask_vec[15] = 0; + } + + if (flags & CALIB_USE_INTRINSIC_GUESS) + { + CV_Assert(nparams == NINTRINSIC + nimages * 6); + } + + if (flags & CALIB_FIX_ASPECT_RATIO) + { + mask_vec[0] = 0; + if (aspectRatio <= 0) + CV_Error(Error::StsBadArg, "CALIB_FIX_ASPECT_RATIO is set but aspectRatio is not positive"); + } + + if (flags & CALIB_FIX_TAUX_TAUY) + { + mask_vec[16] = mask_vec[17] = 0; + } + + if (releaseObject) + { + int s = NINTRINSIC + nimages * 6; + std::copy(matM.ptr(), matM.ptr(0, maxPoints - 1) + 3, + param_m.ptr() + s); + mask_vec[s + 0] = 0; + mask_vec[s + 1] = 0; + mask_vec[s + 2] = 0; + mask_vec[s + iFixedPoint * 3 + 0] = 0; + mask_vec[s + iFixedPoint * 3 + 1] = 0; + mask_vec[s + iFixedPoint * 3 + 2] = 0; + mask_vec[nparams - 1] = 0; + } + + Mat mask(mask_vec); + int nparams_nz = countNonZero(mask); + + if (nparams_nz >= 2 * total) + { + CV_Error_(Error::StsBadArg, + ("There should be less vars to optimize (having %d) than the " + "number of residuals (%d = 2 per point)", nparams_nz, 2 * total)); + } + + // Initialize extrinsic parameters + for (int i = 0, pos = 0; i < nimages; i++) + { + int ni = npoints.at(i); + + int s = NINTRINSIC + i*6; + Mat _ri = param_m.rowRange(s, s + 3); + Mat _ti = param_m.rowRange(s + 3, s + 6); + + Mat _Mi = matM.colRange(pos, pos + ni); + Mat _mi = _m.colRange(pos, pos + ni); + + findExtrinsicCameraParams2(_Mi, _mi, Mat(A), _k, _ri, _ti, 0); + + pos += ni; + } + + // 3. run the optimization + Mat_ prev_param = param_m.clone(); + Mutex globalMutex; + + // Compute initial error + Mat allErrorsBuf(1, total, CV_64FC2); + reprojErr = 0; + int so = NINTRINSIC + nimages * 6; + int pos = 0; + for (int i = 0; i < nimages; i++) + { + int ni = npoints.at(i); + int si = NINTRINSIC + i * 6; + + Mat _ri = param_m.rowRange(si, si + 3); + Mat _ti = param_m.rowRange(si + 3, si + 6); + + Mat _Mi = matM.colRange(pos, pos + ni); + if (releaseObject) + { + _Mi = param_m.rowRange(so, so + ni * 3); + _Mi = _Mi.reshape(3, 1); + } + Mat _mi = _m.colRange(pos, pos + ni); + Mat _me = allErrorsBuf.colRange(pos, pos + ni); + + double fx = param_m(0), fy = param_m(1), cx = param_m(2), cy = param_m(3); + if (flags & CALIB_FIX_ASPECT_RATIO) + fx = fy * aspectRatio; + Matx33d intrin(fx, 0, cx, 0, fy, cy, 0, 0, 1); + Mat dist = param_m.rowRange(4, 4 + 14); + + projectPoints(_Mi, _ri, _ti, intrin, dist, _me); + subtract(_me, _mi, _me); + + reprojErr += norm(_me, NORM_L2SQR); + pos += ni; + } + + bool recomputeFinalErrors = (termCrit.maxCount == 0); + bool jacobianAtCurrentParams = false; + if (termCrit.maxCount > 0) + { + solver.reset(); + + parallel_for_(Range(0, nimages), + JAccumulator(solver, matM, _m, npoints, param_m, + flags, aspectRatio, NINTRINSIC, + releaseObject, maxPoints, globalMutex)); + jacobianAtCurrentParams = true; + // JAccumulator acc(solver, matM, _m, npoints, param_m, + // flags, aspectRatio, NINTRINSIC, + // releaseObject, maxPoints, globalMutex); + // acc(Range(0, nimages)); + solver.step(mask); + + double prevErr = reprojErr; + // Apply the initial step + param_m.copyTo(prev_param); + // 1. Global parameters (Intrinsics + Objects) + for (int kk = 0; kk < NINTRINSIC; kk++) + param_m(kk) -= solver.getGlobalUpdate().at(kk); + + if (releaseObject) + { + int param_obj_start = NINTRINSIC + nimages * 6; + int num_obj_params = solver.n_global - NINTRINSIC; + for (int kk = 0; kk < num_obj_params; kk++) + param_m(param_obj_start + kk) -= solver.getGlobalUpdate().at(NINTRINSIC + kk); + } + for (int i = 0; i < nimages; i++) + { + int si = NINTRINSIC + i * 6; + for (int kk = 0; kk < 6; kk++) + param_m(si + kk) -= solver.getLocalUpdate(i).at(kk); + } + jacobianAtCurrentParams = false; + for (int iter = 0; iter < termCrit.maxCount; iter++) + { + if (flags & CALIB_FIX_ASPECT_RATIO) + { + param_m(0) = param_m(1) * aspectRatio; + } + + reprojErr = 0; + int pos_iter = 0; + for (int i = 0; i < nimages; i++) + { + int ni = npoints.at(i); + int si = NINTRINSIC + i * 6; + + Mat _ri = param_m.rowRange(si, si + 3); + Mat _ti = param_m.rowRange(si + 3, si + 6); + + Mat _Mi = matM.colRange(pos_iter, pos_iter + ni); + if (releaseObject) + { + _Mi = param_m.rowRange(so, so + ni * 3).reshape(3, 1); + } + Mat _mi = _m.colRange(pos_iter, pos_iter + ni); + Mat _me = allErrorsBuf.colRange(pos_iter, pos_iter + ni); + + double fx = param_m(0), fy = param_m(1), cx = param_m(2), cy = param_m(3); + Matx33d intrin(fx, 0, cx, 0, fy, cy, 0, 0, 1); + Mat dist = param_m.rowRange(4, 4 + 14); + + projectPoints(_Mi, _ri, _ti, intrin, dist, _me); + subtract(_me, _mi, _me); + + double viewErr = norm(_me, NORM_L2SQR); + if (!perViewErr.empty()) + { + perViewErr.at(i) = std::sqrt(viewErr / ni); + } + reprojErr += viewErr; + pos_iter += ni; + } + + // Accept or reject the step + if (reprojErr < prevErr) + { + // Step accepted + solver.lambdaLg10 = std::max(solver.lambdaLg10 - 1, -16); + prevErr = reprojErr; + + // Check convergence BEFORE saving new prev_param + double paramChange = norm(param_m, prev_param, NORM_L2) / + (norm(prev_param, NORM_L2) + DBL_EPSILON); + param_m.copyTo(prev_param); + if (paramChange < termCrit.epsilon) + { + break; + } + + // Compute new Jacobian at new position + solver.reset(); + parallel_for_(Range(0, nimages), + JAccumulator(solver, matM, _m, npoints, param_m, + flags, aspectRatio, NINTRINSIC, + releaseObject, maxPoints, globalMutex)); + jacobianAtCurrentParams = true; + }else + { + // Step rejected, increase lambda and recompute step + if (++solver.lambdaLg10 > 16) + { + // Lambda exhausted, restore best params + prev_param.copyTo(param_m); + recomputeFinalErrors = true; + break; + } + + // Revert params + prev_param.copyTo(param_m); + } + + // Compute step (possibly with new lambda) + solver.step(mask); + + // Apply step. Update layouts: + // deltaGlobal: [Intrinsics (0..NINTRINSIC-1) | Object points (NINTRINSIC..n_global-1)] + // param_m: [Intrinsics | Extrinsic parameters | Object points] + + // Apply Intrinsics update + for (int kk = 0; kk < NINTRINSIC; kk++) + { + param_m(kk) -= solver.getGlobalUpdate().at(kk); + } + + // Apply Objects update (if any) + if (releaseObject) + { + int param_obj_start = NINTRINSIC + nimages * 6; + int num_obj_params = solver.n_global - NINTRINSIC; + for (int kk = 0; kk < num_obj_params; kk++) + { + param_m(param_obj_start + kk) -= solver.getGlobalUpdate().at(NINTRINSIC + kk); + } + } + + // Apply Local parameters (Extrinsics) + for (int i = 0; i < nimages; i++) + { + int si = NINTRINSIC + i * 6; + for (int kk = 0; kk < 6; kk++) + param_m(si + kk) -= solver.getLocalUpdate(i).at(kk); + } + jacobianAtCurrentParams = false; + } + } + + if (flags & CALIB_FIX_ASPECT_RATIO) + { + param_m(0) = param_m(1) * aspectRatio; + } + + if (recomputeFinalErrors) + { + Mat finalErrorsBuf; + Mat* errorsPtr = &allErrorsBuf; + if (!stdDevs.empty()) + { + finalErrorsBuf.create(1, total, CV_64FC2); + errorsPtr = &finalErrorsBuf; + } + + Mat& errors = *errorsPtr; + reprojErr = 0; + pos = 0; + for (int i = 0; i < nimages; i++) + { + int ni = npoints.at(i); + int si = NINTRINSIC + i * 6; + + Mat _ri = param_m.rowRange(si, si + 3); + Mat _ti = param_m.rowRange(si + 3, si + 6); + + Mat _Mi = matM.colRange(pos, pos + ni); + if (releaseObject) + { + _Mi = param_m.rowRange(so, so + ni * 3).reshape(3, 1); + } + Mat _mi = _m.colRange(pos, pos + ni); + Mat _me = errors.colRange(pos, pos + ni); + + double fx = param_m(0), fy = param_m(1), cx = param_m(2), cy = param_m(3); + Matx33d intrin(fx, 0, cx, 0, fy, cy, 0, 0, 1); + Mat dist = param_m.rowRange(4, 4 + 14); + + projectPoints(_Mi, _ri, _ti, intrin, dist, _me); + subtract(_me, _mi, _me); + + double viewErr = norm(_me, NORM_L2SQR); + if (!perViewErr.empty()) + { + perViewErr.at(i) = std::sqrt(viewErr / ni); + } + reprojErr += viewErr; + pos += ni; + } + } + + if (!stdDevs.empty()) + { + if (!jacobianAtCurrentParams) + { + solver.reset(); + parallel_for_(Range(0, nimages), + JAccumulator(solver, matM, _m, npoints, param_m, + flags, aspectRatio, NINTRINSIC, + releaseObject, maxPoints, globalMutex)); + jacobianAtCurrentParams = true; + } + Mat JtJ = Mat::zeros(nparams, nparams, CV_64F); + + // Manual copy for U to avoid assertion + Mat srcU = solver.U(Rect(0, 0, NINTRINSIC, NINTRINSIC)); + Mat dstU = JtJ(Rect(0, 0, NINTRINSIC, NINTRINSIC)); + for(int r=0; r(r); + double* dptr = dstU.ptr(r); + for(int c=0; c(r); + double* dptr = dstV.ptr(r); + for(int c=0; c mask_copy(mask.ptr(), mask.ptr() + nparams); + subMatrix(JtJ, JtJN, mask_copy, mask_copy); + completeSymm(JtJN, false); + cv::invert(JtJN, JtJinv, DECOMP_SVD); + + int nErrors = 2 * total - nparams_nz; + double sigma2 = norm(allErrorsBuf, NORM_L2SQR) / nErrors; + + int j = 0; + for (int s = 0; s < nparams; s++) + { + stdDevs.at(s) = mask.at(s) ? + std::sqrt(JtJinv.at(j, j) * sigma2) : 0.0; + if (mask.at(s)) + { + j++; + } + } + } + + // Store optimization results + A = Matx33d(param_m(0), 0, param_m(2), 0, param_m(1), param_m(3), 0, 0, 1); + Mat(A).convertTo(cameraMatrix, cameraMatrix.type()); + + _k = Mat(distCoeffs.size(), CV_64F, param_m.ptr() + 4); + _k.convertTo(distCoeffs, distCoeffs.type()); + + + if (!newObjPoints.empty() && releaseObject) + { + int s = NINTRINSIC + nimages * 6; + Mat _Mi = param_m.rowRange(s, s + maxPoints * 3); + _Mi.reshape(3, 1).convertTo(newObjPoints, newObjPoints.type()); + + } + for (int i = 0; i < nimages; i++) + { + if (!rvecs.empty()) + { + Mat src = param_m.rowRange(NINTRINSIC + i * 6, NINTRINSIC + i * 6 + 3); + if (rvecs.rows == nimages && rvecs.cols * rvecs.channels() == 9) + { + Mat dst(3, 3, rvecs.depth(), rvecs.ptr(i)); + Rodrigues(src, A); + Mat(A).convertTo(dst, dst.type()); + } + else + { + Mat dst(3, 1, rvecs.depth(), rvecs.rows == 1 ? + rvecs.data + i * rvecs.elemSize() : rvecs.ptr(i)); + src.convertTo(dst, dst.type()); + } + } + if (!tvecs.empty()) + { + Mat src = param_m.rowRange(NINTRINSIC + i * 6 + 3, NINTRINSIC + i * 6 + 6); + Mat dst(3, 1, tvecs.depth(), tvecs.rows == 1 ? + tvecs.data + i * tvecs.elemSize() : tvecs.ptr(i)); + src.convertTo(dst, dst.type()); + } + } + return std::sqrt(reprojErr/total); +} + //////////////////////////////// Stereo Calibration /////////////////////////////////// static double stereoCalibrateImpl( @@ -713,14 +1797,19 @@ static double stereoCalibrateImpl( distCoeffs.convertTo(tdist, CV_64F); } - if( !(flags & (CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS))) - { - Mat mIntr(A[k], /* copyData = */ false); - Mat mDist(distInitial[k], /* copyData = */ false); - calibrateCameraInternal(objectPoints, imagePoints[k], - _npoints, imageSize, 0, mIntr, mDist, - Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit); - } + if( !(flags & (CALIB_FIX_INTRINSIC | CALIB_USE_INTRINSIC_GUESS))) + { + Mat mIntr(A[k], /* copyData = */ false); + Mat mDist(distInitial[k], /* copyData = */ false); + if( flags & CALIB_DISABLE_SCHUR_COMPLEMENT ) + calibrateCameraInternalBouguet(objectPoints, imagePoints[k], + _npoints, imageSize, 0, mIntr, mDist, + Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit); + else + calibrateCameraInternalSchur(objectPoints, imagePoints[k], + _npoints, imageSize, 0, mIntr, mDist, + Mat(), Mat(), Mat(), Mat(), Mat(), flags, termCrit); + } } if( flags & CALIB_SAME_FOCAL_LENGTH ) @@ -1437,51 +2526,60 @@ double calibrateCameraRO(InputArrayOfArrays _objectPoints, errorsM = _perViewErrors.getMat(); } - double reprojErr = calibrateCameraInternal( - objPt, imgPt, npoints, imageSize, iFixedPoint, - cameraMatrix, distCoeffs, - rvecM, tvecM, - newObjPt, - stdDeviationsM, - errorsM, flags, criteria); + double reprojErr = 0; + if( flags & CALIB_DISABLE_SCHUR_COMPLEMENT ) + { + reprojErr = calibrateCameraInternalBouguet( + objPt, imgPt, npoints, imageSize, iFixedPoint, + cameraMatrix, distCoeffs, + rvecM, tvecM, + newObjPt, + stdDeviationsM, + errorsM, flags, criteria); + } + else + { + reprojErr = calibrateCameraInternalSchur( + objPt, imgPt, npoints, imageSize, iFixedPoint, + cameraMatrix, distCoeffs, + rvecM, tvecM, + newObjPt, + stdDeviationsM, + errorsM, flags, criteria); + } if( stddev_needed ) { stdDeviationsM.rowRange(0, CALIB_NINTRINSIC).copyTo(stdDeviationsIntrinsics); } - - if ( stddev_ext_needed ) + if( stddev_ext_needed ) { - int s = CALIB_NINTRINSIC; - stdDeviationsM.rowRange(s, s + nimages*6).copyTo(stdDeviationsExtrinsics); + stdDeviationsM.rowRange(CALIB_NINTRINSIC, CALIB_NINTRINSIC + nimages*6).copyTo(stdDeviationsExtrinsics); } - if( stddev_obj_needed ) { - int s = CALIB_NINTRINSIC + nimages*6; - stdDeviationsM.rowRange(s, s + np*3).copyTo(stdDeviationsObjPoints); - } - - // overly complicated and inefficient rvec/ tvec handling to support vector - for(int i = 0; i < nimages; i++ ) - { - if( rvecs_needed && rvecs_mat_vec) - { - _rvecs.create(3, 1, CV_64F, i, true); - Mat rv = _rvecs.getMat(i); - memcpy(rv.ptr(), rvecM.ptr(i), 3*sizeof(double)); - } - if( tvecs_needed && tvecs_mat_vec) - { - _tvecs.create(3, 1, CV_64F, i, true); - Mat tv = _tvecs.getMat(i); - memcpy(tv.ptr(), tvecM.ptr(i), 3*sizeof(double)); - } + int si = CALIB_NINTRINSIC + nimages*6; + stdDeviationsM.rowRange(si, stdDeviationsM.rows).copyTo(stdDeviationsObjPoints); } cameraMatrix.copyTo(_cameraMatrix); distCoeffs.copyTo(_distCoeffs); + for(int i = 0; i < nimages; i++ ) + { + if( rvecs_needed && rvecs_mat_vec ) + { + _rvecs.create(3, 1, CV_64F, i, true); + Mat rv = _rvecs.getMat(i); + Mat(rvecM.row(i).t()).copyTo(rv); + } + if( tvecs_needed && tvecs_mat_vec ) + { + _tvecs.create(3, 1, CV_64F, i, true); + Mat tv = _tvecs.getMat(i); + Mat(tvecM.row(i).t()).copyTo(tv); + } + } return reprojErr; }