Naming Conventions

examples/example_constraint_manifold/CartPoleUtils.cpp

@@ -249,9 +249,9 @@ OptimizerSetting CartPole::getOptSetting() const {
 }
 
 /* ************************************************************************* */
-BasisKeyFunc CartPole::getBasisKeyFunc(bool unactuated_as_constraint) const {
+BasisKeyFunction CartPole::getBasisKeyFunction(bool unactuated_as_constraint) const {
   if (unactuated_as_constraint) {
-    BasisKeyFunc basis_key_func =
+    BasisKeyFunction basisKeyFunction =
         [=](const KeyVector& keys) -> KeyVector {
       KeyVector basis_keys;
       for (const Key& key : keys) {
@@ -268,9 +268,9 @@ BasisKeyFunc CartPole::getBasisKeyFunc(bool unactuated_as_constraint) const {
       }
       return basis_keys;
     };
-    return basis_key_func;
+    return basisKeyFunction;
   } else {
-    BasisKeyFunc basis_key_func =
+    BasisKeyFunction basisKeyFunction =
         [](const KeyVector& keys) -> KeyVector {
       KeyVector basis_keys;
       for (const Key& key : keys) {
@@ -287,7 +287,7 @@ BasisKeyFunc CartPole::getBasisKeyFunc(bool unactuated_as_constraint) const {
       }
       return basis_keys;
     };
-    return basis_key_func;
+    return basisKeyFunction;
   }
 }
 

examples/example_constraint_manifold/CartPoleUtils.h

@@ -14,7 +14,7 @@
 #pragma once
 
 #include <gtdynamics/dynamics/DynamicsGraph.h>
-#include <gtdynamics/cmopt/NonlinearMOptimizer.h>
+#include <gtdynamics/cmopt/NonlinearManifoldOptimizer.h>
 #include <gtdynamics/universal_robot/Robot.h>
 #include <gtdynamics/universal_robot/sdf.h>
 #include <gtdynamics/utils/Initializer.h>
@@ -95,7 +95,7 @@ class CartPole {
   void printJointAngles(const Values& values, size_t num_steps) const;
 
   /// Return function that select basis keys for constraint manifolds.
-  BasisKeyFunc getBasisKeyFunc(bool unactuated_as_constraint = true) const;
+  BasisKeyFunction getBasisKeyFunction(bool unactuated_as_constraint = true) const;
 
 protected:
   /// Default optimizer setting.

examples/example_constraint_manifold/QuadrupedUtils.cpp

@@ -319,17 +319,17 @@ Values Vision60Robot::getInitValuesStep(const int t, const Pose3 &base_pose,
   }
 
   Values known_values;
-  LevenbergMarquardtParams lm_params;
-  // lm_params.setVerbosityLM("SUMMARY");
-  lm_params.setlambdaUpperBound(1e20);
+  LevenbergMarquardtParams lmParams;
+  // lmParams.setVerbosityLM("SUMMARY");
+  lmParams.setlambdaUpperBound(1e20);
 
   // solve q level
   NonlinearFactorGraph graph_q = getConstraintsGraphStepQ(t);
   graph_q.addPrior<Pose3>(PoseKey(base_id, t), base_pose,
                           graph_builder.opt().p_cost_model);
 
   Values init_values_q = SubValues(init_values_t, graph_q.keys());
-  LevenbergMarquardtOptimizer optimizer_q(graph_q, init_values_q, lm_params);
+  LevenbergMarquardtOptimizer optimizer_q(graph_q, init_values_q, lmParams);
   auto results_q = optimizer_q.optimize();
   if (graph_q.error(results_q) > 1e-5) {
     std::cout << "solving q fails! error: " << graph_q.error(results_q) << "\n";
@@ -342,7 +342,7 @@ Values Vision60Robot::getInitValuesStep(const int t, const Pose3 &base_pose,
                             graph_builder.opt().v_cost_model);
   graph_v = ConstVarGraph(graph_v, known_values);
   Values init_values_v = SubValues(init_values_t, graph_v.keys());
-  LevenbergMarquardtOptimizer optimizer_v(graph_v, init_values_v, lm_params);
+  LevenbergMarquardtOptimizer optimizer_v(graph_v, init_values_v, lmParams);
   auto results_v = optimizer_v.optimize();
   if (graph_v.error(results_v) > 1e-5) {
     std::cout << "solving v fails! error: " << graph_v.error(results_v) << "\n";
@@ -360,7 +360,7 @@ Values Vision60Robot::getInitValuesStep(const int t, const Pose3 &base_pose,
   }
   graph_ad = ConstVarGraph(graph_ad, known_values);
   Values init_values_ad = SubValues(init_values_t, graph_ad.keys());
-  LevenbergMarquardtOptimizer optimizer_ad(graph_ad, init_values_ad, lm_params);
+  LevenbergMarquardtOptimizer optimizer_ad(graph_ad, init_values_ad, lmParams);
   auto results_ad = optimizer_ad.optimize();
   if (graph_ad.error(results_ad) > 1e-5) {
     std::cout << "solving ad fails! error: " << graph_ad.error(results_ad)

examples/example_constraint_manifold/QuadrupedUtils.h

@@ -179,7 +179,7 @@ class Vision60Robot {
   const OptimizerSetting &opt() const { return graph_builder.opt(); }
 
   /// Return function that select basis keys for constraint manifolds.
-  BasisKeyFunc getBasisKeyFunc() const {
+  BasisKeyFunction getBasisKeyFunction() const {
     if (express_redundancy) {
       return &findBasisKeysRedundancy;
     } else {

examples/example_constraint_manifold/main_arm_kinematic_planning.cpp

@@ -411,7 +411,7 @@ void kinematic_planning(const ArmBenchmarkArgs& args) {
     auto moptParams =
         ConstrainedOptBenchmark::DefaultMoptParamsSV(&FindBasisKeys);
     auto* retractLm =
-        &moptParams.cc_params->retractor_creator->params()->lm_params;
+        &moptParams.constraintManifoldParams->retractorCreator->params()->lmParams;
     retractLm->linearSolverType =
         gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY;
     retractLm->setlambdaUpperBound(1e2);

examples/example_constraint_manifold/main_cablerobot.cpp

@@ -150,11 +150,11 @@ void kinematic_planning(const ConstrainedOptBenchmark::Options& runOptions) {
   runner.setOuterLmBaseParams(lmParams);
   runner.setMoptFactory([](ConstrainedOptBenchmark::Method) {
     auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams();
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.linearSolverType =
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.linearSolverType =
         gtsam::NonlinearOptimizerParams::MULTIFRONTAL_QR;
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.setlambdaUpperBound(1e2);
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.setlambdaUpperBound(1e2);
     return moptParams;
   });
 

examples/example_constraint_manifold/main_cartpole.cpp

@@ -107,10 +107,10 @@ void dynamic_planning(size_t numSteps,
   runner.setOuterLmBaseParams(lmParams);
   runner.setMoptFactory([&](ConstrainedOptBenchmark::Method) {
     auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV(
-        cartpole.getBasisKeyFunc(true));
-    auto retractorParams = moptParams.cc_params->retractor_creator->params();
-    retractorParams->check_feasible = true;
-    retractorParams->lm_params.linearSolverType =
+        cartpole.getBasisKeyFunction(true));
+    auto retractorParams = moptParams.constraintManifoldParams->retractorCreator->params();
+    retractorParams->checkFeasible = true;
+    retractorParams->lmParams.linearSolverType =
         gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY;
     return moptParams;
   });

examples/example_constraint_manifold/main_connected_poses.cpp

@@ -12,7 +12,7 @@
  * @author Yetong Zhang
  */
 
-#include <gtdynamics/cmopt/NonlinearMOptimizer.h>
+#include <gtdynamics/cmopt/NonlinearManifoldOptimizer.h>
 #include <gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h>
 #include <gtsam/constrained/NonlinearEqualityConstraint.h>
 #include <gtsam/geometry/Pose2.h>
@@ -203,7 +203,7 @@ Values get_init_values(
 
 void PrintCMComponentDebug(const EConsOptProblem& problem,
                            const ManifoldOptimizerParameters& mopt_params,
-                           const LevenbergMarquardtParams& lm_params,
+                           const LevenbergMarquardtParams& lmParams,
                            bool debug_enabled) {
   if (!debug_enabled) return;
 
@@ -212,22 +212,22 @@ void PrintCMComponentDebug(const EConsOptProblem& problem,
             << ", costs dim: " << problem.costsDimension()
             << ", values dim: " << problem.valuesDimension() << "\n";
 
-  NonlinearMOptimizer debug_optimizer(mopt_params, lm_params);
-  auto mopt_problem = debug_optimizer.initializeMoptProblem(
+  NonlinearManifoldOptimizer debug_optimizer(mopt_params, lmParams);
+  auto mopt_problem = debug_optimizer.initializeManifoldOptimizationProblem(
       problem.costs(), problem.constraints(), problem.initValues());
 
   mopt_problem.print("[CM DEBUG] ");
 
   std::map<Key, Key> key_component_map;
-  for (const Key& cm_key : mopt_problem.manifold_keys_) {
-    const auto& cm = mopt_problem.values_.at(cm_key).cast<ConstraintManifold>();
+  for (const Key& cm_key : mopt_problem.manifoldKeys) {
+    const auto& cm = mopt_problem.values.at(cm_key).cast<ConstraintManifold>();
     for (const Key& base_key : cm.values().keys()) {
       key_component_map[base_key] = cm_key;
     }
   }
-  for (const Key& cm_key : mopt_problem.fixed_manifolds_.keys()) {
+  for (const Key& cm_key : mopt_problem.fixedManifolds.keys()) {
     const auto& cm =
-        mopt_problem.fixed_manifolds_.at(cm_key).cast<ConstraintManifold>();
+        mopt_problem.fixedManifolds.at(cm_key).cast<ConstraintManifold>();
     for (const Key& base_key : cm.values().keys()) {
       key_component_map[base_key] = cm_key;
     }
@@ -265,8 +265,8 @@ void kinematic_planning(const ConnectedPosesArgs& args) {
 
   auto moptFactory = [](ConstrainedOptBenchmark::Method) {
     auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams();
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.linearSolverType =
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.linearSolverType =
         gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY;
     return moptParams;
   };

examples/example_constraint_manifold/main_quadruped.cpp

@@ -36,7 +36,7 @@
 
 #include "QuadrupedUtils.h"
 #include "gtdynamics/cmopt/ConstraintManifold.h"
-#include "gtdynamics/cmopt/TspaceBasis.h"
+#include "gtdynamics/cmopt/TangentSpaceBasis.h"
 #include "gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h"
 #include "gtdynamics/constrained_optimizer/ConstrainedOptimizer.h"
 #include "gtdynamics/factors/ContactPointFactor.h"
@@ -130,17 +130,17 @@ void TrajectoryOptimization(
   });
   runner.setMoptFactory([&](ConstrainedOptBenchmark::Method) {
     auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV(
-        vision60.getBasisKeyFunc());
-    moptParams.cc_params->retractor_creator->params()->use_basis_keys = true;
-    moptParams.cc_params->retractor_creator->params()->sigma = 1.0;
-    moptParams.cc_params->retractor_creator->params()->apply_base_retraction =
+        vision60.getBasisKeyFunction());
+    moptParams.constraintManifoldParams->retractorCreator->params()->useBasisKeys = true;
+    moptParams.constraintManifoldParams->retractorCreator->params()->sigma = 1.0;
+    moptParams.constraintManifoldParams->retractorCreator->params()->applyBaseRetraction =
         true;
-    moptParams.cc_params->retractor_creator->params()->check_feasible = true;
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.linearSolverType =
+    moptParams.constraintManifoldParams->retractorCreator->params()->checkFeasible = true;
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.linearSolverType =
         gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY;
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.setlambdaUpperBound(1e2);
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.setlambdaUpperBound(1e2);
     return moptParams;
   });
   runner.setResultCallback(

examples/example_constraint_manifold/main_range_constraint.cpp

@@ -215,8 +215,8 @@ void kinematic_planning(const RangeConstraintArgs& args) {
   runner.setOuterLmBaseParams(LevenbergMarquardtParams());
   runner.setMoptFactory([](ConstrainedOptBenchmark::Method) {
     auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams();
-    moptParams.cc_params->retractor_creator->params()
-        ->lm_params.linearSolverType =
+    moptParams.constraintManifoldParams->retractorCreator->params()
+        ->lmParams.linearSolverType =
         gtsam::NonlinearOptimizerParams::SEQUENTIAL_CHOLESKY;
     return moptParams;
   });

examples/scripts/nithya00_constrained_opt_benchmark.cpp

@@ -96,8 +96,8 @@ int main(int argc, char** argv) {
   auto problem = ConstrainedOptProblem::EqConstrainedOptProblem(graph, constraints);
 
   /// Solve the constraint problem with Penalty Method optimizer.
-  auto penalty_params = std::make_shared<gtsam::PenaltyOptimizerParams>();
-  gtsam::PenaltyOptimizer penalty_optimizer(problem, init_values, penalty_params);
+  auto penaltyParams = std::make_shared<gtsam::PenaltyOptimizerParams>();
+  gtsam::PenaltyOptimizer penalty_optimizer(problem, init_values, penaltyParams);
   Values penalty_results = penalty_optimizer.optimize();
 
   /// Solve the constraint problem with Augmented Lagrangian optimizer.

examples/scripts/rss01_estimation.cpp

@@ -47,52 +47,52 @@ void SaveValues(const std::string file_name, const Values &values) {
 /* ************************************************************************* */
 IEConstraintManifold::Params::shared_ptr GetIECMParamsManual() {
   auto iecm_params = std::make_shared<IEConstraintManifold::Params>();
-  iecm_params->retractor_creator =
+  iecm_params->retractorCreator =
       std::make_shared<UniversalIERetractorCreator>(
           std::make_shared<HalfSphereRetractor>(half_sphere));
-  iecm_params->e_basis_creator = std::make_shared<OrthonormalBasisCreator>();
+  iecm_params->equalityBasisCreator = std::make_shared<OrthonormalBasisCreator>();
   return iecm_params;
 }
 
 /* ************************************************************************* */
 IEConstraintManifold::Params::shared_ptr GetECMParamsManual() {
   auto iecm_params = std::make_shared<IEConstraintManifold::Params>();
-  iecm_params->retractor_creator =
+  iecm_params->retractorCreator =
       std::make_shared<UniversalIERetractorCreator>(
           std::make_shared<SphereRetractor>(half_sphere));
-  iecm_params->e_basis_creator = std::make_shared<OrthonormalBasisCreator>();
+  iecm_params->equalityBasisCreator = std::make_shared<OrthonormalBasisCreator>();
   return iecm_params;
 }
 
 // /* *************************************************************************
 // */ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() {
 //   auto iecm_params = std::make_shared<IEConstraintManifold::Params>();
-//   iecm_params->retractor_creator =
+//   iecm_params->retractorCreator =
 //       std::make_shared<UniversalIERetractorCreator>(
 //           std::make_shared<HalfSphereRetractor>(half_sphere));
-//   iecm_params->e_basis_creator = std::make_shared<OrthonormalBasisCreator>();
+//   iecm_params->equalityBasisCreator = std::make_shared<OrthonormalBasisCreator>();
 //   return iecm_params;
 // }
 
 /* ************************************************************************* */
 IEConstraintManifold::Params::shared_ptr GetIECMParamsCR() {
   auto iecm_params = std::make_shared<IEConstraintManifold::Params>();
-  iecm_params->e_basis_creator = std::make_shared<OrthonormalBasisCreator>();
+  iecm_params->equalityBasisCreator = std::make_shared<OrthonormalBasisCreator>();
   auto retractor_params = std::make_shared<IERetractorParams>();
-  retractor_params->lm_params = LevenbergMarquardtParams();
-  retractor_params->lm_params.setlambdaUpperBound(1e10);
-  retractor_params->lm_params.setAbsoluteErrorTol(1e-10);
-  retractor_params->check_feasible = true;
-  retractor_params->ensure_feasible = true;
-  retractor_params->feasible_threshold = 1e-5;
-  retractor_params->use_varying_sigma = true;
-  retractor_params->metric_sigmas = std::make_shared<VectorValues>();
+  retractor_params->lmParams = LevenbergMarquardtParams();
+  retractor_params->lmParams.setlambdaUpperBound(1e10);
+  retractor_params->lmParams.setAbsoluteErrorTol(1e-10);
+  retractor_params->checkFeasible = true;
+  retractor_params->ensureFeasible = true;
+  retractor_params->feasibleThreshold = 1e-5;
+  retractor_params->useVaryingSigma = true;
+  retractor_params->metricSigmas = std::make_shared<VectorValues>();
   auto retract_penalty_params = std::make_shared<PenaltyParameters>();
   retract_penalty_params->initial_mu = 1.0;
   retract_penalty_params->mu_increase_rate = 10.0;
   retract_penalty_params->num_iterations = 3;
-  retractor_params->penalty_params = retract_penalty_params;
-  iecm_params->retractor_creator =
+  retractor_params->penaltyParams = retract_penalty_params;
+  iecm_params->retractorCreator =
       std::make_shared<BarrierRetractorCreator>(retractor_params);
   return iecm_params;
 }
@@ -162,13 +162,13 @@ IEConsOptProblem CreateProblem() {
 }
 
 /* ************************************************************************* */
-std::pair<IEResultSummary, IELMItersDetails> SecondPhaseOptimization(
+std::pair<IEResultSummary, IELMOptimizationDetails> SecondPhaseOptimization(
     const Values values, std::string exp_name) {
   auto problem = CreateProblem();
   problem.values_ = values;
 
   IELMParams ie_params;
-  ie_params.boundary_approach_rate_threshold = 1e10;
+  ie_params.boundaryApproachRateThreshold = 1e10;
   return OptimizeIE_CMCOptLM(problem, ie_params, GetIECMParamsCR(), exp_name,
                              false);
 }
@@ -178,23 +178,23 @@ int main(int argc, char **argv) {
   auto problem = CreateProblem();
 
   IELMParams ie_params;
-  // ie_params.lm_params.minModelFidelity = 0.5;
+  // ie_params.lmParams.minModelFidelity = 0.5;
 
   // soft constraints
   std::cout << "optimize soft...\n";
-  LevenbergMarquardtParams lm_params;
-  auto soft_result = OptimizeIE_Soft(problem, lm_params, 1e2);
+  LevenbergMarquardtParams lmParams;
+  auto soft_result = OptimizeIE_Soft(problem, lmParams, 1e2);
 
   // penalty method
   std::cout << "optimize penalty...\n";
-  auto penalty_params = std::make_shared<PenaltyParameters>();
-  penalty_params->initial_mu = 1;
-  penalty_params->num_iterations = 4;
-  penalty_params->mu_increase_rate = 10;
-  penalty_params->store_iter_details = true;
-  penalty_params->store_lm_details = true;
-  penalty_params->lm_params.setVerbosityLM("SUMMARY");
-  auto penalty_result = OptimizeIE_Penalty(problem, penalty_params);
+  auto penaltyParams = std::make_shared<PenaltyParameters>();
+  penaltyParams->initial_mu = 1;
+  penaltyParams->num_iterations = 4;
+  penaltyParams->mu_increase_rate = 10;
+  penaltyParams->store_iter_details = true;
+  penaltyParams->store_lm_details = true;
+  penaltyParams->lmParams.setVerbosityLM("SUMMARY");
+  auto penalty_result = OptimizeIE_Penalty(problem, penaltyParams);
 
   // augmented Lagrangian
   std::cout << "optimize augmented Lagrangian...\n";
@@ -225,7 +225,7 @@ int main(int argc, char **argv) {
 
   // // // IEGD method
   // // std::cout << "optimize CMOpt(IE-GD)...\n";
-  // // GDParams gd_params;
+  // // GradientDescentParams gd_params;
   // // // gd_params.muLowerBound = 1e-10;
   // // // gd_params.verbose = true;
   // // auto iegd_result = OptimizeIE_CMCOptGD(problem, gd_params,
@@ -238,7 +238,7 @@ int main(int argc, char **argv) {
 
   // // // IELM cost-aware projection
   // // std::cout << "optimize CMOpt(IE-LM-CR)...\n";
-  // // // ie_params.lm_params.setVerbosityLM("SUMMARY");
+  // // // ie_params.lmParams.setVerbosityLM("SUMMARY");
   // // auto ielm_cr_result =
   // //     OptimizeIE_CMCOptLM(problem, ie_params, GetIECMParamsCR(),
   // "CMOpt(IE-LM-CR)");