From 402228d52e661521bf26fef50f0d222749632850 Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Fri, 23 Aug 2019 14:02:55 +0200
Subject: [PATCH] small changes to mapping

---
 include/basalt/utils/imu_types.h  |  2 ++
 src/vi_estimator/keypoint_vio.cpp |  1 +
 src/vi_estimator/keypoint_vo.cpp  |  1 +
 src/vi_estimator/nfr_mapper.cpp   | 24 ++++++++++++++----------
 4 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h
index dd5ef4f..e0d2301 100644
--- a/include/basalt/utils/imu_types.h
+++ b/include/basalt/utils/imu_types.h
@@ -274,6 +274,7 @@ struct MargData {
   Eigen::map<int64_t, PoseStateWithLin> frame_poses;
   std::set<int64_t> kfs_all;
   std::set<int64_t> kfs_to_marg;
+  bool use_imu;
 
   std::vector<OpticalFlowResult::Ptr> opt_flow_res;
 };
@@ -316,6 +317,7 @@ void serialize(Archive& ar, basalt::MargData& m) {
   ar(m.frame_states);
   ar(m.kfs_all);
   ar(m.kfs_to_marg);
+  ar(m.use_imu);
 }
 
 }  // namespace cereal
diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index 5eb5439..6385e8f 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -626,6 +626,7 @@ void KeypointVioEstimator::marginalize(
         m->frame_states = frame_states;
         m->kfs_all = kf_ids_all;
         m->kfs_to_marg = kfs_to_marg;
+        m->use_imu = true;
 
         for (int64_t t : m->kfs_all) {
           m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t));
diff --git a/src/vi_estimator/keypoint_vo.cpp b/src/vi_estimator/keypoint_vo.cpp
index e5d4d7d..3840037 100644
--- a/src/vi_estimator/keypoint_vo.cpp
+++ b/src/vi_estimator/keypoint_vo.cpp
@@ -537,6 +537,7 @@ void KeypointVoEstimator::marginalize(
           m->frame_states = frame_states;
           m->kfs_all = kf_ids_all;
           m->kfs_to_marg = kfs_to_marg;
+          m->use_imu = false;
 
           for (int64_t t : m->kfs_all) {
             m->opt_flow_res.emplace_back(prev_opt_flow_res.at(t));
diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp
index d2c695c..52101f0 100644
--- a/src/vi_estimator/nfr_mapper.cpp
+++ b/src/vi_estimator/nfr_mapper.cpp
@@ -122,17 +122,19 @@ void NfrMapper::processMargData(MargData& m) {
     //                << std::endl;
   }
 
-  Eigen::MatrixXd marg_H_new;
-  Eigen::VectorXd marg_b_new;
-  BundleAdjustmentBase::marginalizeHelper(m.abs_H, m.abs_b, idx_to_keep,
-                                          idx_to_marg, marg_H_new, marg_b_new);
+  if (!idx_to_marg.empty()) {
+    Eigen::MatrixXd marg_H_new;
+    Eigen::VectorXd marg_b_new;
+    BundleAdjustmentBase::marginalizeHelper(
+        m.abs_H, m.abs_b, idx_to_keep, idx_to_marg, marg_H_new, marg_b_new);
 
-  //    std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
-  //              << marg_H_new.cols() << std::endl;
+    //    std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
+    //              << marg_H_new.cols() << std::endl;
 
-  m.abs_H = marg_H_new;
-  m.abs_b = marg_b_new;
-  m.aom = aom_new;
+    m.abs_H = marg_H_new;
+    m.abs_b = marg_b_new;
+    m.aom = aom_new;
+  }
 
   BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
 
@@ -193,7 +195,9 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
       rpf.cov_inv.setIdentity();
     }
 
-    roll_pitch_factors.emplace_back(rpf);
+    if (m.use_imu) {
+      roll_pitch_factors.emplace_back(rpf);
+    }
   }
 
   for (int64_t other_id : m.kfs_all) {