Add a couple things in visual feature front-end to config; move final…

… refinement after online portion

include/file_io/cv_file_storage/config_file_storage_io.h

@@ -1280,22 +1280,29 @@ class SerializableVisualFeatureParams
 
     int enforce_min_pixel_parallax_requirement =
         data_.enforce_min_pixel_parallax_requirement_ ? 1 : 0;
-    fs << kEnforceMinPixelParallaxRequirement
+    fs << kEnforceMinPixelParallaxRequirementLabel
        << enforce_min_pixel_parallax_requirement;
 
     int enforce_min_robot_pose_parallax_requirement =
         data_.enforce_min_robot_pose_parallax_requirement_ ? 1 : 0;
-    fs << kEnforceMinRobotPoseParallaxRequirement
+    fs << kEnforceMinRobotPoseParallaxRequirementLabel
        << enforce_min_robot_pose_parallax_requirement;
 
-    fs << kInlierEpipolarErrThresh << (double)data_.inlier_epipolar_err_thresh_;
-    fs << kCheckPastNFramesForEpipolarErr
+    fs << kInlierEpipolarErrThreshLabel
+       << (double)data_.inlier_epipolar_err_thresh_;
+    fs << kCheckPastNFramesForEpipolarErrLabel
        << (int)data_.check_past_n_frames_for_epipolar_err_;
 
     int enforce_epipolar_error_requirement =
         data_.enforce_epipolar_error_requirement_ ? 1 : 0;
-    fs << kEnforceEpipolarErrorRequirement
+    fs << kEnforceEpipolarErrorRequirementLabel
        << (int)enforce_epipolar_error_requirement;
+
+    int early_votes_return_int = data_.early_votes_return_ ? 1 : 0;
+    fs << kEarlyVotesReturnLabel << (int)early_votes_return_int;
+
+    fs << kVisualFeatureInlierMajorityPercentageLabel
+       << data_.visual_feature_inlier_majority_percentage_;
     fs << "}";
   }
 
@@ -1309,25 +1316,32 @@ class SerializableVisualFeatureParams
         data_.min_visual_feature_parallax_robot_orient_requirement_;
 
     int enforce_min_pixel_parallax_requirement =
-        node[kEnforceMinPixelParallaxRequirement];
+        node[kEnforceMinPixelParallaxRequirementLabel];
     data_.enforce_min_pixel_parallax_requirement_ =
         enforce_min_pixel_parallax_requirement != 0;
     int enforce_min_robot_pose_parallax_requirement =
-        node[kEnforceMinRobotPoseParallaxRequirement];
+        node[kEnforceMinRobotPoseParallaxRequirementLabel];
     data_.enforce_min_robot_pose_parallax_requirement_ =
         enforce_min_robot_pose_parallax_requirement != 0;
 
-    double inlier_epipolar_err_thresh = (double)node[kInlierEpipolarErrThresh];
+    double inlier_epipolar_err_thresh =
+        (double)node[kInlierEpipolarErrThreshLabel];
     data_.inlier_epipolar_err_thresh_ = inlier_epipolar_err_thresh;
     int check_past_n_frames_for_epipolar_err =
-        (int)node[kCheckPastNFramesForEpipolarErr];
+        (int)node[kCheckPastNFramesForEpipolarErrLabel];
     data_.check_past_n_frames_for_epipolar_err_ =
         check_past_n_frames_for_epipolar_err;
 
     int enforce_epipolar_error_requirement =
-        node[kEnforceEpipolarErrorRequirement];
+        node[kEnforceEpipolarErrorRequirementLabel];
     data_.enforce_epipolar_error_requirement_ =
         enforce_epipolar_error_requirement != 0;
+
+    int early_votes_return_int = node[kEarlyVotesReturnLabel];
+    data_.early_votes_return_ = early_votes_return_int != 0;
+
+    data_.visual_feature_inlier_majority_percentage_ =
+        node[kVisualFeatureInlierMajorityPercentageLabel];
   }
 
  protected:
@@ -1345,16 +1359,19 @@ class SerializableVisualFeatureParams
   inline static const std::string
       kMinVisualFeatureParallaxRobotOrientRequirementLabel =
           "min_visual_feature_parallax_robot_orient_requirement";
-  inline static const std::string kEnforceMinPixelParallaxRequirement =
+  inline static const std::string kEnforceMinPixelParallaxRequirementLabel =
       "enforce_min_pixel_parallax_requirement";
