diff --git a/src/stella_vslam/optimize/pose_optimizer_factory.h b/src/stella_vslam/optimize/pose_optimizer_factory.h index f29dec4f..09d39c80 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 95e2a3f9..bb4a3676 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 e5ee8d25..9145215a 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; };