recover corrupted graph

include/glim/mapping/async_global_mapping.hpp

@@ -86,6 +86,7 @@ class AsyncGlobalMapping {
 
   int optimization_interval;
   std::atomic_bool request_to_optimize;
+  std::atomic_bool request_to_recover;
   std::atomic<double> request_to_find_overlapping_submaps;
 
   std::mutex global_mapping_mutex;

include/glim/mapping/callbacks.hpp

@@ -132,6 +132,12 @@ struct GlobalMappingCallbacks {
    */
   static CallbackSlot<void()> request_to_optimize;
 
+  /**
+   * @brief Request the global mapping module to detect and recover from a graph corruption
+   * @note  This is a special inverse-direction callback slot
+   */
+  static CallbackSlot<void()> request_to_recover;
+
   /**
    * @brief Request the global mapping module to find new overlapping submaps
    * @param min_overlap  Minimum overlap rate between submaps

include/glim/mapping/global_mapping.hpp

@@ -87,6 +87,9 @@ class GlobalMapping : public GlobalMappingBase {
   void update_submaps();
   gtsam_points::ISAM2ResultExt update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values);
 
+  void recover_graph() override;
+  std::pair<gtsam::NonlinearFactorGraph, gtsam::Values> recover_graph(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values) const;
+
 private:
   using Params = GlobalMappingParams;
   Params params;

include/glim/mapping/global_mapping_base.hpp

@@ -52,6 +52,11 @@ class GlobalMappingBase {
    */
   virtual void optimize();
 
+  /**
+   * @brief Request to detect and recover graph corruption
+   */
+  virtual void recover_graph();
+
   /**
    * @brief Save the mapping result
    * @param path  Save path

src/glim/mapping/async_global_mapping.cpp

@@ -12,9 +12,11 @@ AsyncGlobalMapping::AsyncGlobalMapping(const std::shared_ptr<glim::GlobalMapping
   optimization_interval(optimization_interval),
   logger(create_module_logger("global")) {
   request_to_optimize = false;
+  request_to_recover = false;
   request_to_find_overlapping_submaps.store(-1.0);
 
   GlobalMappingCallbacks::request_to_optimize.add([this] { request_to_optimize = true; });
+  GlobalMappingCallbacks::request_to_recover.add([this] { request_to_recover = true; });
   GlobalMappingCallbacks::request_to_find_overlapping_submaps.add([this](double min_overlap) { request_to_find_overlapping_submaps.store(min_overlap); });
 
   kill_switch = false;
@@ -96,6 +98,13 @@ void AsyncGlobalMapping::run() {
         last_optimization_time = std::chrono::high_resolution_clock::now();
       }
 
+      if (request_to_recover) {
+        std::lock_guard<std::mutex> lock(global_mapping_mutex);
+        request_to_recover = false;
+        global_mapping->recover_graph();
+        last_optimization_time = std::chrono::high_resolution_clock::now();
+      }
+
       std::this_thread::sleep_for(std::chrono::milliseconds(100));
       continue;
     }

src/glim/mapping/callbacks.cpp

@@ -23,5 +23,6 @@ CallbackSlot<void(gtsam_points::ISAM2Ext&, gtsam::NonlinearFactorGraph&, gtsam::
 CallbackSlot<void(gtsam_points::ISAM2Ext&, const gtsam_points::ISAM2ResultExt& result)> GlobalMappingCallbacks::on_smoother_update_result;
 
 CallbackSlot<void()> GlobalMappingCallbacks::request_to_optimize;
+CallbackSlot<void()> GlobalMappingCallbacks::request_to_recover;
 CallbackSlot<void(double)> GlobalMappingCallbacks::request_to_find_overlapping_submaps;
 }  // namespace glim

src/glim/mapping/global_mapping.cpp

@@ -521,8 +521,8 @@ gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearF
       isam2.reset(new gtsam_points::ISAM2ExtDummy(isam2_params));
     }
 
-    isam2->update(factors, values);
-    logger->warn("isam2 was reset");
+    logger->warn("reset isam2");
+    return update_isam2(factors, values);
   }
 
   return result;
@@ -711,20 +711,30 @@ bool GlobalMapping::load(const std::string& path) {
 
   gtsam::Values values;
   gtsam::NonlinearFactorGraph graph;
+  bool needs_recover = false;
 
   try {
     logger->info("deserializing factor graph");
     gtsam::deserializeFromBinaryFile(path + "/graph.bin", graph);
   } catch (boost::archive::archive_exception e) {
     logger->error("failed to deserialize factor graph!!");
     logger->error(e.what());
+  } catch (std::exception& e) {
+    logger->error("failed to deserialize factor graph!!");
+    logger->error(e.what());
+    needs_recover = true;
   }
+
   try {
     logger->info("deserializing values");
     gtsam::deserializeFromBinaryFile(path + "/values.bin", values);
   } catch (boost::archive::archive_exception e) {
-    logger->error("failed to deserialize factor graph!!");
+    logger->error("failed to deserialize values!!");
+    logger->error(e.what());
+  } catch (std::exception& e) {
+    logger->error("failed to deserialize values!!");
     logger->error(e.what());
+    needs_recover = true;
   }
 
   logger->info("creating matching cost factors");
@@ -756,6 +766,21 @@ bool GlobalMapping::load(const std::string& path) {
     }
   }
 
+  const size_t num_factors_before = graph.size();
+  const auto remove_loc = std::remove_if(graph.begin(), graph.end(), [](const auto& factor) { return factor == nullptr; });
+  graph.erase(remove_loc, graph.end());
+  if (graph.size() != num_factors_before) {
+    logger->warn("removed {} invalid factors", num_factors_before - graph.size());
+    needs_recover = true;
+  }
+
+  if (needs_recover) {
+    logger->warn("recovering factor graph");
+    const auto recovered = recover_graph(graph, values);
+    graph.add(recovered.first);
+    values.insert_or_assign(recovered.second);
+  }
+
   logger->info("optimize");
   Callbacks::on_smoother_update(*isam2, graph, values);
   auto result = update_isam2(graph, values);
@@ -769,4 +794,141 @@ bool GlobalMapping::load(const std::string& path) {
   return true;
 }
 
+void GlobalMapping::recover_graph() {
+  const auto recovered = recover_graph(isam2->getFactorsUnsafe(), isam2->calculateEstimate());
+  update_isam2(recovered.first, recovered.second);
+}
+
+// Recover the graph by adding missing values and factors
+std::pair<gtsam::NonlinearFactorGraph, gtsam::Values> GlobalMapping::recover_graph(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values) const {
+  logger->info("recovering graph");
+  bool enable_imu = false;
+  for (const auto& value : values) {
+    const char chr = gtsam::Symbol(value.key).chr();
+    enable_imu |= (chr == 'e' || chr == 'v' || chr == 'b');
+  }
+  for (const auto& factor : graph) {
+    enable_imu |= boost::dynamic_pointer_cast<gtsam::ImuFactor>(factor) != nullptr;
+  }
+
+  logger->info("enable_imu={}", enable_imu);
+
+  logger->info("creating connectivity map");
+  bool prior_exists = false;
+  std::unordered_map<gtsam::Key, std::set<gtsam::Key>> connectivity_map;
+  for (const auto& factor : graph) {
+    if (!factor) {
+      continue;
+    }
+
+    for (const auto key : factor->keys()) {
+      for (const auto key2 : factor->keys()) {
+        connectivity_map[key].insert(key2);
+      }
+    }
+
+    if (factor->keys().size() == 1 && factor->keys()[0] == X(0)) {
+      prior_exists |= boost::dynamic_pointer_cast<gtsam_points::LinearDampingFactor>(factor) != nullptr;
+    }
+  }
+
+  if (!prior_exists) {
+    logger->warn("X0 prior is missing");
+    new_factors->emplace_shared<gtsam_points::LinearDampingFactor>(X(0), 6, params.init_pose_damping_scale);
+  }
+
+  logger->info("fixing missing values and factors");
+  const auto prior_noise3 = gtsam::noiseModel::Isotropic::Precision(3, 1e6);
+  const auto prior_noise6 = gtsam::noiseModel::Isotropic::Precision(6, 1e6);
+
+  gtsam::NonlinearFactorGraph new_factors;
+  gtsam::Values new_values;
+  for (int i = 0; i < submaps.size(); i++) {
+    if (!values.exists(X(i))) {
+      logger->warn("X{} is missing", i);
+      new_values.insert(X(i), gtsam::Pose3(submaps[i]->T_world_origin.matrix()));
+    }
+
+    if (connectivity_map[X(i)].count(X(i + 1)) == 0 && i != submaps.size() - 1) {
+      logger->warn("X{} -> X{} is missing", i, i + 1);
+
+      const Eigen::Isometry3d delta = submaps[i]->origin_odom_frame()->T_world_sensor().inverse() * submaps[i + 1]->origin_odom_frame()->T_world_sensor();
+      new_factors.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(i), X(i + 1), gtsam::Pose3(delta.matrix()), prior_noise6);
+    }
+
+    if (!enable_imu) {
+      continue;
+    }
+
+    const auto submap = submaps[i];
+    const gtsam::imuBias::ConstantBias imu_biasL(submap->frames.front()->imu_bias);
+    const gtsam::imuBias::ConstantBias imu_biasR(submap->frames.back()->imu_bias);
+    const Eigen::Vector3d v_origin_imuL = submap->T_world_origin.linear().inverse() * submap->frames.front()->v_world_imu;
+    const Eigen::Vector3d v_origin_imuR = submap->T_world_origin.linear().inverse() * submap->frames.back()->v_world_imu;
+
+    if (i != 0) {
+      if (!values.exists(E(i * 2))) {
+        logger->warn("E{} is missing", i * 2);
+        new_values.insert(E(i * 2), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_L).matrix()));
+      }
+      if (!values.exists(V(i * 2))) {
+        logger->warn("V{} is missing", i * 2);
+        new_values.insert(V(i * 2), (submap->T_world_origin.linear() * v_origin_imuL).eval());
+      }
+      if (!values.exists(B(i * 2))) {
+        logger->warn("B{} is missing", i * 2);
+        new_values.insert(B(i * 2), imu_biasL);
+      }
+
+      if (connectivity_map[X(i)].count(E(i * 2)) == 0) {
+        logger->warn("X{} -> E{} is missing", i, i * 2);
+        new_factors.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(i), E(i * 2), gtsam::Pose3(submap->T_origin_endpoint_L.matrix()), prior_noise6);
+      }
+      if (connectivity_map[X(i)].count(V(i * 2)) == 0) {
+        logger->warn("X{} -> V{} is missing", i, i * 2);
+        new_factors.emplace_shared<gtsam_points::RotateVector3Factor>(X(i), V(i * 2), v_origin_imuL, prior_noise3);
+      }
+      if (connectivity_map[B(i * 2)].count(B(i * 2)) == 0) {
+        logger->warn("B{} -> B{} is missing", i * 2, i * 2);
+        new_factors.emplace_shared<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(B(i * 2), imu_biasL, prior_noise6);
+      }
+
+      if (connectivity_map[B(i * 2)].count(B(i * 2 + 1)) == 0) {
+        logger->warn("B{} -> B{} is missing", i * 2, i * 2 + 1);
+        new_factors.emplace_shared<gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>>(B(i * 2), B(i * 2 + 1), gtsam::imuBias::ConstantBias(), prior_noise6);
+      }
+    }
+
+    if (!values.exists(E(i * 2 + 1))) {
+      logger->warn("E{} is missing", i * 2 + 1);
+      new_values.insert(E(i * 2 + 1), gtsam::Pose3((submap->T_world_origin * submap->T_origin_endpoint_R).matrix()));
+    }
+    if (!values.exists(V(i * 2 + 1))) {
+      logger->warn("V{} is missing", i * 2 + 1);
+      new_values.insert(V(i * 2 + 1), (submap->T_world_origin.linear() * v_origin_imuR).eval());
+    }
+    if (!values.exists(B(i * 2 + 1))) {
+      logger->warn("B{} is missing", i * 2 + 1);
+      new_values.insert(B(i * 2 + 1), imu_biasR);
+    }
+
+    if (connectivity_map[X(i)].count(E(i * 2 + 1)) == 0) {
+      logger->warn("X{} -> E{} is missing", i, i * 2 + 1);
+      new_factors.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(i), E(i * 2 + 1), gtsam::Pose3(submap->T_origin_endpoint_R.matrix()), prior_noise6);
+    }
+    if (connectivity_map[X(i)].count(V(i * 2 + 1)) == 0) {
+      logger->warn("X{} -> V{} is missing", i, i * 2 + 1);
+      new_factors.emplace_shared<gtsam_points::RotateVector3Factor>(X(i), V(i * 2 + 1), v_origin_imuR, prior_noise3);
+    }
+    if (connectivity_map[B(i * 2 + 1)].count(B(i * 2 + 1)) == 0) {
+      logger->warn("B{} -> B{} is missing", i * 2 + 1, i * 2 + 1);
+      new_factors.emplace_shared<gtsam::PriorFactor<gtsam::imuBias::ConstantBias>>(B(i * 2 + 1), imu_biasR, prior_noise6);
+    }
+  }
+
+  logger->info("recovering done");
+
+  return {new_factors, new_values};
+}
+
 }  // namespace glim

src/glim/mapping/global_mapping_base.cpp

@@ -26,6 +26,8 @@ void GlobalMappingBase::find_overlapping_submaps(double min_overlap) {}
 
 void GlobalMappingBase::optimize() {}
 
+void GlobalMappingBase::recover_graph() {}
+
 std::shared_ptr<GlobalMappingBase> GlobalMappingBase::load_module(const std::string& so_name) {
   return load_module_from_so<GlobalMappingBase>(so_name, "create_global_mapping_module");
 }

src/glim/viewer/interactive_viewer.cpp

@@ -209,6 +209,11 @@ void InteractiveViewer::drawable_selection() {
     GlobalMappingCallbacks::request_to_find_overlapping_submaps(min_overlap);
   }
 
+  if (ImGui::Button("Recover graph")) {
+    logger->info("recovering graph...");
+    GlobalMappingCallbacks::request_to_recover();
+  }
+
   if (ImGui::Button("Optimize")) {
     logger->info("optimizing...");
     GlobalMappingCallbacks::request_to_optimize();