non-parallel compute_error

src/vi_estimator/ba_base.cpp

@@ -131,28 +131,15 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
 }
 
 void BundleAdjustmentBase::computeError(
-    double& error_all,
+    double& error,
     std::map<int, std::vector<std::pair<TimeCamId, double>>>* outliers,
     double outlier_threshold) const {
-  error_all = 0;
+  error = 0;
 
-  std::vector<TimeCamId> obs_tcid_vec;
   for (const auto& kv : lmdb.getObservations()) {
-    obs_tcid_vec.emplace_back(kv.first);
-  }
+    const TimeCamId& tcid_h = kv.first;
 
-  std::mutex res_mutex;
-
-  tbb::parallel_for(
-      tbb::blocked_range<size_t>(0, obs_tcid_vec.size()),
-      [&](const tbb::blocked_range<size_t>& range) {
-        double error = 0;
-
-        for (size_t r = range.begin(); r != range.end(); ++r) {
-          auto kv = lmdb.getObservations().find(obs_tcid_vec[r]);
-          const TimeCamId& tcid_h = kv->first;
-
-          for (const auto& obs_kv : kv->second) {
+    for (const auto& obs_kv : kv.second) {
       const TimeCamId& tcid_t = obs_kv.first;
 
       if (tcid_h != tcid_t) {
@@ -174,8 +161,7 @@ void BundleAdjustmentBase::computeError(
 
                 Eigen::Vector2d res;
 
-                      bool valid =
-                          linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
+                bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
 
                 if (valid) {
                   double e = res.norm();
@@ -189,8 +175,8 @@ void BundleAdjustmentBase::computeError(
                   double obs_weight =
                       huber_weight / (obs_std_dev * obs_std_dev);
 
-                        error += (2 - huber_weight) * obs_weight *
-                                 res.transpose() * res;
+                  error +=
+                      (2 - huber_weight) * obs_weight * res.transpose() * res;
                 } else {
                   if (outliers) {
                     (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1);
@@ -227,8 +213,8 @@ void BundleAdjustmentBase::computeError(
                   double obs_weight =
                       huber_weight / (obs_std_dev * obs_std_dev);
 
-                        error += (2 - huber_weight) * obs_weight *
-                                 res.transpose() * res;
+                  error +=
+                      (2 - huber_weight) * obs_weight * res.transpose() * res;
                 } else {
                   if (outliers) {
                     (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
@@ -240,10 +226,6 @@ void BundleAdjustmentBase::computeError(
       }
     }
   }
-
-        std::scoped_lock l(res_mutex);
-        error_all += error;
-      });
 }
 
 void BundleAdjustmentBase::linearizeHelper(

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

@@ -47,7 +47,7 @@ NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
     : config(config),
       lambda(1e-10),
       min_lambda(1e-32),
-      max_lambda(100),
+      max_lambda(1000),
       lambda_vee(2) {
   this->calib = calib;
   this->obs_std_dev = config.mapper_obs_std_dev;
@@ -288,7 +288,7 @@ 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();
 
     bool converged = false;
     bool step = false;