Add Options for GlobalOptimizer

- num_iter - use_huber_kernel
stella-cv · Oct 4, 2024 · 092de3e · 092de3e
1 parent 4be17ea
commit 092de3e
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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