small changes to mapping

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

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));

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));

src/vi_estimator/nfr_mapper.cpp

@@ -122,10 +122,11 @@ void NfrMapper::processMargData(MargData& m) {
     //                << std::endl;
   }
 
+  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);
+    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;
@@ -133,6 +134,7 @@ void NfrMapper::processMargData(MargData& m) {
     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,8 +195,10 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
       rpf.cov_inv.setIdentity();
     }
 
+    if (m.use_imu) {
       roll_pitch_factors.emplace_back(rpf);
     }
+  }
 
   for (int64_t other_id : m.kfs_all) {
     if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) {