Update initializer (Initialization with markers is now more stable)

src/stella_vslam/global_optimization_module.cc

@@ -19,7 +19,8 @@ global_optimization_module::global_optimization_module(data::map_database* map_d
       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))),
+          util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["use_huber_kernel"].as<bool>(false),
+          util::yaml_optional_ref(yaml_node, "GlobalOptimizer")["verbose"].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)) {

src/stella_vslam/module/initializer.cc

@@ -24,10 +24,11 @@ initializer::initializer(data::map_database* map_db,
       min_num_triangulated_pts_(yaml_node["min_num_triangulated_pts"].as<unsigned int>(50)),
       parallax_deg_thr_(yaml_node["parallax_deg_threshold"].as<float>(1.0)),
       reproj_err_thr_(yaml_node["reprojection_error_threshold"].as<float>(4.0)),
-      num_ba_iters_(yaml_node["num_ba_iterations"].as<unsigned int>(20)),
+      num_ba_iters_(yaml_node["num_ba_iterations"].as<unsigned int>(100)),
       scaling_factor_(yaml_node["scaling_factor"].as<float>(1.0)),
       use_fixed_seed_(yaml_node["use_fixed_seed"].as<bool>(false)),
-      required_keyframes_for_marker_initialization_(yaml_node["required_keyframes_for_marker_initialization"].as<unsigned int>(3)) {
+      gain_threshold_(yaml_node["gain_threshold"].as<float>(1e-5)),
+      verbose_(yaml_node["verbose"].as<bool>(false)) {
     spdlog::debug("CONSTRUCT: module::initializer");
 }
 
@@ -271,17 +272,19 @@ bool initializer::create_map_for_monocular(data::bow_vocabulary* bow_vocab, data
             // Set the association to the new marker
             keyfrm->add_marker(marker);
             marker->observations_.emplace(keyfrm->id_, keyfrm);
-
-            marker_initializer::check_marker_initialization(*marker, required_keyframes_for_marker_initialization_);
         }
     };
     assign_marker_associations(init_keyfrm);
     assign_marker_associations(curr_keyfrm);
 
     // global bundle adjustment
-    const auto global_bundle_adjuster = optimize::global_bundle_adjuster(num_ba_iters_, true);
+    const auto global_bundle_adjuster = optimize::global_bundle_adjuster(num_ba_iters_, true, verbose_);
     std::vector<std::shared_ptr<data::keyframe>> keyfrms{init_keyfrm, curr_keyfrm};
-    global_bundle_adjuster.optimize_for_initialization(keyfrms, lms, markers);
+    if (markers.size() > 0) {
+        // Adjust map scale with reference to marker width.
+        global_bundle_adjuster.optimize_for_initialization(keyfrms, lms, markers, gain_threshold_, true);
+    }
+    global_bundle_adjuster.optimize_for_initialization(keyfrms, lms, markers, gain_threshold_, false);
 
     if (indefinite_scale) {
         // scale the map so that the median of depths is 1.0

src/stella_vslam/module/initializer.h

@@ -87,6 +87,10 @@ class initializer {
     const float scaling_factor_;
     //! Use fixed random seed for RANSAC if true
     const bool use_fixed_seed_;
+    //! Gain threshold (for g2o)
+    const float gain_threshold_;
+    //! Verbosity (for g2o)
+    const bool verbose_;
 
     //-----------------------------------------
     // for monocular camera model

src/stella_vslam/module/loop_bundle_adjuster.cc

@@ -15,8 +15,12 @@ namespace module {
 
 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) {}
+                                           const bool use_huber_kernel,
+                                           const bool verbose)
+    : map_db_(map_db),
+      num_iter_(num_iter),
+      use_huber_kernel_(use_huber_kernel),
+      verbose_(verbose) {}
 
 void loop_bundle_adjuster::set_mapping_module(mapping_module* mapper) {
     mapper_ = mapper;
@@ -47,7 +51,7 @@ void loop_bundle_adjuster::optimize(const std::shared_ptr<data::keyframe>& curr_
     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;
     eigen_alloc_unord_map<unsigned int, std::array<Vec3_t, 4>> marker_to_pos_w_after_global_BA;
-    const auto global_BA = optimize::global_bundle_adjuster(num_iter_, use_huber_kernel_);
+    const auto global_BA = optimize::global_bundle_adjuster(num_iter_, use_huber_kernel_, verbose_);
     bool ok = global_BA.optimize(curr_keyfrm->graph_node_->get_keyframes_from_root(),
                                  optimized_keyfrm_ids, optimized_landmark_ids,
                                  optimized_marker_ids,
@@ -185,13 +189,15 @@ void loop_bundle_adjuster::optimize(const std::shared_ptr<data::keyframe>& curr_
         }
 
         for (const auto& mkr : markers) {
-            if (!optimized_marker_ids.count(mkr->id_)) // not optimized
+            if (!optimized_marker_ids.count(mkr->id_)) {
                 continue;
+            }
 
             // Update all corners
             const std::array<Vec3_t, 4>& new_corners = marker_to_pos_w_after_global_BA.at(mkr->id_);
-            for (size_t c = 0; c < 4; c++)
-                mkr->corners_pos_w_[c] = new_corners[c];
+            for (size_t corner_idx = 0; corner_idx < 4; corner_idx++) {
+                mkr->corners_pos_w_[corner_idx] = new_corners[corner_idx];
+            }
         }
 
         mapper_->resume();

src/stella_vslam/module/loop_bundle_adjuster.h

@@ -21,7 +21,8 @@ class loop_bundle_adjuster {
      */
     explicit loop_bundle_adjuster(data::map_database* map_db,
                                   const unsigned int num_iter = 10,
-                                  const bool use_huber_kernel = false);
+                                  const bool use_huber_kernel = false,
+                                  const bool verbose = false);
 
     /**
      * Destructor
@@ -57,8 +58,10 @@ class loop_bundle_adjuster {
 
     //! number of iteration for optimization
     const unsigned int num_iter_ = 10;
-
+    //! True if using Huber kernel (for g2o)
     const bool use_huber_kernel_ = false;
+    //! Verbosity (for g2o)
+    const bool verbose_ = false;
 
     //-----------------------------------------
     // thread management

src/stella_vslam/optimize/global_bundle_adjuster.cc

@@ -4,7 +4,6 @@
 #include "stella_vslam/data/map_database.h"
 #include "stella_vslam/marker_model/base.h"
 #include "stella_vslam/optimize/global_bundle_adjuster.h"
-#include "stella_vslam/optimize/local_bundle_adjuster_g2o.h"
 #include "stella_vslam/optimize/terminate_action.h"
 #include "stella_vslam/optimize/internal/landmark_vertex_container.h"
 #include "stella_vslam/optimize/internal/marker_vertex_container.h"
@@ -35,6 +34,8 @@ void optimize_impl(g2o::SparseOptimizer& optimizer,
                    std::unordered_set<unsigned int>& mkr_has_vtx,
                    unsigned int num_iter,
                    bool use_huber_kernel,
+                   bool fix_markers,
+                   bool verbose,
                    bool* const force_stop_flag) {
     // 2. Construct an optimizer
 
@@ -137,7 +138,7 @@ void optimize_impl(g2o::SparseOptimizer& optimizer,
 
         // Only optimize if requested, and if enough keyframes were available to
         // initialize it correctly
-        if (!mkr->keep_fixed_ && !mkr->initialized_before_)
+        if (!fix_markers && !mkr->keep_fixed_ && !mkr->initialized_before_)
             continue;
 
         // Indicate that a vertex container will be available, this can be
@@ -146,7 +147,7 @@ void optimize_impl(g2o::SparseOptimizer& optimizer,
         mkr_has_vtx.insert(mkr->id_);
 
         // Convert the corners to the g2o vertex, then set it to the optimizer
-        auto corner_vertices = marker_vtx_container.create_vertices(mkr, mkr->keep_fixed_);
+        auto corner_vertices = marker_vtx_container.create_vertices(mkr, fix_markers || mkr->keep_fixed_);
         for (unsigned int corner_idx = 0; corner_idx < corner_vertices.size(); ++corner_idx) {
             const auto corner_vtx = corner_vertices[corner_idx];
             optimizer.addVertex(corner_vtx);
@@ -168,7 +169,7 @@ void optimize_impl(g2o::SparseOptimizer& optimizer,
                 const float x_right = -1.0;
                 const float inv_sigma_sq = 1.0;
 
-                const auto sqrt_chi_sq = (mkr->keep_fixed_) ? 0.0 : sqrt_chi_sq_2D;
+                const auto sqrt_chi_sq = (fix_markers || mkr->keep_fixed_) ? 0.0 : sqrt_chi_sq_2D;
 
                 auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, nullptr, corner_vtx,
                                                             0, undist_pt.x, undist_pt.y, x_right,
@@ -182,19 +183,27 @@ void optimize_impl(g2o::SparseOptimizer& optimizer,
     // 5. Perform optimization
 
     optimizer.initializeOptimization();
+    optimizer.setVerbose(verbose);
     optimizer.optimize(num_iter);
 
     if (force_stop_flag && *force_stop_flag) {
         return;
     }
 }
 
-global_bundle_adjuster::global_bundle_adjuster(const unsigned int num_iter, const bool use_huber_kernel)
-    : num_iter_(num_iter), use_huber_kernel_(use_huber_kernel) {}
+global_bundle_adjuster::global_bundle_adjuster(
+    const unsigned int num_iter,
+    const bool use_huber_kernel,
+    const bool verbose)
+    : num_iter_(num_iter),
+      use_huber_kernel_(use_huber_kernel),
+      verbose_(verbose) {}
 
 void global_bundle_adjuster::optimize_for_initialization(const std::vector<std::shared_ptr<data::keyframe>>& keyfrms,
                                                          const std::vector<std::shared_ptr<data::landmark>>& lms,
                                                          const std::vector<std::shared_ptr<data::marker>>& markers,
+                                                         float gain_threshold,
+                                                         bool fix_markers,
                                                          bool* const force_stop_flag) const {
     std::vector<bool> is_optimized_lm(lms.size(), true);
 
@@ -205,12 +214,15 @@ void global_bundle_adjuster::optimize_for_initialization(const std::vector<std::
     internal::landmark_vertex_container lm_vtx_container(vtx_id_offset, lms.size());
     // Container of the landmark vertices
     internal::marker_vertex_container marker_vtx_container(vtx_id_offset, markers.size());
-    std::unordered_set<unsigned int> mkr_has_vtx; // Not really used here, we're not updating markers in this function
+    std::unordered_set<unsigned int> mkr_has_vtx;
 
     g2o::SparseOptimizer optimizer;
+    auto terminateAction = new terminate_action;
+    terminateAction->setGainThreshold(gain_threshold);
+    optimizer.addPostIterationAction(terminateAction);
 
     optimize_impl(optimizer, keyfrms, lms, markers, is_optimized_lm, keyfrm_vtx_container, lm_vtx_container, marker_vtx_container,
-                  mkr_has_vtx, num_iter_, use_huber_kernel_, force_stop_flag);
+                  mkr_has_vtx, num_iter_, use_huber_kernel_, fix_markers, verbose_, force_stop_flag);
 
     if (force_stop_flag && *force_stop_flag) {
         return;
@@ -247,6 +259,21 @@ void global_bundle_adjuster::optimize_for_initialization(const std::vector<std::
         lm->set_pos_in_world(pos_w);
         lm->update_mean_normal_and_obs_scale_variance();
     }
+
+    for (auto& mkr : markers) {
+        if (fix_markers || mkr->keep_fixed_) // No need to update
+            continue;
+        if (!mkr->initialized_before_)
+            continue;
+        // It's possible that initialized_before_ got changed since it was
+        // checked the last time, this actually tests if a vtx was made
+        if (mkr_has_vtx.find(mkr->id_) == mkr_has_vtx.end())
+            continue;
+
+        for (size_t corner_idx = 0; corner_idx < 4; corner_idx++) {
+            mkr->corners_pos_w_[corner_idx] = marker_vtx_container.get_vertex(mkr, corner_idx)->estimate();
+        }
+    }
 }
 
 bool global_bundle_adjuster::optimize(const std::vector<std::shared_ptr<data::keyframe>>& keyfrms,
@@ -309,7 +336,7 @@ bool global_bundle_adjuster::optimize(const std::vector<std::shared_ptr<data::ke
     optimizer.addPostIterationAction(terminateAction);
 
     optimize_impl(optimizer, keyfrms, lms, markers, is_optimized_lm, keyfrm_vtx_container, lm_vtx_container, marker_vtx_container,
-                  mkr_has_vtx, num_iter_, use_huber_kernel_, force_stop_flag);
+                  mkr_has_vtx, num_iter_, use_huber_kernel_, false, verbose_, force_stop_flag);
 
     if (force_stop_flag && *force_stop_flag && !terminateAction->stopped_by_terminate_action_) {
         return false;
@@ -362,15 +389,16 @@ bool global_bundle_adjuster::optimize(const std::vector<std::shared_ptr<data::ke
 
         bool changed = false;
         std::array<Vec3_t, 4> new_pos_corners;
-        for (size_t c = 0; c < 4; c++) {
-            auto orig_pos = mkr->corners_pos_w_[c];
-            auto vtx = marker_vtx_container.get_vertex(mkr, c);
+        for (size_t corner_idx = 0; corner_idx < 4; corner_idx++) {
+            auto orig_pos = mkr->corners_pos_w_[corner_idx];
+            auto vtx = marker_vtx_container.get_vertex(mkr, corner_idx);
             auto new_pos = vtx->estimate();
 
-            if (orig_pos != new_pos)
+            if (orig_pos != new_pos) {
                 changed = true;
+            }
 
-            new_pos_corners[c] = new_pos;
+            new_pos_corners[corner_idx] = new_pos;
         }
 
         if (!changed)

src/stella_vslam/optimize/global_bundle_adjuster.h

@@ -15,8 +15,12 @@ class global_bundle_adjuster {
      * Constructor
      * @param num_iter
      * @param use_huber_kernel
+     * @param verbose
      */
-    explicit global_bundle_adjuster(const unsigned int num_iter = 10, const bool use_huber_kernel = true);
+    explicit global_bundle_adjuster(
+        unsigned int num_iter = 10,
+        bool use_huber_kernel = true,
+        bool verbose = false);
 
     /**
      * Destructor
@@ -26,6 +30,8 @@ class global_bundle_adjuster {
     void optimize_for_initialization(const std::vector<std::shared_ptr<data::keyframe>>& keyfrms,
                                      const std::vector<std::shared_ptr<data::landmark>>& lms,
                                      const std::vector<std::shared_ptr<data::marker>>& markers,
+                                     float gain_threshold,
+                                     bool fix_markers,
                                      bool* const force_stop_flag = nullptr) const;
 
     /**
@@ -50,9 +56,10 @@ class global_bundle_adjuster {
 private:
     //! number of iterations of optimization
     unsigned int num_iter_;
-
     //! use Huber loss or not
     const bool use_huber_kernel_;
+    //! Verbosity (for g2o)
+    const bool verbose_ = false;
 };
 
 } // namespace optimize

src/stella_vslam/optimize/local_bundle_adjuster_g2o.cc

@@ -411,19 +411,20 @@ void local_bundle_adjuster_g2o::optimize(data::map_database* map_db,
         // Also update the marker positions
         for (auto& id_mkr_pair : local_mkrs) {
             auto& mkr = id_mkr_pair.second;
-            if (mkr->keep_fixed_)
+            if (mkr->keep_fixed_) {
                 continue;
-            if (!mkr->initialized_before_)
+            }
+            if (!mkr->initialized_before_) {
                 continue;
+            }
             // It's possible that initialized_before_ got changed since it was
             // checked the last time, this actually tests if a vtx was made
-            if (mkr_has_vtx.find(mkr->id_) == mkr_has_vtx.end())
+            if (mkr_has_vtx.find(mkr->id_) == mkr_has_vtx.end()) {
                 continue;
+            }
 
-            for (size_t c = 0; c < 4; c++) {
-                auto vtx = marker_vtx_container.get_vertex(mkr, c);
-                auto new_pos = vtx->estimate();
-                mkr->corners_pos_w_[c] = new_pos;
+            for (size_t corner_idx = 0; corner_idx < 4; corner_idx++) {
+                mkr->corners_pos_w_[corner_idx] = marker_vtx_container.get_vertex(mkr, corner_idx)->estimate();
             }
         }
     }