non-parallel compute_error

src/vi_estimator/ba_base.cpp

@@ -131,119 +131,101 @@ 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;
+
+    for (const auto& obs_kv : kv.second) {
+      const TimeCamId& tcid_t = obs_kv.first;
+
+      if (tcid_h != tcid_t) {
+        PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
+        PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
+
+        Sophus::SE3d T_t_h_sophus =
+            computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
+                           state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
+
+        Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
+
+        std::visit(
+            [&](const auto& cam) {
+              for (size_t i = 0; i < obs_kv.second.size(); i++) {
+                const KeypointObservation& kpt_obs = obs_kv.second[i];
+                const KeypointPosition& kpt_pos =
+                    lmdb.getLandmark(kpt_obs.kpt_id);
+
+                Eigen::Vector2d res;
+
+                bool valid = linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
+
+                if (valid) {
+                  double e = res.norm();
+
+                  if (outliers && e > outlier_threshold) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e);
+                  }
+
+                  double huber_weight =
+                      e < huber_thresh ? 1.0 : huber_thresh / e;
+                  double obs_weight =
+                      huber_weight / (obs_std_dev * obs_std_dev);
+
+                  error +=
+                      (2 - huber_weight) * obs_weight * res.transpose() * res;
+                } else {
+                  if (outliers) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1);
+                  }
+                }
+              }
+            },
+            calib.intrinsics[tcid_t.cam_id].variant);
+
+      } else {
+        // target and host are the same
+        // residual does not depend on the pose
+        // it just depends on the point
+
+        std::visit(
+            [&](const auto& cam) {
+              for (size_t i = 0; i < obs_kv.second.size(); i++) {
+                const KeypointObservation& kpt_obs = obs_kv.second[i];
+                const KeypointPosition& kpt_pos =
+                    lmdb.getLandmark(kpt_obs.kpt_id);
+
+                Eigen::Vector2d res;
+
+                bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res);
+                if (valid) {
+                  double e = res.norm();
+
+                  if (outliers && e > outlier_threshold) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
+                  }
+
+                  double huber_weight =
+                      e < huber_thresh ? 1.0 : huber_thresh / e;
+                  double obs_weight =
+                      huber_weight / (obs_std_dev * obs_std_dev);
+
+                  error +=
+                      (2 - huber_weight) * obs_weight * res.transpose() * res;
+                } else {
+                  if (outliers) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
+                  }
+                }
+              }
+            },
+            calib.intrinsics[tcid_t.cam_id].variant);
+      }
+    }
   }
-
-  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) {
-            const TimeCamId& tcid_t = obs_kv.first;
-
-            if (tcid_h != tcid_t) {
-              PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
-              PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
-
-              Sophus::SE3d T_t_h_sophus =
-                  computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
-                                 state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
-
-              Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
-
-              std::visit(
-                  [&](const auto& cam) {
-                    for (size_t i = 0; i < obs_kv.second.size(); i++) {
-                      const KeypointObservation& kpt_obs = obs_kv.second[i];
-                      const KeypointPosition& kpt_pos =
-                          lmdb.getLandmark(kpt_obs.kpt_id);
-
-                      Eigen::Vector2d res;
-
-                      bool valid =
-                          linearizePoint(kpt_obs, kpt_pos, T_t_h, cam, res);
-
-                      if (valid) {
-                        double e = res.norm();
-
-                        if (outliers && e > outlier_threshold) {
-                          (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, e);
-                        }
-
-                        double huber_weight =
-                            e < huber_thresh ? 1.0 : huber_thresh / e;
-                        double obs_weight =
-                            huber_weight / (obs_std_dev * obs_std_dev);
-
-                        error += (2 - huber_weight) * obs_weight *
-                                 res.transpose() * res;
-                      } else {
-                        if (outliers) {
-                          (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1);
-                        }
-                      }
-                    }
-                  },
-                  calib.intrinsics[tcid_t.cam_id].variant);
-
-            } else {
-              // target and host are the same
-              // residual does not depend on the pose
-              // it just depends on the point
-
-              std::visit(
-                  [&](const auto& cam) {
-                    for (size_t i = 0; i < obs_kv.second.size(); i++) {
-                      const KeypointObservation& kpt_obs = obs_kv.second[i];
-                      const KeypointPosition& kpt_pos =
-                          lmdb.getLandmark(kpt_obs.kpt_id);
-
-                      Eigen::Vector2d res;
-
-                      bool valid = linearizePoint(kpt_obs, kpt_pos, cam, res);
-                      if (valid) {
-                        double e = res.norm();
-
-                        if (outliers && e > outlier_threshold) {
-                          (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
-                        }
-
-                        double huber_weight =
-                            e < huber_thresh ? 1.0 : huber_thresh / e;
-                        double obs_weight =
-                            huber_weight / (obs_std_dev * obs_std_dev);
-
-                        error += (2 - huber_weight) * obs_weight *
-                                 res.transpose() * res;
-                      } else {
-                        if (outliers) {
-                          (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
-                        }
-                      }
-                    }
-                  },
-                  calib.intrinsics[tcid_t.cam_id].variant);
-            }
-          }
-        }
-
-        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;