added restore and backup functions

include/basalt/vi_estimator/ba_base.h

@@ -368,6 +368,18 @@ class BundleAdjustmentBase {
     AccumT accum;
   };
 
+  inline void backup() {
+    for (auto& kv : frame_states) kv.second.backup();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : frame_states) kv.second.restore();
+    for (auto& kv : frame_poses) kv.second.backup();
+    lmdb.restore();
+  }
+
   // protected:
   PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const {
     auto it = frame_poses.find(t_ns);

include/basalt/vi_estimator/landmark_database.h

@@ -106,6 +106,14 @@ class LandmarkDatabase {
 
   void removeObservations(int lm_id, const std::set<TimeCamId>& obs);
 
+  inline void backup() {
+    for (auto& kv : kpts) kv.second.backup();
+  }
+
+  inline void restore() {
+    for (auto& kv : kpts) kv.second.restore();
+  }
+
  private:
   Eigen::unordered_map<int, KeypointPosition> kpts;
   Eigen::map<TimeCamId,

src/vi_estimator/keypoint_vio.cpp

@@ -971,6 +971,9 @@ void KeypointVioEstimator::optimize() {
       };
       tbb::parallel_for(keys_range, update_points_func);
 
+      // backup();
+      // restore();
+
       if (config.vio_debug) {
         double after_update_marg_prior_error = 0;
         double after_update_vision_error = 0, after_update_imu_error = 0,