Merge branch 'landmark_database' into 'master'

Added Landmark database

See merge request basalt/basalt!24

CMakeLists.txt

@@ -214,6 +214,7 @@ add_library(basalt SHARED
   src/vi_estimator/vio_estimator.cpp
   src/vi_estimator/ba_base.cpp
   src/vi_estimator/nfr_mapper.cpp
+  src/vi_estimator/landmark_database.cpp
   src/utils/keypoints.cpp)
 
 

data/euroc_config.json

@@ -12,11 +12,14 @@
         "config.vio_max_kfs": 7,
         "config.vio_min_frames_after_kf": 5,
         "config.vio_new_kf_keypoints_thresh": 0.7,
-        "config.vio_max_iterations": 5,
         "config.vio_debug": false,
         "config.vio_obs_std_dev": 0.5,
         "config.vio_obs_huber_thresh": 1.0,
         "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
+        
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

data/euroc_ds_calib.json

@@ -159,30 +159,30 @@
             }
         ],
         "calib_accel_bias": [
-            0.001725405479279035,
+          -0.003025405479279035,
-            0.1500005286487319,
+          0.1200005286487319,
-            0.06708820471592454,
+          0.06708820471592454,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0
+          0.0
-        ],
+      ],
-        "calib_gyro_bias": [
+      "calib_gyro_bias": [
-            -0.002186848441668376,
+          -0.002186848441668376,
-            0.024427823167917037,
+          0.020427823167917037,
-            0.07668367023977922,
+          0.07668367023977922,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0,
+          0.0,
-            0.0
+          0.0
-        ],
+      ],
         "imu_update_rate": 200.0,
         "accel_noise_std": [
             0.016,

data/euroc_eucm_calib.json

@@ -159,8 +159,8 @@
             }
         ],
         "calib_accel_bias": [
-            0.001725405479279035,
+            -0.003025405479279035,
-            0.1500005286487319,
+            0.1200005286487319,
             0.06708820471592454,
             0.0,
             0.0,
@@ -171,7 +171,7 @@
         ],
         "calib_gyro_bias": [
             -0.002186848441668376,
-            0.024427823167917037,
+            0.020427823167917037,
             0.07668367023977922,
             0.0,
             0.0,

data/tumvi_512_config.json

@@ -8,15 +8,18 @@
         "config.optical_flow_epipolar_error": 0.005,
         "config.optical_flow_levels": 3,
         "config.optical_flow_skip_frames": 1,
+
         "config.vio_max_states": 3,
         "config.vio_max_kfs": 7,
         "config.vio_min_frames_after_kf": 5,
         "config.vio_new_kf_keypoints_thresh": 0.7,
-        "config.vio_max_iterations": 5,
         "config.vio_debug": false,
         "config.vio_obs_std_dev": 0.5,
         "config.vio_obs_huber_thresh": 1.0,
         "config.vio_min_triangulation_dist": 0.05,
+        "config.vio_outlier_threshold": 3.0,
+        "config.vio_filter_iteration": 4,
+        "config.vio_max_iterations": 7,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,

include/basalt/utils/vio_config.h

@@ -56,9 +56,12 @@ struct VioConfig {
   int vio_max_kfs;
   int vio_min_frames_after_kf;
   float vio_new_kf_keypoints_thresh;
-  int vio_max_iterations;
   bool vio_debug;
 
+  double vio_outlier_threshold;
+  int vio_filter_iteration;
+  int vio_max_iterations;
+
   double vio_obs_std_dev;
   double vio_obs_huber_thresh;
   double vio_min_triangulation_dist;

include/basalt/vi_estimator/ba_base.h

@@ -34,7 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
-#include <basalt/utils/imu_types.h>
+#include <basalt/vi_estimator/landmark_database.h>
 
 #include <tbb/blocked_range.h>
 
@@ -42,22 +42,6 @@ namespace basalt {
 
 class BundleAdjustmentBase {
  public:
-  // keypoint position defined relative to some frame
-  struct KeypointPosition {
-    TimeCamId kf_id;
-    Eigen::Vector2d dir;
-    double id;
-
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  };
-
-  struct KeypointObservation {
-    int kpt_id;
-    Eigen::Vector2d pos;
-
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  };
-
   struct RelLinDataBase {
     std::vector<std::pair<TimeCamId, TimeCamId>> order;
 
@@ -113,7 +97,10 @@ class BundleAdjustmentBase {
     double error;
   };
 
-  void computeError(double& error) const;
+  void computeError(double& error,
+                    std::map<int, std::vector<std::pair<TimeCamId, double>>>*
+                        outliers = nullptr,
+                    double outlier_threshold = 0) const;
 
   void linearizeHelper(
       Eigen::vector<RelLinData>& rld_vec,
@@ -125,6 +112,8 @@ class BundleAdjustmentBase {
   static void linearizeRel(const RelLinData& rld, Eigen::MatrixXd& H,
                            Eigen::VectorXd& b);
 
+  void filterOutliers(double outlier_threshold, int min_num_obs);
+
   template <class CamT>
   static bool linearizePoint(
       const KeypointObservation& kpt_obs, const KeypointPosition& kpt_pos,
@@ -397,10 +386,7 @@ class BundleAdjustmentBase {
   Eigen::map<int64_t, PoseStateWithLin> frame_poses;
 
   // Point management
-  Eigen::unordered_map<int, KeypointPosition> kpts;
+  LandmarkDatabase lmdb;
-  Eigen::map<TimeCamId,
-             Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
-      obs;
 
   double obs_std_dev;
   double huber_thresh;

include/basalt/vi_estimator/landmark_database.h

@@ -0,0 +1,107 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#pragma once
+
+#include <basalt/utils/imu_types.h>
+
+namespace basalt {
+
+// keypoint position defined relative to some frame
+struct KeypointPosition {
+  TimeCamId kf_id;
+  Eigen::Vector2d dir;
+  double id;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct KeypointObservation {
+  int kpt_id;
+  Eigen::Vector2d pos;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+class LandmarkDatabase {
+ public:
+  // Non-const
+  void addLandmark(int lm_id, const KeypointPosition& pos);
+
+  void removeKeyframes(const std::set<FrameId>& kfs_to_marg,
+                       const std::set<FrameId>& poses_to_marg,
+                       const std::set<FrameId>& states_to_marg_all);
+
+  void addObservation(const TimeCamId& tcid_target,
+                      const KeypointObservation& o);
+
+  KeypointPosition& getLandmark(int lm_id);
+
+  // Const
+  const KeypointPosition& getLandmark(int lm_id) const;
+
+  std::vector<TimeCamId> getHostKfs() const;
+
+  std::vector<KeypointPosition> getLandmarksForHost(
+      const TimeCamId& tcid) const;
+
+  const Eigen::map<TimeCamId,
+                   Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>&
+  getObservations() const;
+
+  bool landmarkExists(int lm_id) const;
+
+  size_t numLandmarks() const;
+
+  int numObservations() const;
+
+  int numObservations(int lm_id) const;
+
+  void removeLandmark(int lm_id);
+
+  void removeObservations(int lm_id, const std::set<TimeCamId>& obs);
+
+ private:
+  Eigen::unordered_map<int, KeypointPosition> kpts;
+  Eigen::map<TimeCamId,
+             Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
+      obs;
+
+  std::unordered_map<TimeCamId, std::set<int>> host_to_kpts;
+
+  int num_observations = 0;
+  Eigen::unordered_map<int, int> kpts_num_obs;
+};
+
+}  // namespace basalt

scripts/install_mac_os_deps.sh

@@ -10,6 +10,7 @@ brew install \
      tbb \
      glew \
      eigen \
-     ccache
+     ccache \
+     lz4
 
 brew install llvm

src/mapper.cpp

@@ -108,6 +108,7 @@ void detect();
 void match();
 void tracks();
 void optimize();
+void filter();
 
 constexpr int UI_WIDTH = 200;
 
@@ -135,6 +136,10 @@ Button detect_btn("ui.detect", &detect);
 Button match_btn("ui.match", &match);
 Button tracks_btn("ui.tracks", &tracks);
 Button optimize_btn("ui.optimize", &optimize);
+
+pangolin::Var<double> outlier_threshold("ui.outlier_threshold", 3.0, 0.01, 10);
+
+Button filter_btn("ui.filter", &filter);
 Button align_btn("ui.aling_svd", &alignButton);
 
 pangolin::OpenGlRenderState camera;
@@ -325,6 +330,8 @@ int main(int argc, char** argv) {
     match();
     tracks();
     optimize();
+    filter();
+    optimize();
 
     if (!result_path.empty()) {
       double error = alignButton();
@@ -562,7 +569,7 @@ void load_data(const std::string& calib_path, const std::string& cache_path) {
 
 void computeEdgeVis() {
   edges_vis.clear();
-  for (const auto& kv1 : nrf_mapper->obs) {
+  for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
     for (const auto& kv2 : kv1.second) {
       Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
                                .at(kv1.first.frame_id)
@@ -644,3 +651,8 @@ void tracks() {
   //                                            mapper_point_ids);
   computeEdgeVis();
 }
+
+void filter() {
+  nrf_mapper->filterOutliers(outlier_threshold, 4);
+  nrf_mapper->get_current_points(mapper_points, mapper_point_ids);
+}

src/mapper_sim.cpp

@@ -479,7 +479,7 @@ void extractNonlinearFactors(basalt::MargData& m) {
 
 void computeEdgeVis() {
   edges_vis.clear();
-  for (const auto& kv1 : nrf_mapper->obs) {
+  for (const auto& kv1 : nrf_mapper->lmdb.getObservations()) {
     for (const auto& kv2 : kv1.second) {
       Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
                                .at(kv1.first.frame_id)

src/utils/vio_config.cpp

@@ -57,11 +57,14 @@ VioConfig::VioConfig() {
   vio_max_kfs = 7;
   vio_min_frames_after_kf = 5;
   vio_new_kf_keypoints_thresh = 0.7;
-  vio_max_iterations = 5;
+
   vio_debug = false;
   vio_obs_std_dev = 0.5;
   vio_obs_huber_thresh = 1.0;
   vio_min_triangulation_dist = 0.05;
+  vio_outlier_threshold = 3.0;
+  vio_filter_iteration = 4;
+  vio_max_iterations = 7;
 
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
@@ -118,6 +121,8 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.vio_new_kf_keypoints_thresh));
   ar(CEREAL_NVP(config.vio_debug));
   ar(CEREAL_NVP(config.vio_max_iterations));
+  ar(CEREAL_NVP(config.vio_outlier_threshold));
+  ar(CEREAL_NVP(config.vio_filter_iteration));
 
   ar(CEREAL_NVP(config.vio_obs_std_dev));
   ar(CEREAL_NVP(config.vio_obs_huber_thresh));

src/vi_estimator/ba_base.cpp

@@ -120,7 +120,7 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
 
     Eigen::Vector3d inc_p = rld.Hll.at(lm_idx) * (rld.bl.at(lm_idx) - H_l_p_x);
 
-    KeypointPosition& kpt = kpts[lm_idx];
+    KeypointPosition& kpt = lmdb.getLandmark(lm_idx);
     kpt.dir -= inc_p.head<2>();
     kpt.id -= inc_p[2];
 
@@ -128,10 +128,13 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
   }
 }
 
-void BundleAdjustmentBase::computeError(double& error) const {
+void BundleAdjustmentBase::computeError(
+    double& error,
+    std::map<int, std::vector<std::pair<TimeCamId, double>>>* outliers,
+    double outlier_threshold) const {
   error = 0;
 
-  for (const auto& kv : obs) {
+  for (const auto& kv : lmdb.getObservations()) {
     const TimeCamId& tcid_h = kv.first;
 
     for (const auto& obs_kv : kv.second) {
@@ -151,7 +154,8 @@ void BundleAdjustmentBase::computeError(double& error) const {
             [&](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 = kpts.at(kpt_obs.kpt_id);
+                const KeypointPosition& kpt_pos =
+                    lmdb.getLandmark(kpt_obs.kpt_id);
 
                 Eigen::Vector2d res;
 
@@ -159,6 +163,11 @@ void BundleAdjustmentBase::computeError(double& error) const {
 
                 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 =
@@ -166,6 +175,10 @@ void BundleAdjustmentBase::computeError(double& error) const {
 
                   error +=
                       (2 - huber_weight) * obs_weight * res.transpose() * res;
+                } else {
+                  if (outliers) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -1);
+                  }
                 }
               }
             },
@@ -180,13 +193,19 @@ void BundleAdjustmentBase::computeError(double& error) const {
             [&](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 = kpts.at(kpt_obs.kpt_id);
+                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 =
@@ -194,6 +213,10 @@ void BundleAdjustmentBase::computeError(double& error) const {
 
                   error +=
                       (2 - huber_weight) * obs_weight * res.transpose() * res;
+                } else {
+                  if (outliers) {
+                    (*outliers)[kpt_obs.kpt_id].emplace_back(tcid_t, -2);
+                  }
                 }
               }
             },
@@ -216,7 +239,7 @@ void BundleAdjustmentBase::linearizeHelper(
   std::vector<TimeCamId> obs_tcid_vec;
   for (const auto& kv : obs_to_lin) {
     obs_tcid_vec.emplace_back(kv.first);
-    rld_vec.emplace_back(kpts.size(), kv.second.size());
+    rld_vec.emplace_back(lmdb.numLandmarks(), kv.second.size());
   }
 
   tbb::parallel_for(
@@ -264,7 +287,8 @@ void BundleAdjustmentBase::linearizeHelper(
                   [&](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 = kpts.at(kpt_obs.kpt_id);
+                      const KeypointPosition& kpt_pos =
+                          lmdb.getLandmark(kpt_obs.kpt_id);
 
                       Eigen::Vector2d res;
                       Eigen::Matrix<double, 2, POSE_SIZE> d_res_d_xi;
@@ -318,7 +342,8 @@ void BundleAdjustmentBase::linearizeHelper(
                   [&](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 = kpts.at(kpt_obs.kpt_id);
+                      const KeypointPosition& kpt_pos =
+                          lmdb.getLandmark(kpt_obs.kpt_id);
 
                       Eigen::Vector2d res;
                       Eigen::Matrix<double, 2, 3> d_res_d_p;
@@ -401,14 +426,10 @@ void BundleAdjustmentBase::get_current_points(
   points.clear();
   ids.clear();
 
-  for (const auto& kv_kpt : kpts) {
+  for (const auto& tcid_host : lmdb.getHostKfs()) {
-    Eigen::Vector3d pt_cam =
-        StereographicParam<double>::unproject(kv_kpt.second.dir).head<3>();
-    pt_cam /= kv_kpt.second.id;
-
     Sophus::SE3d T_w_i;
 
-    int64_t id = kv_kpt.second.kf_id.frame_id;
+    int64_t id = tcid_host.frame_id;
     if (frame_states.count(id) > 0) {
       PoseVelBiasStateWithLin state = frame_states.at(id);
       T_w_i = state.getState().T_w_i;
@@ -421,16 +442,65 @@ void BundleAdjustmentBase::get_current_points(
       std::abort();
     }
 
-    const Sophus::SE3d& T_i_c = calib.T_i_c[kv_kpt.second.kf_id.cam_id];
+    const Sophus::SE3d& T_i_c = calib.T_i_c[tcid_host.cam_id];
+    Eigen::Matrix4d T_w_c = (T_w_i * T_i_c).matrix();
 
-    // std::cerr << "T_w_i\n" << T_w_i.matrix() << std::endl;
+    for (const KeypointPosition& kpt_pos :
+         lmdb.getLandmarksForHost(tcid_host)) {
+      Eigen::Vector4d pt_cam =
+          StereographicParam<double>::unproject(kpt_pos.dir);
+      pt_cam[3] = kpt_pos.id;
 
-    points.emplace_back(T_w_i * T_i_c * pt_cam);
+      Eigen::Vector4d pt_w = T_w_c * pt_cam;
 
-    ids.emplace_back(kv_kpt.first);
+      points.emplace_back(pt_w.head<3>() / pt_w[3]);
+      ids.emplace_back(1);
+    }
   }
 }
 
+void BundleAdjustmentBase::filterOutliers(double outlier_threshold,
+                                          int min_num_obs) {
+  double error;
+  std::map<int, std::vector<std::pair<TimeCamId, double>>> outliers;
+  computeError(error, &outliers, outlier_threshold);
+
+  //  std::cout << "============================================" << std::endl;
+  //  std::cout << "Num landmarks: " << lmdb.numLandmarks() << " with outliners
+  //  "
+  //            << outliers.size() << std::endl;
+
+  for (const auto& kv : outliers) {
+    int num_obs = lmdb.numObservations(kv.first);
+    int num_outliers = kv.second.size();
+
+    bool remove = false;
+
+    if (num_obs - num_outliers < min_num_obs) remove = true;
+
+    //    std::cout << "\tlm_id: " << kv.first << " num_obs: " << num_obs
+    //              << " outliers: " << num_outliers << " [";
+
+    for (const auto& kv2 : kv.second) {
+      if (kv2.second == -2) remove = true;
+
+      //      std::cout << kv2.second << ", ";
+    }
+
+    //    std::cout << "] " << std::endl;
+
+    if (remove) {
+      lmdb.removeLandmark(kv.first);
+    } else {
+      std::set<TimeCamId> outliers;
+      for (const auto& kv2 : kv.second) outliers.emplace(kv2.first);
+      lmdb.removeObservations(kv.first, outliers);
+    }
+  }
+
+  // std::cout << "============================================" << std::endl;
+}
+
 void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
                                              Eigen::VectorXd& abs_b,
                                              const std::set<int>& idx_to_keep,

src/vi_estimator/keypoint_vio.cpp

@@ -70,8 +70,8 @@ KeypointVioEstimator::KeypointVioEstimator(
   marg_H(5, 5) = prior_weight;
 
   // small prior to avoid jumps in bias
-  marg_H.diagonal().segment<3>(9).array() = 1e1;
+  marg_H.diagonal().segment<3>(9).array() = 1e2;
-  marg_H.diagonal().segment<3>(12).array() = 1e2;
+  marg_H.diagonal().segment<3>(12).array() = 1e3;
 
   std::cout << "marg_H\n" << marg_H << std::endl;
 
@@ -119,6 +119,8 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
 
     ImuData::Ptr data;
     imu_data_queue.pop(data);
+    data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
+    data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
 
     while (true) {
       vision_data_queue.pop(curr_frame);
@@ -163,14 +165,17 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
 
         while (data->t_ns <= prev_frame->t_ns) {
           imu_data_queue.pop(data);
-
           if (!data.get()) break;
+          data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
+          data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
         }
 
         while (data->t_ns <= curr_frame->t_ns) {
           meas->integrate(*data, accel_cov, gyro_cov);
           imu_data_queue.pop(data);
           if (!data.get()) break;
+          data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
+          data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
         }
 
         if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) {
@@ -240,15 +245,15 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
     for (const auto& kv_obs : opt_flow_meas->observations[i]) {
       int kpt_id = kv_obs.first;
 
-      auto it = kpts.find(kpt_id);
+      if (lmdb.landmarkExists(kpt_id)) {
-      if (it != kpts.end()) {
+        const TimeCamId& tcid_host = lmdb.getLandmark(kpt_id).kf_id;
-        const TimeCamId& tcid_host = it->second.kf_id;
 
         KeypointObservation kobs;
         kobs.kpt_id = kpt_id;
         kobs.pos = kv_obs.second.translation().cast<double>();
 
-        obs[tcid_host][tcid_target].push_back(kobs);
+        lmdb.addObservation(tcid_target, kobs);
+        // obs[tcid_host][tcid_target].push_back(kobs);
 
         if (num_points_connected.count(tcid_host.frame_id) == 0) {
           num_points_connected[tcid_host.frame_id] = 0;
@@ -343,7 +348,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
           kpt_pos.kf_id = tcidl;
           kpt_pos.dir = StereographicParam<double>::project(p0_triangulated);
           kpt_pos.id = p0_triangulated[3];
-          kpts[lm_id] = kpt_pos;
+          lmdb.addLandmark(lm_id, kpt_pos);
 
           num_points_added++;
           valid_kp = true;
@@ -352,7 +357,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
 
       if (valid_kp) {
         for (const auto& kv_obs : kp_obs) {
-          obs[tcidl][kv_obs.first].push_back(kv_obs.second);
+          lmdb.addObservation(kv_obs.first, kv_obs.second);
         }
       }
     }
@@ -674,17 +679,16 @@ void KeypointVioEstimator::marginalize(
                  Eigen::map<TimeCamId, Eigen::vector<KeypointObservation>>>
           obs_to_lin;
 
-      for (auto it = obs.cbegin(); it != obs.cend();) {
+      for (auto it = lmdb.getObservations().cbegin();
+           it != lmdb.getObservations().cend();) {
         if (kfs_to_marg.count(it->first.frame_id) > 0) {
           for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
                ++it2) {
             if (it2->first.frame_id <= last_state_to_marg)
               obs_to_lin[it->first].emplace(*it2);
           }
-          it = obs.erase(it);
-        } else {
-          ++it;
         }
+        ++it;
       }
 
       double rld_error;
@@ -701,15 +705,6 @@ void KeypointVioEstimator::marginalize(
 
         linearizeAbs(rel_H, rel_b, rld, aom, accum);
       }
-
-      // remove points
-      for (auto it = kpts.cbegin(); it != kpts.cend();) {
-        if (kfs_to_marg.count(it->second.kf_id.frame_id) > 0) {
-          it = kpts.erase(it);
-        } else {
-          ++it;
-        }
-      }
     }
 
     linearizeAbsIMU(aom, accum.getH(), accum.getB(), imu_error, bg_error,
@@ -809,16 +804,7 @@ void KeypointVioEstimator::marginalize(
       prev_opt_flow_res.erase(id);
     }
 
-    for (auto it = obs.begin(); it != obs.end(); ++it) {
+    lmdb.removeKeyframes(kfs_to_marg, poses_to_marg, states_to_marg_all);
-      for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) {
-        if (poses_to_marg.count(it2->first.frame_id) > 0 ||
-            states_to_marg_all.count(it2->first.frame_id) > 0) {
-          it2 = it->second.erase(it2);
-        } else {
-          ++it2;
-        }
-      }
-    }
 
     AbsOrderMap marg_order_new;
 
@@ -925,7 +911,7 @@ void KeypointVioEstimator::optimize() {
 
       double rld_error;
       Eigen::vector<RelLinData> rld_vec;
-      linearizeHelper(rld_vec, obs, rld_error);
+      linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
 
       BundleAdjustmentBase::LinearizeAbsReduce<DenseAccumulator<double>> lopt(
           aom);
@@ -1022,6 +1008,10 @@ void KeypointVioEstimator::optimize() {
         }
       }
 
+      if (iter == config.vio_filter_iteration) {
+        filterOutliers(config.vio_outlier_threshold, 4);
+      }
+
       if (inc.array().abs().maxCoeff() < 1e-4) break;
 
       // std::cerr << "LT\n" << LT << std::endl;
@@ -1037,7 +1027,7 @@ void KeypointVioEstimator::optimize() {
 
 void KeypointVioEstimator::computeProjections(
     std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {
-  for (const auto& kv : obs) {
+  for (const auto& kv : lmdb.getObservations()) {
     const TimeCamId& tcid_h = kv.first;
 
     for (const auto& obs_kv : kv.second) {
@@ -1061,7 +1051,8 @@ void KeypointVioEstimator::computeProjections(
             [&](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 = kpts.at(kpt_obs.kpt_id);
+                const KeypointPosition& kpt_pos =
+                    lmdb.getLandmark(kpt_obs.kpt_id);
 
                 Eigen::Vector2d res;
                 Eigen::Vector4d proj;
@@ -1084,7 +1075,8 @@ void KeypointVioEstimator::computeProjections(
             [&](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 = kpts.at(kpt_obs.kpt_id);
+                const KeypointPosition& kpt_pos =
+                    lmdb.getLandmark(kpt_obs.kpt_id);
 
                 Eigen::Vector2d res;
                 Eigen::Vector4d proj;

src/vi_estimator/landmark_database.cpp

@@ -0,0 +1,206 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/vi_estimator/landmark_database.h>
+
+namespace basalt {
+
+void LandmarkDatabase::addLandmark(int lm_id, const KeypointPosition &pos) {
+  kpts[lm_id] = pos;
+  host_to_kpts[pos.kf_id].emplace(lm_id);
+}
+
+void LandmarkDatabase::removeKeyframes(
+    const std::set<FrameId> &kfs_to_marg,
+    const std::set<FrameId> &poses_to_marg,
+    const std::set<FrameId> &states_to_marg_all) {
+  // remove points
+  for (auto it = kpts.cbegin(); it != kpts.cend();) {
+    if (kfs_to_marg.count(it->second.kf_id.frame_id) > 0) {
+      auto num_obs_it = kpts_num_obs.find(it->first);
+      num_observations -= num_obs_it->second;
+      kpts_num_obs.erase(num_obs_it);
+
+      it = kpts.erase(it);
+    } else {
+      ++it;
+    }
+  }
+
+  for (auto it = obs.cbegin(); it != obs.cend();) {
+    if (kfs_to_marg.count(it->first.frame_id) > 0) {
+      it = obs.erase(it);
+    } else {
+      ++it;
+    }
+  }
+
+  for (auto it = obs.begin(); it != obs.end(); ++it) {
+    for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) {
+      if (poses_to_marg.count(it2->first.frame_id) > 0 ||
+          states_to_marg_all.count(it2->first.frame_id) > 0 ||
+          kfs_to_marg.count(it->first.frame_id) > 0) {
+        for (const auto &v : it2->second) kpts_num_obs.at(v.kpt_id)--;
+
+        it2 = it->second.erase(it2);
+      } else {
+        ++it2;
+      }
+    }
+  }
+
+  for (auto it = host_to_kpts.cbegin(); it != host_to_kpts.cend();) {
+    if (kfs_to_marg.count(it->first.frame_id) > 0 ||
+        states_to_marg_all.count(it->first.frame_id) > 0 ||
+        poses_to_marg.count(it->first.frame_id) > 0) {
+      it = host_to_kpts.erase(it);
+    } else {
+      ++it;
+    }
+  }
+}
+
+std::vector<TimeCamId> LandmarkDatabase::getHostKfs() const {
+  std::vector<TimeCamId> res;
+  for (const auto &kv : obs) res.emplace_back(kv.first);
+  return res;
+}
+
+std::vector<KeypointPosition> LandmarkDatabase::getLandmarksForHost(
+    const TimeCamId &tcid) const {
+  std::vector<KeypointPosition> res;
+  for (const auto &v : host_to_kpts.at(tcid)) res.emplace_back(kpts.at(v));
+  return res;
+}
+
+void LandmarkDatabase::addObservation(const TimeCamId &tcid_target,
+                                      const KeypointObservation &o) {
+  auto it = kpts.find(o.kpt_id);
+  BASALT_ASSERT(it != kpts.end());
+
+  auto &obs_vec = obs[it->second.kf_id][tcid_target];
+
+  // Check that the point observation is inserted only once
+  for (const auto &oo : obs_vec) {
+    BASALT_ASSERT(oo.kpt_id != o.kpt_id);
+  }
+
+  obs_vec.emplace_back(o);
+
+  num_observations++;
+  kpts_num_obs[o.kpt_id]++;
+}
+
+KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) {
+  return kpts.at(lm_id);
+}
+
+const KeypointPosition &LandmarkDatabase::getLandmark(int lm_id) const {
+  return kpts.at(lm_id);
+}
+
+const Eigen::map<TimeCamId,
+                 Eigen::map<TimeCamId, Eigen::vector<KeypointObservation> > >
+    &LandmarkDatabase::getObservations() const {
+  return obs;
+}
+
+bool LandmarkDatabase::landmarkExists(int lm_id) const {
+  return kpts.count(lm_id) > 0;
+}
+
+size_t LandmarkDatabase::numLandmarks() const { return kpts.size(); }
+
+int LandmarkDatabase::numObservations() const { return num_observations; }
+
+int LandmarkDatabase::numObservations(int lm_id) const {
+  return kpts_num_obs.at(lm_id);
+}
+
+void LandmarkDatabase::removeLandmark(int lm_id) {
+  auto it = kpts.find(lm_id);
+  BASALT_ASSERT(it != kpts.end());
+
+  host_to_kpts.at(it->second.kf_id).erase(lm_id);
+
+  for (auto &kv : obs.at(it->second.kf_id)) {
+    int idx = -1;
+    for (size_t i = 0; i < kv.second.size(); ++i) {
+      if (kv.second[i].kpt_id == lm_id) {
+        idx = i;
+        break;
+      }
+    }
+
+    if (idx >= 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)--;
+    }
+  }
+
+  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);
+}
+
+void LandmarkDatabase::removeObservations(int lm_id,
+                                          const std::set<TimeCamId> &outliers) {
+  auto it = kpts.find(lm_id);
+  BASALT_ASSERT(it != kpts.end());
+
+  for (auto &kv : obs.at(it->second.kf_id)) {
+    if (outliers.count(kv.first) > 0) {
+      int idx = -1;
+      for (size_t i = 0; i < kv.second.size(); i++) {
+        if (kv.second[i].kpt_id == lm_id) {
+          idx = i;
+          break;
+        }
+      }
+      BASALT_ASSERT(idx >= 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)--;
+    }
+  }
+}
+
+}  // namespace basalt

src/vi_estimator/nfr_mapper.cpp

@@ -244,7 +244,7 @@ void NfrMapper::optimize(int num_iterations) {
 
     double rld_error;
     Eigen::vector<RelLinData> rld_vec;
-    linearizeHelper(rld_vec, obs, rld_error);
+    linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
 
     //      SparseHashAccumulator<double> accum;
     //      accum.reset(aom.total_size);
@@ -662,14 +662,15 @@ void NfrMapper::setup_opt() {
       pos.dir = StereographicParam<double>::project(pos_3d);
       pos.id = pos_3d[3];
 
-      kpts[kv.first] = pos;
+      lmdb.addLandmark(kv.first, pos);
 
       for (const auto& obs_kv : kv.second) {
         KeypointObservation ko;
         ko.kpt_id = kv.first;
         ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second];
 
-        obs[tcid_h][obs_kv.first].emplace_back(ko);
+        lmdb.addObservation(obs_kv.first, ko);
+        // obs[tcid_h][obs_kv.first].emplace_back(ko);
       }
       break;
     }

test/src/test_vio.cpp

@@ -338,7 +338,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
   basalt::ExtendedUnifiedCamera<double> cam =
       basalt::ExtendedUnifiedCamera<double>::getTestProjections()[0];
 
-  basalt::KeypointVioEstimator::KeypointPosition kpt_pos;
+  basalt::KeypointPosition kpt_pos;
 
   Eigen::Vector4d point3d;
   cam.unproject(Eigen::Vector2d::Random() * 50, point3d);
@@ -358,7 +358,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
 
   p_trans = T_t_h * p_trans;
 
-  basalt::KeypointVioEstimator::KeypointObservation kpt_obs;
+  basalt::KeypointObservation kpt_obs;
   cam.project(p_trans, kpt_obs.pos);
 
   Eigen::Vector2d res;
@@ -391,7 +391,7 @@ TEST(VioTestSuite, LinearizePointsTest) {
     test_jacobian(
         "d_res_d_p", d_res_d_p,
         [&](const Eigen::Vector3d& x) {
-          basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos;
+          basalt::KeypointPosition kpt_pos_new = kpt_pos;
 
           kpt_pos_new.dir += x.head<2>();
           kpt_pos_new.id += x[2];