Merge branch 'LM' into 'master'

Added Levengert-Marquadt support

See merge request basalt/basalt!32

data/euroc_config.json

@@ -20,6 +20,9 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,
@@ -33,6 +36,9 @@
         "config.mapper_second_best_test_ratio": 1.2,
         "config.mapper_bow_num_bits": 16,
         "config.mapper_min_triangulation_dist": 0.07,
-        "config.mapper_no_factor_weights": false
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
     }
 }

data/euroc_config_no_weights.json

@@ -20,6 +20,9 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,
@@ -33,6 +36,9 @@
         "config.mapper_second_best_test_ratio": 1.2,
         "config.mapper_bow_num_bits": 16,
         "config.mapper_min_triangulation_dist": 0.07,
-        "config.mapper_no_factor_weights": true
+        "config.mapper_no_factor_weights": true,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
     }
 }

data/tumvi_512_config.json

@@ -21,6 +21,9 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,
@@ -34,6 +37,9 @@
         "config.mapper_second_best_test_ratio": 1.2,
         "config.mapper_bow_num_bits": 16,
         "config.mapper_min_triangulation_dist": 0.07,
-        "config.mapper_no_factor_weights": false
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
     }
 }

include/basalt/utils/imu_types.h

@@ -57,7 +57,9 @@ static const Eigen::Vector3d g(0, 0, -9.81);
 static const Eigen::Vector3d g_dir(0, 0, -1);
 }  // namespace constants
 
