improve calibration

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`. 

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<std::string, double> *stats = nullptr);
 
   void saveCalib();
@@ -155,6 +155,9 @@ class CamCalib {
 
   pangolin::Var<bool> opt_intr;
 
+  pangolin::Var<bool> opt_until_convg;
+  pangolin::Var<double> stop_thresh;
+
   std::shared_ptr<pangolin::Plotter> vign_plotter;
   std::shared_ptr<pangolin::Plotter> polar_plotter;
   std::shared_ptr<pangolin::Plotter> azimuth_plotter;

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<std::string, double> *stats = nullptr);
 
   void saveCalib();
@@ -170,6 +170,9 @@ class CamImuCalib {
 
   pangolin::Var<double> huber_thresh;
 
+  pangolin::Var<bool> opt_until_convg;
+  pangolin::Var<double> stop_thresh;
+
   pangolin::Plotter *plotter;
   pangolin::View *img_view_display;
 

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,
+  bool optimize(bool opt_intrinsics, double huber_thresh, double stop_thresh,
-                int &num_points, double &reprojection_error) {
+                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 "
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
-                  << eopt.num_points << std::endl;
+                  << " 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 "
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
-                  << eopt.num_points << std::endl;
+                  << " num points " << eopt.num_points << std::endl;
 
         lambda = std::max(
             min_lambda,

include/basalt/optimization/spline_optimize.h

@@ -302,6 +302,14 @@ class SplineOptimization {
     //    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();
     ccd.aprilgrid_corner_pos_3d = &aprilgrid_corner_pos_3d;
@@ -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 huber_thresh, double stop_thresh, double& error,
-                double& reprojection_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();
-      double max_inc = inc_full.array().abs().maxCoeff(&maxIndex);
 
-      //      std::cout << "max_inc " << max_inc << " idx " << maxIndex << "
+      if (max_inc < stop_thresh) converged = true;
-      //      size "
-      //                << inc_full.size() << std::endl;
-
-      if (max_inc < 1e-10) converged = true;
 
       Calibration<Scalar> calib_backup = *calib;
       MocapCalibration<Scalar> 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 "
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
-                  << eopt.num_points << std::endl;
+                  << " 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 "
+                  << " max_inc: " << max_inc << " Error: " << eopt.error
-                  << eopt.num_points << std::endl;
+                  << " num points " << eopt.num_points << std::endl;
 
         lambda = std::max(
             min_lambda,

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<std::string, double> *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,7 +740,7 @@ void CamCalib::optimizeWithParam(bool print_info,
 
     auto start = std::chrono::high_resolution_clock::now();
 
-    bool converged = calib_opt->optimize(opt_intr, huber_thresh, 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() {

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<std::string, double> *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(
+    converged = calib_opt->optimize(opt_intr, opt_poses, opt_corners,
-        opt_intr, opt_poses, opt_corners, opt_cam_time_offset, opt_imu_scale,
+                                    opt_cam_time_offset, opt_imu_scale,
-        opt_mocap, huber_thresh, error, num_points, reprojection_error);
+                                    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() {

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,
+    spline_opt.optimize(false, true, false, false, true, false, 0.002, 1e-10,
-                        num_inliers, reprojection_error);
+                        error, num_inliers, reprojection_error);
 
   ASSERT_TRUE(
       spline_opt.getAccelBias().getParam().isApprox(accel_bias_full.getParam()))