-  inline static const std::string kEnforceMinRobotPoseParallaxRequirement =
+  inline static const std::string kEnforceMinRobotPoseParallaxRequirementLabel =
       "enforce_min_robot_pose_parallax_requirement";
-  inline static const std::string kInlierEpipolarErrThresh =
+  inline static const std::string kInlierEpipolarErrThreshLabel =
       "inlier_epipolar_err_thresh";
-  inline static const std::string kCheckPastNFramesForEpipolarErr =
+  inline static const std::string kCheckPastNFramesForEpipolarErrLabel =
       "check_past_n_frames_for_epipolar_err";
-  inline static const std::string kEnforceEpipolarErrorRequirement =
+  inline static const std::string kEnforceEpipolarErrorRequirementLabel =
       "enforce_epipolar_error_requirement_";
+  inline static const std::string kEarlyVotesReturnLabel = "early_votes_return";
+  inline static const std::string kVisualFeatureInlierMajorityPercentageLabel =
+      "visual_feature_inlier_majority_percentage";
 };
 
 static void write(cv::FileStorage &fs,

include/refactoring/configuration/full_ov_slam_config.h

@@ -21,7 +21,7 @@ namespace vslam_types_refactor {
 
 // NOTE: This should be incremented every time the format of the configuration
 // data changes
-const static int kCurrentConfigSchemaVersion = 12;
+const static int kCurrentConfigSchemaVersion = 13;
 
 struct VisualFeatureParams {
   double reprojection_error_std_dev_;
@@ -35,6 +35,9 @@ struct VisualFeatureParams {
   size_t check_past_n_frames_for_epipolar_err_;
   bool enforce_epipolar_error_requirement_;
 
+  bool early_votes_return_ = true;
+  double visual_feature_inlier_majority_percentage_ = 0.5;
+
   bool operator==(const VisualFeatureParams &rhs) const {
     return (reprojection_error_std_dev_ == rhs.reprojection_error_std_dev_) &&
            (min_visual_feature_parallax_pixel_requirement_ ==
@@ -51,7 +54,10 @@ struct VisualFeatureParams {
            (check_past_n_frames_for_epipolar_err_ ==
             rhs.check_past_n_frames_for_epipolar_err_) &&
            (enforce_epipolar_error_requirement_ ==
-            rhs.enforce_epipolar_error_requirement_);
+            rhs.enforce_epipolar_error_requirement_) &&
+           (early_votes_return_ == rhs.early_votes_return_) &&
+           (visual_feature_inlier_majority_percentage_ ==
+            rhs.visual_feature_inlier_majority_percentage_);
   }
 
   bool operator!=(const VisualFeatureParams &rhs) const {

include/refactoring/offline/offline_problem_runner.h

@@ -199,6 +199,20 @@ class OfflineProblemRunner {
       }
     }
 
+    // Run the final optimization (refinement)
+    if (!runOptimizationIteration(0,
+                                  max_frame_id,
+                                  problem_data,
+                                  optimization_factors_enabled_params,
+                                  optimization_scope_params,
+                                  max_frame_id,
+                                  opt_logger,
+                                  pose_graph,
+                                  problem,
+                                  1)) {
+      return false;
+    }
+
     visualization_callback_(problem_data,
                             pose_graph,
                             0,
@@ -366,7 +380,7 @@ class OfflineProblemRunner {
     bool run_visual_feature_opt = true;
     if (global_ba) {
       bool run_pgo;
-      if (next_frame_id == max_frame_id) {
+      if ((next_frame_id == max_frame_id) && (attempt_num > 0)) {
         run_pgo = optimization_factors_enabled_params
                       .use_pose_graph_on_final_global_ba_;
         if (run_pgo) {
@@ -811,7 +825,7 @@ class OfflineProblemRunner {
 
     // Until there are no more objects to merge, check if a merge should be
     // performed, and then if so, rerun the optimization to update the estimates
-    int post_process_round = 1;
+    int post_process_round = 2;
     while (object_merger_(pose_graph)) {
       ceres::Problem merged_problem;
       optimizer_.clearPastOptimizationData();

include/refactoring/visual_feature_frontend/visual_feature_front_end.h

@@ -228,7 +228,9 @@ class VisualFeatureFrontend {
       const bool &enforce_min_robot_pose_parallax_requirement,
       const double &inlier_epipolar_err_thresh,
       const size_t &check_pase_n_frames_for_epipolar_err,
-      const bool &enforce_epipolar_error_requirement)
+      const bool &enforce_epipolar_error_requirement,
+      const bool &early_votes_return,
+      const double &inlier_majority_percentage)
       : gba_checker_(gba_checker),
         reprojection_error_provider_(reprojection_error_provider),
         min_visual_feature_parallax_pixel_requirement_(
@@ -245,7 +247,9 @@ class VisualFeatureFrontend {
         check_pase_n_frames_for_epipolar_err_(
             check_pase_n_frames_for_epipolar_err),
         enforce_epipolar_error_requirement_(
-            enforce_epipolar_error_requirement) {}
+            enforce_epipolar_error_requirement),
+        early_votes_return_(early_votes_return),
+        inlier_majority_percentage_(inlier_majority_percentage) {}
 
   /**
    * @brief
@@ -474,8 +478,9 @@ class VisualFeatureFrontend {
   double inlier_epipolar_err_thresh_ = 8.0;
   size_t check_pase_n_frames_for_epipolar_err_ = 5;
   bool enforce_epipolar_error_requirement_ = true;
+  bool early_votes_return_ = true;
 
-  const double inlier_majority_percentage_ = 0.5;
+  double inlier_majority_percentage_ = 0.5;
 
  private:
   void getFactorsByFeatureIdFromPoseGraph_(
@@ -586,9 +591,11 @@ class VisualFeatureFrontend {
         n_voters++;
       }
       // TODO maybe add this to configuration as well
-
-      return (((double)votes) / n_voters) > inlier_majority_percentage_;
+      if (early_votes_return_) {
+        return (((double)votes) / n_voters) > inlier_majority_percentage_;
+      }
     }
+    return (((double)votes) / n_voters) > inlier_majority_percentage_;
   }
 
   bool isReprojectionErrorFactorInlierInPoseGraph_(

include/run_optimization_utils/optimization_runner.h

@@ -313,7 +313,9 @@ bool runFullOptimization(
           .enforce_min_robot_pose_parallax_requirement_,
       config.visual_feature_params_.inlier_epipolar_err_thresh_,
       config.visual_feature_params_.check_past_n_frames_for_epipolar_err_,
-      config.visual_feature_params_.enforce_epipolar_error_requirement_);
+      config.visual_feature_params_.enforce_epipolar_error_requirement_,
+      config.visual_feature_params_.early_votes_return_,
+      config.visual_feature_params_.visual_feature_inlier_majority_percentage_);
   std::function<void(const MainProbData &,
                      const MainPgPtr &,
                      const FrameId &,

src/refactoring/configuration/write_configuration.cpp

@@ -164,6 +164,9 @@ int main(int argc, char **argv) {
       5;
   configuration.visual_feature_params_.enforce_epipolar_error_requirement_ =
       true;
+  configuration.visual_feature_params_.early_votes_return_ = true;
+  configuration.visual_feature_params_
+      .visual_feature_inlier_majority_percentage_ = 0.5;
 
   // Set up defaults for the different types of optimization that can be
   // overridden later
@@ -240,7 +243,8 @@ int main(int argc, char **argv) {
   configuration.ltm_tunable_params_.min_col_norm_ = 5e-9;
   configuration.ltm_solver_params_ =
       base_solver_params;  // TODO is this the one we want?
-  configuration.ltm_tunable_params_.fallback_to_prev_for_failed_extraction_ = true;
+  configuration.ltm_tunable_params_.fallback_to_prev_for_failed_extraction_ =
+      true;
   configuration.ltm_solver_params_.max_num_iterations_ = 300;
 
   pose_graph_optimization::ObjectVisualPoseGraphResidualParams residual_params;

test/file_io/cv_file_storage/config_file_storage_io_tests.cc

@@ -38,6 +38,8 @@ TEST(FullOVSLAMConfigIO, ReadWriteFullOBSLAMConfig) {
       1.3;
   visual_feature_params.min_visual_feature_parallax_pixel_requirement_ = 1.4;
   visual_feature_params.reprojection_error_std_dev_ = 1.5;
+  visual_feature_params.early_votes_return_ = false;
+  visual_feature_params.visual_feature_inlier_majority_percentage_ = 0.7;
   orig_config.visual_feature_params_ = visual_feature_params;
 
   pose_graph_optimization::OptimizationSolverParams