-struct PoseVelBiasStateWithLin : private PoseVelBiasState {
+struct PoseVelBiasStateWithLin {
+  using VecN = PoseVelBiasState::VecN;
+
   PoseVelBiasStateWithLin() {
     linearized = false;
     delta.setZero();
@@ -67,14 +69,14 @@ struct PoseVelBiasStateWithLin : private PoseVelBiasState {
                           const Eigen::Vector3d& vel_w_i,
                           const Eigen::Vector3d& bias_gyro,
                           const Eigen::Vector3d& bias_accel, bool linearized)
-      : PoseVelBiasState(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel),
+      : linearized(linearized),
-        linearized(linearized) {
+        state_linearized(t_ns, T_w_i, vel_w_i, bias_gyro, bias_accel) {
     delta.setZero();
-    state_current = *this;
+    state_current = state_linearized;
   }
 
   PoseVelBiasStateWithLin(const PoseVelBiasState& other)
-      : PoseVelBiasState(other), linearized(false) {
+      : linearized(false), state_linearized(other) {
     delta.setZero();
     state_current = other;
   }
@@ -88,115 +90,152 @@ struct PoseVelBiasStateWithLin : private PoseVelBiasState {
 
   void applyInc(const VecN& inc) {
     if (!linearized) {
-      PoseVelBiasState::applyInc(inc);
+      state_linearized.applyInc(inc);
     } else {
       delta += inc;
-      state_current = *this;
+      state_current = state_linearized;
       state_current.applyInc(delta);
     }
   }
 
   inline const PoseVelBiasState& getState() const {
     if (!linearized) {
-      return *this;
+      return state_linearized;
     } else {
       return state_current;
     }
   }
 
-  inline const PoseVelBiasState& getStateLin() const { return *this; }
+  inline const PoseVelBiasState& getStateLin() const {
+    return state_linearized;
+  }
 
   inline bool isLinearized() const { return linearized; }
   inline const VecN& getDelta() const { return delta; }
-  inline int64_t getT_ns() const { return t_ns; }
+  inline int64_t getT_ns() const { return state_linearized.t_ns; }
 
   friend struct PoseStateWithLin;
 
+  inline void backup() {
+    backup_delta = delta;
+    backup_state_linearized = state_linearized;
+    backup_state_current = state_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    state_linearized = backup_state_linearized;
+    state_current = backup_state_current;
+  }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
   bool linearized;
   VecN delta;
+  PoseVelBiasState state_linearized, state_current;
 
-  PoseVelBiasState state_current;
+  VecN backup_delta;
+  PoseVelBiasState backup_state_linearized, backup_state_current;
 
   friend class cereal::access;
 
   template <class Archive>
   void serialize(Archive& ar) {
-    ar(T_w_i);
+    ar(state_linearized.T_w_i);
-    ar(vel_w_i);
+    ar(state_linearized.vel_w_i);
-    ar(bias_gyro);
+    ar(state_linearized.bias_gyro);
-    ar(bias_accel);
+    ar(state_linearized.bias_accel);
     ar(state_current.T_w_i);
     ar(state_current.vel_w_i);
     ar(state_current.bias_gyro);
     ar(state_current.bias_accel);
     ar(delta);
     ar(linearized);
-    ar(t_ns);
+    ar(state_linearized.t_ns);
   }
 };
 
-struct PoseStateWithLin : private PoseState {
+struct PoseStateWithLin {
+  using VecN = PoseState::VecN;
+
   PoseStateWithLin() {
     linearized = false;
     delta.setZero();
   };
 
   PoseStateWithLin(int64_t t_ns, const Sophus::SE3d& T_w_i)
-      : PoseState(t_ns, T_w_i), linearized(false) {
+      : linearized(false), pose_linearized(t_ns, T_w_i) {
     delta.setZero();
     T_w_i_current = T_w_i;
   }
 
   PoseStateWithLin(const PoseVelBiasStateWithLin& other)
-      : PoseState(other.t_ns, other.T_w_i),
+      : linearized(other.linearized),
-        linearized(other.linearized),
+        delta(other.delta.head<6>()),
-        delta(other.delta.head<6>()) {
+        pose_linearized(other.state_linearized.t_ns,
-    T_w_i_current = T_w_i;
+                        other.state_linearized.T_w_i) {
-    incPose(delta, T_w_i_current);
+    T_w_i_current = pose_linearized.T_w_i;
+    PoseState::incPose(delta, T_w_i_current);
   }
 
   inline const Sophus::SE3d& getPose() const {
     if (!linearized) {
-      return T_w_i;
+      return pose_linearized.T_w_i;
     } else {
       return T_w_i_current;
     }
   }
 
-  inline const Sophus::SE3d& getPoseLin() const { return T_w_i; }
+  inline const Sophus::SE3d& getPoseLin() const {
+    return pose_linearized.T_w_i;
+  }
 
   inline void applyInc(const VecN& inc) {
     if (!linearized) {
-      incPose(inc, T_w_i);
+      PoseState::incPose(inc, pose_linearized.T_w_i);
     } else {
       delta += inc;
-      T_w_i_current = T_w_i;
+      T_w_i_current = pose_linearized.T_w_i;
-      incPose(delta, T_w_i_current);
+      PoseState::incPose(delta, T_w_i_current);
     }
   }
 
   inline bool isLinearized() const { return linearized; }
   inline const VecN& getDelta() const { return delta; }
-  inline int64_t getT_ns() const { return t_ns; }
+  inline int64_t getT_ns() const { return pose_linearized.t_ns; }
+
+  inline void backup() {
+    backup_delta = delta;
+    backup_pose_linearized = pose_linearized;
+    backup_T_w_i_current = T_w_i_current;
+  }
+
+  inline void restore() {
+    delta = backup_delta;
+    pose_linearized = backup_pose_linearized;
+    T_w_i_current = backup_T_w_i_current;
+  }
 
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  private:
   bool linearized;
   VecN delta;
-
+  PoseState pose_linearized;
   Sophus::SE3d T_w_i_current;
 
+  VecN backup_delta;
+  PoseState backup_pose_linearized;
+  Sophus::SE3d backup_T_w_i_current;
+
   friend class cereal::access;
 
   template <class Archive>
   void serialize(Archive& ar) {
-    ar(T_w_i);
+    ar(pose_linearized.T_w_i);
     ar(T_w_i_current);
     ar(delta);
     ar(linearized);
-    ar(t_ns);
+    ar(pose_linearized.t_ns);
   }
 };
 

include/basalt/utils/vio_config.h

@@ -68,6 +68,10 @@ struct VioConfig {
 
   bool vio_enforce_realtime;
 
+  bool vio_use_lm;
+  double vio_lm_lambda_min;
+  double vio_lm_lambda_max;
+
   double mapper_obs_std_dev;
   double mapper_obs_huber_thresh;
   int mapper_detection_num_points;
@@ -81,5 +85,9 @@ struct VioConfig {
   int mapper_bow_num_bits;
   double mapper_min_triangulation_dist;
   bool mapper_no_factor_weights;
+
+  bool mapper_use_lm;
+  double mapper_lm_lambda_min;
+  double mapper_lm_lambda_max;
 };
 }  // namespace basalt

include/basalt/vi_estimator/ba_base.h

@@ -368,6 +368,18 @@ class BundleAdjustmentBase {
     AccumT accum;
   };
 
+  inline void backup() {
+    for (auto& kv : frame_states) kv.second.backup();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : frame_states) kv.second.restore();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.restore();
+  }
+
   // protected:
   PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const {
     auto it = frame_poses.find(t_ns);

include/basalt/vi_estimator/keypoint_vio.h

@@ -228,6 +228,8 @@ class KeypointVioEstimator : public VioEstimatorBase,
 
   VioConfig config;
 
+  double lambda, min_lambda, max_lambda, lambda_vee;
+
   int64_t msckf_kf_id;
 
   std::shared_ptr<std::thread> processing_thread;

include/basalt/vi_estimator/landmark_database.h

@@ -44,7 +44,21 @@ struct KeypointPosition {
   Eigen::Vector2d dir;
   double id;
 
+  inline void backup() {
+    backup_dir = dir;
+    backup_id = id;
+  }
+
+  inline void restore() {
+    dir = backup_dir;
+    id = backup_id;
+  }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  Eigen::Vector2d backup_dir;
+  double backup_id;
 };
 
 struct KeypointObservation {
@@ -92,6 +106,14 @@ class LandmarkDatabase {
 
   void removeObservations(int lm_id, const std::set<TimeCamId>& obs);
 
+  inline void backup() {
+    for (auto& kv : kpts) kv.second.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : kpts) kv.second.restore();
+  }
+
  private:
   Eigen::unordered_map<int, KeypointPosition> kpts;
   Eigen::map<TimeCamId,

include/basalt/vi_estimator/nfr_mapper.h

@@ -205,5 +205,7 @@ class NfrMapper : public BundleAdjustmentBase {
   std::shared_ptr<HashBow<256>> hash_bow_database;
 
   VioConfig config;
+
+  double lambda, min_lambda, max_lambda, lambda_vee;
 };
 }  // namespace basalt

src/utils/vio_config.cpp

@@ -68,6 +68,10 @@ VioConfig::VioConfig() {
 
   vio_enforce_realtime = false;
 
+  vio_use_lm = false;
+  vio_lm_lambda_min = 1e-32;
+  vio_lm_lambda_max = 1e2;
+
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
   mapper_detection_num_points = 800;
@@ -81,6 +85,10 @@ VioConfig::VioConfig() {
   mapper_bow_num_bits = 16;
   mapper_min_triangulation_dist = 0.07;
   mapper_no_factor_weights = false;
+
+  mapper_use_lm = false;
+  mapper_lm_lambda_min = 1e-32;
+  mapper_lm_lambda_max = 1e2;
 }
 
 void VioConfig::save(const std::string& filename) {
@@ -132,6 +140,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
 
   ar(CEREAL_NVP(config.vio_enforce_realtime));
 
+  ar(CEREAL_NVP(config.vio_use_lm));
+  ar(CEREAL_NVP(config.vio_lm_lambda_min));
+  ar(CEREAL_NVP(config.vio_lm_lambda_max));
+
   ar(CEREAL_NVP(config.mapper_obs_std_dev));
   ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
   ar(CEREAL_NVP(config.mapper_detection_num_points));
@@ -145,5 +157,9 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.mapper_bow_num_bits));
   ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
   ar(CEREAL_NVP(config.mapper_no_factor_weights));
+
+  ar(CEREAL_NVP(config.mapper_use_lm));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_min));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_max));
 }
 }  // namespace cereal

src/vi_estimator/ba_base.cpp

@@ -465,8 +465,9 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold,
   std::map<int, std::vector<std::pair<TimeCamId, double>>> outliers;
   computeError(error, &outliers, outlier_threshold);
 
-  //  std::cout << "============================================" << std::endl;
+  //  std::cout << "============================================" <<
-  //  std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners
+  //  std::endl; std::cout << "Num landmarks: " << lmdb.numLandmarks() << "
+  //  with outliners
   //  "
   //            << outliers.size() << std::endl;
 
@@ -498,7 +499,8 @@ void BundleAdjustmentBase::filterOutliers(double outlier_threshold,
     }
   }
 
-  // std::cout << "============================================" << std::endl;
+  // std::cout << "============================================" <<
+  // std::endl;
 }
 
 void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
