improve LM

include/basalt/optimization/accumulator.h

@@ -129,65 +129,6 @@ class DenseAccumulator {
   VectorX b;
 };
 
-template <typename Scalar = double>
-class SparseAccumulator {
- public:
-  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
-  typedef Eigen::Triplet<Scalar> T;
-  typedef Eigen::SparseMatrix<Scalar> SparseMatrix;
-
-  template <int ROWS, int COLS, typename Derived>
-  inline void addH(int si, int sj, const Eigen::MatrixBase<Derived>& data) {
-    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, ROWS, COLS);
-
-    for (int i = 0; i < ROWS; i++) {
-      for (int j = 0; j < COLS; j++) {
-        triplets.emplace_back(si + i, sj + j, data(i, j));
-      }
-    }
-  }
-
-  template <int ROWS, typename Derived>
-  inline void addB(int i, const Eigen::MatrixBase<Derived>& data) {
-    b.template segment<ROWS>(i) += data;
-  }
-
-  inline VectorX solve() const {
-    SparseMatrix sm(b.rows(), b.rows());
-
-    auto triplets_copy = triplets;
-    for (int i = 0; i < b.rows(); i++) {
-      triplets_copy.emplace_back(i, i, 0.000001);
-    }
-
-    sm.setFromTriplets(triplets_copy.begin(), triplets_copy.end());
-
-    // Eigen::IOFormat CleanFmt(2);
-    // std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl;
-
-    Eigen::SimplicialLDLT<SparseMatrix> chol(sm);
-    return chol.solve(-b);
-    // return sm.toDense().ldlt().solve(-b);
-  }
-
-  inline void reset(int opt_size) {
-    triplets.clear();
-    b.setZero(opt_size);
-  }
-
-  inline void join(const SparseAccumulator<Scalar>& other) {
-    triplets.reserve(triplets.size() + other.triplets.size());
-    triplets.insert(triplets.end(), other.triplets.begin(),
-                    other.triplets.end());
-    b += other.b;
-  }
-
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- private:
-  std::vector<T> triplets;
-  VectorX b;
-};
-
 template <typename Scalar = double>
 class SparseHashAccumulator {
  public:
@@ -243,6 +184,8 @@ class SparseHashAccumulator {
 
   inline VectorX Hdiagonal() const { return smm.diagonal(); }
 
+  inline VectorX& getB() { return b; }
+
   inline VectorX solve(const VectorX* diagonal) const {
     auto t2 = std::chrono::high_resolution_clock::now();
 

include/basalt/optimization/poses_optimize.h

@@ -60,7 +60,8 @@ class PosesOptimization {
       typename Eigen::vector<AprilgridCornersData>::const_iterator;
 
  public:
-  PosesOptimization() : lambda(1e-6), min_lambda(1e-12), max_lambda(10) {}
+  PosesOptimization()
+      : lambda(1e-6), min_lambda(1e-12), max_lambda(10), lambda_vee(2) {}
 
   bool initializeIntrinsics(
       size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@@ -126,7 +127,8 @@ class PosesOptimization {
   bool calibInitialized() const { return calib != nullptr; }
   bool initialized() const { return true; }
 
-  void optimize(bool opt_intrinsics, double huber_thresh, double &error,
+  // Returns true when converged
+  bool optimize(bool opt_intrinsics, double huber_thresh, double &error,
                 int &num_points, double &reprojection_error) {
     error = 0;
     num_points = 0;
@@ -152,10 +154,11 @@ class PosesOptimization {
     lopt.accum.setup_solver();
     Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
 
+    bool converged = false;
     bool step = false;
     int max_iter = 10;
 
-    while (!step && max_iter > 0) {
+    while (!step && max_iter > 0 && !converged) {
       Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup =
           timestam_to_pose;
       Calibration<Scalar> calib_backup = *calib;
@@ -165,6 +168,7 @@ 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;
 
       for (auto &kv : timestam_to_pose) {
         kv.second *=
@@ -184,20 +188,34 @@ class PosesOptimization {
       ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
       tbb::parallel_reduce(april_range, eopt);
 
-      if (eopt.error > lopt.error) {
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc.dot(inc * lambda - lopt.accum.getB());
+
+      // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
         std::cout << "\t[REJECTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
                   << " Error: " << eopt.error << " num points "
                   << eopt.num_points << std::endl;
-        lambda = std::min(max_lambda, 2 * lambda);
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
 
         timestam_to_pose = timestam_to_pose_backup;
         *calib = calib_backup;
       } else {
         std::cout << "\t[ACCEPTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
                   << " Error: " << eopt.error << " num points "
                   << eopt.num_points << std::endl;
 
-        lambda = std::max(min_lambda, lambda / 2);
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
 
         error = eopt.error;
         num_points = eopt.num_points;
@@ -207,6 +225,12 @@ class PosesOptimization {
       }
       max_iter--;
     }
+
+    if (converged) {
+      std::cout << "[CONVERGED]" << std::endl;
+    }
+
+    return converged;
   }
 
   void recompute_size() {
@@ -288,7 +312,7 @@ class PosesOptimization {
   typename LinearizePosesOpt<
       Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
 
-  Scalar lambda, min_lambda, max_lambda;
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   size_t problem_size;
 

include/basalt/optimization/spline_optimize.h

@@ -90,8 +90,9 @@ class SplineOptimization {
   SplineOptimization(int64_t dt_ns = 1e7)
       : pose_var(1e-4),
         lambda(1e-12),
-        min_lambda(1e-12),
+        min_lambda(1e-18),
         max_lambda(10),
+        lambda_vee(2),
         spline(dt_ns),
         dt_ns(dt_ns) {
     pose_var_inv = 1.0 / pose_var;
@@ -349,13 +350,13 @@ class SplineOptimization {
     //              << std::endl;
   }
 
-  void optimize(bool use_intr, bool use_poses, bool use_april_corners,
+  // 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) {
     // std::cerr << "optimize num_knots " << num_knots << std::endl;
 
-    for (int i = 0; i < 1; i++) {
     ccd.opt_intrinsics = use_intr;
     ccd.opt_cam_time_offset = opt_cam_time_offset;
     ccd.opt_imu_scale = opt_imu_scale;
@@ -408,15 +409,17 @@ class SplineOptimization {
     lopt.accum.setup_solver();
     Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
 
+    bool converged = false;
     bool step = false;
     int max_iter = 10;
 
-      while (!step && max_iter > 0) {
+    while (!step && max_iter > 0 && !converged) {
       Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
       for (int i = 0; i < Hdiag_lambda.size(); i++)
         Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
 
       VectorX inc_full = -lopt.accum.solve(&Hdiag_lambda);
+      if (inc_full.array().abs().maxCoeff() < 1e-10) converged = true;
 
       Calibration<Scalar> calib_backup = *calib;
       MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;
@@ -441,11 +444,20 @@ class SplineOptimization {
       tbb::parallel_reduce(accel_range, eopt);
       tbb::parallel_reduce(gyro_range, eopt);
 
-        if (eopt.error > lopt.error) {
+      double f_diff = (lopt.error - eopt.error);
+      double l_diff = 0.5 * inc_full.dot(inc_full * lambda - lopt.accum.getB());
+
+      std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
+
+      double step_quality = f_diff / l_diff;
+
+      if (step_quality < 0) {
         std::cout << "\t[REJECTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
                   << " Error: " << eopt.error << " num points "
                   << eopt.num_points << std::endl;
-          lambda = std::min(max_lambda, 2 * lambda);
+        lambda = std::min(max_lambda, lambda_vee * lambda);
+        lambda_vee *= 2;
 
         spline = spline_backup;
         *calib = calib_backup;
@@ -454,10 +466,15 @@ class SplineOptimization {
 
       } else {
         std::cout << "\t[ACCEPTED] lambda:" << lambda
+                  << " step_quality: " << step_quality
                   << " Error: " << eopt.error << " num points "
                   << eopt.num_points << std::endl;
 
-          lambda = std::max(min_lambda, lambda / 2);
+        lambda = std::max(
+            min_lambda,
+            lambda *
+                std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+        lambda_vee = 2;
 
         error = eopt.error;
         num_points = eopt.num_points;
@@ -467,7 +484,12 @@ class SplineOptimization {
       }
       max_iter--;
     }
+
+    if (converged) {
+      std::cout << "[CONVERGED]" << std::endl;
     }
+
+    return converged;
   }
 
   typename Calibration<Scalar>::Ptr calib;
@@ -529,7 +551,7 @@ class SplineOptimization {
         1e9 * inc_full[mocap_block_offset + 2 * POSE_SIZE + 1];
   }
 
-  Scalar lambda, min_lambda, max_lambda;
+  Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   int64_t min_time_us, max_time_us;
 

src/calibration/cam_calib.cpp

@@ -731,8 +731,8 @@ void CamCalib::optimizeWithParam(bool print_info,
 
     auto start = std::chrono::high_resolution_clock::now();
 
-    calib_opt->optimize(opt_intr, huber_thresh, error, num_points,
+    bool converged = calib_opt->optimize(opt_intr, huber_thresh, error,
-                        reprojection_error);
+                                         num_points, reprojection_error);
 
     auto finish = std::chrono::high_resolution_clock::now();
 
@@ -768,6 +768,8 @@ void CamCalib::optimizeWithParam(bool print_info,
                        .count()
                 << "ms." << std::endl;
 
+      if (converged) std::cout << "Optimization Converged !!" << std::endl;
+
       std::cout << "==================================" << std::endl;
     }
 

src/calibration/cam_imu_calib.cpp

@@ -681,9 +681,9 @@ void CamImuCalib::optimizeWithParam(bool print_info,
 
     auto start = std::chrono::high_resolution_clock::now();
 
-    calib_opt->optimize(opt_intr, opt_poses, opt_corners, opt_cam_time_offset,
+    bool converged = calib_opt->optimize(
-                        opt_imu_scale, opt_mocap, huber_thresh, error,
+        opt_intr, opt_poses, opt_corners, opt_cam_time_offset, opt_imu_scale,
-                        num_points, reprojection_error);
+        opt_mocap, huber_thresh, error, num_points, reprojection_error);
 
     auto finish = std::chrono::high_resolution_clock::now();
 
@@ -752,6 +752,8 @@ void CamImuCalib::optimizeWithParam(bool print_info,
                        .count()
                 << "ms." << std::endl;
 
+      if (converged) std::cout << "Optimization Converged !!" << std::endl;
+
       std::cout << "==================================" << std::endl;
     }