Adjust structure for DLLS Params

DanMcGann · DanMcGann · commit fa9b4e787c06 · 2026-03-01T18:50:05.000-05:00
diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h
@@ -258,14 +258,22 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
  * trust region is not correlated between algorithm iterations.
  */
 struct GTSAM_EXPORT DoglegLineSearchImpl {
+  struct Params {
+    double minDelta;  ///< Minimum allowed small delta
+    double maxDelta;  ///< Maximum allowed delta
+    double stepSize;  ///< Increase of trust region for outward steps
+    double sufficientDecreaseCoeff;  ///< Coefficient for suff. decrease check
+    bool verbose;                    ///< Whether to print debug information
+  };
+
   /**
    * Compute the update point for one iteration of the Dogleg Line Search
-   * algorithm, starting with a trust region of |N| * min_delta the algorithm
-   * searches trust regions from |N| * min_delta to max_delta where |N| is the
+   * algorithm, starting with a trust region of |N| * minDelta the algorithm
+   * searches trust regions from |N| * minDelta to maxDelta where |N| is the
    * number of variables in the system. The algorithm returns the search point
    * with minimum cost that meets the Wolfe Conditions. Evaluation points for
    * the line search are computed according to a geometric series step_{k+1} =
-   * step_size * step_k.
+   * stepSize * step_k.
    *
    *
    * @tparam M The type of the Bayes' net or tree, currently
@@ -274,12 +282,7 @@ struct GTSAM_EXPORT DoglegLineSearchImpl {
    * @tparam F For normal usage this will be NonlinearFactorGraph<VALUES>.
    * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate
    * the error function.
-   * @param min_delta The minimum trust region size (scaled by size of system)
-   * @param max_delta The maximum trust region size (scaled by size of system)
-   * @param step_size The multiplicative factor for the geometric series that
-   * defines the evaluation points for the line search
-   * @param sufficient_decrease_coeff The coefficient for sufficient decrease to
-   * ensure that the linesearch meets the Wolfe conditions
+   * @param params The parameters for dogleg line search
    * @param Rd The Bayes' net or tree as described above.
    * @param f The original nonlinear factor graph with which to evaluate the
    * accuracy of \f$ M(\delta x) \f$ to adjust \f$ \delta \f$.
@@ -289,83 +292,81 @@ struct GTSAM_EXPORT DoglegLineSearchImpl {
    * update \c dx_d, and the resulting nonlinear error \c f_error.
    */
   template <class M, class F, class VALUES>
-  static DoglegOptimizerImpl::IterationResult Iterate(
-      const double min_delta, const double max_delta, const double step_size,
-      const double sufficient_decrease_coeff, const VectorValues& dx_u,
-      const VectorValues& dx_n, const M& Rd, const F& f, const VALUES& x0,
-      const bool verbose = false);
+  static DoglegOptimizerImpl::IterationResult Iterate(const Params& params,
+                                                      const VectorValues& dx_u,
+                                                      const VectorValues& dx_n,
+                                                      const M& Rd, const F& f,
+                                                      const VALUES& x0);
 };
 
 /* ************************************************************************* */
 template <class M, class F, class VALUES>
 typename DoglegOptimizerImpl::IterationResult DoglegLineSearchImpl::Iterate(
-    const double min_delta, const double max_delta, const double step_size,
-    const double sufficient_decrease_coeff, const VectorValues& dx_u,
-    const VectorValues& dx_n, const M& Rd, const F& f, const VALUES& x0,
-    const bool verbose) {
-  if (verbose)
-    std::cout << "DoglegLineSearch | minDelta: " << min_delta
-              << " maxDelta: " << max_delta << " stepSize: " << step_size
-              << std::endl;
-  const double F_init_error = f.error(x0);
-  const double gn_step = dx_n.norm();
-  const size_t num_vars = dx_n.size();
-
-  double max_step, step;
+    const Params& params, const VectorValues& dx_u, const VectorValues& dx_n,
+    const M& Rd, const F& f, const VALUES& x0) {
+  if (params.verbose)
+    std::cout << "DoglegLineSearch | minDelta: " << params.minDelta
+              << " maxDelta: " << params.maxDelta
+              << " stepSize: " << params.stepSize << std::endl;
+  const double fInitError = f.error(x0);
+  const double gnStep = dx_n.norm();
+  const size_t numVars = dx_n.size();
+
+  double maxStep, step;
   /// The search bounds are scaled by the number of variables in the system
-  max_step = std::min(max_delta * num_vars, gn_step);
-  step = std::min(min_delta * num_vars, gn_step);
-  step = std::min(step, max_step);  // Edge case min_step > max_step
+  maxStep = std::min(params.maxDelta * numVars, gnStep);
+  step = std::min(params.minDelta * numVars, gnStep);
+  step = std::min(step, maxStep);  // Edge case min_step > maxStep
 
-  if (verbose)
-    std::cout << "Search Region: [ " << step << " -> " << max_step << "]"
+  if (params.verbose)
+    std::cout << "Search Region: [ " << step << " -> " << maxStep << "]"
               << std::endl;
 
   DoglegOptimizerImpl::IterationResult result;
   result.dx_d =
-      DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, verbose);
+      DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, params.verbose);
   result.f_error = f.error(x0.retract(result.dx_d));
   result.delta = step;
