Add separate optimization solver params for pre-pgo tracking and post…

…-pgo VF adjustment
ut-amrl · Dec 13, 2023 · 9d77230 · 9d77230
1 parent 6477b98
commit 9d77230
Show file tree

Hide file tree

Showing 8 changed files with 564 additions and 5 deletions.
diff --git a/config/update_revision_base.json b/config/update_revision_base.json
diff --git a/include/file_io/cv_file_storage/config_file_storage_io.h b/include/file_io/cv_file_storage/config_file_storage_io.h
@@ -678,6 +678,17 @@ class SerializablePoseGraphPlusObjectsOptimizationParams
     fs << kFinalPgoOptimizationSolverParamsLabel
        << SerializableOptimizationSolverParams(
               data_.final_pgo_optimization_solver_params_);
+
+    fs << kPostPgoVfAdjustmentSolverParamsLabel
+       << SerializableOptimizationSolverParams(
+              data_.post_pgo_vf_adjustment_solver_params_);
+    fs << kFinalPostPgoVfAdjustmentSolverParamsLabel
+       << SerializableOptimizationSolverParams(
+              data_.final_post_pgo_vf_adjustment_solver_params_);
+    fs << kPrePgoTrackingSolverParamsLabel
+       << SerializableOptimizationSolverParams(
+              data_.pre_pgo_tracking_solver_params_);
+
     fs << "}";
   }
 
@@ -711,6 +722,24 @@ class SerializablePoseGraphPlusObjectsOptimizationParams
         ser_final_pgo_optimization_solver_params_;
     data_.final_pgo_optimization_solver_params_ =
         ser_final_pgo_optimization_solver_params_.getEntry();
+
+    SerializableOptimizationSolverParams
+        ser_post_pgo_vf_adjustment_solver_params;
+    node[kPostPgoVfAdjustmentSolverParamsLabel] >>
+        ser_post_pgo_vf_adjustment_solver_params;
+    data_.post_pgo_vf_adjustment_solver_params_ =
+        ser_post_pgo_vf_adjustment_solver_params.getEntry();
+    SerializableOptimizationSolverParams
+        ser_final_post_pgo_vf_adjustment_solver_params;
+    node[kFinalPostPgoVfAdjustmentSolverParamsLabel] >>
+        ser_final_post_pgo_vf_adjustment_solver_params;
+    data_.final_post_pgo_vf_adjustment_solver_params_ =
+        ser_final_post_pgo_vf_adjustment_solver_params.getEntry();
+    SerializableOptimizationSolverParams ser_pre_pgo_tracking_solver_params;
+    node[kPrePgoTrackingSolverParamsLabel] >>
+        ser_pre_pgo_tracking_solver_params;
+    data_.pre_pgo_tracking_solver_params_ =
+        ser_pre_pgo_tracking_solver_params.getEntry();
   }
 
  protected:
@@ -731,6 +760,12 @@ class SerializablePoseGraphPlusObjectsOptimizationParams
       "pgo_optimization_solver_params";
   inline static const std::string kFinalPgoOptimizationSolverParamsLabel =
       "final_pgo_optimization_solver_params";
