From 389104030769fed09617d00bd35d1f7ebb5e3a0f Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 10 Jul 2019 11:49:23 +0200 Subject: [PATCH] improve calibration --- doc/Calibration.md | 7 +++- include/basalt/calibration/cam_calib.h | 5 ++- include/basalt/calibration/cam_imu_calib.h | 5 ++- include/basalt/optimization/poses_optimize.h | 15 +++++---- include/basalt/optimization/spline_optimize.h | 33 ++++++++++--------- src/calibration/cam_calib.cpp | 23 +++++++++---- src/calibration/cam_imu_calib.cpp | 24 ++++++++++---- test/src/test_spline_opt.cpp | 4 +-- 8 files changed, 77 insertions(+), 39 deletions(-) diff --git a/doc/Calibration.md b/doc/Calibration.md index 5e6badf..a386d51 100644 --- a/doc/Calibration.md +++ b/doc/Calibration.md @@ -29,7 +29,7 @@ After that, you should see the calibration GUI: The buttons in the GUI are located in the order which you should follow to calibrate the camera. After pressing a button the system will print the output to the command line: * `load_dataset` loads the dataset. -* `detect_corners` starts corner detection in the backround thread. Since it is the most time consuming part of the calibration process, the detected corners are cached and loaded if you run the executable again pointing to the same result folder path. +* `detect_corners` starts corner detection in the background thread. Since it is the most time consuming part of the calibration process, the detected corners are cached and loaded if you run the executable again pointing to the same result folder path. * `init_cam_intr` computes an initial guess for camera intrinsics. * `init_cam_poses` computes an initial guess for camera poses given the current intrinsics. * `init_cam_extr` computes an initial transformation between the cameras. @@ -48,6 +48,9 @@ You can also control the process using the following buttons: * `show_ids` toggles the ID visualization for every point. * `huber_thresh` controls the threshold for the huber norm in pixels for the optimization. * `opt_intr` controls if the optimization can change the intrinsics. For some datasets it might be helpful to disable this option for several first iterations of the optimization. +* `opt_until_convg` runs the optimization until convergence. +* `stop_thresh` defines the stopping criteria. Optimization will stop when the maximum increment is smaller than this value. + ### Camera + IMU + Mocap calibration After calibrating the camera you can run the camera + IMU + Mocap calibration. The result path should point to the **same folder as before**: @@ -91,6 +94,8 @@ The following options control the optimization process: * `opt_imu_scale` enables IMU axis scaling, rotation and misalignment calibration. This option should be used only for refinement when the optimization already converged. * `opt_mocap` enables Mocap optimization. You should run it only after pressing `init_mocap`. * `huber_thresh` controls the threshold for the huber norm in pixels for the optimization. +* `opt_until_convg` runs the optimization until convergence. +* `stop_thresh` defines the stopping criteria. Optimization will stop when the maximum increment is smaller than this value. **NOTE:** In this case we use a pre-calibrated sequence, so most of refinements or Mocap to IMU calibration will not have any visible effect. If you want to test this functionality use the "raw" sequences, for example `http://vision.in.tum.de/tumvi/raw/dataset-calib-cam3.bag` and `http://vision.in.tum.de/tumvi/raw/dataset-calib-imu1.bag`. diff --git a/include/basalt/calibration/cam_calib.h b/include/basalt/calibration/cam_calib.h index 8b02f46..cb8685b 100644 --- a/include/basalt/calibration/cam_calib.h +++ b/include/basalt/calibration/cam_calib.h @@ -90,7 +90,7 @@ class CamCalib { void optimize(); - void optimizeWithParam(bool print_info, + bool optimizeWithParam(bool print_info, std::map *stats = nullptr); void saveCalib(); @@ -155,6 +155,9 @@ class CamCalib { pangolin::Var opt_intr; + pangolin::Var opt_until_convg; + pangolin::Var stop_thresh; + std::shared_ptr vign_plotter; std::shared_ptr polar_plotter; std::shared_ptr azimuth_plotter; diff --git a/include/basalt/calibration/cam_imu_calib.h b/include/basalt/calibration/cam_imu_calib.h index 7d15107..532e852 100644 --- a/include/basalt/calibration/cam_imu_calib.h +++ b/include/basalt/calibration/cam_imu_calib.h @@ -89,7 +89,7 @@ class CamImuCalib { void optimize(); - void optimizeWithParam(bool print_info, + bool optimizeWithParam(bool print_info, std::map *stats = nullptr); void saveCalib(); @@ -170,6 +170,9 @@ class CamImuCalib { pangolin::Var huber_thresh; + pangolin::Var opt_until_convg; + pangolin::Var stop_thresh; + pangolin::Plotter *plotter; pangolin::View *img_view_display; diff --git a/include/basalt/optimization/poses_optimize.h b/include/basalt/optimization/poses_optimize.h index 849c43f..e56b363 100644 --- a/include/basalt/optimization/poses_optimize.h +++ b/include/basalt/optimization/poses_optimize.h @@ -128,8 +128,8 @@ class PosesOptimization { bool initialized() const { return true; } // Returns true when converged - bool optimize(bool opt_intrinsics, double huber_thresh, double &error, - int &num_points, double &reprojection_error) { + bool optimize(bool opt_intrinsics, double huber_thresh, double stop_thresh, + double &error, int &num_points, double &reprojection_error) { error = 0; num_points = 0; @@ -168,7 +168,8 @@ class PosesOptimization { Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda); - if (inc.array().abs().maxCoeff() < 1e-10) converged = true; + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < stop_thresh) converged = true; for (auto &kv : timestam_to_pose) { kv.second *= @@ -198,8 +199,8 @@ class PosesOptimization { if (step_quality < 0) { std::cout << "\t[REJECTED] lambda:" << lambda << " step_quality: " << step_quality - << " Error: " << eopt.error << " num points " - << eopt.num_points << std::endl; + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::min(max_lambda, lambda_vee * lambda); lambda_vee *= 2; @@ -208,8 +209,8 @@ class PosesOptimization { } else { std::cout << "\t[ACCEPTED] lambda:" << lambda << " step_quality: " << step_quality - << " Error: " << eopt.error << " num points " - << eopt.num_points << std::endl; + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::max( min_lambda, diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index 25e65af..e42e7ca 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -299,8 +299,16 @@ class SplineOptimization { recompute_size(); - // std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl; - // std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl; + // std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl; + // std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl; + + while (mocap_measurements.front().timestamp_ns <= + spline.minTimeNs() + spline.getDtNs()) + mocap_measurements.pop_front(); + + while (mocap_measurements.back().timestamp_ns >= + spline.maxTimeNs() - spline.getDtNs()) + mocap_measurements.pop_back(); ccd.calibration = calib.get(); ccd.mocap_calibration = mocap_calib.get(); @@ -354,8 +362,8 @@ class SplineOptimization { // Returns true when converged bool optimize(bool use_intr, bool use_poses, bool use_april_corners, bool opt_cam_time_offset, bool opt_imu_scale, bool use_mocap, - double huber_thresh, double& error, int& num_points, - double& reprojection_error) { + double huber_thresh, double stop_thresh, double& error, + int& num_points, double& reprojection_error) { // std::cerr << "optimize num_knots " << num_knots << std::endl; ccd.opt_intrinsics = use_intr; @@ -423,14 +431,9 @@ class SplineOptimization { Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda); - typename VectorX::Index maxIndex; - double max_inc = inc_full.array().abs().maxCoeff(&maxIndex); + double max_inc = inc_full.array().abs().maxCoeff(); - // std::cout << "max_inc " << max_inc << " idx " << maxIndex << " - // size " - // << inc_full.size() << std::endl; - - if (max_inc < 1e-10) converged = true; + if (max_inc < stop_thresh) converged = true; Calibration calib_backup = *calib; MocapCalibration mocap_calib_backup = *mocap_calib; @@ -468,8 +471,8 @@ class SplineOptimization { if (step_quality < 0) { std::cout << "\t[REJECTED] lambda:" << lambda << " step_quality: " << step_quality - << " Error: " << eopt.error << " num points " - << eopt.num_points << std::endl; + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::min(max_lambda, lambda_vee * lambda); lambda_vee *= 2; @@ -481,8 +484,8 @@ class SplineOptimization { } else { std::cout << "\t[ACCEPTED] lambda:" << lambda << " step_quality: " << step_quality - << " Error: " << eopt.error << " num points " - << eopt.num_points << std::endl; + << " max_inc: " << max_inc << " Error: " << eopt.error + << " num points " << eopt.num_points << std::endl; lambda = std::max( min_lambda, diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index 84ce73e..6e0beb7 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -70,7 +70,9 @@ CamCalib::CamCalib(const std::string &dataset_path, show_vign("ui.show_vign", false, false, true), show_ids("ui.show_ids", false, false, true), huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0), - opt_intr("ui.opt_intr", true, false, true) { + opt_intr("ui.opt_intr", true, false, true), + opt_until_convg("ui.opt_until_convg", false, false, true), + stop_thresh("ui.stop_thresh", 1e-8, 1e-10, 0.01, true) { if (show_gui) initGui(); if (!fs::exists(cache_path)) { @@ -266,6 +268,11 @@ void CamCalib::renderingLoop() { } } + if (opt_until_convg) { + bool converged = optimizeWithParam(true); + if (converged) opt_until_convg = false; + } + pangolin::FinishFrame(); } } @@ -707,22 +714,24 @@ void CamCalib::loadDataset() { void CamCalib::optimize() { optimizeWithParam(true); } -void CamCalib::optimizeWithParam(bool print_info, +bool CamCalib::optimizeWithParam(bool print_info, std::map *stats) { if (calib_init_poses.empty()) { std::cerr << "No initial camera poses. Press init_cam_poses initialize " "camera poses " << std::endl; - return; + return true; } if (!calib_opt.get() || !calib_opt->calibInitialized()) { std::cerr << "No initial intrinsics. Press init_intrinsics initialize " "intrinsics" << std::endl; - return; + return true; } + bool converged = true; + if (calib_opt) { // calib_opt->compute_projections(); double error; @@ -731,8 +740,8 @@ void CamCalib::optimizeWithParam(bool print_info, auto start = std::chrono::high_resolution_clock::now(); - bool converged = calib_opt->optimize(opt_intr, huber_thresh, error, - num_points, reprojection_error); + converged = calib_opt->optimize(opt_intr, huber_thresh, stop_thresh, error, + num_points, reprojection_error); auto finish = std::chrono::high_resolution_clock::now(); @@ -777,6 +786,8 @@ void CamCalib::optimizeWithParam(bool print_info, computeProjections(); } } + + return converged; } void CamCalib::saveCalib() { diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index d1567f2..a4b11e6 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -78,7 +78,9 @@ CamImuCalib::CamImuCalib(const std::string &dataset_path, opt_cam_time_offset("ui.opt_cam_time_offset", false, false, true), opt_imu_scale("ui.opt_imu_scale", false, false, true), opt_mocap("ui.opt_mocap", false, false, true), - huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0) { + huber_thresh("ui.huber_thresh", 4.0, 0.1, 10.0), + opt_until_convg("ui.opt_until_convg", false, false, true), + stop_thresh("ui.stop_thresh", 1e-8, 1e-10, 0.01, true) { if (show_gui) initGui(); } @@ -186,6 +188,11 @@ void CamImuCalib::renderingLoop() { } } + if (opt_until_convg) { + bool converged = optimizeWithParam(true); + if (converged) opt_until_convg = false; + } + pangolin::FinishFrame(); } } @@ -668,13 +675,15 @@ void CamImuCalib::loadDataset() { void CamImuCalib::optimize() { optimizeWithParam(true); } -void CamImuCalib::optimizeWithParam(bool print_info, +bool CamImuCalib::optimizeWithParam(bool print_info, std::map *stats) { if (!calib_opt.get() || !calib_opt->calibInitialized()) { std::cerr << "Initalize optimization first!" << std::endl; - return; + return true; } + bool converged = true; + if (calib_opt) { // calib_opt->compute_projections(); double error; @@ -683,9 +692,10 @@ void CamImuCalib::optimizeWithParam(bool print_info, auto start = std::chrono::high_resolution_clock::now(); - bool converged = calib_opt->optimize( - opt_intr, opt_poses, opt_corners, opt_cam_time_offset, opt_imu_scale, - opt_mocap, huber_thresh, error, num_points, reprojection_error); + converged = calib_opt->optimize(opt_intr, opt_poses, opt_corners, + opt_cam_time_offset, opt_imu_scale, + opt_mocap, huber_thresh, stop_thresh, error, + num_points, reprojection_error); auto finish = std::chrono::high_resolution_clock::now(); @@ -769,6 +779,8 @@ void CamImuCalib::optimizeWithParam(bool print_info, drawPlots(); } } + + return converged; } void CamImuCalib::saveCalib() { diff --git a/test/src/test_spline_opt.cpp b/test/src/test_spline_opt.cpp index f0a1447..08e7c55 100644 --- a/test/src/test_spline_opt.cpp +++ b/test/src/test_spline_opt.cpp @@ -62,8 +62,8 @@ TEST(SplineOpt, SplineOptTest) { double reprojection_error; int num_inliers; for (int i = 0; i < 10; i++) - spline_opt.optimize(false, true, false, false, true, false, 0.002, error, - num_inliers, reprojection_error); + spline_opt.optimize(false, true, false, false, true, false, 0.002, 1e-10, + error, num_inliers, reprojection_error); ASSERT_TRUE( spline_opt.getAccelBias().getParam().isApprox(accel_bias_full.getParam()))