From ddc2d135789a43f728863642db4a9553b9791495 Mon Sep 17 00:00:00 2001
From: ymd-stella <world.applepie@gmail.com>
Date: Sun, 28 Apr 2024 14:33:06 +0900
Subject: [PATCH] Add parameters for pose_optimizer_g2o

- num_trials_robust
- num_trials
- num_each_iter
---
 .../optimize/pose_optimizer_factory.h             |  6 +++++-
 src/stella_vslam/optimize/pose_optimizer_g2o.cc   | 13 +++++++++----
 src/stella_vslam/optimize/pose_optimizer_g2o.h    | 15 +++++++++++----
 3 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/stella_vslam/optimize/pose_optimizer_factory.h b/src/stella_vslam/optimize/pose_optimizer_factory.h
index f29dec4fb..09d39c805 100644
--- a/src/stella_vslam/optimize/pose_optimizer_factory.h
+++ b/src/stella_vslam/optimize/pose_optimizer_factory.h
@@ -18,7 +18,11 @@ class pose_optimizer_factory {
     static std::unique_ptr<pose_optimizer> create(const YAML::Node& yaml_node) {
         const auto& backend = yaml_node["backend"].as<std::string>("g2o");
         if (backend == "g2o") {
-            return std::unique_ptr<pose_optimizer>(new pose_optimizer_g2o());
+            YAML::Node g2o_node = util::yaml_optional_ref(yaml_node, "g2o");
+            return std::unique_ptr<pose_optimizer>(new pose_optimizer_g2o(
+                g2o_node["num_trials_robust"].as<unsigned int>(2),
+                g2o_node["num_trials"].as<unsigned int>(2),
+                g2o_node["num_each_iter"].as<unsigned int>(10)));
         }
         else if (backend == "gtsam") {
 #ifdef USE_GTSAM
diff --git a/src/stella_vslam/optimize/pose_optimizer_g2o.cc b/src/stella_vslam/optimize/pose_optimizer_g2o.cc
index 95e2a3f94..bb4a36762 100644
--- a/src/stella_vslam/optimize/pose_optimizer_g2o.cc
+++ b/src/stella_vslam/optimize/pose_optimizer_g2o.cc
@@ -20,8 +20,8 @@
 namespace stella_vslam {
 namespace optimize {
 
-pose_optimizer_g2o::pose_optimizer_g2o(const unsigned int num_trials, const unsigned int num_each_iter)
-    : num_trials_(num_trials), num_each_iter_(num_each_iter) {}
+pose_optimizer_g2o::pose_optimizer_g2o(const unsigned int num_trials_robust, const unsigned int num_trials, const unsigned int num_each_iter)
+    : num_trials_robust_(num_trials_robust), num_trials_(num_trials), num_each_iter_(num_each_iter) {}
 
 unsigned int pose_optimizer_g2o::optimize(const data::frame& frm, Mat44_t& optimized_pose, std::vector<bool>& outlier_flags) const {
     auto num_valid_obs = optimize(frm.get_pose_cw(), frm.frm_obs_, frm.orb_params_, frm.camera_,
@@ -114,7 +114,12 @@ unsigned int pose_optimizer_g2o::optimize(const Mat44_t& cam_pose_cw, const data
     // 4. Perform robust Bundle Adjustment (BA)
 
     unsigned int num_bad_obs = 0;
-    for (unsigned int trial = 0; trial < num_trials_; ++trial) {
+    if (num_trials_robust_ == 0) {
+        for (auto& pose_opt_edge_wrap : pose_opt_edge_wraps) {
+            pose_opt_edge_wrap.edge_->setRobustKernel(nullptr);
+        }
+    }
+    for (unsigned int trial = 0; trial < num_trials_robust_ + num_trials_; ++trial) {
         optimizer.initializeOptimization();
         optimizer.optimize(num_each_iter_);
 
@@ -150,7 +155,7 @@ unsigned int pose_optimizer_g2o::optimize(const Mat44_t& cam_pose_cw, const data
                 }
             }
 
-            if (trial == num_trials_ - 2) {
+            if (num_trials_ != 0 && trial + 1 == num_trials_robust_) {
                 edge->setRobustKernel(nullptr);
             }
         }
diff --git a/src/stella_vslam/optimize/pose_optimizer_g2o.h b/src/stella_vslam/optimize/pose_optimizer_g2o.h
index e5ee8d250..9145215a9 100644
--- a/src/stella_vslam/optimize/pose_optimizer_g2o.h
+++ b/src/stella_vslam/optimize/pose_optimizer_g2o.h
@@ -27,10 +27,14 @@ class pose_optimizer_g2o : public pose_optimizer {
 public:
     /**
      * Constructor
+     * @param num_trials_robust
      * @param num_trials
      * @param num_each_iter
      */
-    explicit pose_optimizer_g2o(const unsigned int num_trials = 4, const unsigned int num_each_iter = 10);
+    explicit pose_optimizer_g2o(
+        unsigned int num_trials_robust = 2,
+        unsigned int num_trials = 2,
+        unsigned int num_each_iter = 10);
 
     /**
      * Destructor
@@ -53,10 +57,13 @@ class pose_optimizer_g2o : public pose_optimizer {
                           std::vector<bool>& outlier_flags) const override;
 
 private:
-    //! robust optimizationの試行回数
-    const unsigned int num_trials_ = 4;
+    //! Number of robust optimization (with outlier rejection) attempts
+    const unsigned int num_trials_robust_ = 2;
 
-    //! 毎回のoptimizationのiteration回数
+    //! Number of optimization (with outlier rejection) attempts
+    const unsigned int num_trials_ = 2;
+
+    //! Maximum number of iterations for each optimization
     const unsigned int num_each_iter_ = 10;
 };