diff --git a/applications/calibration-ceres/src/calibration.cpp b/applications/calibration-ceres/src/calibration.cpp index 1f468bb0f677f059ca6d20583654a14e502bbfbd..2c09538e3672742b9874b287994cb3dde8d92e37 100644 --- a/applications/calibration-ceres/src/calibration.cpp +++ b/applications/calibration-ceres/src/calibration.cpp @@ -142,7 +142,9 @@ double ftl::calibration::computeExtrinsicParameters(const Mat &K1, const Mat &D1 double error_after = ba.reprojectionError(); // bundle adjustment didn't work correctly if these checks fail - CHECK(error_before > error_after); + if (error_before < error_after) { + LOG(WARNING) << "error before < error_after (" << error_before << " <" << error_after << ")"; + } CHECK((cv::countNonZero(params1.rvec()) == 0) && (cv::countNonZero(params1.tvec()) == 0)); R = params2.rmat(); diff --git a/applications/calibration-ceres/src/calibration_data.cpp b/applications/calibration-ceres/src/calibration_data.cpp index 02ede5ce6b1793592d14a3af840d7cc773a89bfc..28b77fa8b6b21a07abb8b7e32f46dab0f4363505 100644 --- a/applications/calibration-ceres/src/calibration_data.cpp +++ b/applications/calibration-ceres/src/calibration_data.cpp @@ -180,7 +180,7 @@ int CalibrationData::addObservations(const vector<bool>& visible, const vector<v return retval; } -pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::getVisible(const vector<int> &cameras) { +pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::_getVisible(const vector<int> &cameras) { int n_points = npoints(); vector<vector<Point2d>> observations(cameras.size()); @@ -204,9 +204,9 @@ pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationDat } vector<vector<Point2d>> CalibrationData::getObservations(const vector<int> &cameras) { - return getVisible(cameras).first; + return _getVisible(cameras).first; } vector<reference_wrapper<Point3d>> CalibrationData::getPoints(const vector<int> &cameras) { - return getVisible(cameras).second; + return _getVisible(cameras).second; } diff --git a/applications/calibration-ceres/src/calibration_data.hpp b/applications/calibration-ceres/src/calibration_data.hpp index da4c00eaf210433990222ddd26a3f4b843017058..e98ed3d3c0f86a97e78961eeeb2e102581da9e14 100644 --- a/applications/calibration-ceres/src/calibration_data.hpp +++ b/applications/calibration-ceres/src/calibration_data.hpp @@ -77,11 +77,14 @@ public: void reserve(int n_points) { points_.reserve(n_points); + points_camera_.reserve(n_points*n_cameras_); observations_.reserve(n_points*n_cameras_); visible_.reserve(n_points*n_cameras_); } - std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> getVisible(const std::vector<int> &cameras); + // TODO: method for save poinst_camera_, could return (for example) + // vector<pair<Point3d*>, vector<Point3d*>> + std::vector<std::vector<cv::Point2d>> getObservations(const std::vector<int> &cameras); /* Get points corresponding to observations returned by getObservations() or getObservationsPtr() */ std::vector<std::reference_wrapper<cv::Point3d>> getPoints(const std::vector<int> &cameras); @@ -101,11 +104,19 @@ public: int ncameras() const { return n_cameras_; } private: + std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> _getVisible(const std::vector<int> &cameras); + int n_cameras_; + // cameras std::vector<Camera> cameras_; + // points for each observation std::vector<cv::Point3d> points_; + // points estimated from cameras' observations + std::vector<cv::Point3d> points_camera_; + // observations std::vector<cv::Point2d> observations_; + // visibility std::vector<bool> visible_; }; diff --git a/applications/calibration-ceres/src/main.cpp b/applications/calibration-ceres/src/main.cpp index f73f97dcbb8620bf588a01a4692829233ad1c868..087c3283ba303eceab82ac565c626ec0051e403e 100644 --- a/applications/calibration-ceres/src/main.cpp +++ b/applications/calibration-ceres/src/main.cpp @@ -104,7 +104,8 @@ void calibrate(const string &fname) { // Needs better solution which allows simple access to all estimations. // Required for calculating average coordinates and to know which points // are missing. - vector<tuple<int, vector<Point3d>, vector<reference_wrapper<Point3d>>>> points_all; + + vector<tuple<int, vector<Point3d>>> points_all; transformations[make_pair(refcamera, refcamera)] = make_pair(Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1)); @@ -129,7 +130,7 @@ void calibrate(const string &fname) { Mat t_; vector<Point3d> points; - auto [observations, points_ref] = data.getVisible({current, to}); + auto observations = data.getObservations({current, to}); double err = computeExtrinsicParameters( data.getCamera(current).intrinsicMatrix(), data.getCamera(current).distortionCoefficients(), @@ -141,7 +142,7 @@ void calibrate(const string &fname) { LOG(INFO) << current << " -> " << to << " (RMS error: " << err << ")"; transformations[edge] = make_pair(R_, t_); - points_all.push_back(make_tuple(current, points, points_ref)); + points_all.push_back(make_tuple(current, points)); } const auto [R_update, t_update] = transformations[edge]; @@ -158,7 +159,8 @@ void calibrate(const string &fname) { } // TODO: see points_all comment - for (auto& [i, points, points_ref] : points_all) { + /* + for (auto& [i, points] : points_all) { Mat R = data.getCamera(i).rmat(); Mat t = data.getCamera(i).tvec(); @@ -177,7 +179,7 @@ void calibrate(const string &fname) { } } } - + */ vector<int> idx; BundleAdjustment ba; @@ -213,11 +215,9 @@ void calibrate(const string &fname) { LOG(INFO) << "Finale reprojection error: " << ba.reprojectionError(); - // calibration done + // calibration done, updated values in data } - int main(int argc, char* argv[]) { - calibrate(""); return 0; } diff --git a/applications/calibration-ceres/src/optimization.cpp b/applications/calibration-ceres/src/optimization.cpp index 493bdc6abeef0510b25512becf4616274b7c04c3..aafe2a38a960c3349521a0d2f88369cafb0f8597 100644 --- a/applications/calibration-ceres/src/optimization.cpp +++ b/applications/calibration-ceres/src/optimization.cpp @@ -393,7 +393,14 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op ceres::Solver::Options options; options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose; - options.max_num_iterations = bundle_adjustment_options.max_iter; + + if (bundle_adjustment_options.max_iter > 0) { + options.max_num_iterations = bundle_adjustment_options.max_iter; + } + + if (bundle_adjustment_options.num_threads > 0) { + options.num_threads = bundle_adjustment_options.num_threads; + } ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); diff --git a/applications/calibration-ceres/src/optimization.hpp b/applications/calibration-ceres/src/optimization.hpp index 9fbd16129581fbb9e360919f07a7dcd78fc41220..3b0716c7b6b73ab6cc25bdcc2cedf65b33a5da22 100644 --- a/applications/calibration-ceres/src/optimization.hpp +++ b/applications/calibration-ceres/src/optimization.hpp @@ -83,7 +83,8 @@ public: bool optimize_motion = true; bool optmize_structure = true; - int max_iter = 50; + int num_threads = -1; + int max_iter = -1; bool verbose = false; }; diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp index b6622f70e62dc54453d7a5eb058019d889d1b621..4cbd5f28466d52e6cbdf55f774e85f9cbad94925 100644 --- a/applications/calibration-multi/src/main.cpp +++ b/applications/calibration-multi/src/main.cpp @@ -232,7 +232,8 @@ void calibrateRPC( ftl::net::Universe* net, while(true) { try { if (params.optimize_intrinsic) { - setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {D1, D2}); + // never update distortion during extrinsic calibration + setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {Mat(0, 0, CV_64FC1), Mat(0, 0, CV_64FC1)}); } setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2); setPoseRPC(net, nstream, getMat4x4(R[c], t[c])); @@ -667,6 +668,8 @@ void runCameraCalibrationPath( ftl::Configurable* root, cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR); cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR); + camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera()); + camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera()); if (res.empty()) res = rgb_new[2*idx].size(); } } @@ -721,6 +724,27 @@ void runCameraCalibrationPath( ftl::Configurable* root, std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0); calib.loadInput(path + filename, params.idx_cameras); + for (size_t i = 0; i < nstreams.size(); i++) { + while(true) { + try { + if (camera_parameters[2*i].empty() || camera_parameters[2*i+1].empty()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + continue; + } + vector<Mat> D = getDistortionParametersRPC(net, nstreams[i]); + LOG(INFO) << "K[" << 2*i << "] = \n" << camera_parameters[2*i]; + LOG(INFO) << "D[" << 2*i << "] = " << D[0]; + LOG(INFO) << "K[" << 2*i+1 << "] = \n" << camera_parameters[2*i+1]; + LOG(INFO) << "D[" << 2*i+1 << "] = " << D[1]; + calib.setCameraParameters(2*i, camera_parameters[2*i], D[0]); + calib.setCameraParameters(2*i+1, camera_parameters[2*i+1], D[1]); + break; + } + catch (...) {} + } + } + + Mat out; vector<Mat> map1, map2; vector<cv::Rect> roi; diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp index 60f6209821f5dce2ea31ba0e842e4b862ab152ce..b9627ce252211ef9075e165270d6d88376038814 100644 --- a/applications/calibration-multi/src/multicalibrate.cpp +++ b/applications/calibration-multi/src/multicalibrate.cpp @@ -146,9 +146,9 @@ Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) { } void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs) { - DCHECK(idx < n_cameras_); - DCHECK(K.size() == Size(3, 3)); - DCHECK(distCoeffs.size() == Size(5, 1)); + CHECK(idx < n_cameras_); + CHECK(K.size() == Size(3, 3)); + CHECK(distCoeffs.total() == 5); K.convertTo(K_[idx], CV_64FC1); distCoeffs.convertTo(dist_coeffs_[idx], CV_64FC1); } @@ -529,6 +529,7 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0)); vector<Point3d> points3d; + double err = ftl::calibration::computeExtrinsicParameters(K1, dist_coeffs_[camera_from], K2, dist_coeffs_[camera_to], points1, points2, object_points_, R2, t2, points3d); calculateTransform(R2, t2, R1, t1, rmat, tvec); diff --git a/components/common/cpp/include/ftl/calibration.hpp b/components/common/cpp/include/ftl/calibration.hpp index 719da54d8c591f0077c43a47387617d9a786f24d..00662685237cdabf5e9d8a855ced5d1493f4a4e5 100644 --- a/components/common/cpp/include/ftl/calibration.hpp +++ b/components/common/cpp/include/ftl/calibration.hpp @@ -9,6 +9,9 @@ namespace calibration { namespace validate { +/** + * @brief Valid translation for stereo camera. + */ bool translationStereo(const cv::Mat &t); bool rotationMatrix(const cv::Mat &M); @@ -34,6 +37,11 @@ bool distortionCoefficients(const cv::Mat &D, cv::Size size); } +/** + * @brief Scale camera intrinsic matrix + * @param size_new New resolution + * @param size_old Original (camera matrix) resolution + */ cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_new, const cv::Size &size_old); } diff --git a/components/common/cpp/src/calibration.cpp b/components/common/cpp/src/calibration.cpp index 4ee3425a7806ad876ed62c8c485f9f9aa2224acb..6a6bd8fa31197b72bec301a510a603c8b45213f1 100644 --- a/components/common/cpp/src/calibration.cpp +++ b/components/common/cpp/src/calibration.cpp @@ -106,8 +106,8 @@ bool ftl::calibration::validate::distortionCoefficients(const Mat &D, Size size) double dist_prev_n = 0; double dist_prev_p = 0; - for (int r2_ = 0; r2_ < diagonal; r2_++) { - double r2 = r2_; + for (int r = 0; r < diagonal; r++) { + double r2 = r*r; double r4 = r2*r2; double r6 = r4*r2;