@@ -552,7 +554,8 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
 
   //  H_mm_inv = abs_H.bottomRightCorner(marg_size, marg_size)
   //                 .fullPivHouseholderQr()
-  //                 .solve(Eigen::MatrixXd::Identity(marg_size, marg_size));
+  //                 .solve(Eigen::MatrixXd::Identity(marg_size,
+  //                 marg_size));
 
   abs_H.topRightCorner(keep_size, marg_size) *= H_mm_inv;
 

src/vi_estimator/keypoint_vio.cpp

@@ -53,7 +53,11 @@ KeypointVioEstimator::KeypointVioEstimator(
       frames_after_kf(0),
       g(g),
       initialized(false),
-      config(config) {
+      config(config),
+      lambda(config.vio_lm_lambda_min),
+      min_lambda(config.vio_lm_lambda_min),
+      max_lambda(config.vio_lm_lambda_max),
+      lambda_vee(2) {
   this->obs_std_dev = config.vio_obs_std_dev;
   this->huber_thresh = config.vio_obs_huber_thresh;
   this->calib = calib;
@@ -949,13 +953,29 @@ void KeypointVioEstimator::optimize() {
       double error_total =
           rld_error + imu_error + marg_prior_error + ba_error + bg_error;
 
+      if (config.vio_debug)
+        std::cout << "[LINEARIZE] Error: " << error_total << " num points "
+                  << std::endl;
+
       lopt.accum.setup_solver();
       Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
-      Hdiag *= 1e-12;
-      for (int i = 0; i < Hdiag.size(); i++)
-        Hdiag[i] = std::max(Hdiag[i], 1e-12);
 
-      const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
+      bool converged = false;
+
+      if (config.vio_use_lm) {  // Use Levenberg–Marquardt
+        bool step = false;
+        int max_iter = 10;
+
+        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);
+
+          const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+          double max_inc = inc.array().abs().maxCoeff();
+          if (max_inc < 1e-4) converged = true;
+
+          backup();
 
           // apply increment to poses
           for (auto& kv : frame_poses) {
@@ -979,6 +999,91 @@ void KeypointVioEstimator::optimize() {
           };
           tbb::parallel_for(keys_range, update_points_func);
 
+          double after_update_marg_prior_error = 0;
+          double after_update_vision_error = 0, after_update_imu_error = 0,
+                 after_bg_error = 0, after_ba_error = 0;
+
+          computeError(after_update_vision_error);
+          computeImuError(aom, after_update_imu_error, after_bg_error,
+                          after_ba_error, frame_states, imu_meas,
+                          gyro_bias_weight, accel_bias_weight, g);
+          computeMargPriorError(after_update_marg_prior_error);
+
+          double after_error_total =
+              after_update_vision_error + after_update_imu_error +
+              after_update_marg_prior_error + after_bg_error + after_ba_error;
+
+          double f_diff = (error_total - after_error_total);
+          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) {
+            if (config.vio_debug)
+              std::cout << "\t[REJECTED] lambda:" << lambda
+                        << " step_quality: " << step_quality
+                        << " max_inc: " << max_inc
+                        << " Error: " << after_error_total << std::endl;
+            lambda = std::min(max_lambda, lambda_vee * lambda);
+            lambda_vee *= 2;
+
+            restore();
+          } else {
+            if (config.vio_debug)
+              std::cout << "\t[ACCEPTED] lambda:" << lambda
+                        << " step_quality: " << step_quality
+                        << " max_inc: " << max_inc
+                        << " Error: " << after_error_total << std::endl;
+
+            lambda = std::max(
+                min_lambda,
+                lambda *
+                    std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+            lambda_vee = 2;
+
+            step = true;
+          }
+          max_iter--;
+        }
+
+        if (config.vio_debug && converged) {
+          std::cout << "[CONVERGED]" << std::endl;
+        }
+      } else {  // Use Gauss-Newton
+        Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
+        for (int i = 0; i < Hdiag_lambda.size(); i++)
+          Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+        const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+        double max_inc = inc.array().abs().maxCoeff();
+        if (max_inc < 1e-4) converged = true;
+
+        // apply increment to poses
+        for (auto& kv : frame_poses) {
+          int idx = aom.abs_order_map.at(kv.first).first;
+          kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
+        }
+
+        // apply increment to states
+        for (auto& kv : frame_states) {
+          int idx = aom.abs_order_map.at(kv.first).first;
+          kv.second.applyInc(-inc.segment<POSE_VEL_BIAS_SIZE>(idx));
+        }
+
+        // Update points
+        tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+        auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
+          for (size_t i = r.begin(); i != r.end(); ++i) {
+            const auto& rld = rld_vec[i];
+            updatePoints(aom, rld, inc);
+          }
+        };
+        tbb::parallel_for(keys_range, update_points_func);
+      }
+
       if (config.vio_debug) {
         double after_update_marg_prior_error = 0;
         double after_update_vision_error = 0, after_update_imu_error = 0,
@@ -1014,8 +1119,7 @@ void KeypointVioEstimator::optimize() {
                   << " bg_error: " << after_bg_error
                   << " ba_error: " << after_ba_error
                   << " marg prior: " << after_update_marg_prior_error
-                  << " total: " << after_error_total << " max_inc "
+                  << " total: " << after_error_total << " error_diff "
-                  << inc.array().abs().maxCoeff() << " error_diff "
                   << error_diff << " time : " << elapsed.count()
                   << "(us),  num_states " << frame_states.size()
                   << " num_poses " << frame_poses.size() << std::endl;
@@ -1029,7 +1133,7 @@ void KeypointVioEstimator::optimize() {
         filterOutliers(config.vio_outlier_threshold, 4);
       }
 
-      if (inc.array().abs().maxCoeff() < 1e-4) break;
+      if (converged) break;
 
       // std::cerr << "LT\n" << LT << std::endl;
       // std::cerr << "z_p\n" << z_p.transpose() << std::endl;
@@ -1040,7 +1144,7 @@ void KeypointVioEstimator::optimize() {
   if (config.vio_debug) {
     std::cout << "=================================" << std::endl;
   }
-}
+}  // namespace basalt
 
 void KeypointVioEstimator::computeProjections(
     std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {

src/vi_estimator/landmark_database.cpp

@@ -155,6 +155,8 @@ void LandmarkDatabase::removeLandmark(int lm_id) {
 
   host_to_kpts.at(it->second.kf_id).erase(lm_id);
 
+  std::set<TimeCamId> to_remove;
+
   for (auto &kv : obs.at(it->second.kf_id)) {
     int idx = -1;
     for (size_t i = 0; i < kv.second.size(); ++i) {
@@ -165,14 +167,22 @@ void LandmarkDatabase::removeLandmark(int lm_id) {
     }
 
     if (idx >= 0) {
+      BASALT_ASSERT(kv.second.size() > 0);
+
       std::swap(kv.second[idx], kv.second[kv.second.size() - 1]);
       kv.second.resize(kv.second.size() - 1);
 
       num_observations--;
       kpts_num_obs.at(lm_id)--;
+
+      if (kv.second.size() == 0) to_remove.insert(kv.first);
     }
   }
 
+  for (const auto &v : to_remove) {
+    obs.at(it->second.kf_id).erase(v);
+  }
+
   BASALT_ASSERT_STREAM(kpts_num_obs.at(lm_id) == 0, kpts_num_obs.at(lm_id));
   kpts_num_obs.erase(lm_id);
   kpts.erase(lm_id);
@@ -183,6 +193,8 @@ void LandmarkDatabase::removeObservations(int lm_id,
   auto it = kpts.find(lm_id);
   BASALT_ASSERT(it != kpts.end());
 
+  std::set<TimeCamId> to_remove;
+
   for (auto &kv : obs.at(it->second.kf_id)) {
     if (outliers.count(kv.first) > 0) {
       int idx = -1;
@@ -193,14 +205,21 @@ void LandmarkDatabase::removeObservations(int lm_id,
         }
       }
       BASALT_ASSERT(idx >= 0);
+      BASALT_ASSERT(kv.second.size() > 0);
 
       std::swap(kv.second[idx], kv.second[kv.second.size() - 1]);
       kv.second.resize(kv.second.size() - 1);
 
       num_observations--;
       kpts_num_obs.at(lm_id)--;
+
+      if (kv.second.size() == 0) to_remove.insert(kv.first);
     }
   }
+
+  for (const auto &v : to_remove) {
+    obs.at(it->second.kf_id).erase(v);
+  }
 }
 
 }  // namespace basalt

src/vi_estimator/nfr_mapper.cpp

@@ -44,7 +44,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace basalt {
 
 NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
-    : config(config) {
+    : config(config),
+      lambda(config.mapper_lm_lambda_min),
+      min_lambda(config.mapper_lm_lambda_min),
+      max_lambda(config.mapper_lm_lambda_max),
+      lambda_vee(2) {
   this->calib = calib;
   this->obs_std_dev = config.mapper_obs_std_dev;
   this->huber_thresh = config.mapper_obs_huber_thresh;
@@ -274,7 +278,7 @@ void NfrMapper::optimize(int num_iterations) {
 
     double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
 
-    std::cout << "iter " << iter
+    std::cout << "[LINEARIZE] iter " << iter
               << " before_update_error: vision: " << rld_error
               << " rel_error: " << lopt.rel_error
               << " roll_pitch_error: " << lopt.roll_pitch_error
@@ -284,10 +288,24 @@ void NfrMapper::optimize(int num_iterations) {
     lopt.accum.print_info = true;
 
     lopt.accum.setup_solver();
-    Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
+    const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
-    Hdiag.setConstant(Hdiag.size(), 1e-6);
 
-    const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
+    bool converged = false;
+
+    if (config.vio_use_lm) {  // Use Levenberg–Marquardt
+      bool step = false;
+      int max_iter = 10;
+
+      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);
+
+        const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+        double max_inc = inc.array().abs().maxCoeff();
+        if (max_inc < 1e-5) converged = true;
+
+        backup();
 
         // apply increment to poses
         for (auto& kv : frame_poses) {
@@ -314,31 +332,88 @@ void NfrMapper::optimize(int num_iterations) {
         computeRelPose(after_rel_error);
         computeRollPitch(after_roll_pitch_error);
 
-    double after_error_total =
+        double after_error_total = after_update_vision_error + after_rel_error +
-        after_update_vision_error + after_rel_error + after_roll_pitch_error;
+                                   after_roll_pitch_error;
 
-    double error_diff = error_total - after_error_total;
+        double f_diff = (error_total - after_error_total);
+        double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
 
-    auto t2 = std::chrono::high_resolution_clock::now();
+        std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
 
-    auto elapsed =
+        double step_quality = f_diff / l_diff;
-        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
 
-    std::cout << "iter " << iter
+        if (step_quality < 0) {
-              << "  after_update_error: vision: " << after_update_vision_error
+          std::cout << "\t[REJECTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc
+                    << " vision_error: " << after_update_vision_error
                     << " rel_error: " << after_rel_error
                     << " roll_pitch_error: " << after_roll_pitch_error
-              << " total: " << after_error_total << " max_inc "
+                    << " total: " << after_error_total << std::endl;
-              << inc.array().abs().maxCoeff() << " error_diff " << error_diff
+          lambda = std::min(max_lambda, lambda_vee * lambda);
-              << " time : " << elapsed.count() << "(us),  num_states "
+          lambda_vee *= 2;
-              << frame_states.size() << " num_poses " << frame_poses.size()
+
-              << std::endl;
+          restore();
+        } else {
+          std::cout << "\t[ACCEPTED] lambda:" << lambda
+                    << " step_quality: " << step_quality
+                    << " max_inc: " << max_inc
+                    << " vision_error: " << after_update_vision_error
+                    << " rel_error: " << after_rel_error
+                    << " roll_pitch_error: " << after_roll_pitch_error
+                    << " total: " << after_error_total << std::endl;
+
+          lambda = std::max(
+              min_lambda,
+              lambda *
+                  std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+          lambda_vee = 2;
+
+          step = true;
+        }
+
+        max_iter--;
 
         if (after_error_total > error_total) {
           std::cout << "increased error after update!!!" << std::endl;
         }
+      }
+    } else {  // Use Gauss-Newton
+      Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
+      for (int i = 0; i < Hdiag_lambda.size(); i++)
+        Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
 
-    if (inc.array().abs().maxCoeff() < 1e-4) break;
+      const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc.array().abs().maxCoeff();
+      if (max_inc < 1e-5) converged = true;
+
+      // apply increment to poses
+      for (auto& kv : frame_poses) {
+        int idx = aom.abs_order_map.at(kv.first).first;
+        BASALT_ASSERT(!kv.second.isLinearized());
+        kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
+      }
+
+      // Update points
+      tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+      auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
+        for (size_t i = r.begin(); i != r.end(); ++i) {
+          const auto& rld = rld_vec[i];
+          updatePoints(aom, rld, inc);
+        }
+      };
+      tbb::parallel_for(keys_range, update_points_func);
+    }
+
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto elapsed =
+        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+    std::cout << "iter " << iter << " time : " << elapsed.count()
+              << "(us),  num_states " << frame_states.size() << " num_poses "
+              << frame_poses.size() << std::endl;
+
+    if (converged) break;
 
     // std::cerr << "LT\n" << LT << std::endl;
     // std::cerr << "z_p\n" << z_p.transpose() << std::endl;
@@ -508,7 +583,8 @@ void NfrMapper::match_all() {
 
       // std::cout << "Closest frames for " << tcid << ": ";
       for (const auto& otcid_score : results) {
-        // std::cout << otcid_score.first << "(" << otcid_score.second << ") ";
+        // std::cout << otcid_score.first << "(" << otcid_score.second << ")
+        // ";
         if (otcid_score.first.frame_id != tcid.frame_id &&
             otcid_score.second > config.mapper_frames_to_match_threshold) {
           match_pair m;