Fix indeterminant linear system (#92)

* add mapping tools for standard viewer

* fix indeterminant linear system

* prevent previous submap with a small overlap being isolated

* quit callback for extension modules

include/glim/util/extension_module.hpp

@@ -22,6 +22,11 @@ class ExtensionModule {
    */
   virtual bool ok() const { return true; }
 
+  /**
+   * @brief Called when the system is quitting.
+   */
+  virtual void at_exit(const std::string& dump_path) {}
+
   /**
    * @brief Load an extension module from a dynamic library
    * @param so_name  Dynamic library name

include/glim/viewer/standard_viewer.hpp

@@ -79,6 +79,9 @@ class StandardViewer : public ExtensionModule {
   std::vector<std::pair<boost::weak_ptr<gtsam::NonlinearFactor>, FactorLineGetter>> odometry_factor_lines;
   std::unordered_map<std::uint64_t, Eigen::Isometry3f> odometry_poses;
 
+  bool show_mapping_tools;
+  float min_overlap;
+
   double point_size;
   bool point_size_metric;
   bool point_shape_circle;

src/glim/mapping/global_mapping.cpp

@@ -158,6 +158,7 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) {
   }
 
   if (params.enable_imu) {
+    logger->debug("create IMU factor");
     if (submap->odom_frames.front()->frame_id != FrameID::IMU) {
       logger->warn("odom frames are not estimated in the IMU frame while global mapping requires IMU estimation");
     }
@@ -210,13 +211,9 @@ void GlobalMapping::insert_submap(const SubMap::Ptr& submap) {
   }
 
   Callbacks::on_smoother_update(*isam2, *new_factors, *new_values);
-  try {
-    auto result = update_isam2(*new_factors, *new_values);
-    Callbacks::on_smoother_update_result(*isam2, result);
-  } catch (std::exception& e) {
-    logger->error("an exception was caught during global map optimization!!");
-    logger->error(e.what());
-  }
+  auto result = update_isam2(*new_factors, *new_values);
+  Callbacks::on_smoother_update_result(*isam2, result);
+
   new_values.reset(new gtsam::Values);
   new_factors.reset(new gtsam::NonlinearFactorGraph);
 
@@ -414,6 +411,7 @@ boost::shared_ptr<gtsam::NonlinearFactorGraph> GlobalMapping::create_matching_co
 
   const auto& current_submap = submaps.back();
 
+  double previous_overlap = 0.0;
   for (int i = 0; i < current; i++) {
     const double dist = (submaps[i]->T_world_origin.translation() - current_submap->T_world_origin.translation()).norm();
     if (dist > params.max_implicit_loop_distance) {
@@ -423,6 +421,7 @@ boost::shared_ptr<gtsam::NonlinearFactorGraph> GlobalMapping::create_matching_co
     const Eigen::Isometry3d delta = submaps[i]->T_world_origin.inverse() * current_submap->T_world_origin;
     const double overlap = gtsam_points::overlap_auto(submaps[i]->voxelmaps.back(), current_submap->frame, delta);
 
+    previous_overlap = i == current - 1 ? overlap : previous_overlap;
     if (overlap < params.min_implicit_loop_overlap) {
       continue;
     }
@@ -447,6 +446,14 @@ boost::shared_ptr<gtsam::NonlinearFactorGraph> GlobalMapping::create_matching_co
     }
   }
 
+  if (previous_overlap < std::max(0.25, params.min_implicit_loop_overlap)) {
+    logger->warn("previous submap has only a small overlap with the current submap ({})", previous_overlap);
+    logger->warn("create a between factor to prevent the submap from being isolated");
+    const int last = current - 1;
+    const gtsam::Pose3 init_delta = gtsam::Pose3((submaps[last]->T_world_origin.inverse() * submaps[current]->T_world_origin).matrix());
+    factors->add(gtsam::make_shared<gtsam::BetweenFactor<gtsam::Pose3>>(X(last), X(current), init_delta, gtsam::noiseModel::Isotropic::Precision(6, 1e6)));
+  }
+
   return factors;
 }
 
@@ -459,14 +466,53 @@ void GlobalMapping::update_submaps() {
 gtsam_points::ISAM2ResultExt GlobalMapping::update_isam2(const gtsam::NonlinearFactorGraph& new_factors, const gtsam::Values& new_values) {
   gtsam_points::ISAM2ResultExt result;
 
+  gtsam::Key indeterminant_nearby_key = 0;
+  try {
 #ifdef GTSAM_USE_TBB
-  auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
-  arena->execute([&] {
+    auto arena = static_cast<tbb::task_arena*>(tbb_task_arena.get());
+    arena->execute([&] {
 #endif
-    result = isam2->update(new_factors, new_values);
+      result = isam2->update(new_factors, new_values);
 #ifdef GTSAM_USE_TBB
-  });
+    });
 #endif
+  } catch (const gtsam::IndeterminantLinearSystemException& e) {
+    logger->error("an indeterminant lienar system exception was caught during global map optimization!!");
+    logger->error(e.what());
+    indeterminant_nearby_key = e.nearbyVariable();
+  } catch (const std::exception& e) {
+    logger->error("an exception was caught during global map optimization!!");
+    logger->error(e.what());
+  }
+
+  if (indeterminant_nearby_key != 0) {
+    const gtsam::Symbol symbol(indeterminant_nearby_key);
+    if (symbol.chr() == 'v' || symbol.chr() == 'b' || symbol.chr() == 'e') {
+      indeterminant_nearby_key = X(symbol.index() / 2);
+    }
+    logger->warn("insert a damping factor at {} to prevent corruption", std::string(gtsam::Symbol(indeterminant_nearby_key)));
+
+    gtsam::Values values = isam2->getLinearizationPoint();
+    gtsam::NonlinearFactorGraph factors = isam2->getFactorsUnsafe();
+    factors.emplace_shared<gtsam_points::LinearDampingFactor>(indeterminant_nearby_key, 6, 1e4);
+
+    gtsam::ISAM2Params isam2_params;
+    if (params.use_isam2_dogleg) {
+      gtsam::ISAM2DoglegParams dogleg_params;
+      isam2_params.setOptimizationParams(dogleg_params);
+    }
+    isam2_params.relinearizeSkip = params.isam2_relinearize_skip;
+    isam2_params.setRelinearizeThreshold(params.isam2_relinearize_thresh);
+
+    if (params.enable_optimization) {
+      isam2.reset(new gtsam_points::ISAM2Ext(isam2_params));
+    } else {
+      isam2.reset(new gtsam_points::ISAM2ExtDummy(isam2_params));
+    }
+
+    isam2->update(factors, values);
+    logger->warn("isam2 was reset");
+  }
 
   return result;
 }

src/glim/viewer/standard_viewer.cpp

@@ -59,6 +59,9 @@ StandardViewer::StandardViewer() : logger(create_module_logger("viewer")) {
   z_range = config.param("standard_viewer", "default_z_range", Eigen::Vector2d(-2.0, 4.0)).cast<float>();
   auto_z_range << 0.0f, 0.0f;
 
+  show_mapping_tools = false;
+  min_overlap = 0.2f;
+
   points_alpha = config.param("standard_viewer", "points_alpha", 1.0);
   factors_alpha = config.param("standard_viewer", "factors_alpha", 1.0);
 
@@ -730,6 +733,11 @@ void StandardViewer::drawable_selection() {
     show_submaps = show_factors = show_mapping;
   }
 
+  ImGui::SameLine();
+  if (ImGui::Button("Tools")) {
+    show_mapping_tools = true;
+  }
+
   ImGui::Checkbox("submaps", &show_submaps);
   ImGui::SameLine();
   ImGui::Checkbox("factors", &show_factors);
@@ -750,6 +758,21 @@ void StandardViewer::drawable_selection() {
     ImGui::Text("bias:%.3f %.3f %.3f %.3f %.3f %.3f", last_imu_bias[0], last_imu_bias[1], last_imu_bias[2], last_imu_bias[3], last_imu_bias[4], last_imu_bias[5]);
     ImGui::End();
   }
+
+  if (show_mapping_tools) {
+    ImGui::Begin("mapping tools", &show_mapping_tools, ImGuiWindowFlags_AlwaysAutoResize);
+    ImGui::DragFloat("Min overlap", &min_overlap, 0.01f, 0.01f, 1.0f);
+    if (ImGui::Button("Find overlapping submaps")) {
+      logger->info("finding overlapping submaps...");
+      GlobalMappingCallbacks::request_to_find_overlapping_submaps(min_overlap);
+    }
+
+    if (ImGui::Button("Optimize")) {
+      logger->info("optimizing...");
+      GlobalMappingCallbacks::request_to_optimize();
+    }
+    ImGui::End();
+  }
 }
 }  // namespace glim