diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index 8350abe3..72e69ce0 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -31,7 +31,7 @@ class CeresCameraIntrinsicsOptimizer { public: static constexpr int POSE_OPT_DIM = 7; - static constexpr int INTRINSICS_DIM = 9; + static constexpr int INTRINSICS_DIM = 12; static constexpr int ROTATION_W_INDEX = 0; static constexpr int ROTATION_X_INDEX = 1; @@ -61,6 +61,13 @@ class CeresCameraIntrinsicsOptimizer */ void setTangentialDistortion(bool use_tangential_distortion); + /*! + * Sets the number of rational distortion coefficients + * @param[in] rational_distortion_coefficients number of radial distortion coefficients + * optimized + */ + void setRationalDistortionCoefficients(int rational_distortion_coefficients); + /*! * Sets the verbose mode * @param[in] verbose whether or not to use tangential distortion @@ -118,6 +125,7 @@ class CeresCameraIntrinsicsOptimizer protected: int radial_distortion_coefficients_; bool use_tangential_distortion_; + int rational_distortion_coefficients_; bool verbose_; cv::Mat_ camera_matrix_; cv::Mat_ distortion_coeffs_; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp index 8750d1e3..099416f4 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -46,12 +46,13 @@ struct ReprojectionResidual ReprojectionResidual( const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, - bool use_tangential_distortion) + bool use_tangential_distortion, int rational_distortion_coeffs) { object_point_ = Eigen::Vector3d(object_point.x, object_point.y, object_point.z); image_point_ = Eigen::Vector2d(image_point.x, image_point.y); radial_distortion_coeffs_ = radial_distortion_coeffs; use_tangential_distortion_ = use_tangential_distortion; + rational_distortion_coeffs_ = rational_distortion_coeffs; } /*! @@ -76,22 +77,32 @@ struct ReprojectionResidual Vector3 object_point_ccs = board_quaternion * (T(1.0) * object_point_) + board_translation; const T null_value = T(0.0); + int distortion_index = 4; const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX]; const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX]; const T & fx = camera_intrinsics[INTRINSICS_FX_INDEX]; const T & fy = camera_intrinsics[INTRINSICS_FY_INDEX]; - const T & k1 = radial_distortion_coeffs_ > 0 ? camera_intrinsics[4] : null_value; - const T & k2 = radial_distortion_coeffs_ > 1 ? camera_intrinsics[5] : null_value; - const T & k3 = radial_distortion_coeffs_ > 2 ? camera_intrinsics[6] : null_value; - const T & p1 = - use_tangential_distortion_ ? camera_intrinsics[4 + radial_distortion_coeffs_] : null_value; - const T & p2 = - use_tangential_distortion_ ? camera_intrinsics[5 + radial_distortion_coeffs_] : null_value; + const T & k1 = + radial_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + const T & k2 = + radial_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + const T & k3 = + radial_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; + const T & p1 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value; + const T & p2 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value; + const T & k4 = + rational_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + const T & k5 = + rational_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + const T & k6 = + rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; const T xp = object_point_ccs.x() / object_point_ccs.z(); const T yp = object_point_ccs.y() / object_point_ccs.z(); const T r2 = xp * xp + yp * yp; - const T d = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + const T d = dn / dd; const T xy = xp * yp; const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); @@ -115,13 +126,15 @@ struct ReprojectionResidual */ static ceres::CostFunction * createResidual( const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, - bool use_tangential_distortion) + bool use_tangential_distortion, int rational_distortion_coeffs) { auto f = new ReprojectionResidual( - object_point, image_point, radial_distortion_coeffs, use_tangential_distortion); + object_point, image_point, radial_distortion_coeffs, use_tangential_distortion, + rational_distortion_coeffs); - int distortion_coefficients = - radial_distortion_coeffs + 2 * static_cast(use_tangential_distortion); + int distortion_coefficients = radial_distortion_coeffs + + 2 * static_cast(use_tangential_distortion) + + rational_distortion_coeffs; ceres::CostFunction * cost_function = nullptr; switch (distortion_coefficients) { @@ -149,6 +162,20 @@ struct ReprojectionResidual cost_function = new ceres::AutoDiffCostFunction(f); break; + case 6: + cost_function = + new ceres::AutoDiffCostFunction(f); + break; + case 7: + cost_function = + new ceres::AutoDiffCostFunction(f); + break; + case 8: + cost_function = + new ceres::AutoDiffCostFunction(f); + break; + default: + throw std::runtime_error("Invalid number of distortion coefficients"); } return cost_function; @@ -158,6 +185,7 @@ struct ReprojectionResidual Eigen::Vector2d image_point_; int radial_distortion_coeffs_; bool use_tangential_distortion_; + int rational_distortion_coeffs_; }; #endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__REPROJECTION_RESIDUAL_HPP_ diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp index 7372bf58..a27e4ea2 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp @@ -45,6 +45,12 @@ void CeresCameraIntrinsicsOptimizer::setTangentialDistortion(bool use_tangential use_tangential_distortion_ = use_tangential_distortion; } +void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients( + int rational_distortion_coefficients) +{ + rational_distortion_coefficients_ = rational_distortion_coefficients; +} + void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; } void CeresCameraIntrinsicsOptimizer::setData( @@ -53,6 +59,40 @@ void CeresCameraIntrinsicsOptimizer::setData( const std::vector> & image_points, const std::vector & rvecs, const std::vector & tvecs) { + if (camera_matrix.cols != 3 || camera_matrix.rows != 3) { + throw std::runtime_error("The camera matrix should be a 3x3 matrix"); + } + + if ( + rational_distortion_coefficients_ > 0 && distortion_coeffs.cols * distortion_coeffs.rows != 8) { + throw std::runtime_error( + "8 distortion coefficients are expected when the rational model is used"); + } + + if ( + rational_distortion_coefficients_ == 0 && + distortion_coeffs.cols * distortion_coeffs.rows != 5) { + throw std::runtime_error( + "5 distortion coefficients are expected when the polynomial model is used"); + } + + if (object_points.size() != image_points.size()) { + throw std::runtime_error( + "The number of object points should be equal to the number of image points"); + } + + if (rvecs.size() != tvecs.size()) { + throw std::runtime_error("The number of rvecs should be equal to the number of tvecs"); + } + + if (rvecs.size() != object_points.size()) { + throw std::runtime_error("The number of rvecs should be equal to the number of object points"); + } + + if (object_points.size() == 0) { + throw std::runtime_error("There should be at least one calibration sample"); + } + camera_matrix_ = camera_matrix.clone(); distortion_coeffs_ = distortion_coeffs.clone(); object_points_ = object_points; @@ -97,17 +137,24 @@ double CeresCameraIntrinsicsOptimizer::getSolution( void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() { // Convert the intrinsics + int num_dist_coefficients = distortion_coeffs_.size().area(); intrinsics_placeholder_[INTRINSICS_CX_INDEX] = camera_matrix_(0, 2); intrinsics_placeholder_[INTRINSICS_CY_INDEX] = camera_matrix_(1, 2); intrinsics_placeholder_[INTRINSICS_FX_INDEX] = camera_matrix_(0, 0); intrinsics_placeholder_[INTRINSICS_FY_INDEX] = camera_matrix_(1, 1); + // plumb bob double k1 = distortion_coeffs_(0); double k2 = distortion_coeffs_(1); double p1 = distortion_coeffs_(2); double p2 = distortion_coeffs_(3); double k3 = distortion_coeffs_(4); + // rational + double k4 = num_dist_coefficients >= 6 ? distortion_coeffs_(5) : 0.0; + double k5 = num_dist_coefficients >= 7 ? distortion_coeffs_(6) : 0.0; + double k6 = num_dist_coefficients >= 8 ? distortion_coeffs_(7) : 0.0; + int index = 4; if (radial_distortion_coefficients_ > 0) { @@ -124,6 +171,16 @@ void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() intrinsics_placeholder_[index++] = p2; } + if (rational_distortion_coefficients_ > 0) { + intrinsics_placeholder_[index++] = k4; + } + if (rational_distortion_coefficients_ > 1) { + intrinsics_placeholder_[index++] = k5; + } + if (rational_distortion_coefficients_ > 2) { + intrinsics_placeholder_[index++] = k6; + } + // Convert the revcs, tvecs into the placeholders pose_placeholders_.resize(object_points_.size()); @@ -156,12 +213,14 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() camera_matrix_(0, 0) = intrinsics_placeholder_[INTRINSICS_FX_INDEX]; camera_matrix_(1, 1) = intrinsics_placeholder_[INTRINSICS_FY_INDEX]; - distortion_coeffs_ = cv::Mat_::zeros(5, 1); double & k1 = distortion_coeffs_(0); double & k2 = distortion_coeffs_(1); double & p1 = distortion_coeffs_(2); double & p2 = distortion_coeffs_(3); double & k3 = distortion_coeffs_(4); + double & k4 = distortion_coeffs_(5); + double & k5 = distortion_coeffs_(6); + double & k6 = distortion_coeffs_(7); int index = 4; @@ -179,6 +238,16 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() p2 = intrinsics_placeholder_[index++]; } + if (rational_distortion_coefficients_ > 0) { + k4 = intrinsics_placeholder_[index++]; + } + if (rational_distortion_coefficients_ > 1) { + k5 = intrinsics_placeholder_[index++]; + } + if (rational_distortion_coefficients_ > 2) { + k6 = intrinsics_placeholder_[index++]; + } + // Convert the revcs, tvecs into the placeholders rvecs_.resize(pose_placeholders_.size()); tvecs_.resize(pose_placeholders_.size()); @@ -253,7 +322,8 @@ void CeresCameraIntrinsicsOptimizer::evaluate() for (std::size_t point_index = 0; point_index < view_object_points.size(); point_index++) { auto f = ReprojectionResidual( view_object_points[point_index], view_image_points[point_index], - radial_distortion_coefficients_, use_tangential_distortion_); + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_); std::array residuals; f(intrinsics_placeholder_.data(), pose_placeholder.data(), residuals.data()); total_ceres_error += residuals[0] * residuals[0] + residuals[1] * residuals[1]; @@ -278,7 +348,8 @@ void CeresCameraIntrinsicsOptimizer::solve() problem.AddResidualBlock( ReprojectionResidual::createResidual( view_object_points[point_index], view_image_points[point_index], - radial_distortion_coefficients_, use_tangential_distortion_), + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_), nullptr, // L2 intrinsics_placeholder_.data(), pose_placeholder.data()); } @@ -302,6 +373,7 @@ void CeresCameraIntrinsicsOptimizer::solve() options.function_tolerance = 1e-10; options.gradient_tolerance = 1e-14; options.num_threads = 8; + options.max_num_consecutive_invalid_steps = 1000; options.use_inner_iterations = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 29c05119..926d7f8c 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -45,19 +45,22 @@ calibrate( const std::vector & image_points_eigen_list, const Eigen::MatrixXd & initial_camera_matrix_eigen, const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs, - bool use_tangential_distortion, bool verbose) + bool use_tangential_distortion, int num_rational_coeffs, bool verbose) { - (void)num_radial_coeffs; - (void)use_tangential_distortion; - (void)verbose; - if ( initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 || object_points_eigen_list.size() != image_points_eigen_list.size() || num_radial_coeffs < 0 || - num_radial_coeffs > 3 || - std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols() > 1) || + num_radial_coeffs > 3 || num_rational_coeffs < 0 || num_rational_coeffs > 3 || + std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1 || (initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols()) != 5) { std::cout << "Invalid parameters" << std::endl; + std::cout << "\t object_points_list.size(): " << object_points_eigen_list.size() << std::endl; + std::cout << "\t image_points_list.size(): " << image_points_eigen_list.size() << std::endl; + std::cout << "\t initial_camera_matrix:\n" << initial_camera_matrix_eigen << std::endl; + std::cout << "\t initial_dist_coeffs:\n" << initial_dist_coeffs_eigen << std::endl; + std::cout << "\t num_radial_coeffs: " << num_radial_coeffs << std::endl; + std::cout << "\t num_rational_coeffs: " << num_rational_coeffs << std::endl; + std::cout << "\t use_tangential_distortion: " << use_tangential_distortion << std::endl; return std::tuple< double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, std::vector>(); @@ -115,6 +118,7 @@ calibrate( CeresCameraIntrinsicsOptimizer optimizer; optimizer.setRadialDistortionCoefficients(num_radial_coeffs); optimizer.setTangentialDistortion(use_tangential_distortion); + optimizer.setRationalDistortionCoefficients(num_rational_coeffs); optimizer.setVerbose(verbose); optimizer.setData( initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv, @@ -179,7 +183,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics )pbdoc", py::arg("object_points_list"), py::arg("image_points_list"), py::arg("initial_camera_matrix"), - py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), + py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), py::arg("num_rational_coeffs"), py::arg("use_tangential_distortion"), py::arg("verbose") = false); #ifdef VERSION_INFO diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index ecb44e3c..e991832b 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -30,16 +30,22 @@ int main(int argc, char ** argv) { - CV_UNUSED(argc); - CV_UNUSED(argv); + if (argc != 5) { + std::cout + << "Usage: " << argv[0] + << " " + << std::endl; + return 1; + } // Global config int cols = 6; int rows = 8; std::size_t max_samples = 50; std::size_t mini_calibration_samples = 20; - bool use_tangent_distortion = true; - int num_radial_distortion_coeffs = 3; + int num_radial_distortion_coeffs = atoi(argv[2]); + bool use_tangent_distortion = atoi(argv[3]); + int num_rational_distortion_coeffs = atoi(argv[4]); // Placeholders std::vector> all_object_points; @@ -102,16 +108,29 @@ int main(int argc, char ** argv) params.minDistBetweenBlobs = (min_dist_between_blobs_percentage * std::max(size.height, size.width) / 100.0); - cv::Size pattern(cols, rows); // w x h format - std::vector centers; // this will be filled by the detected centers + cv::Size pattern(cols, rows); // w x h format + std::vector circle_centers, + chessboard_centers; // this will be filled by the detected centers - bool found = cv::findCirclesGrid( - grayscale_img, pattern, centers, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, + bool found_circles = cv::findCirclesGrid( + grayscale_img, pattern, circle_centers, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, blobDetector); - if (found) { + bool found_chessboard = findChessboardCorners( + grayscale_img, pattern, chessboard_centers, + cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK); + + if (found_chessboard) + cornerSubPix( + grayscale_img, chessboard_centers, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1)); + + if (found_circles) { + all_object_points.push_back(template_points); + all_image_points.push_back(circle_centers); + } else if (found_chessboard) { all_object_points.push_back(template_points); - all_image_points.push_back(centers); + all_image_points.push_back(chessboard_centers); } if (all_object_points.size() >= max_samples) { @@ -145,18 +164,24 @@ int main(int argc, char ** argv) all_image_points.begin() + mini_calibration_samples); cv::Mat_ mini_opencv_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ mini_opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ mini_opencv_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); cv::Mat_ opencv_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ opencv_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); cv::Mat_ undistorted_camera_matrix = cv::Mat_::zeros(3, 3); cv::Mat_ ceres_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ ceres_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ ceres_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); int flags = 0; - if (!use_tangent_distortion) { + /* if (!use_tangent_distortion) { flags |= cv::CALIB_ZERO_TANGENT_DIST; } @@ -170,13 +195,36 @@ int main(int argc, char ** argv) if (num_radial_distortion_coeffs < 1) { flags |= cv::CALIB_FIX_K1; - } + } */ + + /* flags |= cv2.CALIB_ZERO_TANGENT_DIST if not self.use_tangential_distortion.value else 0 + flags |= cv2.CALIB_RATIONAL_MODEL if self.radial_distortion_coefficients.value > 3 else 0 + flags |= cv2.CALIB_FIX_K6 if self.radial_distortion_coefficients.value < 6 else 0 + flags |= cv2.CALIB_FIX_K5 if self.radial_distortion_coefficients.value < 5 else 0 + flags |= cv2.CALIB_FIX_K4 if self.radial_distortion_coefficients.value < 4 else 0 + flags |= cv2.CALIB_FIX_K3 if self.radial_distortion_coefficients.value < 3 else 0 + flags |= cv2.CALIB_FIX_K2 if self.radial_distortion_coefficients.value < 2 else 0 + flags |= cv2.CALIB_FIX_K1 if self.radial_distortion_coefficients.value < 1 else 0 */ + + flags |= use_tangent_distortion ? 0 : cv::CALIB_ZERO_TANGENT_DIST; + flags |= num_rational_distortion_coeffs > 0 ? cv::CALIB_RATIONAL_MODEL : 0; + flags |= num_rational_distortion_coeffs < 3 ? cv::CALIB_FIX_K6 : 0; + flags |= num_rational_distortion_coeffs < 2 ? cv::CALIB_FIX_K5 : 0; + flags |= num_rational_distortion_coeffs < 1 ? cv::CALIB_FIX_K4 : 0; + flags |= num_radial_distortion_coeffs < 3 ? cv::CALIB_FIX_K3 : 0; + flags |= num_radial_distortion_coeffs < 2 ? cv::CALIB_FIX_K2 : 0; + flags |= num_radial_distortion_coeffs < 1 ? cv::CALIB_FIX_K1 : 0; auto mini_opencv_start = std::chrono::high_resolution_clock::now(); double mini_reproj_error = cv::calibrateCamera( mini_calibration_object_points, mini_calibration_image_points, size, mini_opencv_camera_matrix, mini_opencv_dist_coeffs, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs, flags); + // When the rational mode is enabled in opencv, it just created a vector of the maximum dimension + if (mini_opencv_dist_coeffs.rows * mini_opencv_dist_coeffs.cols > 8) { + mini_opencv_dist_coeffs = mini_opencv_dist_coeffs.rowRange(0, 8); + } + auto mini_opencv_stop = std::chrono::high_resolution_clock::now(); std::cout << "Mini opencv calibration results" << std::endl; @@ -224,6 +272,7 @@ int main(int argc, char ** argv) CeresCameraIntrinsicsOptimizer optimizer; optimizer.setRadialDistortionCoefficients(num_radial_distortion_coeffs); optimizer.setTangentialDistortion(use_tangent_distortion); + optimizer.setRationalDistortionCoefficients(num_rational_distortion_coeffs); optimizer.setVerbose(true); optimizer.setData( mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points,