From 7b78cc95a9961bcbd6408749ea750e37cec45541 Mon Sep 17 00:00:00 2001
From: ymd-stella <7959916+ymd-stella@users.noreply.github.com>
Date: Fri, 4 Oct 2024 15:28:28 +0900
Subject: [PATCH] Add Options for GlobalOptimizer (#621)

* Add Options for GlobalOptimizer

- num_iter
- use_huber_kernel

* Update clang-format-lint-action
---
 .github/workflows/main.yml                      | 2 +-
 src/stella_vslam/global_optimization_module.cc  | 5 ++++-
 src/stella_vslam/module/loop_bundle_adjuster.cc | 8 +++++---
 src/stella_vslam/module/loop_bundle_adjuster.h  | 6 +++++-
 4 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 4b96f597d..bfe4f0668 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -26,7 +26,7 @@ jobs:
         shell: bash
     steps:
       - uses: actions/checkout@v3
-      - uses: DoozyX/clang-format-lint-action@v0.12
+      - uses: DoozyX/clang-format-lint-action@v0.18.1
         with:
           source: "."
           exclude: "./3rd"
diff --git a/src/stella_vslam/global_optimization_module.cc b/src/stella_vslam/global_optimization_module.cc
index 4829fd768..e68f9aaf0 100644
--- a/src/stella_vslam/global_optimization_module.cc
+++ b/src/stella_vslam/global_optimization_module.cc
@@ -16,7 +16,10 @@ global_optimization_module::global_optimization_module(data::map_database* map_d
                                                        data::bow_vocabulary* bow_vocab, const YAML::Node& yaml_node,
                                                        const bool fix_scale)
     : loop_detector_(new module::loop_detector(bow_db, bow_vocab, util::yaml_optional_ref(yaml_node, "LoopDetector"), fix_scale)),
-      loop_bundle_adjuster_(new module::loop_bundle_adjuster(map_db)),
+      loop_bundle_adjuster_(new module::loop_bundle_adjuster(
+          map_db,
+          util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["num_iter"].as<unsigned int>(10),
+          util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["use_huber_kernel"].as<bool>(false))),
       map_db_(map_db),
       graph_optimizer_(new optimize::graph_optimizer(util::yaml_optional_ref(yaml_node, "GraphOptimizer"), fix_scale)),
       thr_neighbor_keyframes_(util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["thr_neighbor_keyframes"].as<unsigned int>(15)) {
diff --git a/src/stella_vslam/module/loop_bundle_adjuster.cc b/src/stella_vslam/module/loop_bundle_adjuster.cc
index d8278b184..9b1d86630 100644
--- a/src/stella_vslam/module/loop_bundle_adjuster.cc
+++ b/src/stella_vslam/module/loop_bundle_adjuster.cc
@@ -12,8 +12,10 @@
 namespace stella_vslam {
 namespace module {
 
-loop_bundle_adjuster::loop_bundle_adjuster(data::map_database* map_db, const unsigned int num_iter)
-    : map_db_(map_db), num_iter_(num_iter) {}
+loop_bundle_adjuster::loop_bundle_adjuster(data::map_database* map_db,
+                                           const unsigned int num_iter,
+                                           const bool use_huber_kernel)
+    : map_db_(map_db), num_iter_(num_iter), use_huber_kernel_(use_huber_kernel) {}
 
 void loop_bundle_adjuster::set_mapping_module(mapping_module* mapper) {
     mapper_ = mapper;
@@ -42,7 +44,7 @@ void loop_bundle_adjuster::optimize(const std::shared_ptr<data::keyframe>& curr_
     std::unordered_set<unsigned int> optimized_landmark_ids;
     eigen_alloc_unord_map<unsigned int, Vec3_t> lm_to_pos_w_after_global_BA;
     eigen_alloc_unord_map<unsigned int, Mat44_t> keyfrm_to_pose_cw_after_global_BA;
-    const auto global_BA = optimize::global_bundle_adjuster(num_iter_, false);
+    const auto global_BA = optimize::global_bundle_adjuster(num_iter_, use_huber_kernel_);
     bool ok = global_BA.optimize(curr_keyfrm->graph_node_->get_keyframes_from_root(),
                                  optimized_keyfrm_ids, optimized_landmark_ids,
                                  lm_to_pos_w_after_global_BA,
diff --git a/src/stella_vslam/module/loop_bundle_adjuster.h b/src/stella_vslam/module/loop_bundle_adjuster.h
index 99266706a..d90daec70 100644
--- a/src/stella_vslam/module/loop_bundle_adjuster.h
+++ b/src/stella_vslam/module/loop_bundle_adjuster.h
@@ -19,7 +19,9 @@ class loop_bundle_adjuster {
     /**
      * Constructor
      */
-    explicit loop_bundle_adjuster(data::map_database* map_db, const unsigned int num_iter = 10);
+    explicit loop_bundle_adjuster(data::map_database* map_db,
+                                  const unsigned int num_iter = 10,
+                                  const bool use_huber_kernel = false);
 
     /**
      * Destructor
@@ -56,6 +58,8 @@ class loop_bundle_adjuster {
     //! number of iteration for optimization
     const unsigned int num_iter_ = 10;
 
+    const bool use_huber_kernel_ = false;
+
     //-----------------------------------------
     // thread management