+  inline static const std::string kPostPgoVfAdjustmentSolverParamsLabel =
+      "post_pgo_vf_adjustment_solver_params";
+  inline static const std::string kFinalPostPgoVfAdjustmentSolverParamsLabel =
+      "final_post_pgo_vf_adjustment_solver_params";
+  inline static const std::string kPrePgoTrackingSolverParamsLabel =
+      "pre_pgo_tracking_solver_params";
 };
 
 static void write(

diff --git a/include/refactoring/configuration/full_ov_slam_config.h b/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 = 13;
+const static int kCurrentConfigSchemaVersion = 14;
 
 struct VisualFeatureParams {
   double reprojection_error_std_dev_;

diff --git a/include/refactoring/offline/offline_problem_runner.h b/include/refactoring/offline/offline_problem_runner.h
@@ -477,7 +477,7 @@ class OfflineProblemRunner {
                   std::make_shared<ceres::Solver::Summary>();
               if (!optimizer_.solveOptimization(
                       &problem,
-                      iteration_params.phase_one_opt_params_,
+                      pgo_solver_params_.pre_pgo_tracking_solver_params_,
                       ceres_callbacks,
                       null_logger,
                       nullptr,

diff --git a/include/refactoring/optimization/optimization_solver_params.h b/include/refactoring/optimization/optimization_solver_params.h
@@ -177,6 +177,9 @@ struct PoseGraphPlusObjectsOptimizationParams {
   RelativePoseCovarianceOdomModelParams relative_pose_cov_params_;
   OptimizationSolverParams pgo_optimization_solver_params_;
   OptimizationSolverParams final_pgo_optimization_solver_params_;
+  OptimizationSolverParams post_pgo_vf_adjustment_solver_params_;
+  OptimizationSolverParams final_post_pgo_vf_adjustment_solver_params_;
+  OptimizationSolverParams pre_pgo_tracking_solver_params_;
 
   bool operator==(const PoseGraphPlusObjectsOptimizationParams &rhs) const {
     return (relative_pose_factor_huber_loss_ ==
@@ -189,7 +192,13 @@ struct PoseGraphPlusObjectsOptimizationParams {
            (pgo_optimization_solver_params_ ==
             rhs.pgo_optimization_solver_params_) &&
            (final_pgo_optimization_solver_params_ ==
-            rhs.final_pgo_optimization_solver_params_);
+            rhs.final_pgo_optimization_solver_params_) &&
+           (post_pgo_vf_adjustment_solver_params_ ==
+            rhs.post_pgo_vf_adjustment_solver_params_) &&
+           (final_post_pgo_vf_adjustment_solver_params_ ==
+            rhs.final_post_pgo_vf_adjustment_solver_params_) &&
+           (pre_pgo_tracking_solver_params_ ==
+            rhs.pre_pgo_tracking_solver_params_);
   }
 
   bool operator!=(const PoseGraphPlusObjectsOptimizationParams &rhs) const {

diff --git a/include/refactoring/optimization/pose_graph_plus_objects_optimizer.h b/include/refactoring/optimization/pose_graph_plus_objects_optimizer.h
@@ -321,8 +321,8 @@ bool runPgoPlusEllipsoids(
       if (!optimizer.solveOptimization(
               &problem,
               final_run
-                  ? pgo_solver_params.final_pgo_optimization_solver_params_
-                  : pgo_solver_params.pgo_optimization_solver_params_,
+                  ? pgo_solver_params.final_post_pgo_vf_adjustment_solver_params_
+                  : pgo_solver_params.post_pgo_vf_adjustment_solver_params_,
               ceres_callbacks,
               null_logger,
               nullptr,

diff --git a/src/refactoring/configuration/write_configuration.cpp b/src/refactoring/configuration/write_configuration.cpp
@@ -226,6 +226,13 @@ int main(int argc, char **argv) {
       global_ba_solver_params;
   configuration.pgo_solver_params_.final_pgo_optimization_solver_params_ =
       final_opt_solver_params;
+  configuration.pgo_solver_params_.final_post_pgo_vf_adjustment_solver_params_ =
+      final_ba_iteration_params.phase_two_opt_params_;
+  configuration.pgo_solver_params_.post_pgo_vf_adjustment_solver_params_ =
+      local_ba_iteration_params.phase_two_opt_params_;
+  configuration.pgo_solver_params_.pre_pgo_tracking_solver_params_ =
+      local_ba_iteration_params.phase_two_opt_params_;
+
   pose_graph_optimization::RelativePoseCovarianceOdomModelParams
       relative_pose_cov_params;
   relative_pose_cov_params.transl_error_mult_for_transl_error_ = 0.1;

diff --git a/test/file_io/cv_file_storage/config_file_storage_io_tests.cc b/test/file_io/cv_file_storage/config_file_storage_io_tests.cc
@@ -131,6 +131,20 @@ TEST(FullOVSLAMConfigIO, ReadWriteFullOBSLAMConfig) {
   orig_config.pgo_solver_params_.enable_visual_feats_only_opt_post_pgo_ = true;
   orig_config.pgo_solver_params_
       .enable_visual_non_opt_feature_adjustment_post_pgo_ = false;
+  orig_config.pgo_solver_params_.final_post_pgo_vf_adjustment_solver_params_ =
+      orig_config.final_ba_iteration_params_.phase_two_opt_params_;
+  orig_config.pgo_solver_params_.final_post_pgo_vf_adjustment_solver_params_
+      .max_num_iterations_ = 567;
+
+  orig_config.pgo_solver_params_.post_pgo_vf_adjustment_solver_params_ =
+      orig_config.local_ba_iteration_params_.phase_two_opt_params_;
+  orig_config.pgo_solver_params_.post_pgo_vf_adjustment_solver_params_
+      .max_num_iterations_ = 523;
+
+  orig_config.pgo_solver_params_.pre_pgo_tracking_solver_params_ =
+      orig_config.local_ba_iteration_params_.phase_two_opt_params_;
+  orig_config.pgo_solver_params_.pre_pgo_tracking_solver_params_
+      .function_tolerance_ = 1.2857;
 
   LongTermMapExtractionTunableParams ltm_tunable_params;
   ltm_tunable_params.far_feature_threshold_ = 2.4;