Changes to LM

include/basalt/optimization/accumulator.h

@@ -220,24 +220,11 @@ class SparseHashAccumulator {
     b.template segment<ROWS>(i) += data;
   }
 
-  inline VectorX solve(Scalar alpha = 1e-6) const {
-    SparseMatrix sm(b.rows(), b.rows());
-
-    auto t1 = std::chrono::high_resolution_clock::now();
+  inline void setup_solver() {
     std::vector<T> triplets;
     triplets.reserve(hash_map.size() * 36 + b.rows());
 
-    if (alpha > 0)
-      for (int i = 0; i < b.rows(); i++) {
-        triplets.emplace_back(i, i, alpha);
-      }
-
     for (const auto& kv : hash_map) {
-      // if (kv.first[2] != kv.second.rows()) std::cerr << "rows mismatch" <<
-      // std::endl;
-      // if (kv.first[3] != kv.second.cols()) std::cerr << "cols mismatch" <<
-      // std::endl;
-
       for (int i = 0; i < kv.second.rows(); i++) {
         for (int j = 0; j < kv.second.cols(); j++) {
           triplets.emplace_back(kv.first[0] + i, kv.first[1] + j,
@@ -246,14 +233,21 @@ class SparseHashAccumulator {
       }
     }
 
-    sm.setFromTriplets(triplets.begin(), triplets.end());
+    for (int i = 0; i < b.rows(); i++) {
+      triplets.emplace_back(i, i, std::numeric_limits<double>::min());
+    }
 
+    smm = SparseMatrix(b.rows(), b.rows());
+    smm.setFromTriplets(triplets.begin(), triplets.end());
+  }
+
+  inline VectorX Hdiagonal() const { return smm.diagonal(); }
+
+  inline VectorX solve(const VectorX* diagonal) const {
     auto t2 = std::chrono::high_resolution_clock::now();
 
-    // sm.diagonal() *= 1.01;
-
-    // Eigen::IOFormat CleanFmt(2);
-    // std::cerr << "sm\n" << sm.toDense().format(CleanFmt) << std::endl;
+    SparseMatrix sm = smm;
+    if (diagonal) sm.diagonal() += *diagonal;
 
     VectorX res;
 
@@ -272,15 +266,12 @@ class SparseHashAccumulator {
 
     auto t3 = std::chrono::high_resolution_clock::now();
 
-    auto elapsed1 =
-        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
     auto elapsed2 =
         std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
 
     if (print_info) {
-      std::cout << "Forming matrix: " << elapsed1.count() * 1e-6
-                << "s. Solving linear system: " << elapsed2.count() * 1e-6
-                << "s." << std::endl;
+      std::cout << "Solving linear system: " << elapsed2.count() * 1e-6 << "s."
+                << std::endl;
     }
 
     return res;
@@ -330,6 +321,8 @@ class SparseHashAccumulator {
   std::unordered_map<KeyT, MatrixX, KeyHash> hash_map;
 
   VectorX b;
+
+  SparseMatrix smm;
 };
 
 }  // namespace basalt

include/basalt/optimization/poses_optimize.h

@@ -60,7 +60,7 @@ class PosesOptimization {
       typename Eigen::vector<AprilgridCornersData>::const_iterator;
 
  public:
-  PosesOptimization() : lambda(1e-6), min_lambda(1e-6), max_lambda(10) {}
+  PosesOptimization() : lambda(1e-12), min_lambda(1e-12), max_lambda(10) {}
 
   bool initializeIntrinsics(
       size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@@ -149,6 +149,9 @@ class PosesOptimization {
     std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
               << lopt.num_points << std::endl;
 
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
     bool step = false;
     int max_iter = 10;
 
@@ -157,7 +160,11 @@ class PosesOptimization {
           timestam_to_pose;
       Calibration<Scalar> calib_backup = *calib;
 
-      Eigen::VectorXd inc = -lopt.accum.solve(lambda);
+      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);
+
+      Eigen::VectorXd inc = -lopt.accum.solve(&Hdiag_lambda);
 
       for (auto &kv : timestam_to_pose) {
         kv.second *=

include/basalt/optimization/spline_optimize.h

@@ -89,8 +89,8 @@ class SplineOptimization {
 
   SplineOptimization(int64_t dt_ns = 1e7)
       : pose_var(1e-4),
-        lambda(1),
-        min_lambda(1e-6),
+        lambda(1e-12),
+        min_lambda(1e-12),
         max_lambda(10),
         spline(dt_ns),
         dt_ns(dt_ns) {
@@ -405,11 +405,18 @@ class SplineOptimization {
       std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
                 << lopt.num_points << std::endl;
 
+      lopt.accum.setup_solver();
+      Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+
       bool step = false;
       int max_iter = 10;
 
       while (!step && max_iter > 0) {
-        VectorX inc_full = -lopt.accum.solve(lambda);
+        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);
 
         Calibration<Scalar> calib_backup = *calib;
         MocapCalibration<Scalar> mocap_calib_backup = *mocap_calib;

src/vi_estimator/nfr_mapper.cpp

@@ -270,7 +270,11 @@ void NfrMapper::optimize(int num_iterations) {
     lopt.accum.iterative_solver = true;
     lopt.accum.print_info = true;
 
-    const Eigen::VectorXd inc = lopt.accum.solve();
+    lopt.accum.setup_solver();
+    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+    Hdiag.setConstant(Hdiag.size(), 1e-6);
+
+    const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
 
     // apply increment to poses
     for (auto& kv : frame_poses) {