From 86f11dd1c07a2c313560096e952a87754ec9c3c2 Mon Sep 17 00:00:00 2001
From: "k.koide" <k.koide@aist.go.jp>
Date: Tue, 8 Oct 2024 23:46:07 +0900
Subject: [PATCH] add mapping tools for standard viewer

---
 include/glim/viewer/standard_viewer.hpp |  3 +++
 src/glim/viewer/standard_viewer.cpp     | 23 +++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/include/glim/viewer/standard_viewer.hpp b/include/glim/viewer/standard_viewer.hpp
index 3b67094a..63a477b3 100644
--- a/include/glim/viewer/standard_viewer.hpp
+++ b/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;
diff --git a/src/glim/viewer/standard_viewer.cpp b/src/glim/viewer/standard_viewer.cpp
index 710b8667..d716248c 100644
--- a/src/glim/viewer/standard_viewer.cpp
+++ b/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