-  if (verbose)
+  if (params.verbose)
     std::cout << "Initial Step Error: " << result.f_error << std::endl;
 
   // Validate Step size will terminate search
-  if (step < 1e-12 || step_size < 1.0)
+  if (step < 1e-12 || params.stepSize < 1.0)
     throw std::runtime_error(
         "Invalid DoglegLineSearch configuration. Would cause infinite search.");
 
   // Search Increase delta
   double eps = std::numeric_limits<double>::epsilon();
-  while (step < max_step - eps) {
+  while (step < maxStep - eps) {
     // Compute the trust region for the evaluation point according to the
     // Geometric Series
-    step = std::min(max_step, step * step_size);
+    step = std::min(maxStep, step * params.stepSize);
 
     // Compute the Evaluation Point and evaluate the error
-    VectorValues dx_d =
-        DoglegOptimizerImpl::ComputeDoglegPoint(step, dx_u, dx_n, verbose);
+    VectorValues dx_d = DoglegOptimizerImpl::ComputeDoglegPoint(
+        step, dx_u, dx_n, params.verbose);
     VALUES x_d(x0.retract(dx_d));
-    double new_F_error = f.error(x_d);
+    double fNewError = f.error(x_d);
 
-    if (verbose)
-      std::cout << "Step: " << step << " | Error: " << new_F_error << std::endl;
+    if (params.verbose)
+      std::cout << "Step: " << step << " | Error: " << fNewError << std::endl;
 
     // Check step acceptance conditions
-    bool update_decreased_error = new_F_error < result.f_error;
-    bool update_has_sufficient_decrease =
-        new_F_error < (F_init_error - sufficient_decrease_coeff * step);
+    bool updateDecreasedError = fNewError < result.f_error;
+    bool updateHasSufficientDecrease =
+        fNewError < (fInitError - params.sufficientDecreaseCoeff * step);
 
-    if (verbose)
-      std::cout << "Decrease Error: " << update_decreased_error
-                << " | Suff. Dec.: " << update_has_sufficient_decrease
+    if (params.verbose)
+      std::cout << "Decrease Error: " << updateDecreasedError
+                << " | Suff. Dec.: " << updateHasSufficientDecrease
                 << std::endl;
 
-    if (update_decreased_error && update_has_sufficient_decrease) {
-      result.f_error = new_F_error;
+    if (updateDecreasedError && updateHasSufficientDecrease) {
+      result.f_error = fNewError;
       result.dx_d = dx_d;
       result.delta = step;
 
-      if (verbose) std::cout << "Step Accepted" << std::endl;
+      if (params.verbose) std::cout << "Step Accepted" << std::endl;
     }
   }
 
diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp
@@ -779,10 +779,10 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
     gttoc(Copy_dx_d);
   } else if (std::holds_alternative<ISAM2DoglegLineSearchParams>(
                  params_.optimizationParams)) {
-    const ISAM2DoglegLineSearchParams& dlls_params =
+    const ISAM2DoglegLineSearchParams& isamDllsParams =
         std::get<ISAM2DoglegLineSearchParams>(params_.optimizationParams);
     const double effectiveWildfireThreshold =
-        forceFullSolve ? 0.0 : dlls_params.getWildfireThreshold();
+        forceFullSolve ? 0.0 : isamDllsParams.getWildfireThreshold();
 
     // Timer start
     gttic(DoglegLineSearch_Iterate);
@@ -803,11 +803,9 @@ void ISAM2::updateDelta(bool forceFullSolve) const {
 
     // Do the DogLeg Line Search
     DoglegOptimizerImpl::IterationResult doglegResult(
-        DoglegLineSearchImpl::Iterate(
-            dlls_params.getMinDelta(), dlls_params.getMaxDelta(),
-            dlls_params.getStepSize(), dlls_params.getSufficientDecreaseCoeff(),
-            dx_u, deltaNewton_, *this, nonlinearFactors_, theta_,
-            dlls_params.isVerbose()));
+        DoglegLineSearchImpl::Iterate(isamDllsParams.dllsParams, dx_u,
+                                      deltaNewton_, *this, nonlinearFactors_,
+                                      theta_));
 
     // Update Delta and linear step
     delta_ = doglegResult.dx_d;
diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h
@@ -134,25 +134,18 @@ struct GTSAM_EXPORT ISAM2DoglegParams {
  * ISAM2(const ISAM2Params&).
  */
 struct GTSAM_EXPORT ISAM2DoglegLineSearchParams {
-  double minDelta;  ///< Minimum allowed small delta
-  double maxDelta;  ///< Maximum allowed delta
-  double stepSize;  ///< Increase of trust region for outward steps
-  double sufficientDecreaseCoeff;  ///< Coefficient for suff. decrease check
+  DoglegLineSearchImpl::Params dllsParams;  ///< Params for DoglegLineSearch
   double wildfireThreshold;  ///< Update delta when changes are above thresh
-  bool verbose;              ///< Whether to print debug information
 
   /** Specify parameters as constructor arguments */
   ISAM2DoglegLineSearchParams(double minDelta = 0.02, double maxDelta = 0.5,
                               double stepSize = 1.5,
                               double sufficientDecreaseCoeff = 1e-3,
-                              double wildfireThreshold = 1e-4,
-                              bool verbose = false)
-      : minDelta(minDelta),
-        maxDelta(maxDelta),
-        stepSize(stepSize),
-        sufficientDecreaseCoeff(sufficientDecreaseCoeff),
-        wildfireThreshold(wildfireThreshold),
-        verbose(verbose) {
+                              bool verbose = false,
+                              double wildfireThreshold = 1e-4)
+      : dllsParams{minDelta, maxDelta, stepSize, sufficientDecreaseCoeff,
+                   verbose},
+        wildfireThreshold(wildfireThreshold) {
     if (minDelta < 1e-12 || maxDelta < 1e-12 || stepSize < 1.0) {
       throw std::invalid_argument(
           "ISAM2DoglegLineSearchParams constructed with invalid configuration. "
@@ -163,33 +156,37 @@ struct GTSAM_EXPORT ISAM2DoglegLineSearchParams {
   void print(const std::string str = "") const {
     using std::cout;
     cout << str << "type:                      ISAM2DoglegLineSearchParams\n";
-    cout << str << "minDelta:                 " << minDelta << "\n";
-    cout << str << "maxDelta:                 " << maxDelta << "\n";
-    cout << str << "stepSize:                 " << stepSize << "\n";
-    cout << str << "sufficientDecreaseCoeff: " << sufficientDecreaseCoeff
+    cout << str << "minDelta:                 " << dllsParams.minDelta << "\n";
+    cout << str << "maxDelta:                 " << dllsParams.maxDelta << "\n";
+    cout << str << "stepSize:                 " << dllsParams.stepSize << "\n";
+    cout << str
+         << "sufficientDecreaseCoeff: " << dllsParams.sufficientDecreaseCoeff
          << "\n";
+    cout << str << "verbose:        " << dllsParams.verbose << "\n";
     cout << str << "wildfireThreshold:        " << wildfireThreshold << "\n";
     cout.flush();
   }
   /** Getters **/
-  double getMinDelta() const { return minDelta; }
-  double getMaxDelta() const { return maxDelta; }
-  double getStepSize() const { return stepSize; }
-  double getSufficientDecreaseCoeff() const { return sufficientDecreaseCoeff; }
+  double getMinDelta() const { return dllsParams.minDelta; }
+  double getMaxDelta() const { return dllsParams.maxDelta; }
+  double getStepSize() const { return dllsParams.stepSize; }
+  double getSufficientDecreaseCoeff() const {
+    return dllsParams.sufficientDecreaseCoeff;
+  }
+  bool isVerbose() const { return dllsParams.verbose; }
   double getWildfireThreshold() const { return wildfireThreshold; }
-  bool isVerbose() const { return verbose; }
 
   /** Setters */
-  void setMinDelta(double minDelta) { this->minDelta = minDelta; }
-  void setMaxDelta(double maxDelta) { this->maxDelta = maxDelta; }
-  void setStepSize(double stepSize) { this->stepSize = stepSize; }
+  void setMinDelta(double minDelta) { this->dllsParams.minDelta = minDelta; }
+  void setMaxDelta(double maxDelta) { this->dllsParams.maxDelta = maxDelta; }
+  void setStepSize(double stepSize) { this->dllsParams.stepSize = stepSize; }
   void setSufficientDecreaseCoeff(double sufficientDecreaseCoeff) {
-    this->sufficientDecreaseCoeff = sufficientDecreaseCoeff;
+    this->dllsParams.sufficientDecreaseCoeff = sufficientDecreaseCoeff;
   }
+  void setVerbose(bool verbose) { this->dllsParams.verbose = verbose; }
   void setWildfireThreshold(double wildfireThreshold) {
     this->wildfireThreshold = wildfireThreshold;
   }
-  void setVerbose(bool verbose) { this->verbose = verbose; }
 };
 
 /**
diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i
@@ -437,14 +437,14 @@ class ISAM2DoglegLineSearchParams {
   double getMaxDelta() const;
   double getStepSize() const;
   double getSufficientDecreaseCoeff() const;
-  double getWildfireThreshold() const;
   bool isVerbose() const;
+  double getWildfireThreshold() const;
   void setMinDelta(double min_delta);
   void setMaxDelta(double max_delta);
   void setStepSize(double step_size);
   void setSufficientDecreaseCoeff(double sufficient_decrease_coeff);
-  void setWildfireThreshold(double wildfire_threshold);
   void setVerbose(bool verbose);
+  void setWildfireThreshold(double wildfire_threshold);
 };
 
 class ISAM2ThresholdMapValue {
diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp
@@ -186,7 +186,7 @@ TEST(DoglegOptimizer, IterateLineSearch) {
     VectorValues dx_u = gbn->optimizeGradientSearch();
     VectorValues dx_n = gbn->optimize();
     DoglegOptimizerImpl::IterationResult result = DoglegLineSearchImpl::Iterate(
-        0.02, 0.5, 1.5, 1e-3, dx_u, dx_n, *gbn, fg, config);
+        {0.02, 0.5, 1.5, 1e-3, false}, dx_u, dx_n, *gbn, fg, config);
     EXPECT(result.f_error < fg.error(config));  // Check that error decreases
 
     Values newConfig(config.retract(result.dx_d));
diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp
@@ -318,13 +318,13 @@ TEST(ISAM2, slamlike_solution_dogleg)
 }
 
 /* ************************************************************************* */
-TEST(ISAM2, slamlikeSolutionDoglegLineSearch) {
+TEST(ISAM2, SlamlikeSolutionDoglegLineSearch) {
   // These variables will be reused and accumulate factors and values
   Values fullinit;
   NonlinearFactorGraph fullgraph;
   ISAM2 isam = createSlamlikeISAM2(
       &fullinit, &fullgraph,
-      ISAM2Params(ISAM2DoglegLineSearchParams(0.1, 1.0, 3, 1e-4, 1e-3, false),
+      ISAM2Params(ISAM2DoglegLineSearchParams(0.1, 1.0, 3, 1e-4, false, 1e-3),
                   0.0, 0, false));
 
   // Compare solutions
@@ -356,13 +356,13 @@ TEST(ISAM2, slamlike_solution_dogleg_qr)
 }
 
 /* ************************************************************************* */
-TEST(ISAM2, slamlikeSolutionDoglegLineSearchQr) {
+TEST(ISAM2, SlamlikeSolutionDoglegLineSearchQr) {
   // These variables will be reused and accumulate factors and values
   Values fullinit;
   NonlinearFactorGraph fullgraph;
   ISAM2 isam = createSlamlikeISAM2(
       &fullinit, &fullgraph,
-      ISAM2Params(ISAM2DoglegLineSearchParams(0.1, 10.0, 3, 1e-4, 1e-3, false),
+      ISAM2Params(ISAM2DoglegLineSearchParams(0.1, 10.0, 3, 1e-4, false, 1e-3),
                   0.0, 0, false, false, ISAM2Params::QR));
 
   // Compare solutions
@@ -1023,7 +1023,7 @@ TEST(ISAM2, calculate_nnz)
 }
 
 /* ************************************************************************* */
-TEST(ISAM2, predictUpdateInfo) {
+TEST(ISAM2, PredictUpdateInfo) {
   // Setup iSAM2
   ISAM2Params params;
   params.findUnusedFactorSlots = true;
