diff --git a/examples/example_constraint_manifold/CartPoleUtils.cpp b/examples/example_constraint_manifold/CartPoleUtils.cpp index 58ab8db27..825d0d438 100644 --- a/examples/example_constraint_manifold/CartPoleUtils.cpp +++ b/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; } } diff --git a/examples/example_constraint_manifold/CartPoleUtils.h b/examples/example_constraint_manifold/CartPoleUtils.h index 0bb3068f5..e443181bb 100644 --- a/examples/example_constraint_manifold/CartPoleUtils.h +++ b/examples/example_constraint_manifold/CartPoleUtils.h @@ -14,7 +14,7 @@ #pragma once #include -#include +#include #include #include #include @@ -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. diff --git a/examples/example_constraint_manifold/QuadrupedUtils.cpp b/examples/example_constraint_manifold/QuadrupedUtils.cpp index 8294876f7..3a777afde 100644 --- a/examples/example_constraint_manifold/QuadrupedUtils.cpp +++ b/examples/example_constraint_manifold/QuadrupedUtils.cpp @@ -319,9 +319,9 @@ 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); @@ -329,7 +329,7 @@ Values Vision60Robot::getInitValuesStep(const int t, const Pose3 &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) diff --git a/examples/example_constraint_manifold/QuadrupedUtils.h b/examples/example_constraint_manifold/QuadrupedUtils.h index 52643b174..6b3539ac8 100644 --- a/examples/example_constraint_manifold/QuadrupedUtils.h +++ b/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 { diff --git a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp b/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp index cb027a463..87e6b0b71 100644 --- a/examples/example_constraint_manifold/main_arm_kinematic_planning.cpp +++ b/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); diff --git a/examples/example_constraint_manifold/main_cablerobot.cpp b/examples/example_constraint_manifold/main_cablerobot.cpp index f448b0aa9..fd747d122 100644 --- a/examples/example_constraint_manifold/main_cablerobot.cpp +++ b/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; }); diff --git a/examples/example_constraint_manifold/main_cartpole.cpp b/examples/example_constraint_manifold/main_cartpole.cpp index c4d7633a4..b897a5d5e 100644 --- a/examples/example_constraint_manifold/main_cartpole.cpp +++ b/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; }); diff --git a/examples/example_constraint_manifold/main_connected_poses.cpp b/examples/example_constraint_manifold/main_connected_poses.cpp index d59a9741a..5933816f9 100644 --- a/examples/example_constraint_manifold/main_connected_poses.cpp +++ b/examples/example_constraint_manifold/main_connected_poses.cpp @@ -12,7 +12,7 @@ * @author Yetong Zhang */ -#include +#include #include #include #include @@ -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_component_map; - for (const Key& cm_key : mopt_problem.manifold_keys_) { - const auto& cm = mopt_problem.values_.at(cm_key).cast(); + for (const Key& cm_key : mopt_problem.manifoldKeys) { + const auto& cm = mopt_problem.values.at(cm_key).cast(); 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(); + mopt_problem.fixedManifolds.at(cm_key).cast(); 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; }; diff --git a/examples/example_constraint_manifold/main_quadruped.cpp b/examples/example_constraint_manifold/main_quadruped.cpp index 13aa54bff..8e9a264bc 100644 --- a/examples/example_constraint_manifold/main_quadruped.cpp +++ b/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( diff --git a/examples/example_constraint_manifold/main_range_constraint.cpp b/examples/example_constraint_manifold/main_range_constraint.cpp index fba5f2c1d..5a5aa656a 100644 --- a/examples/example_constraint_manifold/main_range_constraint.cpp +++ b/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; }); diff --git a/examples/scripts/nithya00_constrained_opt_benchmark.cpp b/examples/scripts/nithya00_constrained_opt_benchmark.cpp index 237f63052..d58b8f2da 100644 --- a/examples/scripts/nithya00_constrained_opt_benchmark.cpp +++ b/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::PenaltyOptimizer penalty_optimizer(problem, init_values, penalty_params); + auto penaltyParams = std::make_shared(); + gtsam::PenaltyOptimizer penalty_optimizer(problem, init_values, penaltyParams); Values penalty_results = penalty_optimizer.optimize(); /// Solve the constraint problem with Augmented Lagrangian optimizer. diff --git a/examples/scripts/rss01_estimation.cpp b/examples/scripts/rss01_estimation.cpp index 0a0eac530..0fff802da 100644 --- a/examples/scripts/rss01_estimation.cpp +++ b/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(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); return iecm_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetECMParamsManual() { auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); return iecm_params; } // /* ************************************************************************* // */ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { // auto iecm_params = std::make_shared(); -// iecm_params->retractor_creator = +// iecm_params->retractorCreator = // std::make_shared( // std::make_shared(half_sphere)); -// iecm_params->e_basis_creator = std::make_shared(); +// iecm_params->equalityBasisCreator = std::make_shared(); // return iecm_params; // } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsCR() { auto iecm_params = std::make_shared(); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); auto retractor_params = std::make_shared(); - 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(); + 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(); auto retract_penalty_params = std::make_shared(); 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(retractor_params); return iecm_params; } @@ -162,13 +162,13 @@ IEConsOptProblem CreateProblem() { } /* ************************************************************************* */ -std::pair SecondPhaseOptimization( +std::pair 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(); - 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(); + 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)"); diff --git a/examples/scripts/rss02_cp_limits.cpp b/examples/scripts/rss02_cp_limits.cpp index ae7d619fb..4cacc1683 100644 --- a/examples/scripts/rss02_cp_limits.cpp +++ b/examples/scripts/rss02_cp_limits.cpp @@ -29,26 +29,26 @@ double dt = 0.1; /* ************************************************************************* */ IERetractorParams::shared_ptr GetNominalRetractorParams() { auto retractor_params = std::make_shared(); - 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->prior_sigma = 1e2; + 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->priorSigma = 1e2; auto retract_penalty_params = std::make_shared(); retract_penalty_params->initial_mu = 1.0; retract_penalty_params->mu_increase_rate = 10.0; retract_penalty_params->num_iterations = 6; - retractor_params->penalty_params = retract_penalty_params; + retractor_params->penaltyParams = retract_penalty_params; return retractor_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsManual() { auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(cp.getBasisKeyFunc()); - iecm_params->retractor_creator = + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(cp.getBasisKeyFunction()); + iecm_params->retractorCreator = std::make_shared( std::make_shared(cp)); return iecm_params; @@ -57,9 +57,9 @@ IEConstraintManifold::Params::shared_ptr GetIECMParamsManual() { /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared(GetNominalRetractorParams()); - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); return iecm_params; } @@ -67,11 +67,11 @@ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { IEConstraintManifold::Params::shared_ptr GetIECMParamsCR() { auto iecm_params = std::make_shared(); auto retractor_params_cr = GetNominalRetractorParams(); - retractor_params_cr->use_varying_sigma = true; - retractor_params_cr->metric_sigmas = std::make_shared(); - iecm_params->retractor_creator = + retractor_params_cr->useVaryingSigma = true; + retractor_params_cr->metricSigmas = std::make_shared(); + iecm_params->retractorCreator = std::make_shared(retractor_params_cr); - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); return iecm_params; } @@ -132,13 +132,13 @@ std::vector GetCostTerms() { } /* ************************************************************************* */ -std::pair SecondPhaseOptimization( +std::pair 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); } @@ -150,24 +150,24 @@ int main(int argc, char **argv) { // Parameters IELMParams ie_params; - ie_params.boundary_approach_rate_threshold = 1e10; - // ie_params.lm_params.setVerbosityLM("SUMMARY"); + ie_params.boundaryApproachRateThreshold = 1e10; + // ie_params.lmParams.setVerbosityLM("SUMMARY"); // soft constraints std::cout << "optimize soft...\n"; - LevenbergMarquardtParams lm_params; - // lm_params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtParams lmParams; + // lmParams.setVerbosityLM("SUMMARY"); auto soft_result = - OptimizeIE_Soft(problem, lm_params, 1e8, evaluate_projected); + OptimizeIE_Soft(problem, lmParams, 1e8, evaluate_projected); // penalty method std::cout << "optimize penalty...\n"; - auto penalty_params = std::make_shared(); - penalty_params->initial_mu = 1e0; - penalty_params->mu_increase_rate = 4; - penalty_params->num_iterations = 20; + auto penaltyParams = std::make_shared(); + penaltyParams->initial_mu = 1e0; + penaltyParams->mu_increase_rate = 4; + penaltyParams->num_iterations = 20; auto penalty_result = - OptimizeIE_Penalty(problem, penalty_params, evaluate_projected); + OptimizeIE_Penalty(problem, penaltyParams, evaluate_projected); // augmented Lagrangian std::cout << "optimize augmented Lagrangian...\n"; @@ -209,10 +209,10 @@ int main(int argc, char **argv) { // // IEGD method // std::cout << "optimize CMOpt(IE-GD)...\n"; - // GDParams gd_params; + // GradientDescentParams gd_params; // gd_params.verbose = true; // gd_params.muLowerBound = 1e-10; - // gd_params.init_lambda = 1e-5; + // gd_params.initialLambda = 1e-5; // auto iegd_result = OptimizeIE_CMCOptGD(problem, gd_params, // GetIECMParamsSP()); diff --git a/examples/scripts/rss03_cp_friction.cpp b/examples/scripts/rss03_cp_friction.cpp index b99acfc93..b7badf798 100644 --- a/examples/scripts/rss03_cp_friction.cpp +++ b/examples/scripts/rss03_cp_friction.cpp @@ -40,36 +40,36 @@ double dt = 0.05; /* ************************************************************************* */ IERetractorParams::shared_ptr GetNominalRetractorParams() { auto retractor_params = std::make_shared(); - 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->prior_sigma = 1e2; + 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->priorSigma = 1e2; auto retract_penalty_params = std::make_shared(); 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; + retractor_params->penaltyParams = retract_penalty_params; return retractor_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsManual() { auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(cp)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); return iecm_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared(GetNominalRetractorParams()); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); return iecm_params; } @@ -77,11 +77,11 @@ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { IEConstraintManifold::Params::shared_ptr GetIECMParamsCR() { auto iecm_params = std::make_shared(); auto retractor_params_cr = GetNominalRetractorParams(); - retractor_params_cr->use_varying_sigma = true; - retractor_params_cr->metric_sigmas = std::make_shared(); - iecm_params->retractor_creator = + retractor_params_cr->useVaryingSigma = true; + retractor_params_cr->metricSigmas = std::make_shared(); + iecm_params->retractorCreator = std::make_shared(retractor_params_cr); - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); return iecm_params; } @@ -189,13 +189,13 @@ std::vector GetCostTerms() { } /* ************************************************************************* */ -std::pair SecondPhaseOptimization( +std::pair 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); } @@ -205,24 +205,24 @@ int main(int argc, char **argv) { // Parameters IELMParams ie_params; - ie_params.boundary_approach_rate_threshold = 1e10; - // ie_params.lm_params.setVerbosityLM("SUMMARY"); + ie_params.boundaryApproachRateThreshold = 1e10; + // ie_params.lmParams.setVerbosityLM("SUMMARY"); // soft constraints std::cout << "optimize soft...\n"; - LevenbergMarquardtParams lm_params; - // lm_params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtParams lmParams; + // lmParams.setVerbosityLM("SUMMARY"); auto soft_result = - OptimizeIE_Soft(problem, lm_params, 1e2, eval_proj_cost_progress); + OptimizeIE_Soft(problem, lmParams, 1e2, eval_proj_cost_progress); // penalty method std::cout << "optimize penalty...\n"; - auto penalty_params = std::make_shared(); - penalty_params->initial_mu = 1e2; - penalty_params->mu_increase_rate = 4; - penalty_params->num_iterations = 8; + auto penaltyParams = std::make_shared(); + penaltyParams->initial_mu = 1e2; + penaltyParams->mu_increase_rate = 4; + penaltyParams->num_iterations = 8; auto penalty_result = - OptimizeIE_Penalty(problem, penalty_params, eval_proj_cost_progress); + OptimizeIE_Penalty(problem, penaltyParams, eval_proj_cost_progress); // augmented Lagrangian std::cout << "optimize augmented Lagrangian...\n"; @@ -263,7 +263,7 @@ int main(int argc, char **argv) { // // IEGD method // std::cout << "optimize CMOpt(IE-GD)...\n"; - // GDParams gd_params; + // GradientDescentParams gd_params; // auto iegd_result = OptimizeIE_CMCOptGD(problem, gd_params, // GetIECMParamsSP()); diff --git a/examples/scripts/rss04_quad_jump.cpp b/examples/scripts/rss04_quad_jump.cpp index 6c8906285..8b30a514c 100644 --- a/examples/scripts/rss04_quad_jump.cpp +++ b/examples/scripts/rss04_quad_jump.cpp @@ -150,84 +150,84 @@ std::vector GetCostTerms(const IEVision60RobotMultiPhase& /* ************************************************************************* */ IERetractorParams::shared_ptr GetNominalRetractorParams() { auto retractor_params = std::make_shared(); - 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->prior_sigma = 1e-1; - retractor_params->use_varying_sigma = false; - - auto penalty_params = std::make_shared(); - // penalty_params->lm_params = params_->lm_params; - penalty_params->initial_mu = 10.0; - penalty_params->mu_increase_rate = 10.0; - penalty_params->num_iterations = 2; - auto lm_params1 = retractor_params->lm_params; - auto lm_params2 = retractor_params->lm_params; + 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->priorSigma = 1e-1; + retractor_params->useVaryingSigma = false; + + auto penaltyParams = std::make_shared(); + // penaltyParams->lmParams = params_->lmParams; + penaltyParams->initial_mu = 10.0; + penaltyParams->mu_increase_rate = 10.0; + penaltyParams->num_iterations = 2; + auto lm_params1 = retractor_params->lmParams; + auto lm_params2 = retractor_params->lmParams; // lm_params1.setMaxIterations(20); // lm_params1.setVerbosityLM("SUMMARY"); - penalty_params->iters_lm_params = std::vector(); - for (size_t i = 0; i < penalty_params->num_iterations - 1; i++) { - penalty_params->iters_lm_params.push_back(lm_params1); + penaltyParams->iters_lm_params = std::vector(); + for (size_t i = 0; i < penaltyParams->num_iterations - 1; i++) { + penaltyParams->iters_lm_params.push_back(lm_params1); } - penalty_params->iters_lm_params.push_back(lm_params2); - retractor_params->penalty_params = penalty_params; + penaltyParams->iters_lm_params.push_back(lm_params2); + retractor_params->penaltyParams = penaltyParams; return retractor_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsSP() { auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; - iecm_params->retractor_creator = + iecm_params->equalityBasisBuildFromScratch = false; + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase, GetNominalRetractorParams(), false); - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); return iecm_params; } /* ************************************************************************* */ IEConstraintManifold::Params::shared_ptr GetIECMParamsCR() { auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisBuildFromScratch = false; auto retractor_params_cr = GetNominalRetractorParams(); - retractor_params_cr->use_varying_sigma = true; - retractor_params_cr->metric_sigmas = std::make_shared(); - // retractor_params_cr->scale_varying_sigma = true; - iecm_params->retractor_creator = + retractor_params_cr->useVaryingSigma = true; + retractor_params_cr->metricSigmas = std::make_shared(); + // retractor_params_cr->scaleVaryingSigma = true; + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase, retractor_params_cr, false); - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); return iecm_params; } /* ************************************************************************* */ IELMParams NominalIELMParams() { IELMParams ie_params; - ie_params.boundary_approach_rate_threshold = 1e10; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - // ie_params.lm_params.setMaxIterations(50); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - ie_params.lm_params.setlambdaUpperBound(1e10); - // ie_params.lm_params.lambdaInitial = 1e-6; - ie_params.iqp_max_iters = 100; - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.boundaryApproachRateThreshold = 1e10; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + // ie_params.lmParams.setMaxIterations(50); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + ie_params.lmParams.setlambdaUpperBound(1e10); + // ie_params.lmParams.lambdaInitial = 1e-6; + ie_params.iqpMaxIterations = 100; + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; return ie_params; } /* ************************************************************************* */ -std::pair +std::pair SecondPhaseOptimization(const Values values, std::string exp_name) { auto problem = std::get<0>(CreateProblem()); problem.values_ = values; IELMParams ie_params = NominalIELMParams(); - ie_params.lm_params.setLinearSolverType("MULTIFRONTAL_QR"); - // ie_params.boundary_approach_rate_threshold = 10; + ie_params.lmParams.setLinearSolverType("MULTIFRONTAL_QR"); + // ie_params.boundaryApproachRateThreshold = 10; return OptimizeIE_CMCOptLM(problem, ie_params, GetIECMParamsCR(), exp_name, false); } @@ -248,31 +248,31 @@ void TrajectoryOptimization() { PenaltyItersDetails penalty_iters_details; AugmentedLagrangianItersDetails augl_iters_details; SQPItersDetails sqp_iters_details; - IELMItersDetails cmopt_iters_details; - IELMItersDetails cmcopt_iters_details; + IELMOptimizationDetails cmopt_iters_details; + IELMOptimizationDetails cmcopt_iters_details; IPItersDetails ip_iters_details; // soft constraints if (run_soft) { std::cout << "optimize soft...\n"; - LevenbergMarquardtParams lm_params; - lm_params.setlambdaUpperBound(1e10); - lm_params.setVerbosityLM("SUMMARY"); - std::tie(soft_summary, soft_iters_details) = OptimizeIE_Soft(problem, lm_params, 1e4, evaluate_projected); + LevenbergMarquardtParams lmParams; + lmParams.setlambdaUpperBound(1e10); + lmParams.setVerbosityLM("SUMMARY"); + std::tie(soft_summary, soft_iters_details) = OptimizeIE_Soft(problem, lmParams, 1e4, evaluate_projected); } // penalty method if (run_penalty) { std::cout << "optimize penalty...\n"; - auto penalty_params = std::make_shared(); - penalty_params->initial_mu = 1e-4; - penalty_params->mu_increase_rate = 4; - penalty_params->num_iterations = 16; - penalty_params->lm_params.setVerbosityLM("SUMMARY"); - penalty_params->lm_params.setlambdaUpperBound(1e10); - penalty_params->lm_params.setMaxIterations(30); - std::tie(penalty_summary, penalty_iters_details) = OptimizeIE_Penalty(problem, penalty_params, evaluate_projected); + auto penaltyParams = std::make_shared(); + penaltyParams->initial_mu = 1e-4; + penaltyParams->mu_increase_rate = 4; + penaltyParams->num_iterations = 16; + penaltyParams->lmParams.setVerbosityLM("SUMMARY"); + penaltyParams->lmParams.setlambdaUpperBound(1e10); + penaltyParams->lmParams.setMaxIterations(30); + std::tie(penalty_summary, penalty_iters_details) = OptimizeIE_Penalty(problem, penaltyParams, evaluate_projected); } if (run_augl) { @@ -285,9 +285,9 @@ void TrajectoryOptimization() { // al_params->max_dual_step_size_e = 1e0; // al_params->max_dual_step_size_i = 1e0; - al_params->lm_params.setVerbosityLM("SUMMARY"); - al_params->lm_params.setlambdaUpperBound(1e10); - al_params->lm_params.setMaxIterations(30); + al_params->lmParams.setVerbosityLM("SUMMARY"); + al_params->lmParams.setlambdaUpperBound(1e10); + al_params->lmParams.setMaxIterations(30); if (log_progress) { al_params->store_iter_details = true; @@ -330,9 +330,9 @@ void TrajectoryOptimization() { // // IEGD method // std::cout << "optimize CMOpt(IE-GD)...\n"; - // GDParams gd_params; + // GradientDescentParams gd_params; // gd_params.verbose = true; - // gd_params.init_lambda = 1e-5; + // gd_params.initialLambda = 1e-5; // gd_params.muLowerBound = 1e-15; // gd_params.maxIterations = 20; // auto iegd_result = OptimizeIE_CMCOptGD(problem, gd_params, GetIECMParamsSP(), diff --git a/examples/scripts/yetong01_dome_estimation.cpp b/examples/scripts/yetong01_dome_estimation.cpp index de65ccb35..21f81244c 100644 --- a/examples/scripts/yetong01_dome_estimation.cpp +++ b/examples/scripts/yetong01_dome_estimation.cpp @@ -240,28 +240,28 @@ int main(int argc, char **argv) { problem.initValues().print(); auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); - LevenbergMarquardtParams lm_params; + LevenbergMarquardtParams lmParams; std::cout << "run soft...\n"; - auto soft_result = OptimizeIE_Soft(problem, lm_params, 100); + auto soft_result = OptimizeIE_Soft(problem, lmParams, 100); auto barrier_params = std::make_shared(); barrier_params->num_iterations = 15; std::cout << "run barrier...\n"; auto barrier_result = OptimizeIE_Penalty(problem, barrier_params); - GDParams gd_params; + GradientDescentParams gd_params; std::cout << "run gd...\n"; auto gd_result = OptimizeIE_CMCOptGD(problem, gd_params, iecm_params); IELMParams ie_params; std::cout << "run lm...\n"; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.show_active_constraints = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.showActiveConstraints = true; auto lm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); soft_result.first.printLatex(std::cout); @@ -291,7 +291,7 @@ int main(int argc, char **argv) { << lm_error << "\n"; // for (const auto &iter_details : details) { - // IEOptimizer::PrintIterDetails(iter_details, num_steps, false, + // IEOptimizer::printIterationDetails(iter_details, num_steps, false, // IEHalfSphere::PrintValues, // IEHalfSphere::PrintDelta); diff --git a/examples/scripts/yetong02_half_sphere_optimization.cpp b/examples/scripts/yetong02_half_sphere_optimization.cpp index e138e3867..bebe06b88 100644 --- a/examples/scripts/yetong02_half_sphere_optimization.cpp +++ b/examples/scripts/yetong02_half_sphere_optimization.cpp @@ -39,15 +39,15 @@ int main(int argc, char **argv) { initial_values.insert(point_key, init_point); auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); IEConsOptProblem problem(graph, e_constraints, i_constraints, initial_values); - LevenbergMarquardtParams lm_params; - auto soft_result = OptimizeIE_Soft(problem, lm_params, 100); + LevenbergMarquardtParams lmParams; + auto soft_result = OptimizeIE_Soft(problem, lmParams, 100); auto barrier_params = std::make_shared(); barrier_params->num_iterations = 15; @@ -60,12 +60,12 @@ int main(int argc, char **argv) { sqp_params->lm_params.setlambdaUpperBound(1e10); auto sqp_result = OptimizeIE_SQP(problem, sqp_params); - GDParams gd_params; + GradientDescentParams gd_params; auto gd_result = OptimizeIE_CMCOptGD(problem, gd_params, iecm_params); IELMParams ie_params; - ie_params.lm_params.minModelFidelity = 0.5; - // lm_params.setVerbosityLM("SUMMARY"); + ie_params.lmParams.minModelFidelity = 0.5; + // lmParams.setVerbosityLM("SUMMARY"); auto lm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); soft_result.first.printLatex(std::cout); @@ -87,7 +87,7 @@ int main(int argc, char **argv) { // const auto &details = lm_optimizer.details(); // for (const auto &iter_details : details) { - // IEOptimizer::PrintIterDetails(iter_details, 0, false, + // IEOptimizer::printIterationDetails(iter_details, 0, false, // IEHalfSphere::PrintValues, // IEHalfSphere::PrintDelta); // } @@ -95,7 +95,7 @@ int main(int argc, char **argv) { // // Run GD optimization // { - // GDParams params; + // GradientDescentParams params; // params.maxIterations = 30; // IEGDOptimizer gd_optimizer(params); // auto gd_result = gd_optimizer.optimize(graph, e_constraints, @@ -104,7 +104,7 @@ int main(int argc, char **argv) { // const auto &details = gd_optimizer.details(); // for (const auto &iter_details : details) { - // IEOptimizer::PrintIterDetails(iter_details, 0, false, + // IEOptimizer::printIterationDetails(iter_details, 0, false, // IEHalfSphere::PrintValues, // IEHalfSphere::PrintDelta); // } diff --git a/examples/scripts/yetong03_half_sphere_trajectory.cpp b/examples/scripts/yetong03_half_sphere_trajectory.cpp index b6e91873f..9a3dcee0a 100644 --- a/examples/scripts/yetong03_half_sphere_trajectory.cpp +++ b/examples/scripts/yetong03_half_sphere_trajectory.cpp @@ -1,4 +1,4 @@ -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" #include #include #include @@ -103,25 +103,25 @@ int main(int argc, char **argv) { } auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); IEConsOptProblem problem(graph, e_constraints, i_constraints, initial_values); - LevenbergMarquardtParams lm_params; - auto soft_result = OptimizeIE_Soft(problem, lm_params, 100); + LevenbergMarquardtParams lmParams; + auto soft_result = OptimizeIE_Soft(problem, lmParams, 100); auto barrier_params = std::make_shared(); barrier_params->num_iterations = 15; auto barrier_result = OptimizeIE_Penalty(problem, barrier_params); - GDParams gd_params; + GradientDescentParams gd_params; auto gd_result = OptimizeIE_CMCOptGD(problem, gd_params, iecm_params); IELMParams ie_params; - ie_params.lm_params.minModelFidelity = 0.5; + ie_params.lmParams.minModelFidelity = 0.5; auto lm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); soft_result.first.printLatex(std::cout); @@ -141,7 +141,7 @@ int main(int argc, char **argv) { // const auto &details = lm_optimizer.details(); // for (const auto &iter_details : details) { - // IEOptimizer::PrintIterDetails(iter_details, num_steps, false, + // IEOptimizer::printIterationDetails(iter_details, num_steps, false, // IEHalfSphere::PrintValues, // IEHalfSphere::PrintDelta); // } @@ -149,7 +149,7 @@ int main(int argc, char **argv) { // // Run GD optimization // { - // GDParams params; + // GradientDescentParams params; // params.maxIterations = 30; // IEGDOptimizer gd_optimizer(params); // auto gd_result = gd_optimizer.optimize(graph, e_constraints, @@ -158,7 +158,7 @@ int main(int argc, char **argv) { // const auto &details = gd_optimizer.details(); // for (const auto &iter_details : details) { - // IEOptimizer::PrintIterDetails(iter_details, num_steps, false, + // IEOptimizer::printIterationDetails(iter_details, num_steps, false, // IEHalfSphere::PrintValues, // IEHalfSphere::PrintDelta); // } diff --git a/examples/scripts/yetong04_cart_pole_friction.cpp b/examples/scripts/yetong04_cart_pole_friction.cpp index d4bd50123..ce2102f71 100644 --- a/examples/scripts/yetong04_cart_pole_friction.cpp +++ b/examples/scripts/yetong04_cart_pole_friction.cpp @@ -143,25 +143,25 @@ int main(int argc, char **argv) { IECartPoleWithFriction::PrintValues(initial_values, num_steps); auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(cp)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); IEConsOptProblem problem(graph, e_constraints, i_constraints, initial_values); - LevenbergMarquardtParams lm_params; - auto soft_result = OptimizeIE_Soft(problem, lm_params, 100); + LevenbergMarquardtParams lmParams; + auto soft_result = OptimizeIE_Soft(problem, lmParams, 100); auto barrier_params = std::make_shared(); barrier_params->num_iterations = 15; auto barrier_result = OptimizeIE_Penalty(problem, barrier_params); - GDParams gd_params; + GradientDescentParams gd_params; auto gd_result = OptimizeIE_CMCOptGD(problem, gd_params, iecm_params); IELMParams ie_params; - ie_params.lm_params = lm_params; + ie_params.lmParams = lmParams; auto lm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); soft_result.first.printLatex(std::cout); @@ -202,7 +202,7 @@ int main(int argc, char **argv) { // const auto &details = lm_optimizer.details(); for (const auto &iter_details : lm_result.second) { - IEOptimizer::PrintIterDetails(iter_details, num_steps, false, + IEOptimizer::printIterationDetails(iter_details, num_steps, false, IECartPoleWithFriction::PrintValues, IECartPoleWithFriction::PrintDelta); } @@ -211,9 +211,9 @@ int main(int argc, char **argv) { // // Run GD optimization // { - // GDParams params; + // GradientDescentParams params; // params.maxIterations = 5; - // params.init_lambda = 100; + // params.initialLambda = 100; // IEGDOptimizer gd_optimizer(params); // auto gd_result = gd_optimizer.optimize(graph, e_constraints, // i_constraints, @@ -223,7 +223,7 @@ int main(int argc, char **argv) { // const auto &details = gd_optimizer.details(); // for (const auto &iter_details : details) { - // PrintIterDetails(iter_details, num_steps, true); + // printIterationDetails(iter_details, num_steps, true); // } // IECartPoleWithFriction::PrintValues(gd_result, num_steps); // } diff --git a/examples/scripts/yetong05_cart_pole_limits.cpp b/examples/scripts/yetong05_cart_pole_limits.cpp index e7cbf314e..c34ffb7fd 100644 --- a/examples/scripts/yetong05_cart_pole_limits.cpp +++ b/examples/scripts/yetong05_cart_pole_limits.cpp @@ -1,4 +1,4 @@ -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" #include "gtdynamics/utils/DynamicsSymbol.h" #include #include @@ -68,12 +68,12 @@ int main(int argc, char **argv) { // Parameters auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = std::make_shared(cp.getBasisKeyFunc()); - iecm_params->retractor_creator = + iecm_params->equalityManifoldParams->basisCreator = std::make_shared(cp.getBasisKeyFunction()); + iecm_params->retractorCreator = std::make_shared( std::make_shared(cp)); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; + iecm_params->equalityBasisBuildFromScratch = false; // optimize IELM IELMParams ie_params; @@ -81,7 +81,7 @@ int main(int argc, char **argv) { Values result_values = lm_result.second.back().state.baseValues(); for (const auto &iter_details : lm_result.second) { - IEOptimizer::PrintIterDetails( + IEOptimizer::printIterationDetails( iter_details, num_steps, false, IECartPoleWithLimits::PrintValues, IECartPoleWithLimits::PrintDelta, GTDKeyFormatter); } diff --git a/examples/scripts/yetong06_trajectory_optimization_ground.cpp b/examples/scripts/yetong06_trajectory_optimization_ground.cpp index 8e6b502b6..6d66578db 100644 --- a/examples/scripts/yetong06_trajectory_optimization_ground.cpp +++ b/examples/scripts/yetong06_trajectory_optimization_ground.cpp @@ -37,7 +37,7 @@ #include "gtdynamics/factors/ContactPointFactor.h" #include "gtdynamics/cmcopt/IERetractor.h" #include "gtdynamics/cmopt/ConstraintManifold.h" -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" #include "gtdynamics/utils/DynamicsSymbol.h" #include "gtdynamics/utils/GraphUtils.h" #include "gtdynamics/utils/values.h" @@ -137,30 +137,30 @@ void TrajectoryOptimization() { // Parameters auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(vision60.getBasisKeyFunc()); + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(vision60.getBasisKeyFunction()); auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - // retractor_params->lm_params.setVerbosityLM("SUMMARY"); - // retractor_params->lm_params.minModelFidelity = 0.5; - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + // retractor_params->lmParams.setVerbosityLM("SUMMARY"); + // retractor_params->lmParams.minModelFidelity = 0.5; + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + iecm_params->retractorCreator = std::make_shared( vision60, retractor_params, true); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; + iecm_params->equalityBasisBuildFromScratch = false; IELMParams ie_params; - ie_params.lm_params.setMaxIterations(10); + ie_params.lmParams.setMaxIterations(10); // optimize IELM auto lm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); Values result_values = lm_result.second.back().state.baseValues(); for (const auto &iter_details : lm_result.second) { - IEOptimizer::PrintIterDetails( + IEOptimizer::printIterationDetails( iter_details, num_steps, false, IEVision60Robot::PrintValues, IEVision60Robot::PrintDelta, GTDKeyFormatter); } diff --git a/examples/scripts/yetong07_trajectory_optimization_quadvert.cpp b/examples/scripts/yetong07_trajectory_optimization_quadvert.cpp index cb88f9f50..37d66d010 100644 --- a/examples/scripts/yetong07_trajectory_optimization_quadvert.cpp +++ b/examples/scripts/yetong07_trajectory_optimization_quadvert.cpp @@ -116,36 +116,36 @@ void TrajectoryOptimization() { /* <========================== Optimize IELM ============================> */ /* <=====================================================================> */ auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisBuildFromScratch = false; /* <=========== retractor ===========> */ auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - // retractor_params->use_varying_sigma = true; - // retractor_params->scale_varying_sigma = true; - // retractor_params->metric_sigmas = std::make_shared(); - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + // retractor_params->useVaryingSigma = true; + // retractor_params->scaleVaryingSigma = true; + // retractor_params->metricSigmas = std::make_shared(); + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase, retractor_params, false); - // iecm_params->retractor_creator = + // iecm_params->retractorCreator = // std::make_shared( // vision60_multi_phase, retractor_params, false); /* <=========== t-space basis ===========> */ - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); /* <=========== IELM params ===========> */ IELMParams ie_params; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.lm_params.setMaxIterations(100); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - ie_params.lm_params.setlambdaInitial(1e-2); - ie_params.lm_params.setlambdaUpperBound(1e10); - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.lmParams.setMaxIterations(100); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + ie_params.lmParams.setlambdaInitial(1e-2); + ie_params.lmParams.setlambdaUpperBound(1e10); + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; /* <=========== optimize ===========> */ auto ielm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); diff --git a/examples/scripts/yetong08_retractor_benchmark_quadvert.cpp b/examples/scripts/yetong08_retractor_benchmark_quadvert.cpp index f2c125e7a..1810d7f6c 100644 --- a/examples/scripts/yetong08_retractor_benchmark_quadvert.cpp +++ b/examples/scripts/yetong08_retractor_benchmark_quadvert.cpp @@ -230,59 +230,59 @@ IECM_PARAMS_LIST ConstructExpIECMParams(const EXP_SETTING_LIST &exp_settings, IECM_PARAMS_LIST iecm_params_list; for (const auto &[basis_type, retractor_type, metric_type] : exp_settings) { auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisBuildFromScratch = false; /// Tspace Basis if (basis_type == "Orthonormal") { - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); } else if (basis_type == "EliminationT") { - iecm_params->e_basis_creator = - std::make_shared( + iecm_params->equalityBasisCreator = + std::make_shared( vision60_multi_phase_T); } else if (basis_type == "Eliminationa") { - iecm_params->e_basis_creator = - std::make_shared( + iecm_params->equalityBasisCreator = + std::make_shared( vision60_multi_phase_a); } /// Retractor params auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 1; + retractor_params->lmParams = LevenbergMarquardtParams(); + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 1; if ((metric_type == "cost") || (metric_type == "costscale")) { - retractor_params->use_varying_sigma = true; - retractor_params->metric_sigmas = std::make_shared(); + retractor_params->useVaryingSigma = true; + retractor_params->metricSigmas = std::make_shared(); } if (metric_type == "costscale") { - retractor_params->scale_varying_sigma = true; + retractor_params->scaleVaryingSigma = true; } /// Retractor if (retractor_type == "Barrier") { if (metric_type == "basisT") { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase_T, retractor_params, true); } else if (metric_type == "basisa") { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase_a, retractor_params, true); } else { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared(retractor_params); } } else if (retractor_type == "Hierarchical") { if (metric_type == "basisT") { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase_T, retractor_params, true); } else if (metric_type == "basisa") { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase_a, retractor_params, true); } else { - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase_T, retractor_params, false); } @@ -303,13 +303,13 @@ void RetractorBenchMark() { /* <=========== IELM params ===========> */ IELMParams ie_params; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.lm_params.setMaxIterations(100); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - ie_params.lm_params.setlambdaInitial(1e-2); - ie_params.lm_params.setlambdaUpperBound(1e10); - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.lmParams.setMaxIterations(100); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + ie_params.lmParams.setlambdaInitial(1e-2); + ie_params.lmParams.setlambdaUpperBound(1e10); + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; /* <=========== experimental settings ===========> */ EXP_SETTING_LIST exp_settings; @@ -351,13 +351,13 @@ void MultiStageOptimization() { /* <=========== IELM params ===========> */ IELMParams ie_params; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.lm_params.setMaxIterations(100); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - ie_params.lm_params.setlambdaInitial(1e-5); - ie_params.lm_params.setlambdaUpperBound(1e10); - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.lmParams.setMaxIterations(100); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + ie_params.lmParams.setlambdaInitial(1e-5); + ie_params.lmParams.setlambdaUpperBound(1e10); + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; /* <=========== experimental settings ===========> */ EXP_SETTING_LIST exp_settings1; diff --git a/examples/scripts/yetong09_so2_metric_proj_retraction.cpp b/examples/scripts/yetong09_so2_metric_proj_retraction.cpp index 07bf37b57..64c140973 100644 --- a/examples/scripts/yetong09_so2_metric_proj_retraction.cpp +++ b/examples/scripts/yetong09_so2_metric_proj_retraction.cpp @@ -24,7 +24,7 @@ #include #include "gtdynamics/cmcopt/IERetractor.h" -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" using namespace gtsam; using namespace gtdynamics; @@ -73,15 +73,15 @@ void SaveState(const IELMState &state, int iter_id, void SaveTrial(const IELMTrial &trial, int iter_id, int trial_id, const std::string &folder_path) { Values new_values; - for (const auto &[key, manifold] : trial.nonlinear_update.new_manifolds) { + for (const auto &[key, manifold] : trial.nonlinearUpdate.newManifolds) { new_values.insert(manifold.values()); } double new_x1 = new_values.atDouble(x1_key); double new_x2 = new_values.atDouble(x2_key); - double delta_x1 = trial.linear_update.tangent_vector.at(x1_key)(0); - double delta_x2 = trial.linear_update.tangent_vector.at(x2_key)(0); + double delta_x1 = trial.linearUpdate.tangentVector.at(x1_key)(0); + double delta_x2 = trial.linearUpdate.tangentVector.at(x2_key)(0); std::string file_path = folder_path + "trial_" + std::to_string(iter_id) + "_" + std::to_string(trial_id) + ".txt"; @@ -89,13 +89,13 @@ void SaveTrial(const IELMTrial &trial, int iter_id, int trial_id, file.open(file_path); file << delta_x1 << " " << delta_x2 << "\n"; file << new_x1 << " " << new_x2 << "\n"; - file << trial.linear_update.lambda << "\n"; - file << trial.nonlinear_update.new_error << "\n"; - file << trial.step_is_successful << "\n"; + file << trial.linearUpdate.lambda << "\n"; + file << trial.nonlinearUpdate.newError << "\n"; + file << trial.stepIsSuccessful << "\n"; file.close(); } -void SaveDetails(const IELMItersDetails &iters_details, +void SaveDetails(const IELMOptimizationDetails &iters_details, const std::string &folder_path) { std::filesystem::create_directory(folder_path); // int total_trials = 0; @@ -143,19 +143,19 @@ void OptimizeSO2() { init_values.insert(x2_key, init_point.y()); IELMParams ielm_params; - ielm_params.lm_params.setVerbosityLM("SUMMARY"); - // ielm_params.lm_params.setMaxIterations(1); + ielm_params.lmParams.setVerbosityLM("SUMMARY"); + // ielm_params.lmParams.setMaxIterations(1); auto iecm_params = std::make_shared(); - iecm_params->e_basis_creator = std::make_shared(); - LevenbergMarquardtParams lm_params; - // lm_params.setVerbosityLM("SUMMARY"); - lm_params.minModelFidelity = 0.5; + iecm_params->equalityBasisCreator = std::make_shared(); + LevenbergMarquardtParams lmParams; + // lmParams.setVerbosityLM("SUMMARY"); + lmParams.minModelFidelity = 0.5; //// Optimization with fixed sigmas { - auto barrier_params = std::make_shared(lm_params, 0.1); - barrier_params->init_values_as_x = false; - iecm_params->retractor_creator = + auto barrier_params = std::make_shared(lmParams, 0.1); + barrier_params->initValuesAsX = false; + iecm_params->retractorCreator = std::make_shared(barrier_params); IELMOptimizer optimizer(ielm_params, iecm_params); auto result = @@ -172,9 +172,9 @@ void OptimizeSO2() { //// Optimization with varying sigmas { auto barrier_params = - IERetractorParams::VarySigmas(lm_params); - barrier_params->init_values_as_x = false; - iecm_params->retractor_creator = + IERetractorParams::createVaryingSigmas(lmParams); + barrier_params->initValuesAsX = false; + iecm_params->retractorCreator = std::make_shared(barrier_params); IELMOptimizer optimizer(ielm_params, iecm_params); auto result = diff --git a/examples/scripts/yetong10_trajectory_optimization_quadforward.cpp b/examples/scripts/yetong10_trajectory_optimization_quadforward.cpp index 36c5c5c47..a7d6df7ab 100644 --- a/examples/scripts/yetong10_trajectory_optimization_quadforward.cpp +++ b/examples/scripts/yetong10_trajectory_optimization_quadforward.cpp @@ -127,28 +127,28 @@ void TrajectoryOptimization() { /* <========================== Optimize IELM ============================> */ /* <=====================================================================> */ auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisBuildFromScratch = false; /* <=========== retractor ===========> */ auto retractor_params = std::make_shared(); - 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->prior_sigma = 1e-1; - retractor_params->use_varying_sigma = true; - // retractor_params->scale_varying_sigma = true; - retractor_params->metric_sigmas = std::make_shared(); + 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->priorSigma = 1e-1; + retractor_params->useVaryingSigma = true; + // retractor_params->scaleVaryingSigma = true; + retractor_params->metricSigmas = std::make_shared(); auto barrier_params = std::make_shared(); - // barrier_params->lm_params = params_->lm_params; + // barrier_params->lmParams = params_->lmParams; barrier_params->initial_mu = 10.0; barrier_params->mu_increase_rate = 10.0; barrier_params->num_iterations = 2; - auto lm_params1 = retractor_params->lm_params; - auto lm_params2 = retractor_params->lm_params; + auto lm_params1 = retractor_params->lmParams; + auto lm_params2 = retractor_params->lmParams; // lm_params1.setMaxIterations(20); // lm_params1.setVerbosityLM("SUMMARY"); barrier_params->iters_lm_params = std::vector(); @@ -156,28 +156,28 @@ void TrajectoryOptimization() { barrier_params->iters_lm_params.push_back(lm_params1); } barrier_params->iters_lm_params.push_back(lm_params2); - retractor_params->penalty_params = barrier_params; + retractor_params->penaltyParams = barrier_params; - // iecm_params->retractor_creator = + // iecm_params->retractorCreator = // std::make_shared( // vision60_multi_phase, retractor_params, false); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase, retractor_params, false); /* <=========== t-space basis ===========> */ - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); /* <=========== IELM params ===========> */ IELMParams ie_params; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.lm_params.setMaxIterations(200); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - // ie_params.lm_params.setlambdaInitial(1e-2); - ie_params.lm_params.setlambdaUpperBound(1e10); - ie_params.iqp_max_iters = 100; - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.lmParams.setMaxIterations(200); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + // ie_params.lmParams.setlambdaInitial(1e-2); + ie_params.lmParams.setlambdaUpperBound(1e10); + ie_params.iqpMaxIterations = 100; + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; /* <=========== optimize ===========> */ auto ielm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); diff --git a/examples/scripts/yetong11_jump_land.cpp b/examples/scripts/yetong11_jump_land.cpp index 029a93682..1d43f995c 100644 --- a/examples/scripts/yetong11_jump_land.cpp +++ b/examples/scripts/yetong11_jump_land.cpp @@ -125,28 +125,28 @@ void TrajectoryOptimization() { /* <========================== Optimize IELM ============================> */ /* <=====================================================================> */ auto iecm_params = std::make_shared(); - iecm_params->e_basis_build_from_scratch = false; + iecm_params->equalityBasisBuildFromScratch = false; /* <=========== retractor ===========> */ auto retractor_params = std::make_shared(); - 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->prior_sigma = 1e-1; - retractor_params->use_varying_sigma = true; - // retractor_params->scale_varying_sigma = true; - retractor_params->metric_sigmas = std::make_shared(); + 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->priorSigma = 1e-1; + retractor_params->useVaryingSigma = true; + // retractor_params->scaleVaryingSigma = true; + retractor_params->metricSigmas = std::make_shared(); auto barrier_params = std::make_shared(); - // barrier_params->lm_params = params_->lm_params; + // barrier_params->lmParams = params_->lmParams; barrier_params->initial_mu = 10.0; barrier_params->mu_increase_rate = 10.0; barrier_params->num_iterations = 2; - auto lm_params1 = retractor_params->lm_params; - auto lm_params2 = retractor_params->lm_params; + auto lm_params1 = retractor_params->lmParams; + auto lm_params2 = retractor_params->lmParams; // lm_params1.setMaxIterations(20); // lm_params1.setVerbosityLM("SUMMARY"); barrier_params->iters_lm_params = std::vector(); @@ -154,27 +154,27 @@ void TrajectoryOptimization() { barrier_params->iters_lm_params.push_back(lm_params1); } barrier_params->iters_lm_params.push_back(lm_params2); - retractor_params->penalty_params = barrier_params; + retractor_params->penaltyParams = barrier_params; - // iecm_params->retractor_creator = + // iecm_params->retractorCreator = // std::make_shared( // vision60_multi_phase, retractor_params, false); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( vision60_multi_phase, retractor_params, false); /* <=========== t-space basis ===========> */ - iecm_params->e_basis_creator = OrthonormalBasisCreator::CreateSparse(); + iecm_params->equalityBasisCreator = OrthonormalBasisCreator::createSparse(); /* <=========== IELM params ===========> */ IELMParams ie_params; - ie_params.lm_params.setVerbosityLM("SUMMARY"); - ie_params.lm_params.setMaxIterations(200); - ie_params.lm_params.setLinearSolverType("SEQUENTIAL_QR"); - ie_params.lm_params.setlambdaUpperBound(1e10); - ie_params.iqp_max_iters = 100; - ie_params.show_active_constraints = true; - ie_params.active_constraints_group_as_categories = true; + ie_params.lmParams.setVerbosityLM("SUMMARY"); + ie_params.lmParams.setMaxIterations(200); + ie_params.lmParams.setLinearSolverType("SEQUENTIAL_QR"); + ie_params.lmParams.setlambdaUpperBound(1e10); + ie_params.iqpMaxIterations = 100; + ie_params.showActiveConstraints = true; + ie_params.activeConstraintsGroupedAsCategories = true; /* <=========== optimize ===========> */ auto ielm_result = OptimizeIE_CMCOptLM(problem, ie_params, iecm_params); diff --git a/gtdynamics/cmcopt/IEConstraintManifold.cpp b/gtdynamics/cmcopt/IEConstraintManifold.cpp index f11ed39f7..9176862f3 100644 --- a/gtdynamics/cmcopt/IEConstraintManifold.cpp +++ b/gtdynamics/cmcopt/IEConstraintManifold.cpp @@ -92,8 +92,8 @@ IEConstraintManifold::projectTangentCone(const Vector &xi) const { /* ************************************************************************* */ std::pair IEConstraintManifold::projectTangentCone( - const VectorValues &tangent_vector) const { - Vector xi = e_basis_->computeXi(tangent_vector); + const VectorValues &tangentVector) const { + Vector xi = e_basis_->computeXi(tangentVector); auto result = projectTangentCone(xi); VectorValues projected_tangent_vector = e_basis_->computeTangentVector(result.second); @@ -104,22 +104,22 @@ std::pair IEConstraintManifold::projectTangentCone( IEConstraintManifold IEConstraintManifold::retract(const Vector &xi, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { - auto tangent_vector = e_basis_->computeTangentVector(xi); - return retract(tangent_vector, blocking_indices, retract_info); + IERetractionInfo *retract_info) const { + auto tangentVector = e_basis_->computeTangentVector(xi); + return retract(tangentVector, blocking_indices, retract_info); } /* ************************************************************************* */ IEConstraintManifold IEConstraintManifold::retract(const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { return retractor_->retract(this, delta, blocking_indices, retract_info); } /* ************************************************************************* */ IndexSet IEConstraintManifold::blockingIndices( - const VectorValues &tangent_vector) const { + const VectorValues &tangentVector) const { IndexSet blocking_indices; for (const auto &idx : active_indices_) { @@ -127,8 +127,8 @@ IndexSet IEConstraintManifold::blockingIndices( auto linear_factor = LinearizedIConstraint(i_constraint, values_); Vector error = Vector::Zero(linear_factor->rows()); for (auto it = linear_factor->begin(); it != linear_factor->end(); ++it) { - if (tangent_vector.exists(*it)) { - error += linear_factor->getA(it) * tangent_vector.at(*it); + if (tangentVector.exists(*it)) { + error += linear_factor->getA(it) * tangentVector.at(*it); } } // A negative directional derivative means this active face would be @@ -144,13 +144,13 @@ IndexSet IEConstraintManifold::blockingIndices( /* ************************************************************************* */ IEConstraintManifold IEConstraintManifold::moveToBoundary(const IndexSet &active_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { return retractor_->moveToBoundary(this, active_indices, retract_info); } /* ************************************************************************* */ ConstraintManifold IEConstraintManifold::eConstraintManifold() const { - return ConstraintManifold(e_constraints_, values_, params_->ecm_params, false, + return ConstraintManifold(e_constraints_, values_, params_->equalityManifoldParams, false, e_basis_); } @@ -172,13 +172,13 @@ ConstraintManifold IEConstraintManifold::eConstraintManifold( active_constraints->add(new_active_constraints); auto new_basis = e_basis_->createWithAdditionalConstraints( - new_active_constraints, values_, params_->e_basis_build_from_scratch); - return ConstraintManifold(active_constraints, values_, params_->ecm_params, + new_active_constraints, values_, params_->equalityBasisBuildFromScratch); + return ConstraintManifold(active_constraints, values_, params_->equalityManifoldParams, false, new_basis); } /* ************************************************************************* */ -IndexSet IEConstraintManifold::IdentifyActiveConstraints( +IndexSet IEConstraintManifold::identifyActiveConstraints( const NonlinearInequalityConstraints &i_constraints, const Values &values, const std::optional &active_indices) { if (active_indices) { @@ -202,10 +202,10 @@ IndexSet IEConstraintManifold::IdentifyActiveConstraints( } /* ************************************************************************* */ -TangentCone::shared_ptr IEConstraintManifold::ConstructTangentCone( +TangentCone::shared_ptr IEConstraintManifold::constructTangentCone( const NonlinearInequalityConstraints &i_constraints, const Values &values, const IndexSet &active_indices, - const TspaceBasis::shared_ptr &t_basis) { + const TangentSpaceBasis::shared_ptr &t_basis) { LinearInequalityConstraints constraints; for (const auto &constraint_idx : active_indices) { @@ -232,7 +232,7 @@ TangentCone::shared_ptr IEConstraintManifold::ConstructTangentCone( /* ************************************************************************* */ LinearIConstraintMap -IEConstraintManifold::linearActiveManIConstraints( +IEConstraintManifold::linearActiveManifoldInequalityConstraints( const Key manifold_key) const { LinearIConstraintMap active_constraints; @@ -260,7 +260,7 @@ IEConstraintManifold::linearActiveManIConstraints( /* ************************************************************************* */ LinearIConstraintMap -IEConstraintManifold::linearActiveBaseIConstraints() const { +IEConstraintManifold::linearActiveBaseInequalityConstraints() const { LinearIConstraintMap active_constraints; for (const auto &constraint_idx : active_indices_) { diff --git a/gtdynamics/cmcopt/IEConstraintManifold.h b/gtdynamics/cmcopt/IEConstraintManifold.h index fc19ffd9f..b7286db46 100644 --- a/gtdynamics/cmcopt/IEConstraintManifold.h +++ b/gtdynamics/cmcopt/IEConstraintManifold.h @@ -28,12 +28,12 @@ class IEConstraintManifold { struct Params { using shared_ptr = std::shared_ptr; - ConstraintManifold::Params::shared_ptr ecm_params = + ConstraintManifold::Params::shared_ptr equalityManifoldParams = std::make_shared(); // IERetractType ie_retract_type = IERetractType::Barrier; - IERetractorCreator::shared_ptr retractor_creator; - TspaceBasisCreator::shared_ptr e_basis_creator; - bool e_basis_build_from_scratch = true; + IERetractorCreator::shared_ptr retractorCreator; + TangentSpaceBasisCreator::shared_ptr equalityBasisCreator; + bool equalityBasisBuildFromScratch = true; /** Default constructor. */ Params() = default; }; @@ -47,7 +47,7 @@ class IEConstraintManifold { size_t embedding_dim_; size_t e_constraints_dim_; size_t dim_; - TspaceBasis::shared_ptr e_basis_; + TangentSpaceBasis::shared_ptr e_basis_; TangentCone::shared_ptr i_cone_; IERetractor::shared_ptr retractor_; @@ -62,13 +62,13 @@ class IEConstraintManifold { : params_(params), e_constraints_(e_constraints), i_constraints_(i_constraints), values_(values), active_indices_( - IdentifyActiveConstraints(*i_constraints, values, active_indices)), + identifyActiveConstraints(*i_constraints, values, active_indices)), embedding_dim_(values.dim()), e_constraints_dim_(e_constraints_->dim()), dim_(embedding_dim_ - e_constraints_dim_), - e_basis_(params->e_basis_creator->create(e_constraints_, values_)), - i_cone_(ConstructTangentCone(*i_constraints, values, active_indices_, + e_basis_(params->equalityBasisCreator->create(e_constraints_, values_)), + i_cone_(constructTangentCone(*i_constraints, values, active_indices_, e_basis_)), - retractor_(params->retractor_creator->create(*this)) {} + retractor_(params->retractorCreator->create(*this)) {} /** constructor from other manifold but update the values. */ IEConstraintManifold(const IEConstraintManifold &other, const Values &values, @@ -76,11 +76,11 @@ class IEConstraintManifold { : params_(other.params_), e_constraints_(other.e_constraints_), i_constraints_(other.i_constraints_), values_(values), active_indices_( - IdentifyActiveConstraints(*i_constraints_, values, active_indices)), + identifyActiveConstraints(*i_constraints_, values, active_indices)), embedding_dim_(other.embedding_dim_), e_constraints_dim_(other.e_constraints_dim_), dim_(other.dim_), e_basis_(other.e_basis_->createWithNewValues(values_)), - i_cone_(ConstructTangentCone(*i_constraints_, values_, active_indices_, + i_cone_(constructTangentCone(*i_constraints_, values_, active_indices_, e_basis_)), retractor_(other.retractor_) {} @@ -97,7 +97,7 @@ class IEConstraintManifold { /// Active set A(x): tight inequality indices, as in thesis Eq. (4.3). const IndexSet &activeIndices() const { return active_indices_; } - const TspaceBasis::shared_ptr eBasis() const { return e_basis_; } + const TangentSpaceBasis::shared_ptr eBasis() const { return e_basis_; } /// Tangent cone C_x induced by thesis Eqs. (4.14)-(4.16). const TangentCone::shared_ptr &tangentCone() const { return i_cone_; } @@ -121,28 +121,28 @@ class IEConstraintManifold { /// Same projection as above, but starting from ambient tangent vectors. virtual std::pair - projectTangentCone(const VectorValues &tangent_vector) const; + projectTangentCone(const VectorValues &tangentVector) const; /// Retract using the on-corner rule from thesis Eq. (4.46) when blocking /// inequalities are supplied. virtual IEConstraintManifold retract(const Vector &xi, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const; + IERetractionInfo *retract_info = nullptr) const; /// Retract an ambient tangent update back to a feasible IE manifold point. virtual IEConstraintManifold retract(const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const; + IERetractionInfo *retract_info = nullptr) const; /// Return active inequalities that block the step, matching thesis Eq. (4.45). - IndexSet blockingIndices(const VectorValues &tangent_vector) const; + IndexSet blockingIndices(const VectorValues &tangentVector) const; /// Force a mode change by moving directly onto the requested boundary set. IEConstraintManifold moveToBoundary(const IndexSet &active_indices, - IERetractInfo *retract_info = nullptr) const; + IERetractionInfo *retract_info = nullptr) const; /// Construct an e-constraint manifold that only includes equalily /// constraints. @@ -152,35 +152,35 @@ class IEConstraintManifold { /// equality constraints. Used for EQP-based methods., ConstraintManifold eConstraintManifold(const IndexSet &active_indices) const; - double evalIViolation() const { + double evaluateInequalityViolation() const { return i_constraints_->violationNorm(values_); } - double evalEViolation() const { + double evaluateEqualityViolation() const { return e_constraints_->violationNorm(values_); } /// Linearize active inequalities as Dg_A(x) B_x, the K matrix in thesis /// Eq. (4.15), for manifold-space QPs. LinearIConstraintMap - linearActiveManIConstraints(const Key manifold_key) const; + linearActiveManifoldInequalityConstraints(const Key manifold_key) const; /// Linearize the active inequalities directly in ambient variable space. - LinearIConstraintMap linearActiveBaseIConstraints() const; + LinearIConstraintMap linearActiveBaseInequalityConstraints() const; protected: /// Identify A(x), either from explicit mode information or from which /// inequalities are tight at the current values. - static IndexSet IdentifyActiveConstraints( + static IndexSet identifyActiveConstraints( const NonlinearInequalityConstraints &i_constraints, const Values &values, const std::optional &active_indices = {}); /// Construct the tangent cone in thesis Eqs. (4.14)-(4.16) by forming the /// active Jacobian Dg_A(x) and projecting it through the equality basis B_x. static TangentCone::shared_ptr - ConstructTangentCone(const NonlinearInequalityConstraints &i_constraints, + constructTangentCone(const NonlinearInequalityConstraints &i_constraints, const Values &values, const IndexSet &active_indices, - const TspaceBasis::shared_ptr &t_basis); + const TangentSpaceBasis::shared_ptr &t_basis); }; class IEManifoldValues : public std::map { diff --git a/gtdynamics/cmcopt/IEConstraintManifoldUtils.cpp b/gtdynamics/cmcopt/IEConstraintManifoldUtils.cpp index c2db83471..8e7e93026 100644 --- a/gtdynamics/cmcopt/IEConstraintManifoldUtils.cpp +++ b/gtdynamics/cmcopt/IEConstraintManifoldUtils.cpp @@ -24,16 +24,16 @@ KeyVector IEManifoldValues::keys() const { /* ************************************************************************* */ IEManifoldValues IEManifoldValues::moveToBoundaries( const IndexSetMap &approach_indices_map) const { - IEManifoldValues new_manifolds; + IEManifoldValues newManifolds; for (const auto &[key, manifold] : *this) { if (approach_indices_map.find(key) == approach_indices_map.end()) { - new_manifolds.insert({key, manifold}); + newManifolds.insert({key, manifold}); } else { - new_manifolds.insert( + newManifolds.insert( {key, manifold.moveToBoundary(approach_indices_map.at(key))}); } } - return new_manifolds; + return newManifolds; } } // namespace gtdynamics diff --git a/gtdynamics/cmcopt/IEGDOptimizer.cpp b/gtdynamics/cmcopt/IEGDOptimizer.cpp index bcc3ee386..aec14f2f5 100644 --- a/gtdynamics/cmcopt/IEGDOptimizer.cpp +++ b/gtdynamics/cmcopt/IEGDOptimizer.cpp @@ -21,45 +21,45 @@ using namespace gtsam; /* ************************************************************************* */ -IEGDState IEGDState::FromLastIteration(const IEGDIterDetails &iter_details, +IEGDState IEGDState::fromLastIteration(const IEGDIterationDetails &iter_details, const NonlinearFactorGraph &graph, - const GDParams ¶ms) { + const GradientDescentParams ¶ms) { double lambda; const auto &last_trial = iter_details.trials.back(); const auto &prev_state = iter_details.state; IEGDState state; - if (last_trial.step_is_successful) { - state = IEGDState(last_trial.new_manifolds, graph); + if (last_trial.stepIsSuccessful) { + state = IEGDState(last_trial.newManifolds, graph); } else { // pick the trials with smallest error state = IEGDState(iter_details.state.manifolds, graph); for (const auto &trial : iter_details.trials) { - if (trial.new_error < state.error) { - state = IEGDState(trial.new_manifolds, graph); + if (trial.newError < state.error) { + state = IEGDState(trial.newManifolds, graph); } } } last_trial.setNextLambda(state.lambda, params); state.iterations = prev_state.iterations + 1; - state.totalNumberInnerIterations = - prev_state.totalNumberInnerIterations + iter_details.trials.size(); + state.totalInnerIterations = + prev_state.totalInnerIterations + iter_details.trials.size(); return state; } /* ************************************************************************* */ void IEGDState::computeDescentDirection(const NonlinearFactorGraph &graph) { std::map keymap_var2manifold = - IEOptimizer::Var2ManifoldKeyMap(manifolds); + IEOptimizer::varToManifoldKeyMap(manifolds); NonlinearFactorGraph manifold_graph = - ManifoldOptimizer::ManifoldGraph(graph, keymap_var2manifold); + ManifoldOptimizer::manifoldGraph(graph, keymap_var2manifold); - auto linear_graph = manifold_graph.linearize(e_manifolds); + auto linear_graph = manifold_graph.linearize(equalityManifolds); gradient = linear_graph->gradientAtZero(); - descent_dir = -1 * gradient; - std::tie(blocking_indices_map, projected_descent_dir) = - IEOptimizer::ProjectTangentCone(manifolds, descent_dir); + descentDirection = -1 * gradient; + std::tie(blockingIndicesMap, projectedDescentDirection) = + IEOptimizer::projectTangentCone(manifolds, descentDirection); } /* ************************************************************************* */ @@ -70,11 +70,11 @@ Values IEGDState::baseValues() const { } /* ************************************************************************* */ -void IEGDTrial::setNextLambda(double &new_mu, const GDParams ¶ms) const { - if (forced_indices_map.size() > 0) { +void IEGDTrial::setNextLambda(double &new_mu, const GradientDescentParams ¶ms) const { + if (forcedIndicesMap.size() > 0) { new_mu = lambda; } else { - if (step_is_successful) { + if (stepIsSuccessful) { new_mu = lambda / params.beta; } else { new_mu = lambda * params.beta; @@ -84,22 +84,22 @@ void IEGDTrial::setNextLambda(double &new_mu, const GDParams ¶ms) const { /* ************************************************************************* */ void IEGDTrial::computeDelta(const IEGDState &state) { - delta = lambda * state.projected_descent_dir; - tangent_vector = IEOptimizer::ComputeTangentVector(state.manifolds, delta); - linear_cost_change = state.descent_dir.dot(delta); + delta = lambda * state.projectedDescentDirection; + tangentVector = IEOptimizer::computeTangentVector(state.manifolds, delta); + linearCostChange = state.descentDirection.dot(delta); } /* ************************************************************************* */ void IEGDTrial::computeNewManifolds(const IEGDState &state) { - new_manifolds = IEManifoldValues(); + newManifolds = IEManifoldValues(); for (const auto &[key, manifold] : state.manifolds) { - auto manifold_tv = SubValues(tangent_vector, manifold.values().keys()); - if (state.blocking_indices_map.find(key) != - state.blocking_indices_map.end()) { - const auto &blocking_indices = state.blocking_indices_map.at(key); - new_manifolds.emplace(key, manifold.retract(manifold_tv, blocking_indices)); + auto manifold_tv = SubValues(tangentVector, manifold.values().keys()); + if (state.blockingIndicesMap.find(key) != + state.blockingIndicesMap.end()) { + const auto &blocking_indices = state.blockingIndicesMap.at(key); + newManifolds.emplace(key, manifold.retract(manifold_tv, blocking_indices)); } else { - new_manifolds.emplace(key, manifold.retract(manifold_tv)); + newManifolds.emplace(key, manifold.retract(manifold_tv)); } } } @@ -107,9 +107,9 @@ void IEGDTrial::computeNewManifolds(const IEGDState &state) { /* ************************************************************************* */ void IEGDTrial::computeNewError(const NonlinearFactorGraph &graph, const IEGDState &state) { - new_error = graph.error(new_manifolds.baseValues()); - nonlinear_cost_change = state.error - new_error; - model_fidelity = nonlinear_cost_change / linear_cost_change; + newError = graph.error(newManifolds.baseValues()); + nonlinearCostChange = state.error - newError; + modelFidelity = nonlinearCostChange / linearCostChange; } /* ************************************************************************* */ @@ -125,18 +125,18 @@ void PrintIEGDTrialTitle() { /* ************************************************************************* */ void IEGDTrial::print(const IEGDState &state) const { cout << setw(10) << state.iterations << "|"; - cout << setw(12) << setprecision(4) << new_error << "|"; - cout << setw(12) << setprecision(4) << nonlinear_cost_change << "|"; - cout << setw(12) << setprecision(4) << linear_cost_change << "|"; + cout << setw(12) << setprecision(4) << newError << "|"; + cout << setw(12) << setprecision(4) << nonlinearCostChange << "|"; + cout << setw(12) << setprecision(4) << linearCostChange << "|"; cout << setw(10) << setprecision(2) << lambda << "|"; - cout << setw(10) << setprecision(2) << tangent_vector.norm() << endl; + cout << setw(10) << setprecision(2) << tangentVector.norm() << endl; } // /* ************************************************************************* // */ IEManifoldValues IEGDOptimizer::lineSearch( // const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, // const VectorValues &proj_dir, const VectorValues -// &descent_dir, VectorValues &delta) const { +// &descentDirection, VectorValues &delta) const { // double alpha = 0.2; // double beta = 0.5; // double t = 1; @@ -149,12 +149,12 @@ void IEGDTrial::print(const IEGDState &state) const { // delta = t * proj_dir; // // delta.print("delta"); -// IEManifoldValues new_manifolds = RetractManifolds(manifolds, delta); -// Values new_values = CollectManifoldValues(new_manifolds); +// IEManifoldValues newManifolds = retractManifolds(manifolds, delta); +// Values new_values = CollectManifoldValues(newManifolds); // double new_eval = graph.error(new_values); // double nonlinear_error_decrease = eval - new_eval; -// double linear_error_decrease = descent_dir.dot(delta); +// double linear_error_decrease = descentDirection.dot(delta); // std::cout << "t: " << t << "\tnonlinear: " << nonlinear_error_decrease // << "\tlinear: " << linear_error_decrease << std::endl; @@ -163,7 +163,7 @@ void IEGDTrial::print(const IEGDState &state) const { // } // if (nonlinear_error_decrease > linear_error_decrease * alpha) { -// return new_manifolds; +// return newManifolds; // } // t *= beta; // } @@ -172,9 +172,9 @@ void IEGDTrial::print(const IEGDState &state) const { // } /* ************************************************************************* */ -IEGDIterDetails IEGDOptimizer::iterate(const NonlinearFactorGraph &graph, +IEGDIterationDetails IEGDOptimizer::iterate(const NonlinearFactorGraph &graph, const IEGDState &state) const { - IEGDIterDetails iter_details(state); + IEGDIterationDetails iter_details(state); if (checkModeChange(graph, iter_details)) { return iter_details; } @@ -193,15 +193,15 @@ IEGDIterDetails IEGDOptimizer::iterate(const NonlinearFactorGraph &graph, iter_details.trials.emplace_back(trial); // Check condition 1. - if (trial.step_is_successful) { + if (trial.stepIsSuccessful) { break; } // Check condition 2. double abs_change_tol = std::max(params_.absoluteErrorTol, params_.relativeErrorTol * state.error); - if (trial.linear_cost_change < abs_change_tol) { - if (trial.nonlinear_cost_change < abs_change_tol) { + if (trial.linearCostChange < abs_change_tol) { + if (trial.nonlinearCostChange < abs_change_tol) { break; } } @@ -228,9 +228,9 @@ void IEGDOptimizer::tryLambda(const NonlinearFactorGraph &graph, trial.computeNewError(graph, state); // Check if successful. - trial.step_is_successful = false; - if (trial.nonlinear_cost_change > trial.linear_cost_change * params_.alpha) { - trial.step_is_successful = true; + trial.stepIsSuccessful = false; + if (trial.nonlinearCostChange > trial.linearCostChange * params_.alpha) { + trial.stepIsSuccessful = true; } if (params_.verbose) { @@ -241,17 +241,17 @@ void IEGDOptimizer::tryLambda(const NonlinearFactorGraph &graph, /* ************************************************************************* */ Values IEGDOptimizer::optimizeManifolds( const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, - const Values &unconstrained_values) const { + const Values &unconstrainedValues) const { // construct equivalent factors on e-manifolds // std::cout << "in optimize\n"; - std::map keymap_var2manifold = Var2ManifoldKeyMap(manifolds); + std::map keymap_var2manifold = varToManifoldKeyMap(manifolds); NonlinearFactorGraph manifold_graph = - ManifoldOptimizer::ManifoldGraph(graph, keymap_var2manifold); + ManifoldOptimizer::manifoldGraph(graph, keymap_var2manifold); // std::cout << "graph done\n"; // Construct initial state IEGDState state(manifolds, graph, 0); - state.lambda = params_.init_lambda; + state.lambda = params_.initialLambda; if (params_.verbose) { std::cout << "Initial error: " << state.error << "\n"; @@ -268,8 +268,8 @@ Values IEGDOptimizer::optimizeManifolds( IEGDState prev_state; do { prev_state = state; - IEGDIterDetails iter_details = iterate(graph, state); - state = IEGDState::FromLastIteration(iter_details, graph, params_); + IEGDIterationDetails iter_details = iterate(graph, state); + state = IEGDState::fromLastIteration(iter_details, graph, params_); details_->push_back(iter_details); } while (state.iterations < params_.maxIterations && !checkConvergence(prev_state, state) && @@ -286,7 +286,7 @@ bool IEGDOptimizer::checkMuWithinLimits(const double &lambda) const { /* ************************************************************************* */ bool IEGDOptimizer::checkModeChange( const NonlinearFactorGraph &graph, - IEGDIterDetails ¤t_iter_details) const { + IEGDIterationDetails ¤t_iter_details) const { if (details_->size() == 0) { return false; } @@ -294,7 +294,7 @@ bool IEGDOptimizer::checkModeChange( // Find the first state in the sequence of states that have the same mode. int n = details_->size(); int first_i = n - 1; - while (first_i >= 0 && IsSameMode(current_iter_details.state.manifolds, + while (first_i >= 0 && isSameMode(current_iter_details.state.manifolds, details_->at(first_i).state.manifolds)) { first_i--; } @@ -313,7 +313,7 @@ bool IEGDOptimizer::checkModeChange( size_t trial_idx = details_->back().trials.size() - 1; bool failed_trial_exists = false; while (true) { - if (!details_->at(iter_idx).trials.at(trial_idx).step_is_successful) { + if (!details_->at(iter_idx).trials.at(trial_idx).stepIsSuccessful) { failed_trial_exists = true; break; } @@ -333,18 +333,18 @@ bool IEGDOptimizer::checkModeChange( return false; } - IndexSetMap change_indices_map = IdentifyChangeIndices( + IndexSetMap change_indices_map = identifyChangeIndices( current_iter_details.state.manifolds, - details_->at(iter_idx).trials.at(trial_idx).new_manifolds); + details_->at(iter_idx).trials.at(trial_idx).newManifolds); // Condition2(2): most recent failed trial results in other mode if (change_indices_map.size() == 0) { return false; } - auto approach_indices_map = IdentifyApproachingIndices( + auto approach_indices_map = identifyApproachingIndices( init_iter_dertails.state.manifolds, current_iter_details.state.manifolds, - change_indices_map, params_.boundary_approach_rate_threshold); + change_indices_map, params_.boundaryApproachRateThreshold); // Condition3: approaching boundary with decent rate if (approach_indices_map.size() == 0) { @@ -354,11 +354,11 @@ bool IEGDOptimizer::checkModeChange( // Enforce approaching indices; IEGDTrial trial; trial.lambda = current_iter_details.state.lambda; - trial.forced_indices_map = approach_indices_map; - trial.new_manifolds = current_iter_details.state.manifolds.moveToBoundaries( + trial.forcedIndicesMap = approach_indices_map; + trial.newManifolds = current_iter_details.state.manifolds.moveToBoundaries( approach_indices_map); - trial.new_error = graph.error(trial.new_manifolds.baseValues()); - trial.step_is_successful = true; + trial.newError = graph.error(trial.newManifolds.baseValues()); + trial.stepIsSuccessful = true; current_iter_details.trials.emplace_back(trial); return true; } @@ -370,7 +370,7 @@ bool IEGDOptimizer::checkConvergence(const IEGDState &prev_state, return true; // check if mode changes - if (!IsSameMode(prev_state.manifolds, state.manifolds)) { + if (!isSameMode(prev_state.manifolds, state.manifolds)) { return false; } diff --git a/gtdynamics/cmcopt/IEGDOptimizer.h b/gtdynamics/cmcopt/IEGDOptimizer.h index d1d51619f..6e3fcaa26 100644 --- a/gtdynamics/cmcopt/IEGDOptimizer.h +++ b/gtdynamics/cmcopt/IEGDOptimizer.h @@ -28,18 +28,18 @@ using namespace gtsam; struct IEGDState; struct IEGDTrial; -struct IEGDIterDetails; +struct IEGDIterationDetails; -struct GDParams { +struct GradientDescentParams { double alpha = 0.2; double beta = 0.5; - double init_lambda = 1; + double initialLambda = 1; double absoluteErrorTol = 1e-9; double relativeErrorTol = 1e-9; double errorTol = 1e-9; size_t maxIterations = 100; double muLowerBound = 1e-5; - double boundary_approach_rate_threshold = 5; + double boundaryApproachRateThreshold = 5; bool verbose = false; }; @@ -47,14 +47,14 @@ struct IEGDState { public: IEManifoldValues manifolds; double error = 0; - Values e_manifolds; + Values equalityManifolds; VectorValues gradient; - VectorValues descent_dir; - VectorValues projected_descent_dir; - IndexSetMap blocking_indices_map; + VectorValues descentDirection; + VectorValues projectedDescentDirection; + IndexSetMap blockingIndicesMap; double lambda = 0; // step length size_t iterations = 0; - size_t totalNumberInnerIterations = 0; + size_t totalInnerIterations = 0; IEGDState() {} @@ -63,15 +63,15 @@ struct IEGDState { const NonlinearFactorGraph &graph, size_t _iterations = 0) : manifolds(_manifolds), error(graph.error(manifolds.baseValues())), - e_manifolds(IEOptimizer::EManifolds(_manifolds)), + equalityManifolds(IEOptimizer::equalityManifolds(_manifolds)), iterations(_iterations) { computeDescentDirection(graph); } /// Initialize the new state from the result of previous iteration. - static IEGDState FromLastIteration(const IEGDIterDetails &iter_details, + static IEGDState fromLastIteration(const IEGDIterationDetails &iter_details, const NonlinearFactorGraph &graph, - const GDParams ¶ms); + const GradientDescentParams ¶ms); /// compute gradient, then project neg grad into tangent cone. void computeDescentDirection(const NonlinearFactorGraph &graph); @@ -81,22 +81,22 @@ struct IEGDState { /** Trial for GD inner iteration with certain lambda setting. */ struct IEGDTrial { - IEManifoldValues new_manifolds; - IndexSetMap forced_indices_map; + IEManifoldValues newManifolds; + IndexSetMap forcedIndicesMap; VectorValues delta; - VectorValues tangent_vector; - IndexSetMap blocking_indices_map; + VectorValues tangentVector; + IndexSetMap blockingIndicesMap; - double linear_cost_change; - double new_error; - double nonlinear_cost_change; - double model_fidelity; + double linearCostChange; + double newError; + double nonlinearCostChange; + double modelFidelity; - bool step_is_successful; + bool stepIsSuccessful; double lambda; /// Update lambda for the next trial/state. - void setNextLambda(double &new_mu, const GDParams ¶ms) const; + void setNextLambda(double &new_mu, const GradientDescentParams ¶ms) const; /// Compute the linear update delta and tangent vector. void computeDelta(const IEGDState &state); @@ -112,44 +112,44 @@ struct IEGDTrial { void print(const IEGDState &state) const; }; -struct IEGDIterDetails { +struct IEGDIterationDetails { IEGDState state; std::vector trials; - IEGDIterDetails(const IEGDState &_state) : state(_state), trials() {} + IEGDIterationDetails(const IEGDState &_state) : state(_state), trials() {} }; -typedef std::vector IEGDItersDetails; +typedef std::vector IEGDOptimizationDetails; class IEGDOptimizer : public IEOptimizer { protected: - const GDParams params_; ///< LM parameters - std::shared_ptr details_; + const GradientDescentParams params_; ///< LM parameters + std::shared_ptr details_; public: /** Constructor */ - IEGDOptimizer(const GDParams ¶ms = GDParams(), + IEGDOptimizer(const GradientDescentParams ¶ms = GradientDescentParams(), const IEConstraintManifold::Params::shared_ptr &iecm_params = std::make_shared()) : IEOptimizer(iecm_params), params_(params), - details_(std::make_shared()) {} + details_(std::make_shared()) {} - const IEGDItersDetails &details() const { return *details_; } + const IEGDOptimizationDetails &details() const { return *details_; } // IEManifoldValues lineSearch(const NonlinearFactorGraph &graph, // const IEManifoldValues &manifolds, // const VectorValues &proj_dir, - // const VectorValues &descent_dir, + // const VectorValues &descentDirection, // VectorValues &delta) const; virtual Values optimizeManifolds(const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, - const Values &unconstrained_values) const override; + const Values &unconstrainedValues) const override; /** Perform one iterate, may need to make several trials. */ - IEGDIterDetails iterate(const NonlinearFactorGraph &graph, + IEGDIterationDetails iterate(const NonlinearFactorGraph &graph, const IEGDState &state) const; /** Inner loop, perform a trial with specified lambda parameters, changes @@ -168,7 +168,7 @@ class IEGDOptimizer : public IEOptimizer { * with large enough rate. */ bool checkModeChange(const NonlinearFactorGraph &graph, - IEGDIterDetails ¤t_iter_details) const; + IEGDIterationDetails ¤t_iter_details) const; /** Convergence check including * 1) mode change diff --git a/gtdynamics/cmcopt/IELMOptimizer.cpp b/gtdynamics/cmcopt/IELMOptimizer.cpp index b85448e68..a06280ed8 100644 --- a/gtdynamics/cmcopt/IELMOptimizer.cpp +++ b/gtdynamics/cmcopt/IELMOptimizer.cpp @@ -30,39 +30,39 @@ using namespace gtsam; /* ************************************************************************* */ Values IELMOptimizer::optimizeManifolds( const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, - const Values &unconstrained_values) const { + const Values &unconstrainedValues) const { - const LevenbergMarquardtParams &lm_params = ielm_params_.lm_params; + const LevenbergMarquardtParams &lmParams = ielm_params_.lmParams; // Construct manifold graph. - std::map keymap_var2manifold = Var2ManifoldKeyMap(manifolds); + std::map keymap_var2manifold = varToManifoldKeyMap(manifolds); NonlinearFactorGraph manifold_graph = - ManifoldOptimizer::ManifoldGraph(graph, keymap_var2manifold); + ManifoldOptimizer::manifoldGraph(graph, keymap_var2manifold); // Construct initial state - IELMState state(manifolds, unconstrained_values, graph, manifold_graph, - lm_params.lambdaInitial, lm_params.lambdaFactor, 0); + IELMState state(manifolds, unconstrainedValues, graph, manifold_graph, + lmParams.lambdaInitial, lmParams.lambdaFactor, 0); // check if we're already close enough - if (state.error <= lm_params.errorTol) { + if (state.error <= lmParams.errorTol) { details_->emplace_back(state); return state.baseValues(); } // Iterative loop - if (lm_params.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + if (lmParams.verbosityLM == LevenbergMarquardtParams::SUMMARY) { std::cout << "Initial error: " << state.error << "\n"; - PrintIELMTrialTitle(); + printIELMTrialTitle(); } IELMState prev_state; do { prev_state = state; - IELMIterDetails iter_details = iterate(graph, state); - state = IELMState::FromLastIteration(iter_details, graph, manifold_graph, - lm_params); + IELMIterationDetails iter_details = iterate(graph, state); + state = IELMState::fromLastIteration(iter_details, graph, manifold_graph, + lmParams); details_->push_back(iter_details); - } while (state.iterations < lm_params.maxIterations && + } while (state.iterations < lmParams.maxIterations && !checkConvergence(prev_state, state) && checkLambdaWithinLimits(state.lambda) && std::isfinite(state.error)); details_->emplace_back(state); @@ -70,26 +70,26 @@ Values IELMOptimizer::optimizeManifolds( } /* ************************************************************************* */ -IELMIterDetails IELMOptimizer::iterate(const NonlinearFactorGraph &graph, +IELMIterationDetails IELMOptimizer::iterate(const NonlinearFactorGraph &graph, const IELMState &state) const { - const LevenbergMarquardtParams &lm_params = ielm_params_.lm_params; - if (iecm_params_->retractor_creator->params()->use_varying_sigma) { - *iecm_params_->retractor_creator->params()->metric_sigmas = + const LevenbergMarquardtParams &lmParams = ielm_params_.lmParams; + if (iecm_params_->retractorCreator->params()->useVaryingSigma) { + *iecm_params_->retractorCreator->params()->metricSigmas = state.computeMetricSigmas(graph); } - IELMIterDetails iter_details(state); + IELMIterationDetails iter_details(state); if (checkModeChange(graph, iter_details)) { - if (lm_params.verbosityLM == LevenbergMarquardtParams::SUMMARY) { - PrintIELMTrial(state, iter_details.trials.back(), ielm_params_, true); + if (lmParams.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + printIELMTrial(state, iter_details.trials.back(), ielm_params_, true); } return iter_details; } // Set lambda for first trial. double lambda = state.lambda; - double lambda_factor = state.lambda_factor; + double lambdaFactor = state.lambdaFactor; // Perform trials until any of follwing conditions is met // * 1) trial is successful @@ -98,28 +98,28 @@ IELMIterDetails IELMOptimizer::iterate(const NonlinearFactorGraph &graph, while (true) { // Perform the trial. IELMTrial trial(state, graph, lambda, ielm_params_); - if (lm_params.verbosityLM == LevenbergMarquardtParams::SUMMARY) { - PrintIELMTrial(state, trial, ielm_params_); + if (lmParams.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + printIELMTrial(state, trial, ielm_params_); } iter_details.trials.emplace_back(trial); // Check condition 1. - if (trial.step_is_successful) { + if (trial.stepIsSuccessful) { break; } // Check condition 2. - if (trial.linear_update.solve_successful) { + if (trial.linearUpdate.solveSuccessful) { double abs_change_tol = std::max( - lm_params.absoluteErrorTol, lm_params.relativeErrorTol * state.error); - if (trial.linear_update.cost_change < abs_change_tol && - trial.nonlinear_update.cost_change < abs_change_tol) { + lmParams.absoluteErrorTol, lmParams.relativeErrorTol * state.error); + if (trial.linearUpdate.costChange < abs_change_tol && + trial.nonlinearUpdate.costChange < abs_change_tol) { break; } } // Set lambda for next trial. - trial.setNextLambda(lambda, lambda_factor, lm_params); + trial.setNextLambda(lambda, lambdaFactor, lmParams); // Check condition 3. if (!checkLambdaWithinLimits(lambda)) { @@ -132,7 +132,7 @@ IELMIterDetails IELMOptimizer::iterate(const NonlinearFactorGraph &graph, /* ************************************************************************* */ bool IELMOptimizer::checkModeChange( const NonlinearFactorGraph &graph, - IELMIterDetails ¤t_iter_details) const { + IELMIterationDetails ¤t_iter_details) const { if (details_->size() == 0) { return false; } @@ -140,7 +140,7 @@ bool IELMOptimizer::checkModeChange( // Find the first state in the sequence of states that have the same mode. int n = details_->size(); int first_i = n - 1; - while (first_i >= 0 && IsSameMode(current_iter_details.state.manifolds, + while (first_i >= 0 && isSameMode(current_iter_details.state.manifolds, details_->at(first_i).state.manifolds)) { first_i--; } @@ -160,7 +160,7 @@ bool IELMOptimizer::checkModeChange( bool failed_trial_exists = false; while (true) { const auto &trial = details_->at(iter_idx).trials.at(trial_idx); - if (!trial.step_is_successful && trial.linear_update.solve_successful) { + if (!trial.stepIsSuccessful && trial.linearUpdate.solveSuccessful) { failed_trial_exists = true; break; } @@ -182,19 +182,19 @@ bool IELMOptimizer::checkModeChange( } IndexSetMap change_indices_map = - IdentifyChangeIndices(current_iter_details.state.manifolds, + identifyChangeIndices(current_iter_details.state.manifolds, details_->at(iter_idx) .trials.at(trial_idx) - .nonlinear_update.new_manifolds); + .nonlinearUpdate.newManifolds); // Condition2(2): most recent failed trial results in other mode if (change_indices_map.size() == 0) { return false; } - auto approach_indices_map = IdentifyApproachingIndices( + auto approach_indices_map = identifyApproachingIndices( init_iter_dertails.state.manifolds, current_iter_details.state.manifolds, - change_indices_map, ielm_params_.boundary_approach_rate_threshold); + change_indices_map, ielm_params_.boundaryApproachRateThreshold); // Condition3: approaching boundary with decent rate if (approach_indices_map.size() == 0) { @@ -209,19 +209,19 @@ bool IELMOptimizer::checkModeChange( /* ************************************************************************* */ bool IELMOptimizer::checkLambdaWithinLimits(const double &lambda) const { - return lambda <= ielm_params_.lm_params.lambdaUpperBound && - lambda >= ielm_params_.lm_params.lambdaLowerBound; + return lambda <= ielm_params_.lmParams.lambdaUpperBound && + lambda >= ielm_params_.lmParams.lambdaLowerBound; } /* ************************************************************************* */ bool IELMOptimizer::checkConvergence(const IELMState &prev_state, const IELMState &state) const { - if (state.error <= ielm_params_.lm_params.errorTol) + if (state.error <= ielm_params_.lmParams.errorTol) return true; // check if mode changes - if (!IsSameMode(prev_state.manifolds, state.manifolds)) { + if (!isSameMode(prev_state.manifolds, state.manifolds)) { return false; } // check if diverges @@ -230,9 +230,9 @@ bool IELMOptimizer::checkConvergence(const IELMState &prev_state, // calculate relative error decrease and update currentError double relativeDecrease = absoluteDecrease / prev_state.error; bool converged = - (ielm_params_.lm_params.relativeErrorTol && - (relativeDecrease <= ielm_params_.lm_params.relativeErrorTol)) || - (absoluteDecrease <= ielm_params_.lm_params.absoluteErrorTol); + (ielm_params_.lmParams.relativeErrorTol && + (relativeDecrease <= ielm_params_.lmParams.relativeErrorTol)) || + (absoluteDecrease <= ielm_params_.lmParams.absoluteErrorTol); return converged; } diff --git a/gtdynamics/cmcopt/IELMOptimizer.h b/gtdynamics/cmcopt/IELMOptimizer.h index 45a6f150e..f7089bed9 100644 --- a/gtdynamics/cmcopt/IELMOptimizer.h +++ b/gtdynamics/cmcopt/IELMOptimizer.h @@ -27,11 +27,11 @@ using namespace gtsam; struct IELMParams { IELMParams() {} - double boundary_approach_rate_threshold = 3; - LevenbergMarquardtParams lm_params; - size_t iqp_max_iters = 0; - bool show_active_constraints = false; - bool active_constraints_group_as_categories = false; + double boundaryApproachRateThreshold = 3; + LevenbergMarquardtParams lmParams; + size_t iqpMaxIterations = 0; + bool showActiveConstraints = false; + bool activeConstraintsGroupedAsCategories = false; }; /** @@ -41,19 +41,19 @@ class IELMOptimizer : public IEOptimizer { protected: const IELMParams ielm_params_; - std::shared_ptr details_; + std::shared_ptr details_; public: typedef std::shared_ptr shared_ptr; - const IELMItersDetails &details() const { return *details_; } + const IELMOptimizationDetails &details() const { return *details_; } /** Constructor */ IELMOptimizer(const IELMParams &ielm_params = IELMParams(), const IEConstraintManifold::Params::shared_ptr &iecm_params = std::make_shared()) : IEOptimizer(iecm_params), ielm_params_(ielm_params), - details_(std::make_shared()) {} + details_(std::make_shared()) {} /** Virtual destructor */ ~IELMOptimizer() {} @@ -64,7 +64,7 @@ class IELMOptimizer : public IEOptimizer { /** Perform optimization on manifolds. */ Values optimizeManifolds(const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, - const Values &unconstrained_values) const override; + const Values &unconstrainedValues) const override; /** Convergence check including * 1) mode change @@ -79,7 +79,7 @@ class IELMOptimizer : public IEOptimizer { bool checkLambdaWithinLimits(const double &lambda) const; /** Perform one iterate, may need to make several trials. */ - IELMIterDetails iterate(const NonlinearFactorGraph &graph, + IELMIterationDetails iterate(const NonlinearFactorGraph &graph, const IELMState &state) const; /** Check if a mode change is required. The following conditions need to be @@ -90,7 +90,7 @@ class IELMOptimizer : public IEOptimizer { * with large enough rate. */ bool checkModeChange(const NonlinearFactorGraph &graph, - IELMIterDetails ¤t_iter_details) const; + IELMIterationDetails ¤t_iter_details) const; }; } // namespace gtdynamics diff --git a/gtdynamics/cmcopt/IELMOptimizerState.cpp b/gtdynamics/cmcopt/IELMOptimizerState.cpp index da7d44380..55bfb76e3 100644 --- a/gtdynamics/cmcopt/IELMOptimizerState.cpp +++ b/gtdynamics/cmcopt/IELMOptimizerState.cpp @@ -18,22 +18,22 @@ using namespace gtsam; /* ************************************************************************* */ IELMState::IELMState(const IEManifoldValues &_manifolds, - const Values &unconstrained_values, + const Values &unconstrainedValues, const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, - const double &_lambda, const double &_lambda_factor, + const double &_lambda, const double &_lambdaFactor, size_t _iterations) : manifolds(_manifolds), - values(AllValues(_manifolds, unconstrained_values)), - unconstrained_keys(unconstrained_values.keys()), - error(EvaluateGraphError(graph, manifolds, unconstrained_values)), - lambda(_lambda), lambda_factor(_lambda_factor), iterations(_iterations) { + values(allValues(_manifolds, unconstrainedValues)), + unconstrainedKeys(unconstrainedValues.keys()), + error(evaluateGraphError(graph, manifolds, unconstrainedValues)), + lambda(_lambda), lambdaFactor(_lambdaFactor), iterations(_iterations) { construct(graph, manifold_graph); } /* ************************************************************************* */ IELMState -IELMState::FromLastIteration(const IELMIterDetails &iter_details, +IELMState::fromLastIteration(const IELMIterationDetails &iter_details, const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, const LevenbergMarquardtParams ¶ms) { @@ -41,44 +41,44 @@ IELMState::FromLastIteration(const IELMIterDetails &iter_details, const auto &last_trial = iter_details.trials.back(); const auto &prev_state = iter_details.state; IELMState state; - if (last_trial.step_is_successful) { - state = IELMState(last_trial.nonlinear_update.new_manifolds, - last_trial.nonlinear_update.new_unconstrained_values, - graph, manifold_graph, last_trial.linear_update.lambda, - prev_state.lambda_factor); + if (last_trial.stepIsSuccessful) { + state = IELMState(last_trial.nonlinearUpdate.newManifolds, + last_trial.nonlinearUpdate.newUnconstrainedValues, + graph, manifold_graph, last_trial.linearUpdate.lambda, + prev_state.lambdaFactor); // TODO: will this cause early ending? (converged in this mode, but not for // the overall problem) - if (last_trial.forced_indices_map.size() > 0) { - state.grad_blocking_indices_map.mergeWith(last_trial.forced_indices_map); + if (last_trial.forcedIndicesMap.size() > 0) { + state.gradientBlockingIndicesMap.mergeWith(last_trial.forcedIndicesMap); } } else { // pick the trials with smallest error state = IELMState(prev_state.manifolds, prev_state.unconstrainedValues(), graph, - manifold_graph, prev_state.lambda, prev_state.lambda_factor); + manifold_graph, prev_state.lambda, prev_state.lambdaFactor); for (const auto &trial : iter_details.trials) { - if (trial.linear_update.solve_successful && - trial.nonlinear_update.new_error < prev_state.error) { - state = IELMState(trial.nonlinear_update.new_manifolds, - trial.nonlinear_update.new_unconstrained_values, - graph, manifold_graph, trial.linear_update.lambda, - prev_state.lambda_factor); + if (trial.linearUpdate.solveSuccessful && + trial.nonlinearUpdate.newError < prev_state.error) { + state = IELMState(trial.nonlinearUpdate.newManifolds, + trial.nonlinearUpdate.newUnconstrainedValues, + graph, manifold_graph, trial.linearUpdate.lambda, + prev_state.lambdaFactor); } } } - last_trial.setNextLambda(state.lambda, state.lambda_factor, params); + last_trial.setNextLambda(state.lambda, state.lambdaFactor, params); state.iterations = prev_state.iterations + 1; - state.totalNumberInnerIterations = - prev_state.totalNumberInnerIterations + iter_details.trials.size(); + state.totalInnerIterations = + prev_state.totalInnerIterations + iter_details.trials.size(); return state; } /* ************************************************************************* */ -Values IELMState::AllValues(const IEManifoldValues &manifolds, - const Values &unconstrained_values) { - Values values = IEOptimizer::EManifolds(manifolds); - values.insert(unconstrained_values); +Values IELMState::allValues(const IEManifoldValues &manifolds, + const Values &unconstrainedValues) { + Values values = IEOptimizer::equalityManifolds(manifolds); + values.insert(unconstrainedValues); return values; } @@ -86,8 +86,8 @@ Values IELMState::AllValues(const IEManifoldValues &manifolds, void IELMState::construct(const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph) { // linearize costs - base_linear = graph.linearize(baseValues()); - linear_manifold_graph = manifold_graph.linearize(values); + baseLinear = graph.linearize(baseValues()); + linearManifoldGraph = manifold_graph.linearize(values); // linearize active i-constraints linearizeIConstraints(); @@ -98,15 +98,15 @@ void IELMState::construct(const NonlinearFactorGraph &graph, /* ************************************************************************* */ void IELMState::linearizeIConstraints() { - linear_manifold_i_constraints.resize(0); + linearManifoldInequalityConstraints.resize(0); size_t index = 0; for (const auto &[key, manifold] : manifolds) { - auto man_constraints = manifold.linearActiveManIConstraints(key); - auto base_constraints = manifold.linearActiveBaseIConstraints(); + auto man_constraints = manifold.linearActiveManifoldInequalityConstraints(key); + auto base_constraints = manifold.linearActiveBaseInequalityConstraints(); for (const auto &[constraint_idx, constraint] : man_constraints) { - linear_manifold_i_constraints.push_back(constraint); - linear_base_i_constraints.push_back(base_constraints.at(constraint_idx)); - lic_index_translator.insert(index, key, constraint_idx); + linearManifoldInequalityConstraints.push_back(constraint); + linearBaseInequalityConstraints.push_back(base_constraints.at(constraint_idx)); + linearInequalityIndexTranslator.insert(index, key, constraint_idx); index++; } } @@ -114,16 +114,16 @@ void IELMState::linearizeIConstraints() { /* ************************************************************************* */ void IELMState::computeGradient(const NonlinearFactorGraph &manifold_graph) { - gradient = linear_manifold_graph->gradientAtZero(); - VectorValues descent_dir = -1 * gradient; + gradient = linearManifoldGraph->gradientAtZero(); + VectorValues descentDirection = -1 * gradient; // identify blocking constraints - grad_blocking_indices_map = - IEOptimizer::ProjectTangentCone(manifolds, descent_dir).first; + gradientBlockingIndicesMap = + IEOptimizer::projectTangentCone(manifolds, descentDirection).first; } /* ************************************************************************* */ -double IELMState::EvaluateGraphError(const NonlinearFactorGraph &graph, +double IELMState::evaluateGraphError(const NonlinearFactorGraph &graph, const IEManifoldValues &_manifolds, const Values &_unconstrained_values) { Values all_values = _manifolds.baseValues(); @@ -133,7 +133,7 @@ double IELMState::EvaluateGraphError(const NonlinearFactorGraph &graph, /* ************************************************************************* */ Values IELMState::unconstrainedValues() const { - return SubValues(values, unconstrained_keys); + return SubValues(values, unconstrainedKeys); } /* ************************************************************************* */ @@ -150,7 +150,7 @@ IELMState::computeMetricSigmas(const NonlinearFactorGraph &graph) const { auto linear_graph = graph.linearize(base_values); auto hessian_diag = linear_graph->hessianDiagonal(); // hessian_diag.print("hessian diag:", GTDKeyFormatter); - VectorValues metric_sigmas; + VectorValues metricSigmas; for (auto &[key, value] : hessian_diag) { Vector sigmas_sqr_inv = hessian_diag.at(key); if (sigmas_sqr_inv.norm() < 1e-8) { @@ -164,12 +164,12 @@ IELMState::computeMetricSigmas(const NonlinearFactorGraph &graph) const { sigmas(i) = 1 / sqrt(sigmas_sqr_inv(i)); } } - metric_sigmas.insert(key, sigmas); + metricSigmas.insert(key, sigmas); } - metric_sigmas = 10 * metric_sigmas; - // metric_sigmas.print("metric sigmas:", GTDKeyFormatter); + metricSigmas = 10 * metricSigmas; + // metricSigmas.print("metric sigmas:", GTDKeyFormatter); - return metric_sigmas; + return metricSigmas; } /* ************************************************************************* */ @@ -181,27 +181,27 @@ IELMTrial::IELMTrial(const IELMState &state, const NonlinearFactorGraph &graph, const double &lambda, const IELMParams ¶ms) { // std::cout << "========= " << state.iterations << " ======= \n"; auto start = std::chrono::high_resolution_clock::now(); - step_is_successful = false; + stepIsSuccessful = false; // Compute linear update and linear cost change - linear_update = LinearUpdate(lambda, graph, state, params); - if (!linear_update.solve_successful) { + linearUpdate = LinearUpdate(lambda, graph, state, params); + if (!linearUpdate.solveSuccessful) { return; } // Compute nonlinear update and nonlinear cost change - nonlinear_update = NonlinearUpdate(state, linear_update, graph); + nonlinearUpdate = NonlinearUpdate(state, linearUpdate, graph); // Decide if accept or reject trial - model_fidelity = nonlinear_update.cost_change / linear_update.cost_change; - if (linear_update.cost_change > - std::numeric_limits::epsilon() * linear_update.old_error && - model_fidelity > params.lm_params.minModelFidelity) { - step_is_successful = true; + modelFidelity = nonlinearUpdate.costChange / linearUpdate.costChange; + if (linearUpdate.costChange > + std::numeric_limits::epsilon() * linearUpdate.oldError && + modelFidelity > params.lmParams.minModelFidelity) { + stepIsSuccessful = true; } auto end = std::chrono::high_resolution_clock::now(); - trial_time = + trialTime = std::chrono::duration_cast(end - start) .count() / 1e6; @@ -212,50 +212,50 @@ IELMTrial::IELMTrial(const IELMState &state, const NonlinearFactorGraph &graph, const IndexSetMap &approach_indices_map) { auto start = std::chrono::high_resolution_clock::now(); - forced_indices_map = approach_indices_map; - linear_update = LinearUpdate::Zero(state); - nonlinear_update = NonlinearUpdate(state, forced_indices_map, graph); - step_is_successful = true; + forcedIndicesMap = approach_indices_map; + linearUpdate = LinearUpdate::zero(state); + nonlinearUpdate = NonlinearUpdate(state, forcedIndicesMap, graph); + stepIsSuccessful = true; auto end = std::chrono::high_resolution_clock::now(); - trial_time = + trialTime = std::chrono::duration_cast(end - start) .count() / 1e6; } /* ************************************************************************* */ -void IELMTrial::setNextLambda(double &new_lambda, double &new_lambda_factor, +void IELMTrial::setNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { - if (forced_indices_map.size() > 0) { + if (forcedIndicesMap.size() > 0) { return; } - if (step_is_successful) { - setDecreasedNextLambda(new_lambda, new_lambda_factor, params); + if (stepIsSuccessful) { + setDecreasedNextLambda(new_lambda, newLambdaFactor, params); } else { - setIncreasedNextLambda(new_lambda, new_lambda_factor, params); + setIncreasedNextLambda(new_lambda, newLambdaFactor, params); } } /* ************************************************************************* */ void IELMTrial::setIncreasedNextLambda( - double &new_lambda, double &new_lambda_factor, + double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { - new_lambda *= new_lambda_factor; + new_lambda *= newLambdaFactor; if (!params.useFixedLambdaFactor) { - new_lambda_factor *= 2.0; + newLambdaFactor *= 2.0; } } /* ************************************************************************* */ void IELMTrial::setDecreasedNextLambda( - double &new_lambda, double &new_lambda_factor, + double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { if (params.useFixedLambdaFactor) { - new_lambda /= new_lambda_factor; + new_lambda /= newLambdaFactor; } else { - new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * model_fidelity - 1.0, 3)); - new_lambda_factor *= 2.0; + new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * modelFidelity - 1.0, 3)); + newLambdaFactor *= 2.0; } new_lambda = std::max(params.lambdaLowerBound, new_lambda); } @@ -267,15 +267,15 @@ void IELMTrial::setDecreasedNextLambda( /* ************************************************************************* */ std::map, size_t> IdentifyConstraintType(const IEManifoldValues &state_manifolds, - const IndexSetMap blocking_indices_map, - const IEManifoldValues &new_manifolds) { + const IndexSetMap blockingIndicesMap, + const IEManifoldValues &newManifolds) { std::map, size_t> constraint_type_map; - for (const auto &[key, new_manifold] : new_manifolds) { + for (const auto &[key, new_manifold] : newManifolds) { const auto &state_manifold = state_manifolds.at(key); IndexSet blocking_indices; - if (blocking_indices_map.exists(key)) { - blocking_indices = blocking_indices_map.at(key); + if (blockingIndicesMap.exists(key)) { + blocking_indices = blockingIndicesMap.at(key); } for (const auto &constraint_idx : new_manifold.activeIndices()) { size_t constraint_type = 0; @@ -333,20 +333,20 @@ std::string ColoredStr(const std::string &str, const size_t constraint_type, /* ************************************************************************* */ std::string ConstraintInfoStr(const IEManifoldValues &state_manifolds, - const IndexSetMap blocking_indices_map, - const IEManifoldValues &new_manifolds, + const IndexSetMap blockingIndicesMap, + const IEManifoldValues &newManifolds, const KeyFormatter &key_formatter, - bool step_is_successful, + bool stepIsSuccessful, bool group_as_categories) { - std::string default_color_str = step_is_successful ? "\033[0m" : "\033[090m"; + std::string default_color_str = stepIsSuccessful ? "\033[0m" : "\033[090m"; auto constraint_type_map = IdentifyConstraintType( - state_manifolds, blocking_indices_map, new_manifolds); + state_manifolds, blockingIndicesMap, newManifolds); if (!group_as_categories) { std::string str = ""; - for (const auto &[key, manifold] : new_manifolds) { + for (const auto &[key, manifold] : newManifolds) { for (const auto &constraint_idx : manifold.activeIndices()) { auto constraint_type = constraint_type_map.at({key, constraint_idx}); str += ColoredStr( @@ -361,7 +361,7 @@ std::string ConstraintInfoStr(const IEManifoldValues &state_manifolds, std::map>> category_constraints; - for (const auto &[key, manifold] : new_manifolds) { + for (const auto &[key, manifold] : newManifolds) { for (const auto &constraint_idx : manifold.activeIndices()) { auto category = key_formatter(key); size_t k = constraint_idx; @@ -390,14 +390,14 @@ std::string ConstraintInfoStr(const IEManifoldValues &state_manifolds, } /* ************************************************************************* */ -void PrintIELMTrialTitle() { +void printIELMTrialTitle() { cout << setw(10) << "iter " << "|" << setw(12) << "error " << "|" << setw(12) << "nonlinear " << "|" << setw(12) << "linear " << "|" << setw(12) << "linear_retr" << "|" << setw(10) << "lambda " - << "|" << setw(10) << "num_solves" + << "|" << setw(10) << "numSolves" << "|" << setw(17) << "retract_devi " << "|" << setw(10) << "time " << "|" << setw(10) << "delta_norm" @@ -432,54 +432,54 @@ std::string ConstraintInfoStr(const IEManifoldValues &manifolds, } /* ************************************************************************* */ -void PrintIELMTrial(const IELMState &state, const IELMTrial &trial, +void printIELMTrial(const IELMState &state, const IELMTrial &trial, const IELMParams ¶ms, bool forced, const KeyFormatter &key_formatter) { - if (!trial.step_is_successful) { + if (!trial.stepIsSuccessful) { cout << "\033[90m"; } - const auto &nonlinear_update = trial.nonlinear_update; - const auto &linear_update = trial.linear_update; + const auto &nonlinearUpdate = trial.nonlinearUpdate; + const auto &linearUpdate = trial.linearUpdate; cout << setw(10) << state.iterations << "|"; - cout << setw(12) << setprecision(4) << nonlinear_update.new_error << "|"; - cout << setw(12) << setprecision(4) << nonlinear_update.cost_change << "|"; + cout << setw(12) << setprecision(4) << nonlinearUpdate.newError << "|"; + cout << setw(12) << setprecision(4) << nonlinearUpdate.costChange << "|"; if (!forced) { - cout << setw(12) << setprecision(4) << linear_update.cost_change << "|"; + cout << setw(12) << setprecision(4) << linearUpdate.costChange << "|"; cout << setw(12) << setprecision(4) - << nonlinear_update.linear_cost_change_with_retract_delta << "|"; - cout << setw(10) << setprecision(2) << linear_update.lambda << "|"; - if (!linear_update.solve_successful) { + << nonlinearUpdate.linearCostChangeWithRetractionDelta << "|"; + cout << setw(10) << setprecision(2) << linearUpdate.lambda << "|"; + if (!linearUpdate.solveSuccessful) { cout << "linear solve not successful\n"; return; } - cout << setw(4) << linear_update.num_solves << "|" << setw(5) << std::left - << nonlinear_update.num_retract_iters << std::right << "|"; - cout << setw(8) << VectorMean(nonlinear_update.retract_divate_rates) << "|" + cout << setw(4) << linearUpdate.numSolves << "|" << setw(5) << std::left + << nonlinearUpdate.numRetractionIterations << std::right << "|"; + cout << setw(8) << VectorMean(nonlinearUpdate.retractionDeviationRates) << "|" << setw(8) << std::left - << VectorMax(nonlinear_update.retract_divate_rates) << std::right + << VectorMax(nonlinearUpdate.retractionDeviationRates) << std::right << "|"; - cout << setw(10) << setprecision(2) << trial.trial_time << "|"; - cout << setw(10) << setprecision(4) << linear_update.delta.norm() << "|"; - if (params.show_active_constraints) { + cout << setw(10) << setprecision(2) << trial.trialTime << "|"; + cout << setw(10) << setprecision(4) << linearUpdate.delta.norm() << "|"; + if (params.showActiveConstraints) { cout << ConstraintInfoStr( - state.manifolds, linear_update.blocking_indices_map, - nonlinear_update.new_manifolds, GTDKeyFormatter, - trial.step_is_successful, - params.active_constraints_group_as_categories); + state.manifolds, linearUpdate.blockingIndicesMap, + nonlinearUpdate.newManifolds, GTDKeyFormatter, + trial.stepIsSuccessful, + params.activeConstraintsGroupedAsCategories); } // cout << setw(10) << setprecision(4) << - // linear_update.tangent_vector.norm() + // linearUpdate.tangentVector.norm() // << " "; // cout << setw(10) << setprecision(4) - // << linear_update.tangent_vector.norm() / linear_update.delta.norm() + // << linearUpdate.tangentVector.norm() / linearUpdate.delta.norm() // << " "; } else { std::string forced_i_str = - "forced:" + ConstraintInfoStr(state.manifolds, trial.forced_indices_map, + "forced:" + ConstraintInfoStr(state.manifolds, trial.forcedIndicesMap, key_formatter); cout << forced_i_str; } - if (!trial.step_is_successful) { + if (!trial.stepIsSuccessful) { cout << "\033[0m"; } cout << endl; @@ -494,84 +494,84 @@ IELMTrial::LinearUpdate::LinearUpdate(const double &_lambda, const NonlinearFactorGraph &graph, const IELMState &state, const IELMParams ¶ms) - : lambda(_lambda), num_solves(0) { + : lambda(_lambda), numSolves(0) { // build damped system - GaussianFactorGraph::shared_ptr linear = state.linear_manifold_graph; + GaussianFactorGraph::shared_ptr linear = state.linearManifoldGraph; VectorValues sqrt_hessian_diagonal = - SqrtHessianDiagonal(*linear, params.lm_params); + SqrtHessianDiagonal(*linear, params.lmParams); auto damped_system = buildDampedSystem(*linear, sqrt_hessian_diagonal, state, - params.lm_params); + params.lmParams); IndexSet blocking_indices; // no active constraints - if (state.linear_base_i_constraints.size() == 0) { + if (state.linearBaseInequalityConstraints.size() == 0) { try { - delta = SolveLinear(damped_system, params.lm_params); - solve_successful = true; + delta = SolveLinear(damped_system, params.lmParams); + solveSuccessful = true; } catch (const IndeterminantLinearSystemException &) { - solve_successful = false; + solveSuccessful = false; } - num_solves = 1; + numSolves = 1; } else { // solve IQP init estimate - std::tie(delta, blocking_indices, num_solves, solve_successful) = - InitEstimate(damped_system, state, params.lm_params); - if (!solve_successful) { + std::tie(delta, blocking_indices, numSolves, solveSuccessful) = + initialEstimate(damped_system, state, params.lmParams); + if (!solveSuccessful) { return; } // solve IQP - if (params.iqp_max_iters > 0) { + if (params.iqpMaxIterations > 0) { size_t num_new_solves; - std::tie(delta, blocking_indices, num_new_solves, solve_successful) = - SolveConvexIQP(damped_system, state.linear_manifold_i_constraints, - blocking_indices, delta, params.iqp_max_iters); - num_solves += num_new_solves; + std::tie(delta, blocking_indices, num_new_solves, solveSuccessful) = + SolveConvexIQP(damped_system, state.linearManifoldInequalityConstraints, + blocking_indices, delta, params.iqpMaxIterations); + numSolves += num_new_solves; } } // record linear update info - blocking_indices_map = - state.lic_index_translator.decodeIndices(blocking_indices); - tangent_vector = computeTangentVector(state, delta, state.unconstrained_keys); - CheckSolutionValid(state, tangent_vector, blocking_indices); - old_error = linear->error(VectorValues::Zero(delta)); - new_error = linear->error(delta); - cost_change = old_error - new_error; + blockingIndicesMap = + state.linearInequalityIndexTranslator.decodeIndices(blocking_indices); + tangentVector = computeTangentVector(state, delta, state.unconstrainedKeys); + checkSolutionValid(state, tangentVector, blocking_indices); + oldError = linear->error(VectorValues::Zero(delta)); + newError = linear->error(delta); + costChange = oldError - newError; } /* ************************************************************************* */ -IELMTrial::LinearUpdate IELMTrial::LinearUpdate::Zero(const IELMState &state) { - LinearUpdate linear_update; - linear_update.lambda = state.lambda; - linear_update.delta = state.values.zeroVectors(); - linear_update.tangent_vector = state.baseValues().zeroVectors(); - return linear_update; +IELMTrial::LinearUpdate IELMTrial::LinearUpdate::zero(const IELMState &state) { + LinearUpdate linearUpdate; + linearUpdate.lambda = state.lambda; + linearUpdate.delta = state.values.zeroVectors(); + linearUpdate.tangentVector = state.baseValues().zeroVectors(); + return linearUpdate; } /* ************************************************************************* */ std::tuple -IELMTrial::LinearUpdate::InitEstimate(const GaussianFactorGraph &quadratic_cost, +IELMTrial::LinearUpdate::initialEstimate(const GaussianFactorGraph &quadratic_cost, const IELMState &state, const LevenbergMarquardtParams ¶ms) { IndexSet blocking_indices = - state.lic_index_translator.encodeIndices(state.grad_blocking_indices_map); - size_t num_solves = 0; + state.linearInequalityIndexTranslator.encodeIndices(state.gradientBlockingIndicesMap); + size_t numSolves = 0; while (true) { - num_solves += 1; + numSolves += 1; GaussianFactorGraph graph = quadratic_cost; GaussianFactorGraph constraint_graph = - state.linear_manifold_i_constraints.constraintGraph(blocking_indices); + state.linearManifoldInequalityConstraints.constraintGraph(blocking_indices); graph.push_back(constraint_graph.begin(), constraint_graph.end()); VectorValues delta; try { delta = SolveLinear(graph, params); } catch (const IndeterminantLinearSystemException &) { - return {delta, blocking_indices, num_solves, false}; + return {delta, blocking_indices, numSolves, false}; } // check if satisfy tangent cone, if not, add constraints and recompute @@ -583,7 +583,7 @@ IELMTrial::LinearUpdate::InitEstimate(const GaussianFactorGraph &quadratic_cost, IndexSet man_blocking_indices = manifold.blockingIndices(tv); for (const auto &constraint_idx : man_blocking_indices) { size_t index = - state.lic_index_translator.encoder.at({key, constraint_idx}); + state.linearInequalityIndexTranslator.encoder.at({key, constraint_idx}); if (!blocking_indices.exists(index)) { feasible = false; blocking_indices.insert(index); @@ -591,27 +591,27 @@ IELMTrial::LinearUpdate::InitEstimate(const GaussianFactorGraph &quadratic_cost, } } if (feasible) { - return {delta, blocking_indices, num_solves, true}; + return {delta, blocking_indices, numSolves, true}; } } } /* ************************************************************************* */ -bool IELMTrial::LinearUpdate::CheckSolutionValid( - const IELMState &state, const VectorValues &tangent_vector, +bool IELMTrial::LinearUpdate::checkSolutionValid( + const IELMState &state, const VectorValues &tangentVector, const IndexSet &blocking_indices) { - for (const auto &constraint : state.linear_base_i_constraints) { - if (!constraint->feasible(tangent_vector, 1e-5)) { + for (const auto &constraint : state.linearBaseInequalityConstraints) { + if (!constraint->feasible(tangentVector, 1e-5)) { std::cout << "tangent vector violating constraint: " - << (*constraint)(tangent_vector).transpose() << "\n"; + << (*constraint)(tangentVector).transpose() << "\n"; return false; } } for (const auto &constraint_idx : blocking_indices) { - const auto &constraint = state.linear_base_i_constraints.at(constraint_idx); - if (!constraint->isActive(tangent_vector)) { + const auto &constraint = state.linearBaseInequalityConstraints.at(constraint_idx); + if (!constraint->isActive(tangentVector)) { std::cout << "blocking constraint is not active: " - << (*constraint)(tangent_vector).transpose() << "\n"; + << (*constraint)(tangentVector).transpose() << "\n"; return false; } } @@ -679,19 +679,19 @@ GaussianFactorGraph IELMTrial::LinearUpdate::buildDampedSystem( /* ************************************************************************* */ VectorValues IELMTrial::LinearUpdate::computeTangentVector( const IELMState &state, const VectorValues &delta, - const KeySet &unconstrained_keys) const { - VectorValues tangent_vector; + const KeySet &unconstrainedKeys) const { + VectorValues tangentVector; for (const auto &[key, xi] : delta) { - if (unconstrained_keys.exists(key)) { - tangent_vector.insert(key, delta.at(key)); + if (unconstrainedKeys.exists(key)) { + tangentVector.insert(key, delta.at(key)); } else { const auto &manifold = state.manifolds.at(key); VectorValues tv = manifold.eBasis()->computeTangentVector(xi); - tangent_vector.insert(tv); + tangentVector.insert(tv); } } - return tangent_vector; + return tangentVector; } /* ************************************************************************* */ @@ -700,35 +700,35 @@ VectorValues IELMTrial::LinearUpdate::computeTangentVector( /* ************************************************************************* */ IELMTrial::NonlinearUpdate::NonlinearUpdate(const IELMState &state, - const LinearUpdate &linear_update, + const LinearUpdate &linearUpdate, const NonlinearFactorGraph &graph) { // retract for ie-manifolds - num_retract_iters = 0; + numRetractionIterations = 0; VectorValues retract_delta; for (const auto &[key, manifold] : state.manifolds) { VectorValues tv = - SubValues(linear_update.tangent_vector, manifold.values().keys()); - const auto &blocking_indices_map = linear_update.blocking_indices_map; - IERetractInfo retract_info; - if (blocking_indices_map.find(key) != blocking_indices_map.end()) { - const auto &blocking_indices = blocking_indices_map.at(key); - new_manifolds.emplace( + SubValues(linearUpdate.tangentVector, manifold.values().keys()); + const auto &blockingIndicesMap = linearUpdate.blockingIndicesMap; + IERetractionInfo retract_info; + if (blockingIndicesMap.find(key) != blockingIndicesMap.end()) { + const auto &blocking_indices = blockingIndicesMap.at(key); + newManifolds.emplace( key, manifold.retract(tv, blocking_indices, &retract_info)); } else { - new_manifolds.emplace(key, manifold.retract(tv, {}, &retract_info)); + newManifolds.emplace(key, manifold.retract(tv, {}, &retract_info)); } - num_retract_iters += retract_info.num_lm_iters; + numRetractionIterations += retract_info.numLMIterations; auto [manifold_retract_delta, retract_deviation_rate] = - evaluateRetractionDeviation(manifold, new_manifolds.at(key), tv); - retract_divate_rates.push_back(retract_deviation_rate); + evaluateRetractionDeviation(manifold, newManifolds.at(key), tv); + retractionDeviationRates.push_back(retract_deviation_rate); retract_delta.insert(manifold_retract_delta); } // retract for unconstrained variables VectorValues tangent_vector_unconstrained = - SubValues(linear_update.tangent_vector, state.unconstrained_keys); - new_unconstrained_values = + SubValues(linearUpdate.tangentVector, state.unconstrainedKeys); + newUnconstrainedValues = state.unconstrainedValues().retract(tangent_vector_unconstrained); retract_delta.insert(tangent_vector_unconstrained); @@ -740,20 +740,20 @@ IELMTrial::NonlinearUpdate::NonlinearUpdate(const IELMState &state, // zero_vec_kv.push_back(key); // } // PrintKeyVector(zero_vec_kv, "zero_vec", GTDKeyFormatter); - // PrintKeySet(state.base_linear->keys(), "graph", + // PrintKeySet(state.baseLinear->keys(), "graph", // GTDKeyFormatter); - double base_linear_error = state.base_linear->error(zero_vec); - double base_linear_error_retract = state.base_linear->error(retract_delta); - linear_cost_change_with_retract_delta = + double base_linear_error = state.baseLinear->error(zero_vec); + double base_linear_error_retract = state.baseLinear->error(retract_delta); + linearCostChangeWithRetractionDelta = base_linear_error - base_linear_error_retract; } /* ************************************************************************* */ IELMTrial::NonlinearUpdate::NonlinearUpdate( - const IELMState &state, const IndexSetMap &forced_indices_map, + const IELMState &state, const IndexSetMap &forcedIndicesMap, const NonlinearFactorGraph &graph) { - new_manifolds = state.manifolds.moveToBoundaries(forced_indices_map); - new_unconstrained_values = state.unconstrainedValues(); + newManifolds = state.manifolds.moveToBoundaries(forcedIndicesMap); + newUnconstrainedValues = state.unconstrainedValues(); computeError(graph, state.error); } @@ -762,11 +762,11 @@ std::pair IELMTrial::NonlinearUpdate::evaluateRetractionDeviation( const IEConstraintManifold &manifold, const IEConstraintManifold &new_manifold, - const VectorValues &tangent_vector) { + const VectorValues &tangentVector) { VectorValues retract_delta = manifold.values().localCoordinates(new_manifold.values()); - auto vec_diff = retract_delta - tangent_vector; - double tangent_vector_norm = tangent_vector.norm(); + auto vec_diff = retract_delta - tangentVector; + double tangent_vector_norm = tangentVector.norm(); double retract_deviation_rate; if (abs(tangent_vector_norm) < 1e-10) { retract_deviation_rate = 0; @@ -779,17 +779,17 @@ IELMTrial::NonlinearUpdate::evaluateRetractionDeviation( /* ************************************************************************* */ void IELMTrial::NonlinearUpdate::computeError(const NonlinearFactorGraph &graph, - const double &old_error) { + const double &oldError) { - new_error = IELMState::EvaluateGraphError(graph, new_manifolds, - new_unconstrained_values); - cost_change = old_error - new_error; + newError = IELMState::evaluateGraphError(graph, newManifolds, + newUnconstrainedValues); + costChange = oldError - newError; } /* ************************************************************************* */ -/* <========================= IELMItersDetails ============================> */ +/* <========================= IELMOptimizationDetails ============================> */ /* ************************************************************************* */ -void IELMItersDetails::exportFile(const std::string &state_file_path, +void IELMOptimizationDetails::exportFile(const std::string &state_file_path, const std::string &trial_file_path) const { std::ofstream state_file, trial_file; state_file.open(state_file_path); @@ -802,11 +802,11 @@ void IELMItersDetails::exportFile(const std::string &state_file_path, trial_file << "iterations" << ",lambda" << ",error" - << ",step_is_successful" - << ",linear_cost_change" - << ",linear_cost_change_with_retract_delta" - << ",nonlinear_cost_change" - << ",model_fidelity" + << ",stepIsSuccessful" + << ",linearCostChange" + << ",linearCostChangeWithRetractionDelta" + << ",nonlinearCostChange" + << ",modelFidelity" << ",tangent_vector_norm" << ",num_solves_linear" << ",num_solves_retraction" @@ -819,19 +819,19 @@ void IELMItersDetails::exportFile(const std::string &state_file_path, state_file << state.iterations << "," << state.lambda << "," << state.error << "\n"; for (const auto &trial : iter_details.trials) { - trial_file << state.iterations << "," << trial.linear_update.lambda << "," - << trial.nonlinear_update.new_error << "," - << trial.step_is_successful << "," - << trial.linear_update.cost_change << "," - << trial.nonlinear_update.linear_cost_change_with_retract_delta - << "," << trial.nonlinear_update.cost_change << "," - << trial.model_fidelity << "," - << trial.linear_update.tangent_vector.norm() << "," - << trial.linear_update.num_solves << "," - << trial.nonlinear_update.num_retract_iters << "," - << VectorMean(trial.nonlinear_update.retract_divate_rates) + trial_file << state.iterations << "," << trial.linearUpdate.lambda << "," + << trial.nonlinearUpdate.newError << "," + << trial.stepIsSuccessful << "," + << trial.linearUpdate.costChange << "," + << trial.nonlinearUpdate.linearCostChangeWithRetractionDelta + << "," << trial.nonlinearUpdate.costChange << "," + << trial.modelFidelity << "," + << trial.linearUpdate.tangentVector.norm() << "," + << trial.linearUpdate.numSolves << "," + << trial.nonlinearUpdate.numRetractionIterations << "," + << VectorMean(trial.nonlinearUpdate.retractionDeviationRates) << "," - << VectorMax(trial.nonlinear_update.retract_divate_rates) + << VectorMax(trial.nonlinearUpdate.retractionDeviationRates) << "\n"; } } diff --git a/gtdynamics/cmcopt/IELMOptimizerState.h b/gtdynamics/cmcopt/IELMOptimizerState.h index 06b7d35ac..e96dca628 100644 --- a/gtdynamics/cmcopt/IELMOptimizerState.h +++ b/gtdynamics/cmcopt/IELMOptimizerState.h @@ -28,7 +28,7 @@ using namespace gtsam; struct IELMState; struct IELMTrial; -struct IELMIterDetails; +struct IELMIterationDetails; struct IELMParams; /** State corresponding to each LM iteration. */ @@ -36,19 +36,19 @@ struct IELMState { public: IEManifoldValues manifolds; Values values; - KeySet unconstrained_keys; + KeySet unconstrainedKeys; double error = 0; double lambda = 0; - double lambda_factor = 0; + double lambdaFactor = 0; size_t iterations = 0; - size_t totalNumberInnerIterations = 0; + size_t totalInnerIterations = 0; VectorValues gradient; - IndexSetMap grad_blocking_indices_map; // blocking indices map by neg grad - GaussianFactorGraph::shared_ptr base_linear; - GaussianFactorGraph::shared_ptr linear_manifold_graph; - LinearInequalityConstraints linear_base_i_constraints; - LinearInequalityConstraints linear_manifold_i_constraints; - IndexSetMapTranslator lic_index_translator; + IndexSetMap gradientBlockingIndicesMap; // blocking indices map by neg grad + GaussianFactorGraph::shared_ptr baseLinear; + GaussianFactorGraph::shared_ptr linearManifoldGraph; + LinearInequalityConstraints linearBaseInequalityConstraints; + LinearInequalityConstraints linearManifoldInequalityConstraints; + IndexSetMapTranslator linearInequalityIndexTranslator; /// Default constructor. IELMState() {} @@ -58,10 +58,10 @@ struct IELMState { const Values &_unconstrained_values, const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, const double &_lambda, - const double &_lambda_factor, size_t _iterations = 0); + const double &_lambdaFactor, size_t _iterations = 0); /// Initialize the new state from the result of previous iteration. - static IELMState FromLastIteration(const IELMIterDetails &iter_details, + static IELMState fromLastIteration(const IELMIterationDetails &iter_details, const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, const LevenbergMarquardtParams ¶ms); @@ -72,7 +72,7 @@ struct IELMState { /// Values of all original constrained opt problem variables. Values baseValues() const; - static double EvaluateGraphError(const NonlinearFactorGraph &graph, + static double evaluateGraphError(const NonlinearFactorGraph &graph, const IEManifoldValues &_manifolds, const Values &_unconstrained_values); @@ -81,8 +81,8 @@ struct IELMState { VectorValues computeMetricSigmas(const NonlinearFactorGraph &graph) const; protected: - static Values AllValues(const IEManifoldValues &manifolds, - const Values &unconstrained_values); + static Values allValues(const IEManifoldValues &manifolds, + const Values &unconstrainedValues); void construct(const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph); @@ -104,18 +104,18 @@ struct IELMTrial { /// Perform a trial by moving to specified boundaries. IELMTrial(const IELMState &state, const NonlinearFactorGraph &graph, - const IndexSetMap &forced_indices_map); + const IndexSetMap &forcedIndicesMap); struct LinearUpdate { double lambda; - IndexSetMap blocking_indices_map; + IndexSetMap blockingIndicesMap; VectorValues delta; - VectorValues tangent_vector; - double old_error = 0; - double new_error = 0; - double cost_change = 0; - bool solve_successful = 0; - size_t num_solves = 0; // number of solving linear systems + VectorValues tangentVector; + double oldError = 0; + double newError = 0; + double costChange = 0; + bool solveSuccessful = 0; + size_t numSolves = 0; // number of solving linear systems /** Default constructor. */ LinearUpdate() {} @@ -124,20 +124,20 @@ struct IELMTrial { * 1) linear update as (approx) solution the i-constrained qp problem * 2) blocking constraints as of solving the IQP problem * 3) e-manifolds corresponding to the e-constraints and blocking - * constraints 4) corresponding linear cost change 5) solve_successful + * constraints 4) corresponding linear cost change 5) solveSuccessful * indicating if solving the linear system is successful */ LinearUpdate(const double &_lambda, const NonlinearFactorGraph &graph, const IELMState &state, const IELMParams ¶ms); - static LinearUpdate Zero(const IELMState &state); + static LinearUpdate zero(const IELMState &state); /** Generate an initial estimate for the IQP problem using active * constraints that include the blocking constraint for neg-gradient. - * @return [delta, blocking_indices, num_solves, solve_sucessful] + * @return [delta, blocking_indices, numSolves, solve_sucessful] */ static std::tuple - InitEstimate(const GaussianFactorGraph &quadratic_cost, + initialEstimate(const GaussianFactorGraph &quadratic_cost, const IELMState &state, const LevenbergMarquardtParams ¶ms); @@ -149,12 +149,12 @@ struct IELMTrial { * gradient can be expressed as linear combination of bllcking constraint * jacobians. * @param state State of the optimizer. - * @param tangent_vector Solution to the IQP problem. + * @param tangentVector Solution to the IQP problem. * @param blocking_indices Indices of constraints that are active in IQP * w.r.t. delta. */ - static bool CheckSolutionValid(const IELMState &state, - const VectorValues &tangent_vector, + static bool checkSolutionValid(const IELMState &state, + const VectorValues &tangentVector, const IndexSet &blocking_indices); protected: @@ -178,85 +178,85 @@ struct IELMTrial { VectorValues computeTangentVector(const IELMState &state, const VectorValues &delta, - const KeySet &unconstrained_keys) const; + const KeySet &unconstrainedKeys) const; }; struct NonlinearUpdate { - IEManifoldValues new_manifolds; - Values new_unconstrained_values; - double new_error; - double cost_change; - size_t num_retract_iters = 0; // total number of iterations in LM opt. - std::vector retract_divate_rates; - double linear_cost_change_with_retract_delta = 0; + IEManifoldValues newManifolds; + Values newUnconstrainedValues; + double newError; + double costChange; + size_t numRetractionIterations = 0; // total number of iterations in LM opt. + std::vector retractionDeviationRates; + double linearCostChangeWithRetractionDelta = 0; /** Default constructor. */ NonlinearUpdate() {} /** Compute the new manifolds using the linear update delta and blocking * indices. */ - NonlinearUpdate(const IELMState &state, const LinearUpdate &linear_update, + NonlinearUpdate(const IELMState &state, const LinearUpdate &linearUpdate, const NonlinearFactorGraph &graph); NonlinearUpdate(const IELMState &state, - const IndexSetMap &forced_indices_map, + const IndexSetMap &forcedIndicesMap, const NonlinearFactorGraph &graph); void computeError(const NonlinearFactorGraph &graph, - const double &old_error); + const double &oldError); static std::pair evaluateRetractionDeviation(const IEConstraintManifold &manifold, const IEConstraintManifold &new_manifold, - const VectorValues &tangent_vector); + const VectorValues &tangentVector); }; public: // linear update - LinearUpdate linear_update; + LinearUpdate linearUpdate; // nonlinear update - NonlinearUpdate nonlinear_update; + NonlinearUpdate nonlinearUpdate; // decision making - IndexSetMap forced_indices_map; - double model_fidelity; - bool step_is_successful; - bool stop_searching_lambda; - double trial_time; + IndexSetMap forcedIndicesMap; + double modelFidelity; + bool stepIsSuccessful; + bool stopSearchingLambda; + double trialTime; /// Update lambda for the next trial/state. - void setNextLambda(double &new_lambda, double &new_lambda_factor, + void setNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; /// Set lambda as increased values for next trial/state. - void setIncreasedNextLambda(double &new_lambda, double &new_lambda_factor, + void setIncreasedNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; /// Set lambda as decreased values for next trial/state. - void setDecreasedNextLambda(double &new_lambda, double &new_lambda_factor, + void setDecreasedNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; }; /** Print title of summary info of IELM trials. */ -void PrintIELMTrialTitle(); +void printIELMTrialTitle(); /** Print summary info of an IELM trial. */ -void PrintIELMTrial( +void printIELMTrial( const IELMState &state, const IELMTrial &trial, const IELMParams ¶ms, bool forced = false, const KeyFormatter &key_formatter = GTDKeyFormatter); -struct IELMIterDetails { +struct IELMIterationDetails { IELMState state; std::vector trials; - IELMIterDetails(const IELMState &_state) : state(_state), trials() {} + IELMIterationDetails(const IELMState &_state) : state(_state), trials() {} }; -class IELMItersDetails : public std::vector { +class IELMOptimizationDetails : public std::vector { public: - using base = std::vector; + using base = std::vector; using base::base; void exportFile(const std::string &state_file_path, diff --git a/gtdynamics/cmcopt/IEOptimizer.cpp b/gtdynamics/cmcopt/IEOptimizer.cpp index bcadb6ef7..2a0121166 100644 --- a/gtdynamics/cmcopt/IEOptimizer.cpp +++ b/gtdynamics/cmcopt/IEOptimizer.cpp @@ -21,7 +21,7 @@ using namespace gtsam; /* ************************************************************************* */ std::map -IEOptimizer::Var2ManifoldKeyMap(const IEManifoldValues &manifolds) { +IEOptimizer::varToManifoldKeyMap(const IEManifoldValues &manifolds) { std::map keymap_var2manifold; for (const auto &it : manifolds) { for (const Key &variable_key : it.second.values().keys()) { @@ -38,7 +38,7 @@ struct ComponentInfo { }; /* ************************************************************************* */ -ComponentInfo IdentifyConnectedComponent( +ComponentInfo identifyConnectedComponent( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints, const VariableIndex &e_var_index, @@ -84,7 +84,7 @@ ComponentInfo IdentifyConnectedComponent( /* ************************************************************************* */ std::vector> -IdentifyConnectedComponents( +identifyConnectedComponents( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints) { @@ -99,7 +99,7 @@ IdentifyConnectedComponents( NonlinearInequalityConstraints::shared_ptr>> components; while (!keys.empty()) { - auto component_info = IdentifyConnectedComponent( + auto component_info = identifyConnectedComponent( e_constraints, i_constraints, e_var_index, i_var_index, *keys.begin()); for (const Key &key : component_info.keys) { keys.erase(key); @@ -124,14 +124,14 @@ IdentifyConnectedComponents( } /* ************************************************************************* */ -IEManifoldValues IEOptimizer::IdentifyManifolds( +IEManifoldValues IEOptimizer::identifyManifolds( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints, const Values &values, const IEConstraintManifold::Params::shared_ptr &iecm_params) { // Each thesis Eq. (4.33) connected component becomes one Eq. (4.34) // IEConstraintManifold with its own active set, basis, and tangent cone. - auto components = IdentifyConnectedComponents(e_constraints, i_constraints); + auto components = identifyConnectedComponents(e_constraints, i_constraints); IEManifoldValues ie_manifolds; for (const auto &[component_e_constraints, component_i_constraints] : components) { @@ -151,38 +151,38 @@ IEManifoldValues IEOptimizer::IdentifyManifolds( } /* ************************************************************************* */ -Values IEOptimizer::IdentifyUnconstrainedValues( +Values IEOptimizer::identifyUnconstrainedValues( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints, const Values &values) { - Values unconstrained_values; + Values unconstrainedValues; KeySet constrained_keys = e_constraints.keys(); constrained_keys.merge(i_constraints.keys()); for (const Key& key: values.keys()) { if (!constrained_keys.exists(key)) { - unconstrained_values.insert(key, values.at(key)); + unconstrainedValues.insert(key, values.at(key)); } } - return unconstrained_values; + return unconstrainedValues; } /* ************************************************************************* */ VectorValues -IEOptimizer::ComputeTangentVector(const IEManifoldValues &manifolds, +IEOptimizer::computeTangentVector(const IEManifoldValues &manifolds, const VectorValues &delta) { - VectorValues tangent_vector; + VectorValues tangentVector; for (const auto &it : manifolds) { const Key &key = it.first; const Vector &xi = delta.at(key); VectorValues tv = manifolds.at(key).eBasis()->computeTangentVector(xi); - tangent_vector.insert(tv); + tangentVector.insert(tv); } - return tangent_vector; + return tangentVector; } /* ************************************************************************* */ std::pair -IEOptimizer::ProjectTangentCone(const IEManifoldValues &manifolds, +IEOptimizer::projectTangentCone(const IEManifoldValues &manifolds, const VectorValues &v) { VectorValues proj_v; IndexSetMap active_indices_all; @@ -202,47 +202,47 @@ IEOptimizer::ProjectTangentCone(const IEManifoldValues &manifolds, /* ************************************************************************* */ IEManifoldValues -IEOptimizer::RetractManifolds(const IEManifoldValues &manifolds, +IEOptimizer::retractManifolds(const IEManifoldValues &manifolds, const VectorValues &delta) { - IEManifoldValues new_manifolds; + IEManifoldValues newManifolds; for (const auto &it : manifolds) { const Key &key = it.first; const Vector &xi = delta.at(key); - new_manifolds.emplace(key, it.second.retract(xi)); + newManifolds.emplace(key, it.second.retract(xi)); } - return new_manifolds; + return newManifolds; } /* ************************************************************************* */ -Values IEOptimizer::EManifolds(const IEManifoldValues &manifolds) { - Values e_manifolds; +Values IEOptimizer::equalityManifolds(const IEManifoldValues &manifolds) { + Values equalityManifolds; for (const auto &it : manifolds) { - e_manifolds.insert(it.first, it.second.eConstraintManifold()); + equalityManifolds.insert(it.first, it.second.eConstraintManifold()); } - return e_manifolds; + return equalityManifolds; } /* ************************************************************************* */ std::pair -IEOptimizer::EManifolds(const IEManifoldValues &manifolds, +IEOptimizer::equalityManifolds(const IEManifoldValues &manifolds, const IndexSetMap &active_indices) { - Values e_manifolds; + Values equalityManifolds; Values const_e_manifolds; for (const auto &it : manifolds) { const Key &key = it.first; ConstraintManifold e_manifold = it.second.eConstraintManifold(active_indices.at(key)); if (e_manifold.dim() > 0) { - e_manifolds.insert(key, e_manifold); + equalityManifolds.insert(key, e_manifold); } else { const_e_manifolds.insert(key, e_manifold); } } - return std::make_pair(e_manifolds, const_e_manifolds); + return std::make_pair(equalityManifolds, const_e_manifolds); } /* ************************************************************************* */ -bool IEOptimizer::IsSameMode(const IEManifoldValues &manifolds1, +bool IEOptimizer::isSameMode(const IEManifoldValues &manifolds1, const IEManifoldValues &manifolds2) { for (const auto &it : manifolds1) { const IndexSet &indices1 = it.second.activeIndices(); @@ -259,13 +259,13 @@ bool IEOptimizer::IsSameMode(const IEManifoldValues &manifolds1, /* ************************************************************************* */ IndexSetMap -IEOptimizer::IdentifyChangeIndices(const IEManifoldValues &manifolds, - const IEManifoldValues &new_manifolds) { +IEOptimizer::identifyChangeIndices(const IEManifoldValues &manifolds, + const IEManifoldValues &newManifolds) { IndexSetMap change_indices_map; for (const auto &it : manifolds) { const Key &key = it.first; const IndexSet &indices = it.second.activeIndices(); - const IndexSet &new_indices = new_manifolds.at(key).activeIndices(); + const IndexSet &new_indices = newManifolds.at(key).activeIndices(); IndexSet change_indices; for (const auto &idx : new_indices) { if (indices.find(idx) == indices.end()) { @@ -281,8 +281,8 @@ IEOptimizer::IdentifyChangeIndices(const IEManifoldValues &manifolds, /* ************************************************************************* */ IndexSetMap -IEOptimizer::IdentifyApproachingIndices(const IEManifoldValues &manifolds, - const IEManifoldValues &new_manifolds, +IEOptimizer::identifyApproachingIndices(const IEManifoldValues &manifolds, + const IEManifoldValues &newManifolds, const IndexSetMap &change_indices_map, const double& approach_rate_threshold) { IndexSetMap approach_indices_map; @@ -290,7 +290,7 @@ IEOptimizer::IdentifyApproachingIndices(const IEManifoldValues &manifolds, const Key &key = it.first; const IndexSet &change_indices = it.second; const IEConstraintManifold &manifold = manifolds.at(key); - const IEConstraintManifold &new_manifold = new_manifolds.at(key); + const IEConstraintManifold &new_manifold = newManifolds.at(key); auto i_constraints = manifolds.at(key).iConstraints(); double max_approach_rate = -1; @@ -321,7 +321,7 @@ IEOptimizer::IdentifyApproachingIndices(const IEManifoldValues &manifolds, } /* ************************************************************************* */ -std::string IEOptimizer::IndicesStr(const IndexSetMap &indices_map, +std::string IEOptimizer::indicesString(const IndexSetMap &indices_map, const KeyFormatter &keyFormatter) { std::string str; for (const auto &it : indices_map) { @@ -337,7 +337,7 @@ std::string IEOptimizer::IndicesStr(const IndexSetMap &indices_map, } /* ************************************************************************* */ -std::string IEOptimizer::IndicesStr(const IEManifoldValues &manifolds, +std::string IEOptimizer::indicesString(const IEManifoldValues &manifolds, const KeyFormatter &keyFormatter) { std::string str; for (const auto &it : manifolds) { diff --git a/gtdynamics/cmcopt/IEOptimizer.h b/gtdynamics/cmcopt/IEOptimizer.h index ded067632..38cb5f07d 100644 --- a/gtdynamics/cmcopt/IEOptimizer.h +++ b/gtdynamics/cmcopt/IEOptimizer.h @@ -47,27 +47,27 @@ class IEOptimizer { const Values &initial_values) const { // Split the original problem into IE manifold variables and free // variables before the concrete LM/GD solver takes over. - auto manifolds = IdentifyManifolds(e_constraints, i_constraints, + auto manifolds = identifyManifolds(e_constraints, i_constraints, initial_values, iecm_params_); - Values unconstrained_values = IdentifyUnconstrainedValues( + Values unconstrainedValues = identifyUnconstrainedValues( e_constraints, i_constraints, initial_values); - return optimizeManifolds(graph, manifolds, unconstrained_values); + return optimizeManifolds(graph, manifolds, unconstrainedValues); } /// Solve the reduced problem once the IE manifold blocks are identified. virtual Values optimizeManifolds(const NonlinearFactorGraph &graph, const IEManifoldValues &manifolds, - const Values &unconstrained_values) const = 0; + const Values &unconstrainedValues) const = 0; public: /// Map each original variable key to the connected-component manifold key /// that owns it. static std::map - Var2ManifoldKeyMap(const IEManifoldValues &manifolds); + varToManifoldKeyMap(const IEManifoldValues &manifolds); /// Build one IE manifold per constraint-connected component, following the /// thesis CMC component construction in Eqs. (4.33)-(4.34). - static IEManifoldValues IdentifyManifolds( + static IEManifoldValues identifyManifolds( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints, const Values &values, @@ -75,75 +75,75 @@ class IEOptimizer { /// Extract variables that do not participate in any equality or inequality /// component. - static Values IdentifyUnconstrainedValues( + static Values identifyUnconstrainedValues( const NonlinearEqualityConstraints &e_constraints, const NonlinearInequalityConstraints &i_constraints, const Values &values); /// Lift manifold coordinates xi back to ambient tangent vectors via the /// equality-manifold basis B_x. - static VectorValues ComputeTangentVector(const IEManifoldValues &manifolds, + static VectorValues computeTangentVector(const IEManifoldValues &manifolds, const VectorValues &delta); /// Project each component direction into its tangent cone, matching thesis /// Eq. (4.31) componentwise as in Eq. (4.42). /// Note: xi is given in the equality-only basis before cone projection. static std::pair - ProjectTangentCone(const IEManifoldValues &manifolds, const VectorValues &v); + projectTangentCone(const IEManifoldValues &manifolds, const VectorValues &v); /// Retract each manifold block independently back to the feasible set. - static IEManifoldValues RetractManifolds(const IEManifoldValues &manifolds, + static IEManifoldValues retractManifolds(const IEManifoldValues &manifolds, const VectorValues &delta); /// Drop the inequality state and expose only the equality manifolds. - static Values EManifolds(const IEManifoldValues &manifolds); + static Values equalityManifolds(const IEManifoldValues &manifolds); /// Treat selected active inequalities as temporary equalities, corresponding /// to the active-corner manifold in thesis Eq. (4.10). static std::pair - EManifolds(const IEManifoldValues &manifolds, + equalityManifolds(const IEManifoldValues &manifolds, const IndexSetMap &active_indices); /// Two iterates are in the same mode when their active sets agree on every /// IE manifold component; modes are the corners defined in thesis Eq. (4.9). - static bool IsSameMode(const IEManifoldValues &manifolds1, + static bool isSameMode(const IEManifoldValues &manifolds1, const IEManifoldValues &manifolds2); /// Return newly activated inequality indices, i.e. constraints that entered /// the active set from thesis Eq. (4.3) after a trial step. static IndexSetMap - IdentifyChangeIndices(const IEManifoldValues &manifolds, - const IEManifoldValues &new_manifolds); + identifyChangeIndices(const IEManifoldValues &manifolds, + const IEManifoldValues &newManifolds); /// From manifolds to new manifolds, check which boundaries are approached /// with rate larger than the threshold. If multiple boundaries are approached /// for a manifold, pick the boundary with max approach rate. static IndexSetMap - IdentifyApproachingIndices(const IEManifoldValues &manifolds, - const IEManifoldValues &new_manifolds, + identifyApproachingIndices(const IEManifoldValues &manifolds, + const IEManifoldValues &newManifolds, const IndexSetMap &change_indices_map, const double &approach_rate_threshold); static std::string - IndicesStr(const IndexSetMap &indices_map, + indicesString(const IndexSetMap &indices_map, const KeyFormatter &keyFormatter = DefaultKeyFormatter); static std::string - IndicesStr(const IEManifoldValues &manifolds, + indicesString(const IEManifoldValues &manifolds, const KeyFormatter &keyFormatter = DefaultKeyFormatter); typedef std::function - PrintValuesFunc; + PrintValuesFunction; typedef std::function - PrintDeltaFunc; + PrintDeltaFunction; template static void - PrintIterDetails(const IterDetails &iter_details, const size_t num_steps, + printIterationDetails(const IterDetails &iter_details, const size_t num_steps, bool print_values = false, - PrintValuesFunc print_values_func = NULL, - PrintDeltaFunc print_delta_func = NULL, + PrintValuesFunction print_values_func = NULL, + PrintDeltaFunction print_delta_func = NULL, const KeyFormatter &keyFormatter = DefaultKeyFormatter) { std::string red = "1;31"; std::string green = "1;32"; @@ -157,19 +157,19 @@ class IEOptimizer { std::cout << "\033[" + green + "merror: " << std::setprecision(4) << state.error << "\033[0m\n"; auto state_current_str = - IEOptimizer::IndicesStr(state.manifolds, keyFormatter); + IEOptimizer::indicesString(state.manifolds, keyFormatter); if (state_current_str.size() > 0) { std::cout << "current: " << state_current_str << "\n"; } auto state_grad_blocking_str = - IEOptimizer::IndicesStr(state.grad_blocking_indices_map, keyFormatter); + IEOptimizer::indicesString(state.gradientBlockingIndicesMap, keyFormatter); if (state_grad_blocking_str.size() > 0) { std::cout << "grad blocking: " << state_grad_blocking_str << "\n"; } for (const auto &it : state.manifolds) { - double i_error = it.second.evalIViolation(); - double e_error = it.second.evalEViolation(); + double i_error = it.second.evaluateInequalityViolation(); + double e_error = it.second.evaluateEqualityViolation(); if (e_error > 1e-5) { std::cout << "violating e: " << keyFormatter(it.first) << " " << e_error << "\n"; @@ -186,46 +186,46 @@ class IEOptimizer { std::cout << "gradient: \n"; print_delta_func( - IEOptimizer::ComputeTangentVector(state.manifolds, state.gradient), + IEOptimizer::computeTangentVector(state.manifolds, state.gradient), num_steps); } /// Print trials for (const auto &trial : iter_details.trials) { - std::string color = trial.step_is_successful ? red : blue; - std::cout << "\033[" + color + "mlambda: " << trial.linear_update.lambda + std::string color = trial.stepIsSuccessful ? red : blue; + std::cout << "\033[" + color + "mlambda: " << trial.linearUpdate.lambda << "\terror: " << state.error << " -> " - << trial.nonlinear_update.new_error - << "\tfidelity: " << trial.model_fidelity - << "\tlinear: " << trial.linear_update.cost_change - << "\tnonlinear: " << trial.nonlinear_update.cost_change + << trial.nonlinearUpdate.newError + << "\tfidelity: " << trial.modelFidelity + << "\tlinear: " << trial.linearUpdate.costChange + << "\tnonlinear: " << trial.nonlinearUpdate.costChange << "\033[0m\n"; - auto blocking_str = IEOptimizer::IndicesStr( - trial.linear_update.blocking_indices_map, keyFormatter); + auto blocking_str = IEOptimizer::indicesString( + trial.linearUpdate.blockingIndicesMap, keyFormatter); if (blocking_str.size() > 0) { std::cout << "blocking: " << blocking_str << "\n"; } auto forced_str = - IEOptimizer::IndicesStr(trial.forced_indices_map, keyFormatter); + IEOptimizer::indicesString(trial.forcedIndicesMap, keyFormatter); if (forced_str.size() > 0) { std::cout << "forced: " << forced_str << "\n"; } - auto new_str = IEOptimizer::IndicesStr( - trial.nonlinear_update.new_manifolds, keyFormatter); + auto new_str = IEOptimizer::indicesString( + trial.nonlinearUpdate.newManifolds, keyFormatter); if (new_str.size() > 0) { std::cout << "new: " << new_str << "\n"; } if (print_values) { - if (trial.linear_update.tangent_vector.size() > 0) { + if (trial.linearUpdate.tangentVector.size() > 0) { std::cout << "tangent vector: \n"; - print_delta_func(trial.linear_update.tangent_vector, num_steps); + print_delta_func(trial.linearUpdate.tangentVector, num_steps); } std::cout << "new values: \n"; print_values_func( - trial.nonlinear_update.new_manifolds.baseValues(), + trial.nonlinearUpdate.newManifolds.baseValues(), num_steps); } } diff --git a/gtdynamics/cmcopt/IERetractor.cpp b/gtdynamics/cmcopt/IERetractor.cpp index 762feb55e..e99fc69db 100644 --- a/gtdynamics/cmcopt/IERetractor.cpp +++ b/gtdynamics/cmcopt/IERetractor.cpp @@ -39,11 +39,11 @@ namespace { * Behavioral intent (compatibility): * - Preserve previous flow: solve a penalty-form constrained problem using the * provided cost graph and current equality/inequality constraints. - * - Keep LM settings controlled by `IERetractorParams::lm_params` by copying + * - Keep LM settings controlled by `IERetractorParams::lmParams` by copying * them into `PenaltyOptimizerParams::lmParams` right before solve. * * Notes for review: - * - This helper mutates `params->penalty_params->lmParams` intentionally so the + * - This helper mutates `params->penaltyParams->lmParams` intentionally so the * retractor-level LM tuning remains the single source of truth. * - It is `namespace {}` local on purpose: this is a file-scoped adapter layer, * not reusable public API. @@ -54,10 +54,10 @@ Values RunPenaltyOptimization( const NonlinearInequalityConstraints &i_constraints, const Values &initial_values, const IERetractorParams::shared_ptr ¶ms) { - auto penalty_params = params->penalty_params; - penalty_params->lmParams = params->lm_params; + auto penaltyParams = params->penaltyParams; + penaltyParams->lmParams = params->lmParams; ConstrainedOptProblem problem(cost, e_constraints, i_constraints); - PenaltyOptimizer optimizer(problem, initial_values, penalty_params); + PenaltyOptimizer optimizer(problem, initial_values, penaltyParams); return optimizer.optimize(); } @@ -68,7 +68,7 @@ Values RunPenaltyOptimization( IEConstraintManifold IERetractor::moveToBoundary(const IEConstraintManifold *manifold, const IndexSet &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { VectorValues delta = manifold->values().zeroVectors(); return retract(manifold, delta, blocking_indices, retract_info); } @@ -77,7 +77,7 @@ IERetractor::moveToBoundary(const IEConstraintManifold *manifold, IEConstraintManifold BarrierRetractor::moveToBoundary(const IEConstraintManifold *manifold, const IndexSet &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { // Turning blocking inequalities into equalities picks the boundary face that // the next mode should land on. @@ -96,15 +96,15 @@ BarrierRetractor::moveToBoundary(const IEConstraintManifold *manifold, merit_graph.add(manifold->eConstraints()->penaltyGraph(1.0)); merit_graph.add(manifold->iConstraints()->penaltyGraph(1.0)); LevenbergMarquardtOptimizer optimizer_np(merit_graph, opt_values, - params_->lm_params); + params_->lmParams); Values result = optimizer_np.optimize(); - if (params_->check_feasible) { + if (params_->checkFeasible) { size_t k = DynamicsSymbol(manifold->values().keys().front()).time(); std::string k_string = "(" + std::to_string(k) + ")"; if (!CheckFeasible(merit_graph, result, "penalty move to boundary" + k_string, - params_->feasible_threshold)) { + params_->feasibleThreshold)) { } } @@ -116,7 +116,7 @@ IEConstraintManifold BarrierRetractor::retract(const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { // The prior graph is the quadratic stay-close term centered at x + delta. // This implements the metric-projection objective in thesis Eq. (4.26), and @@ -140,7 +140,7 @@ BarrierRetractor::retract(const IEConstraintManifold *manifold, // First solve the soft penalty subproblem, then recover the hard active set // from the resulting feasible-or-nearly-feasible point. const Values &init_values = - params_->init_values_as_x ? manifold->values() : new_values; + params_->initValuesAsX ? manifold->values() : new_values; Values opt_values = RunPenaltyOptimization( prior_graph, e_constraints, i_constraints, init_values, params_); @@ -167,16 +167,16 @@ BarrierRetractor::retract(const IEConstraintManifold *manifold, NonlinearFactorGraph graph_np = active_constraints.penaltyGraph(1.0); graph_np.add(i_constraints.penaltyGraph(1.0)); LevenbergMarquardtOptimizer optimizer_np(graph_np, opt_values, - params_->lm_params); + params_->lmParams); Values result = optimizer_np.optimize(); // check and ensure feasible - if (params_->check_feasible) { + if (params_->checkFeasible) { size_t k = DynamicsSymbol(manifold->values().keys().front()).time(); std::string k_string = "(" + std::to_string(k) + ")"; if (!CheckFeasible(graph_np, result, "penalty retraction" + k_string, - params_->feasible_threshold)) { - if (params_->ensure_feasible) { + params_->feasibleThreshold)) { + if (params_->ensureFeasible) { return retract(manifold, 0.7 * delta, blocking_indices, retract_info); } } @@ -184,7 +184,7 @@ BarrierRetractor::retract(const IEConstraintManifold *manifold, // record retraction info if (retract_info) { - // retract_info->num_lm_iters = + // retract_info->numLMIterations = // opt_info.num_iters.back() + optimizer_np.iterations(); // TODO } @@ -256,7 +256,7 @@ KinodynamicHierarchicalRetractor::KinodynamicHierarchicalRetractor( IEConstraintManifold KinodynamicHierarchicalRetractor::retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { size_t k = DynamicsSymbol(manifold->values().keys().front()).time(); std::string k_string = "(" + std::to_string(k) + ")"; @@ -274,7 +274,7 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( params_->addPriors(new_values, basis_q_keys_, graph_wp_q); Values init_values_q = SubValues(values, graph_wp_q.keys()); LevenbergMarquardtOptimizer optimizer_wp_q(graph_wp_q, init_values_q, - params_->lm_params); + params_->lmParams); Values results_q = optimizer_wp_q.optimize(); // solve q level without priors @@ -286,12 +286,12 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( } } LevenbergMarquardtOptimizer optimizer_np_q(graph_np_q, results_q, - params_->lm_params); + params_->lmParams); Values new_results_q = optimizer_np_q.optimize(); known_values.insert(new_results_q); - if (params_->check_feasible) { + if (params_->checkFeasible) { if (!CheckFeasible(graph_np_q, new_results_q, "q-level" + k_string, - params_->feasible_threshold)) { + params_->feasibleThreshold)) { failed = true; } } @@ -305,7 +305,7 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( params_->addPriors(new_values, basis_v_keys_, graph_wp_v); Values init_values_v = SubValues(values, graph_wp_v.keys()); LevenbergMarquardtOptimizer optimizer_wp_v(graph_wp_v, init_values_v, - params_->lm_params); + params_->lmParams); Values results_v = optimizer_wp_v.optimize(); // solve v level without priors @@ -318,12 +318,12 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( } } LevenbergMarquardtOptimizer optimizer_np_v(graph_np_v, results_v, - params_->lm_params); + params_->lmParams); Values new_results_v = optimizer_np_v.optimize(); known_values.insert(new_results_v); - if (params_->check_feasible) { + if (params_->checkFeasible) { if (!CheckFeasible(graph_np_v, new_results_v, "v-level" + k_string, - params_->feasible_threshold)) { + params_->feasibleThreshold)) { failed = true; } } @@ -337,7 +337,7 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( params_->addPriors(new_values, basis_ad_keys_, graph_wp_ad); Values init_values_ad = SubValues(values, graph_wp_ad.keys()); LevenbergMarquardtOptimizer optimizer_wp_ad(graph_wp_ad, init_values_ad, - params_->lm_params); + params_->lmParams); Values results_ad = optimizer_wp_ad.optimize(); // solve a and dynamics level without priors @@ -350,36 +350,36 @@ IEConstraintManifold KinodynamicHierarchicalRetractor::retract( } LevenbergMarquardtOptimizer optimizer_np_ad(graph_np_ad, results_ad, - params_->lm_params); + params_->lmParams); Values new_results_ad = optimizer_np_ad.optimize(); known_values.insert(new_results_ad); - if (params_->check_feasible) { + if (params_->checkFeasible) { if (!CheckFeasible(graph_np_ad, new_results_ad, "ad-level" + k_string, - params_->feasible_threshold)) { + params_->feasibleThreshold)) { failed = true; } } if (retract_info) { - retract_info->num_lm_iters = optimizer_wp_q.iterations(); - retract_info->num_lm_iters += optimizer_np_q.iterations(); - retract_info->num_lm_iters += optimizer_wp_v.iterations(); - retract_info->num_lm_iters += optimizer_np_v.iterations(); - retract_info->num_lm_iters += optimizer_wp_ad.iterations(); - retract_info->num_lm_iters += optimizer_np_ad.iterations(); + retract_info->numLMIterations = optimizer_wp_q.iterations(); + retract_info->numLMIterations += optimizer_np_q.iterations(); + retract_info->numLMIterations += optimizer_wp_v.iterations(); + retract_info->numLMIterations += optimizer_np_v.iterations(); + retract_info->numLMIterations += optimizer_wp_ad.iterations(); + retract_info->numLMIterations += optimizer_np_ad.iterations(); } // solve a and dynamics level without priors if (failed) { LevenbergMarquardtOptimizer optimizer_all(merit_graph_, known_values, - params_->lm_params); + params_->lmParams); known_values = optimizer_all.optimize(); if (retract_info) { - retract_info->num_lm_iters += optimizer_all.iterations(); + retract_info->numLMIterations += optimizer_all.iterations(); } if (!CheckFeasible(merit_graph_, known_values, "all-levels" + k_string, - params_->feasible_threshold)) { - if (params_->ensure_feasible) { + params_->feasibleThreshold)) { + if (params_->ensureFeasible) { return *manifold; } } diff --git a/gtdynamics/cmcopt/IERetractor.h b/gtdynamics/cmcopt/IERetractor.h index 42d5f1ea7..834da3f08 100644 --- a/gtdynamics/cmcopt/IERetractor.h +++ b/gtdynamics/cmcopt/IERetractor.h @@ -30,40 +30,40 @@ class IEConstraintManifold; struct IERetractorParams { using shared_ptr = std::shared_ptr; - LevenbergMarquardtParams lm_params = LevenbergMarquardtParams(); - double prior_sigma = 1.0; - bool use_varying_sigma = false; - bool scale_varying_sigma = false; - std::shared_ptr metric_sigmas = NULL; - bool init_values_as_x = true; // - bool check_feasible = true; - double feasible_threshold = 1e-3; - bool ensure_feasible = false; - PenaltyOptimizerParams::shared_ptr penalty_params = + LevenbergMarquardtParams lmParams = LevenbergMarquardtParams(); + double priorSigma = 1.0; + bool useVaryingSigma = false; + bool scaleVaryingSigma = false; + std::shared_ptr metricSigmas = NULL; + bool initValuesAsX = true; // + bool checkFeasible = true; + double feasibleThreshold = 1e-3; + bool ensureFeasible = false; + PenaltyOptimizerParams::shared_ptr penaltyParams = std::make_shared(); IERetractorParams() = default; /** Constructor using the same sigma. */ - IERetractorParams(const LevenbergMarquardtParams &_lm_params, - const double &_prior_sigma) - : lm_params(_lm_params), prior_sigma(_prior_sigma), - use_varying_sigma(false), metric_sigmas(NULL) { - penalty_params->lmParams = lm_params; + IERetractorParams(const LevenbergMarquardtParams &_lmParams, + const double &_priorSigma) + : lmParams(_lmParams), priorSigma(_priorSigma), + useVaryingSigma(false), metricSigmas(NULL) { + penaltyParams->lmParams = lmParams; } /** Constructor using varying sigmas. */ - IERetractorParams(const LevenbergMarquardtParams &_lm_params, + IERetractorParams(const LevenbergMarquardtParams &_lmParams, const std::shared_ptr &_metric_sigmas) - : lm_params(_lm_params), prior_sigma(0.0), use_varying_sigma(true), - metric_sigmas(_metric_sigmas) { - penalty_params->lmParams = lm_params; + : lmParams(_lmParams), priorSigma(0.0), useVaryingSigma(true), + metricSigmas(_metric_sigmas) { + penaltyParams->lmParams = lmParams; } - static IERetractorParams::shared_ptr VarySigmas( - const LevenbergMarquardtParams &_lm_param = LevenbergMarquardtParams()) { + static IERetractorParams::shared_ptr createVaryingSigmas( + const LevenbergMarquardtParams &_lmParam = LevenbergMarquardtParams()) { return std::make_shared( - _lm_param, std::make_shared()); + _lmParam, std::make_shared()); } /// Add the quadratic prior term that pulls the retraction solution toward @@ -71,20 +71,20 @@ struct IERetractorParams { template void addPriors(const Values &values, const CONTAINER &keys, NonlinearFactorGraph &graph) const { - if (use_varying_sigma) { - if (scale_varying_sigma) { - AddGeneralPriors(values, keys, *metric_sigmas, graph, prior_sigma); + if (useVaryingSigma) { + if (scaleVaryingSigma) { + AddGeneralPriors(values, keys, *metricSigmas, graph, priorSigma); } else { - AddGeneralPriors(values, keys, *metric_sigmas, graph); + AddGeneralPriors(values, keys, *metricSigmas, graph); } } else { - AddGeneralPriors(values, keys, prior_sigma, graph); + AddGeneralPriors(values, keys, priorSigma, graph); } } }; -struct IERetractInfo { - size_t num_lm_iters = 0; +struct IERetractionInfo { + size_t numLMIterations = 0; }; /// Base class for the feasibility-restoring map R_x(delta) used after each @@ -109,13 +109,13 @@ class IERetractor { virtual IEConstraintManifold retract(const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const = 0; + IERetractionInfo *retract_info = nullptr) const = 0; /// Zero-step retraction that lands directly on the requested boundary face. virtual IEConstraintManifold moveToBoundary(const IEConstraintManifold *manifold, const IndexSet &blocking_indices, - IERetractInfo *retract_info = nullptr) const; + IERetractionInfo *retract_info = nullptr) const; const IERetractorParams::shared_ptr ¶ms() const { return params_; } }; @@ -143,14 +143,14 @@ class BarrierRetractor : public IERetractor { IEConstraintManifold retract(const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; /// Solve the special case delta = 0 while forcing the requested inequalities /// to become active. IEConstraintManifold moveToBoundary(const IEConstraintManifold *manifold, const IndexSet &blocking_indices, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; }; /** @@ -183,7 +183,7 @@ class KinodynamicHierarchicalRetractor : public IERetractor { IEConstraintManifold retract(const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; }; /* ************************************************************************* */ diff --git a/gtdynamics/cmcopt/README.md b/gtdynamics/cmcopt/README.md index 764e01670..8b3dc440d 100644 --- a/gtdynamics/cmcopt/README.md +++ b/gtdynamics/cmcopt/README.md @@ -35,7 +35,7 @@ Pointers: - [IEOptimizer](IEOptimizer.h#L31) - [IEOptimizer::optimize](IEOptimizer.h#L43) -- [IEOptimizer::IdentifyUnconstrainedValues](IEOptimizer.h#L78) +- [IEOptimizer::identifyUnconstrainedValues](IEOptimizer.h#L78) The public `optimize(...)` entry point builds inequality/equality manifolds from the constraints, keeps unconstrained variables separate, and then hands the transformed problem to either the LM or GD variant. @@ -53,11 +53,11 @@ This is the local component version of the CMC state space in thesis Eq. (4.2), Pointers: -- [IdentifyConnectedComponents](IEOptimizer.cpp#L87) -- [IEOptimizer::IdentifyManifolds](IEOptimizer.cpp#L127) +- [identifyConnectedComponents](IEOptimizer.cpp#L87) +- [IEOptimizer::identifyManifolds](IEOptimizer.cpp#L127) - [IEConstraintManifold](IEConstraintManifold.h#L25) -`IEOptimizer::IdentifyManifolds` builds one [IEConstraintManifold](IEConstraintManifold.h#L25) per connected component. That object is the central mathematical container for the local manifold-with-boundary. +`IEOptimizer::identifyManifolds` builds one [IEConstraintManifold](IEConstraintManifold.h#L25) per connected component. That object is the central mathematical container for the local manifold-with-boundary. ## 3) IEConstraintManifold: Equality Basis + Active Set + Cone @@ -78,8 +78,8 @@ Pointers: - [IEConstraintManifold](IEConstraintManifold.h#L25) - [IEConstraintManifold::activeIndices](IEConstraintManifold.h#L98) -- [IEConstraintManifold::IdentifyActiveConstraints](IEConstraintManifold.cpp#L181) -- [IEConstraintManifold::ConstructTangentCone](IEConstraintManifold.cpp#L205) +- [IEConstraintManifold::identifyActiveConstraints](IEConstraintManifold.cpp#L181) +- [IEConstraintManifold::constructTangentCone](IEConstraintManifold.cpp#L205) - [TangentCone](TangentCone.h#L25) Implementation detail: @@ -103,7 +103,7 @@ In code, active inequalities are either: Pointers: -- [IEConstraintManifold::IdentifyActiveConstraints](IEConstraintManifold.cpp#L181) +- [IEConstraintManifold::identifyActiveConstraints](IEConstraintManifold.cpp#L181) - [IEConstraintManifold constructor](IEConstraintManifold.h#L55) - [IEConstraintManifold::createWithNewValues](IEConstraintManifold.h#L87) @@ -123,9 +123,9 @@ where $B_x$ is the equality tangent basis and $Dg_{\mathcal{A}}(x)$ is the Jacob Pointers: -- [IEConstraintManifold::ConstructTangentCone](IEConstraintManifold.cpp#L205) -- [IEConstraintManifold::linearActiveManIConstraints](IEConstraintManifold.cpp#L235) -- [IEConstraintManifold::linearActiveBaseIConstraints](IEConstraintManifold.cpp#L263) +- [IEConstraintManifold::constructTangentCone](IEConstraintManifold.cpp#L205) +- [IEConstraintManifold::linearActiveManifoldInequalityConstraints](IEConstraintManifold.cpp#L235) +- [IEConstraintManifold::linearActiveBaseInequalityConstraints](IEConstraintManifold.cpp#L263) This is exactly where the code turns active nonlinear inequalities into linearized cone constraints in either manifold coordinates or base-variable coordinates. @@ -144,9 +144,9 @@ Pointers: - [TangentCone::project](TangentCone.cpp#L23) - [IEConstraintManifold::projectTangentCone](IEConstraintManifold.cpp#L79) -- [IEOptimizer::ProjectTangentCone](IEOptimizer.cpp#L185) +- [IEOptimizer::projectTangentCone](IEOptimizer.cpp#L185) -`TangentCone::project` solves the convex IQP. `IEConstraintManifold::projectTangentCone` maps the cone-local blocking set back to original inequality indices. `IEOptimizer::ProjectTangentCone` applies that componentwise across the whole problem. +`TangentCone::project` solves the convex IQP. `IEConstraintManifold::projectTangentCone` maps the cone-local blocking set back to original inequality indices. `IEOptimizer::projectTangentCone` applies that componentwise across the whole problem. Thesis reference: Eq. (4.31) projects a descent direction into the cone for one CMC. Eq. (4.42) applies the same projection componentwise across multiple CMCs. The code generalizes this projection to any trial vector, not only a gradient vector. @@ -159,7 +159,7 @@ Mathematically, these are the active inequalities whose directional derivative w Pointers: - [IEConstraintManifold::blockingIndices](IEConstraintManifold.cpp#L121) -- [IELMState::grad_blocking_indices_map](IELMOptimizerState.h#L46) +- [IELMState::gradientBlockingIndicesMap](IELMOptimizerState.h#L46) - [IELMState::computeGradient](IELMOptimizerState.cpp#L116) Those blocking sets are later passed into the retraction, where they are enforced as equalities. @@ -183,14 +183,14 @@ Pointers: - [IELMState](IELMOptimizerState.h#L35) - [IELMState::linearizeIConstraints](IELMOptimizerState.cpp#L100) - [IELMTrial::LinearUpdate](IELMOptimizerState.cpp#L493) -- [IELMTrial::LinearUpdate::InitEstimate](IELMOptimizerState.cpp#L555) -- [IELMTrial::LinearUpdate::CheckSolutionValid](IELMOptimizerState.cpp#L600) +- [IELMTrial::LinearUpdate::initialEstimate](IELMOptimizerState.cpp#L555) +- [IELMTrial::LinearUpdate::checkSolutionValid](IELMOptimizerState.cpp#L600) How the code matches the math: - `linearizeIConstraints()` builds the linearized inequality constraints at the current state. - `LinearUpdate(...)` builds the damped LM system. -- `InitEstimate(...)` seeds the IQP with blocking constraints coming from the negative gradient. +- `initialEstimate(...)` seeds the IQP with blocking constraints coming from the negative gradient. - `SolveConvexIQP(...)` is then used to refine the constrained step. Thesis reference: Eq. (4.44) is the CMC Levenberg-Marquardt quadratic subproblem, constrained by the cone coordinates for each CMC. @@ -229,9 +229,9 @@ Mathematically, this is a discrete mode change between different active sets. Pointers: -- [IEOptimizer::IsSameMode](IEOptimizer.cpp#L245) -- [IEOptimizer::IdentifyChangeIndices](IEOptimizer.h#L115) -- [IEOptimizer::IdentifyApproachingIndices](IEOptimizer.cpp#L284) +- [IEOptimizer::isSameMode](IEOptimizer.cpp#L245) +- [IEOptimizer::identifyChangeIndices](IEOptimizer.h#L115) +- [IEOptimizer::identifyApproachingIndices](IEOptimizer.cpp#L284) - [IELMOptimizer::checkModeChange](IELMOptimizer.cpp#L133) - [IEConstraintManifold::moveToBoundary](IEConstraintManifold.cpp#L146) - [IEManifoldValues::moveToBoundaries](IEConstraintManifoldUtils.cpp#L25) @@ -256,7 +256,7 @@ Pointers: - [IELMOptimizer::optimizeManifolds](IELMOptimizer.cpp#L31) - [IELMOptimizer::iterate](IELMOptimizer.cpp#L73) - [IELMOptimizer::checkModeChange](IELMOptimizer.cpp#L133) -- [IELMState::FromLastIteration](IELMOptimizerState.cpp#L36) +- [IELMState::fromLastIteration](IELMOptimizerState.cpp#L36) - [IELMOptimizer::checkConvergence](IELMOptimizer.cpp#L217) Thesis reference: this is the implementation counterpart of Algorithm 3, with the linear subproblem from Eq. (4.44), retraction from Eq. (4.46), and model fidelity from Eq. (3.53). @@ -292,7 +292,7 @@ Some CMC-Opt runs use varying metric sigmas when building the retraction priors. Pointers: - [IERetractorParams](IERetractor.h#L31) -- [IERetractorParams::VarySigmas](IERetractor.h#L63) +- [IERetractorParams::createVaryingSigmas](IERetractor.h#L63) - [IELMState::computeMetricSigmas](IELMOptimizerState.cpp#L148) Thesis reference: metric retractions are written generically in Eq. (5.12), with the cost-aware metric idea in Eqs. (5.18), (5.20), and (5.21). For CMCs, the projected gradient under the induced metric is Eq. (5.4). diff --git a/gtdynamics/cmcopt/TangentCone.cpp b/gtdynamics/cmcopt/TangentCone.cpp index 06f94b4a0..7fe482aea 100644 --- a/gtdynamics/cmcopt/TangentCone.cpp +++ b/gtdynamics/cmcopt/TangentCone.cpp @@ -38,9 +38,9 @@ std::pair TangentCone::project(const Vector &xi) const { VectorValues init_values; init_values.insert(x_key, Vector::Zero(dim)); - auto [values, active_indices, num_solves, solve_successful] = + auto [values, active_indices, numSolves, solveSuccessful] = SolveConvexIQP(graph, constraints_, init_active_indices, init_values); - if (!solve_successful) { + if (!solveSuccessful) { std::cout << "solve failed in project T-cone.\n"; return {init_active_indices, init_values.at(x_key)}; } diff --git a/gtdynamics/cmopt/ConstraintManifold.cpp b/gtdynamics/cmopt/ConstraintManifold.cpp index bf0b20a4e..364f29604 100644 --- a/gtdynamics/cmopt/ConstraintManifold.cpp +++ b/gtdynamics/cmopt/ConstraintManifold.cpp @@ -21,9 +21,9 @@ namespace gtdynamics { /* ************************************************************************* */ Values ConstraintManifold::constructValues( - const gtsam::Values &values, - const Retractor::shared_ptr &retractor, bool retract_init) { - if (retract_init) { + const gtsam::Values &values, const Retractor::shared_ptr &retractor, + bool retractInitialValues) { + if (retractInitialValues) { return retractor->retractConstraints(std::move(values)); } else { return values; @@ -52,12 +52,9 @@ ConstraintManifold ConstraintManifold::retract(const gtsam::Vector &xi, Values new_values = retractor_->retract(values_, delta); // Set jacobian as 0 since they are not used for optimization. - if (H1) + if (H1 || H2) throw std::runtime_error( - "ConstraintManifold retract jacobian not implemented."); - if (H2) - throw std::runtime_error( - "ConstraintManifold retract jacobian not implemented."); + "ConstraintManifold retract jacobians not implemented."); // Satisfy the constraints in the connected component. return createWithNewValues(new_values); @@ -71,12 +68,9 @@ gtsam::Vector ConstraintManifold::localCoordinates(const ConstraintManifold &g, Vector xi = basis_->localCoordinates(values_, g.values_); // Set jacobian as 0 since they are not used for optimization. - if (H1) - throw std::runtime_error( - "ConstraintManifold localCoordinates jacobian not implemented."); - if (H2) + if (H1 || H2) throw std::runtime_error( - "ConstraintManifold localCoordinates jacobian not implemented."); + "ConstraintManifold localCoordinates jacobians not implemented."); return xi; } @@ -95,7 +89,8 @@ bool ConstraintManifold::equals(const ConstraintManifold &other, /* ************************************************************************* */ const Values ConstraintManifold::feasibleValues() const { - gtsam::LevenbergMarquardtOptimizer optimizer(constraints_->penaltyGraph(), values_); + gtsam::LevenbergMarquardtOptimizer optimizer(constraints_->penaltyGraph(), + values_); return optimizer.optimize(); } @@ -118,14 +113,14 @@ KeyVector EManifoldValues::keys() const { } /* ************************************************************************* */ -VectorValues -EManifoldValues::computeTangentVector(const VectorValues &delta) const { - VectorValues tangent_vector; +VectorValues EManifoldValues::computeTangentVector( + const VectorValues &delta) const { + VectorValues tangentVector; for (const auto &it : *this) { - tangent_vector.insert( + tangentVector.insert( it.second.basis()->computeTangentVector(delta.at(it.first))); } - return tangent_vector; + return tangentVector; } /* ************************************************************************* */ @@ -146,4 +141,4 @@ std::map EManifoldValues::dims() const { return dims_map; } -} // namespace gtdynamics +} // namespace gtdynamics diff --git a/gtdynamics/cmopt/ConstraintManifold.h b/gtdynamics/cmopt/ConstraintManifold.h index f5760c51b..cd10ff6c0 100644 --- a/gtdynamics/cmopt/ConstraintManifold.h +++ b/gtdynamics/cmopt/ConstraintManifold.h @@ -14,7 +14,7 @@ #pragma once #include -#include +#include #include #include #include @@ -49,7 +49,7 @@ using gtsam::NonlinearEqualityConstraints; * optimization problems. * * The manifold dimension is `embedding_dim - constraint_dim`, and tangent - * space mapping is delegated to `TspaceBasis` while feasibility projection is + * space mapping is delegated to `TangentSpaceBasis` while feasibility projection is * delegated to `Retractor`. * * @see README.md#constraint-manifold @@ -61,21 +61,21 @@ class ConstraintManifold { /** * Parameters that define basis construction and retraction behavior. * - * `basis_creator` controls tangent-space parameterization and - * `retractor_creator` controls how feasibility is enforced after updates. + * `basisCreator` controls tangent-space parameterization and + * `retractorCreator` controls how feasibility is enforced after updates. * * @see README.md#tangent-basis * @see README.md#retraction */ struct Params { using shared_ptr = std::shared_ptr; - TspaceBasisCreator::shared_ptr basis_creator; - RetractorCreator::shared_ptr retractor_creator; + TangentSpaceBasisCreator::shared_ptr basisCreator; + RetractorCreator::shared_ptr retractorCreator; /** Default constructor. */ Params() - : basis_creator(std::make_shared()), - retractor_creator(std::make_shared()) {} + : basisCreator(std::make_shared()), + retractorCreator(std::make_shared()) {} }; protected: @@ -86,7 +86,7 @@ class ConstraintManifold { size_t embedding_dim_; // dimension of embedding space size_t constraint_dim_; // dimension of constraints size_t dim_; // dimension of constraint manifold - TspaceBasis::shared_ptr basis_; // tangent space basis + TangentSpaceBasis::shared_ptr basis_; // tangent space basis public: enum { dimension = Eigen::Dynamic }; @@ -98,7 +98,7 @@ class ConstraintManifold { * @param constraints Equality constraints defining the component. * @param values Initial values of variables in the connected component. * @param params Parameters controlling basis and retraction behavior. - * @param retract_init If true, retract values to satisfy constraints at + * @param retractInitialValues If true, retract values to satisfy constraints at * construction. * @param basis Optional pre-built tangent basis. */ @@ -106,19 +106,19 @@ class ConstraintManifold { const NonlinearEqualityConstraints::shared_ptr constraints, const gtsam::Values &values, const Params::shared_ptr ¶ms = std::make_shared(), - bool retract_init = true, - std::optional basis = {}) + bool retractInitialValues = true, + std::optional basis = {}) : params_(params), constraints_(constraints), - retractor_(params->retractor_creator->create(constraints_)), - values_(constructValues(values, retractor_, retract_init)), + retractor_(params->retractorCreator->create(constraints_)), + values_(constructValues(values, retractor_, retractInitialValues)), embedding_dim_(values_.dim()), constraint_dim_(constraints_->dim()), dim_(embedding_dim_ > constraint_dim_ ? embedding_dim_ - constraint_dim_ : 0), basis_(basis ? *basis : (dim_ == 0 ? createFixedBasis(constraints_, values_) - : params->basis_creator->create(constraints_, + : params->basisCreator->create(constraints_, values_))) {} /** @@ -211,7 +211,7 @@ class ConstraintManifold { bool equals(const ConstraintManifold &other, double tol = 1e-8) const; /// Return the basis of the tangent space. - const TspaceBasis::shared_ptr &basis() const { return basis_; } + const TangentSpaceBasis::shared_ptr &basis() const { return basis_; } /// Return the retractor. const Retractor::shared_ptr &retractor() const { return retractor_; } @@ -220,11 +220,11 @@ class ConstraintManifold { const Values feasibleValues() const; protected: - static TspaceBasis::shared_ptr createFixedBasis( + static TangentSpaceBasis::shared_ptr createFixedBasis( const NonlinearEqualityConstraints::shared_ptr &constraints, const gtsam::Values &values) { - auto basis_params = std::make_shared(); - basis_params->always_construct_basis = false; + auto basis_params = std::make_shared(); + basis_params->alwaysConstructBasis = false; return std::make_shared(constraints, values, basis_params); } @@ -233,18 +233,18 @@ class ConstraintManifold { * Initialize values for the manifold state. * @param values Candidate values of variables in the connected component. * @param retractor Retraction object used for feasibility projection. - * @param retract_init If true, perform constraint retraction. + * @param retractInitialValues If true, perform constraint retraction. * @return Initialized values, optionally retracted to feasibility. */ static Values constructValues(const gtsam::Values &values, const Retractor::shared_ptr &retractor, - bool retract_init); + bool retractInitialValues); /// Make sure the tangent space basis is constructed. /// NOTE: The static mutex creates a global synchronization point across all /// ConstraintManifold instances. This is a known limitation that could cause /// unnecessary contention. A better design would move the mutex to the - /// TspaceBasis class itself. + /// TangentSpaceBasis class itself. void makeSureBasisConstructed() const { if (!basis_->isConstructed()) { static std::mutex basis_mutex; diff --git a/gtdynamics/cmopt/LMManifoldOptimizer.cpp b/gtdynamics/cmopt/LMManifoldOptimizer.cpp index 32a986608..a65f8d6c0 100644 --- a/gtdynamics/cmopt/LMManifoldOptimizer.cpp +++ b/gtdynamics/cmopt/LMManifoldOptimizer.cpp @@ -44,13 +44,13 @@ Values LMManifoldOptimizer::optimize( const NonlinearFactorGraph &costs, const NonlinearEqualityConstraints &constraints, const Values &init_values) const { - auto mopt_problem = initializeMoptProblem(costs, constraints, init_values); + auto mopt_problem = initializeManifoldOptimizationProblem(costs, constraints, init_values); return optimize(costs, mopt_problem); } /* ************************************************************************* */ Values LMManifoldOptimizer::optimize( - const NonlinearFactorGraph &graph, const ManifoldOptProblem &mopt_problem) const { + const NonlinearFactorGraph &graph, const ManifoldOptimizationProblem &mopt_problem) const { // Construct initial state LMState state(graph, mopt_problem, params_.lambdaInitial, params_.lambdaFactor, 0); @@ -64,14 +64,14 @@ Values LMManifoldOptimizer::optimize( // Iterative loop if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { std::cout << "Initial error: " << state.error << "\n"; - LMTrial::PrintTitle(); + LMTrial::printTitle(); } LMState prev_state; do { prev_state = state; - LMIterDetails iter_details = iterate(graph, mopt_problem.graph_, state); - state = LMState::FromLastIteration(iter_details, graph, params_); + LMIterationDetails iter_details = iterate(graph, mopt_problem.graph, state); + state = LMState::fromLastIteration(iter_details, graph, params_); details_->push_back(iter_details); } while (state.iterations < params_.maxIterations && !checkConvergence(prev_state, state) && @@ -81,15 +81,15 @@ Values LMManifoldOptimizer::optimize( } /* ************************************************************************* */ -LMIterDetails +LMIterationDetails LMManifoldOptimizer::iterate(const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, const LMState &state) const { - LMIterDetails iter_details(state); + LMIterationDetails iter_details(state); // Set lambda for first trial. double lambda = state.lambda; - double lambda_factor = state.lambda_factor; + double lambdaFactor = state.lambdaFactor; // Perform trials until any of follwing conditions is met // * 1) trial is successful @@ -104,22 +104,22 @@ LMManifoldOptimizer::iterate(const NonlinearFactorGraph &graph, iter_details.trials.emplace_back(trial); // Check condition 1. - if (trial.step_is_successful) { + if (trial.stepIsSuccessful) { break; } // Check condition 2. - if (trial.linear_update.solve_successful) { + if (trial.linearUpdate.solveSuccessful) { double abs_change_tol = std::max(params_.absoluteErrorTol, params_.relativeErrorTol * state.error); - if (trial.linear_update.cost_change < abs_change_tol && - trial.nonlinear_update.cost_change < abs_change_tol) { + if (trial.linearUpdate.costChange < abs_change_tol && + trial.nonlinearUpdate.costChange < abs_change_tol) { break; } } // Set lambda for next trial. - trial.setNextLambda(lambda, lambda_factor, params_); + trial.setNextLambda(lambda, lambdaFactor, params_); // Check condition 3. if (!checkLambdaWithinLimits(lambda)) { diff --git a/gtdynamics/cmopt/LMManifoldOptimizer.h b/gtdynamics/cmopt/LMManifoldOptimizer.h index c271e85e0..1f19be80e 100644 --- a/gtdynamics/cmopt/LMManifoldOptimizer.h +++ b/gtdynamics/cmopt/LMManifoldOptimizer.h @@ -42,7 +42,7 @@ using gtsam::Values; * `LMManifoldOptimizerState`. * * It is the explicit LM implementation path in `cmopt`, complementary to the - * generic `NonlinearMOptimizer` wrapper around standard GTSAM optimizers. + * generic `NonlinearManifoldOptimizer` wrapper around standard GTSAM optimizers. * * @see README.md#solvers * @see ManifoldOptimizer @@ -52,12 +52,12 @@ using gtsam::Values; class LMManifoldOptimizer : public ManifoldOptimizer { protected: const LevenbergMarquardtParams params_; ///< LM parameters - std::shared_ptr details_; + std::shared_ptr details_; public: typedef std::shared_ptr shared_ptr; - const LMItersDetails &details() const { return *details_; } + const LMOptimizationDetails &details() const { return *details_; } /** * Constructor. @@ -69,7 +69,7 @@ class LMManifoldOptimizer : public ManifoldOptimizer { const LevenbergMarquardtParams ¶ms = LevenbergMarquardtParams()) : ManifoldOptimizer(mopt_params), params_(params), - details_(std::make_shared()) {} + details_(std::make_shared()) {} /** Virtual destructor */ ~LMManifoldOptimizer() {} @@ -96,7 +96,7 @@ class LMManifoldOptimizer : public ManifoldOptimizer { * @return Optimized base-variable values. */ gtsam::Values optimize(const NonlinearFactorGraph &graph, - const ManifoldOptProblem &mopt_problem) const; + const ManifoldOptimizationProblem &mopt_problem) const; /** * Perform one outer LM iteration, potentially with multiple lambda trials. @@ -105,7 +105,7 @@ class LMManifoldOptimizer : public ManifoldOptimizer { * @param state Current optimizer state. * @return Iteration details including all trials. */ - LMIterDetails iterate(const NonlinearFactorGraph &graph, + LMIterationDetails iterate(const NonlinearFactorGraph &graph, const NonlinearFactorGraph &manifold_graph, const LMState &state) const; diff --git a/gtdynamics/cmopt/LMManifoldOptimizerState.cpp b/gtdynamics/cmopt/LMManifoldOptimizerState.cpp index d5eb2d2ba..88102079f 100644 --- a/gtdynamics/cmopt/LMManifoldOptimizerState.cpp +++ b/gtdynamics/cmopt/LMManifoldOptimizerState.cpp @@ -25,44 +25,44 @@ namespace gtdynamics { /* ************************************************************************* */ LMState::LMState(const NonlinearFactorGraph &graph, - const ManifoldOptProblem &problem, const double &_lambda, - const double &_lambda_factor, size_t _iterations) + const ManifoldOptimizationProblem &problem, const double &_lambda, + const double &_lambdaFactor, size_t _iterations) : manifolds(problem.manifolds()), - unconstrained_values(problem.unconstrainedValues()), - const_manifolds(problem.constManifolds()), - error(EvaluateGraphError(graph, manifolds, unconstrained_values, - const_manifolds)), - lambda(_lambda), lambda_factor(_lambda_factor), iterations(_iterations) {} + unconstrainedValues(problem.unconstrainedValues()), + constManifolds(problem.constManifolds()), + error(evaluateGraphError(graph, manifolds, unconstrainedValues, + constManifolds)), + lambda(_lambda), lambdaFactor(_lambdaFactor), iterations(_iterations) {} /* ************************************************************************* */ -LMState LMState::FromLastIteration(const LMIterDetails &iter_details, +LMState LMState::fromLastIteration(const LMIterationDetails &iter_details, const NonlinearFactorGraph &graph, const LevenbergMarquardtParams ¶ms) { double lambda; const auto &last_trial = iter_details.trials.back(); const auto &prev_state = iter_details.state; LMState state; - if (last_trial.step_is_successful) { - state.manifolds = last_trial.nonlinear_update.new_manifolds; - state.unconstrained_values = - last_trial.nonlinear_update.new_unconstrained_values; - state.const_manifolds = prev_state.const_manifolds; - state.error = last_trial.nonlinear_update.new_error; - state.lambda = last_trial.linear_update.lambda; - state.lambda_factor = prev_state.lambda_factor; - last_trial.setNextLambda(state.lambda, state.lambda_factor, params); + if (last_trial.stepIsSuccessful) { + state.manifolds = last_trial.nonlinearUpdate.newManifolds; + state.unconstrainedValues = + last_trial.nonlinearUpdate.newUnconstrainedValues; + state.constManifolds = prev_state.constManifolds; + state.error = last_trial.nonlinearUpdate.newError; + state.lambda = last_trial.linearUpdate.lambda; + state.lambdaFactor = prev_state.lambdaFactor; + last_trial.setNextLambda(state.lambda, state.lambdaFactor, params); } else { // pick the trials with smallest error throw std::runtime_error("not implemented"); } state.iterations = prev_state.iterations + 1; - state.totalNumberInnerIterations = - prev_state.totalNumberInnerIterations + iter_details.trials.size(); + state.totalInnerIterations = + prev_state.totalInnerIterations + iter_details.trials.size(); return state; } /* ************************************************************************* */ -double LMState::EvaluateGraphError(const NonlinearFactorGraph &graph, +double LMState::evaluateGraphError(const NonlinearFactorGraph &graph, const EManifoldValues &_manifolds, const Values &_unconstrained_values, const EManifoldValues &_const_manifolds) { @@ -75,8 +75,8 @@ double LMState::EvaluateGraphError(const NonlinearFactorGraph &graph, /* ************************************************************************* */ Values LMState::baseValues() const { Values base_values = manifolds.baseValues(); - base_values.insert(unconstrained_values); - base_values.insert(const_manifolds.baseValues()); + base_values.insert(unconstrainedValues); + base_values.insert(constManifolds.baseValues()); return base_values; } @@ -90,67 +90,67 @@ LMTrial::LMTrial(const LMState &state, const NonlinearFactorGraph &graph, const double &lambda, const LevenbergMarquardtParams ¶ms) { // std::cout << "========= " << state.iterations << " ======= \n"; auto start = std::chrono::high_resolution_clock::now(); - step_is_successful = false; + stepIsSuccessful = false; // Compute linear update and linear cost change - linear_update = LinearUpdate(lambda, manifold_graph, state, params); - if (!linear_update.solve_successful) { + linearUpdate = LinearUpdate(lambda, manifold_graph, state, params); + if (!linearUpdate.solveSuccessful) { return; } // Compute nonlinear update and nonlinear cost change - nonlinear_update = NonlinearUpdate(state, linear_update, graph); + nonlinearUpdate = NonlinearUpdate(state, linearUpdate, graph); // Decide if accept or reject trial - model_fidelity = nonlinear_update.cost_change / linear_update.cost_change; - if (linear_update.cost_change > - std::numeric_limits::epsilon() * linear_update.old_error && - model_fidelity > params.minModelFidelity) { - step_is_successful = true; + modelFidelity = nonlinearUpdate.costChange / linearUpdate.costChange; + if (linearUpdate.costChange > + std::numeric_limits::epsilon() * linearUpdate.oldError && + modelFidelity > params.minModelFidelity) { + stepIsSuccessful = true; } auto end = std::chrono::high_resolution_clock::now(); - trial_time = + trialTime = std::chrono::duration_cast(end - start) .count() / 1e6; } /* ************************************************************************* */ -void LMTrial::setNextLambda(double &new_lambda, double &new_lambda_factor, +void LMTrial::setNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { - if (step_is_successful) { - setDecreasedNextLambda(new_lambda, new_lambda_factor, params); + if (stepIsSuccessful) { + setDecreasedNextLambda(new_lambda, newLambdaFactor, params); } else { - setIncreasedNextLambda(new_lambda, new_lambda_factor, params); + setIncreasedNextLambda(new_lambda, newLambdaFactor, params); } } /* ************************************************************************* */ void LMTrial::setIncreasedNextLambda( - double &new_lambda, double &new_lambda_factor, + double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { - new_lambda *= new_lambda_factor; + new_lambda *= newLambdaFactor; if (!params.useFixedLambdaFactor) { - new_lambda_factor *= 2.0; + newLambdaFactor *= 2.0; } } /* ************************************************************************* */ void LMTrial::setDecreasedNextLambda( - double &new_lambda, double &new_lambda_factor, + double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const { if (params.useFixedLambdaFactor) { - new_lambda /= new_lambda_factor; + new_lambda /= newLambdaFactor; } else { - new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * model_fidelity - 1.0, 3)); - new_lambda_factor *= 2.0; + new_lambda *= std::max(1.0 / 3.0, 1.0 - pow(2.0 * modelFidelity - 1.0, 3)); + newLambdaFactor *= 2.0; } new_lambda = std::max(params.lambdaLowerBound, new_lambda); } /* ************************************************************************* */ -void LMTrial::PrintTitle() { +void LMTrial::printTitle() { cout << setw(10) << "iter" << " " << setw(12) << "error " << " " << setw(12) << "nonlinear " @@ -164,13 +164,13 @@ void LMTrial::PrintTitle() { /* ************************************************************************* */ void LMTrial::print(const LMState &state) const { cout << setw(10) << state.iterations << " " << setw(12) << setprecision(4) - << nonlinear_update.new_error << " " << setw(12) << setprecision(4) - << nonlinear_update.cost_change << " " << setw(10) << setprecision(4) - << linear_update.cost_change << " " << setw(10) << setprecision(2) - << linear_update.lambda << " " << setw(10) - << (linear_update.solve_successful ? "T " : "F ") << " " << setw(10) - << setprecision(2) << trial_time << setw(10) << setprecision(4) - << linear_update.delta.norm() << endl; + << nonlinearUpdate.newError << " " << setw(12) << setprecision(4) + << nonlinearUpdate.costChange << " " << setw(10) << setprecision(4) + << linearUpdate.costChange << " " << setw(10) << setprecision(2) + << linearUpdate.lambda << " " << setw(10) + << (linearUpdate.solveSuccessful ? "T " : "F ") << " " << setw(10) + << setprecision(2) << trialTime << setw(10) << setprecision(4) + << linearUpdate.delta.norm() << endl; } /* ************************************************************************* */ @@ -184,7 +184,7 @@ LMTrial::LinearUpdate::LinearUpdate(const double &_lambda, const LevenbergMarquardtParams ¶ms) : lambda(_lambda) { // linearize and build damped system - Values state_values = state.unconstrained_values; + Values state_values = state.unconstrainedValues; for (const auto &it : state.manifolds) { state_values.insert(it.first, it.second); } @@ -197,15 +197,15 @@ LMTrial::LinearUpdate::LinearUpdate(const double &_lambda, // solve delta try { delta = SolveLinear(damped_system, params); - solve_successful = true; + solveSuccessful = true; } catch (const gtsam::IndeterminantLinearSystemException &) { - solve_successful = false; + solveSuccessful = false; return; } - tangent_vector = computeTangentVector(delta, state); - old_error = linear->error(VectorValues::Zero(delta)); - new_error = linear->error(delta); - cost_change = old_error - new_error; + tangentVector = computeTangentVector(delta, state); + oldError = linear->error(VectorValues::Zero(delta)); + newError = linear->error(delta); + costChange = oldError - newError; } /* ************************************************************************* */ @@ -224,9 +224,9 @@ LMTrial::LinearUpdate::buildDampedSystem(GaussianFactorGraph damped, const LMState &state) const { noiseModelCache.resize(0); // for each of the variables, add a prior - damped.reserve(damped.size() + state.unconstrained_values.size()); + damped.reserve(damped.size() + state.unconstrainedValues.size()); std::map dims = state.manifolds.dims(); - std::map dims_unconstrained = state.unconstrained_values.dims(); + std::map dims_unconstrained = state.unconstrainedValues.dims(); dims.insert(dims_unconstrained.begin(), dims_unconstrained.end()); for (const auto &key_dim : dims) { const Key &key = key_dim.first; @@ -271,18 +271,18 @@ GaussianFactorGraph LMTrial::LinearUpdate::buildDampedSystem( VectorValues LMTrial::LinearUpdate::computeTangentVector(const VectorValues &delta, const LMState &state) const { - VectorValues tangent_vector; + VectorValues tangentVector; for (const auto &it : state.manifolds) { const Vector &xi = delta.at(it.first); - tangent_vector.insert(it.second.basis()->computeTangentVector(xi)); + tangentVector.insert(it.second.basis()->computeTangentVector(xi)); } - for (const auto &it : state.const_manifolds) { - tangent_vector.insert(it.second.values().zeroVectors()); + for (const auto &it : state.constManifolds) { + tangentVector.insert(it.second.values().zeroVectors()); } - for (const Key &key : state.unconstrained_values.keys()) { - tangent_vector.insert(key, delta.at(key)); + for (const Key &key : state.unconstrainedValues.keys()) { + tangentVector.insert(key, delta.at(key)); } - return tangent_vector; + return tangentVector; } /* ************************************************************************* */ @@ -291,30 +291,30 @@ LMTrial::LinearUpdate::computeTangentVector(const VectorValues &delta, /* ************************************************************************* */ LMTrial::NonlinearUpdate::NonlinearUpdate(const LMState &state, - const LinearUpdate &linear_update, + const LinearUpdate &linearUpdate, const NonlinearFactorGraph &graph) { // retract for manifolds VectorValues tangent_vector_manifold = - SubValues(linear_update.delta, state.manifolds.keys()); - new_manifolds = state.manifolds.retract(tangent_vector_manifold); + SubValues(linearUpdate.delta, state.manifolds.keys()); + newManifolds = state.manifolds.retract(tangent_vector_manifold); // retract for unconstrained variables VectorValues tangent_vector_unconstrained = - SubValues(linear_update.delta, state.unconstrained_values.keys()); - new_unconstrained_values = - state.unconstrained_values.retract(tangent_vector_unconstrained); + SubValues(linearUpdate.delta, state.unconstrainedValues.keys()); + newUnconstrainedValues = + state.unconstrainedValues.retract(tangent_vector_unconstrained); // compute error - new_error = LMState::EvaluateGraphError( - graph, new_manifolds, new_unconstrained_values, state.const_manifolds); - cost_change = state.error - new_error; + newError = LMState::evaluateGraphError( + graph, newManifolds, newUnconstrainedValues, state.constManifolds); + costChange = state.error - newError; } /* ************************************************************************* */ -/* <========================= IELMItersDetails ============================> */ +/* <========================= IELMOptimizationDetails ============================> */ /* ************************************************************************* */ -void LMItersDetails::exportFile(const std::string &state_file_path, +void LMOptimizationDetails::exportFile(const std::string &state_file_path, const std::string &trial_file_path) const { std::ofstream state_file, trial_file; state_file.open(state_file_path); @@ -332,13 +332,13 @@ void LMItersDetails::exportFile(const std::string &state_file_path, << "," << "error" << "," - << "step_is_successful" + << "stepIsSuccessful" << "," - << "linear_cost_change" + << "linearCostChange" << "," - << "nonlinear_cost_change" + << "nonlinearCostChange" << "," - << "model_fidelity" + << "modelFidelity" << "\n"; for (const auto &iter_details : *this) { @@ -346,12 +346,12 @@ void LMItersDetails::exportFile(const std::string &state_file_path, state_file << state.iterations << "," << state.lambda << "," << state.error << "\n"; for (const auto &trial : iter_details.trials) { - trial_file << state.iterations << "," << trial.linear_update.lambda << "," - << trial.nonlinear_update.new_error << "," - << trial.step_is_successful << "," - << trial.linear_update.cost_change << "," - << trial.nonlinear_update.cost_change << "," - << trial.model_fidelity << "\n"; + trial_file << state.iterations << "," << trial.linearUpdate.lambda << "," + << trial.nonlinearUpdate.newError << "," + << trial.stepIsSuccessful << "," + << trial.linearUpdate.costChange << "," + << trial.nonlinearUpdate.costChange << "," + << trial.modelFidelity << "\n"; } } state_file.close(); diff --git a/gtdynamics/cmopt/LMManifoldOptimizerState.h b/gtdynamics/cmopt/LMManifoldOptimizerState.h index 2b60b4a50..1b10ad354 100644 --- a/gtdynamics/cmopt/LMManifoldOptimizerState.h +++ b/gtdynamics/cmopt/LMManifoldOptimizerState.h @@ -37,7 +37,7 @@ using gtsam::VectorValues; struct LMState; struct LMTrial; -struct LMIterDetails; +struct LMIterationDetails; /** * State for one outer iteration of manifold LM optimization. @@ -51,13 +51,13 @@ struct LMIterDetails; struct LMState { public: EManifoldValues manifolds; - Values unconstrained_values; - EManifoldValues const_manifolds; + Values unconstrainedValues; + EManifoldValues constManifolds; double error = 0; double lambda = 0; - double lambda_factor = 0; + double lambdaFactor = 0; size_t iterations = 0; - size_t totalNumberInnerIterations = 0; + size_t totalInnerIterations = 0; LMState() {} @@ -66,11 +66,11 @@ struct LMState { * @param graph Base cost graph. * @param problem Transformed manifold optimization problem. * @param _lambda Initial damping value. - * @param _lambda_factor Lambda scaling factor. + * @param _lambdaFactor Lambda scaling factor. * @param _iterations Current outer iteration index. */ - LMState(const NonlinearFactorGraph &graph, const ManifoldOptProblem &problem, - const double &_lambda, const double &_lambda_factor, + LMState(const NonlinearFactorGraph &graph, const ManifoldOptimizationProblem &problem, + const double &_lambda, const double &_lambdaFactor, size_t _iterations = 0); /** @@ -80,7 +80,7 @@ struct LMState { * @param params LM parameters. * @return New LM state for the next outer iteration. */ - static LMState FromLastIteration(const LMIterDetails &iter_details, + static LMState fromLastIteration(const LMIterationDetails &iter_details, const NonlinearFactorGraph &graph, const LevenbergMarquardtParams ¶ms); @@ -92,7 +92,7 @@ struct LMState { * @param _const_manifolds Fixed manifold values. * @return Total nonlinear graph error. */ - static double EvaluateGraphError(const NonlinearFactorGraph &graph, + static double evaluateGraphError(const NonlinearFactorGraph &graph, const EManifoldValues &_manifolds, const Values &_unconstrained_values, const EManifoldValues &_const_manifolds); @@ -136,11 +136,11 @@ struct LMTrial { struct LinearUpdate { double lambda; VectorValues delta; - VectorValues tangent_vector; - double old_error; - double new_error; - double cost_change; - bool solve_successful; + VectorValues tangentVector; + double oldError; + double newError; + double costChange; + bool solveSuccessful; /** Default constructor. */ LinearUpdate() {} @@ -215,10 +215,10 @@ struct LMTrial { * @see README.md#solvers */ struct NonlinearUpdate { - EManifoldValues new_manifolds; - Values new_unconstrained_values; - double new_error; - double cost_change; + EManifoldValues newManifolds; + Values newUnconstrainedValues; + double newError; + double costChange; /** Default constructor. */ NonlinearUpdate() {} @@ -226,51 +226,51 @@ struct LMTrial { /** * Compute nonlinear trial update from a linear update. * @param state Current LM state. - * @param linear_update Linear trial update. + * @param linearUpdate Linear trial update. * @param graph Base cost graph. */ - NonlinearUpdate(const LMState &state, const LinearUpdate &linear_update, + NonlinearUpdate(const LMState &state, const LinearUpdate &linearUpdate, const NonlinearFactorGraph &graph); }; public: // linear update - LinearUpdate linear_update; + LinearUpdate linearUpdate; // nonlinear update - NonlinearUpdate nonlinear_update; + NonlinearUpdate nonlinearUpdate; // decision making - double model_fidelity; - bool step_is_successful; - bool stop_searching_lambda; - double trial_time; + double modelFidelity; + bool stepIsSuccessful; + bool stopSearchingLambda; + double trialTime; /** * Update lambda for the next trial/state. * @param new_lambda Output lambda value. - * @param new_lambda_factor Output lambda scaling factor. + * @param newLambdaFactor Output lambda scaling factor. * @param params LM parameters. */ - void setNextLambda(double &new_lambda, double &new_lambda_factor, + void setNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; /** * Increase lambda for the next trial/state. * @param new_lambda Output lambda value. - * @param new_lambda_factor Output lambda scaling factor. + * @param newLambdaFactor Output lambda scaling factor. * @param params LM parameters. */ - void setIncreasedNextLambda(double &new_lambda, double &new_lambda_factor, + void setIncreasedNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; /** * Decrease lambda for the next trial/state. * @param new_lambda Output lambda value. - * @param new_lambda_factor Output lambda scaling factor. + * @param newLambdaFactor Output lambda scaling factor. * @param params LM parameters. */ - void setDecreasedNextLambda(double &new_lambda, double &new_lambda_factor, + void setDecreasedNextLambda(double &new_lambda, double &newLambdaFactor, const LevenbergMarquardtParams ¶ms) const; /** @@ -280,7 +280,7 @@ struct LMTrial { void print(const LMState &state) const; /// Print table header for LM trial summaries. - static void PrintTitle(); + static void printTitle(); }; /** @@ -288,11 +288,11 @@ struct LMTrial { * * @see README.md#solvers */ -struct LMIterDetails { +struct LMIterationDetails { LMState state; std::vector trials; - LMIterDetails(const LMState &_state) : state(_state), trials() {} + LMIterationDetails(const LMState &_state) : state(_state), trials() {} }; /** @@ -300,9 +300,9 @@ struct LMIterDetails { * * @see README.md#solvers */ -class LMItersDetails : public std::vector { +class LMOptimizationDetails : public std::vector { public: - using base = std::vector; + using base = std::vector; using base::base; /** diff --git a/gtdynamics/cmopt/ManifoldOptimizer.cpp b/gtdynamics/cmopt/ManifoldOptimizer.cpp index 8b64ed86e..0e4323c3d 100644 --- a/gtdynamics/cmopt/ManifoldOptimizer.cpp +++ b/gtdynamics/cmopt/ManifoldOptimizer.cpp @@ -20,47 +20,47 @@ namespace gtdynamics { /* ************************************************************************* */ -Values ManifoldOptProblem::unconstrainedValues() const { - return SubValues(values_, unconstrained_keys_); +Values ManifoldOptimizationProblem::unconstrainedValues() const { + return SubValues(values, unconstrainedKeys); } /* ************************************************************************* */ -EManifoldValues ManifoldOptProblem::manifolds() const { - EManifoldValues e_manifolds; - for (const Key& key : manifold_keys_) { - e_manifolds.insert({key, values_.at(key)}); +EManifoldValues ManifoldOptimizationProblem::manifolds() const { + EManifoldValues equalityManifolds; + for (const Key& key : manifoldKeys) { + equalityManifolds.insert({key, values.at(key)}); } - return e_manifolds; + return equalityManifolds; } /* ************************************************************************* */ -EManifoldValues ManifoldOptProblem::constManifolds() const { - EManifoldValues e_manifolds; - for (const Key& key : fixed_manifolds_.keys()) { - e_manifolds.insert({key, fixed_manifolds_.at(key)}); +EManifoldValues ManifoldOptimizationProblem::constManifolds() const { + EManifoldValues equalityManifolds; + for (const Key& key : fixedManifolds.keys()) { + equalityManifolds.insert({key, fixedManifolds.at(key)}); } - return e_manifolds; + return equalityManifolds; } /* ************************************************************************* */ -std::pair ManifoldOptProblem::problemDimension() const { - size_t values_dim = values_.dim(); +std::pair ManifoldOptimizationProblem::problemDimension() const { + size_t values_dim = values.dim(); size_t graph_dim = 0; - for (const auto& factor : graph_) { + for (const auto& factor : graph) { graph_dim += factor->dim(); } return std::make_pair(graph_dim, values_dim); } /* ************************************************************************* */ -void ManifoldOptProblem::print(const std::string& s, +void ManifoldOptimizationProblem::print(const std::string& s, const KeyFormatter& keyFormatter) const { std::cout << s; - std::cout << "found " << components_.size() - << " components: " << manifold_keys_.size() << " free, " - << fixed_manifolds_.size() << " fixed\n"; - for (const Key& cm_key : manifold_keys_) { - const auto& cm = values_.at(cm_key).cast(); + std::cout << "found " << components.size() + << " components: " << manifoldKeys.size() << " free, " + << fixedManifolds.size() << " fixed\n"; + for (const Key& cm_key : manifoldKeys) { + const auto& cm = values.at(cm_key).cast(); std::cout << "component " << keyFormatter(cm_key) << ":\tdimension " << cm.dim() << "\n"; for (const auto& key : cm.values().keys()) { @@ -69,8 +69,8 @@ void ManifoldOptProblem::print(const std::string& s, std::cout << "\n"; } std::cout << "fully constrained manifolds:\n"; - for (const auto& cm_key : fixed_manifolds_.keys()) { - const auto& cm = fixed_manifolds_.at(cm_key).cast(); + for (const auto& cm_key : fixedManifolds.keys()) { + const auto& cm = fixedManifolds.at(cm_key).cast(); for (const auto& key : cm.values().keys()) { std::cout << "\t" << keyFormatter(key); } @@ -81,11 +81,11 @@ void ManifoldOptProblem::print(const std::string& s, /* ************************************************************************* */ ManifoldOptimizerParameters::ManifoldOptimizerParameters() : Base(), - cc_params(std::make_shared()), - retract_init(true) {} + constraintManifoldParams(std::make_shared()), + retractInitialValues(true) {} /* ************************************************************************* */ -NonlinearEqualityConstraints::shared_ptr ManifoldOptimizer::IdentifyConnectedComponent( +NonlinearEqualityConstraints::shared_ptr ManifoldOptimizer::identifyConnectedComponent( const NonlinearEqualityConstraints& constraints, const gtsam::Key start_key, gtsam::KeySet& keys, const gtsam::VariableIndex& var_index) { std::set constraint_indices; @@ -117,7 +117,7 @@ NonlinearEqualityConstraints::shared_ptr ManifoldOptimizer::IdentifyConnectedCom /* ************************************************************************* */ std::vector -ManifoldOptimizer::IdentifyConnectedComponents( +ManifoldOptimizer::identifyConnectedComponents( const NonlinearEqualityConstraints& constraints) { // Get all the keys in constraints. gtsam::VariableIndex constraint_var_index(constraints); @@ -128,14 +128,14 @@ ManifoldOptimizer::IdentifyConnectedComponents( while (!constraint_keys.empty()) { Key key = *constraint_keys.begin(); constraint_keys.erase(key); - components.emplace_back(IdentifyConnectedComponent( + components.emplace_back(identifyConnectedComponent( constraints, key, constraint_keys, constraint_var_index)); } return components; } /* ************************************************************************* */ -NonlinearFactorGraph ManifoldOptimizer::ManifoldGraph( +NonlinearFactorGraph ManifoldOptimizer::manifoldGraph( const NonlinearFactorGraph& graph, const std::map& var2man_keymap, const Values& fc_manifolds) { NonlinearFactorGraph manifold_graph; @@ -162,20 +162,20 @@ NonlinearFactorGraph ManifoldOptimizer::ManifoldGraph( } /* ************************************************************************* */ -ManifoldOptProblem ManifoldOptimizer::problemTransform( +ManifoldOptimizationProblem ManifoldOptimizer::transformProblem( const EConsOptProblem& equalityConstrainedProblem) const { - ManifoldOptProblem mopt_problem; - mopt_problem.components_ = - IdentifyConnectedComponents(equalityConstrainedProblem.e_constraints_); - constructMoptValues(equalityConstrainedProblem, mopt_problem); - constructMoptGraph(equalityConstrainedProblem, mopt_problem); + ManifoldOptimizationProblem mopt_problem; + mopt_problem.components = + identifyConnectedComponents(equalityConstrainedProblem.e_constraints_); + constructManifoldOptimizationValues(equalityConstrainedProblem, mopt_problem); + constructManifoldOptimizationGraph(equalityConstrainedProblem, mopt_problem); return mopt_problem; } /* ************************************************************************* */ -void ManifoldOptimizer::constructMoptValues( +void ManifoldOptimizer::constructManifoldOptimizationValues( const EConsOptProblem& equalityConstrainedProblem, - ManifoldOptProblem& mopt_problem) const { + ManifoldOptimizationProblem& mopt_problem) const { constructManifoldValues(equalityConstrainedProblem, mopt_problem); constructUnconstrainedValues(equalityConstrainedProblem, mopt_problem); } @@ -183,10 +183,10 @@ void ManifoldOptimizer::constructMoptValues( /* ************************************************************************* */ void ManifoldOptimizer::constructManifoldValues( const EConsOptProblem& equalityConstrainedProblem, - ManifoldOptProblem& mopt_problem) const { - for (size_t i = 0; i < mopt_problem.components_.size(); i++) { + ManifoldOptimizationProblem& mopt_problem) const { + for (size_t i = 0; i < mopt_problem.components.size(); i++) { // Find the values of variables in the component. - const auto& component = mopt_problem.components_.at(i); + const auto& component = mopt_problem.components.at(i); const KeySet component_keys = component->keys(); if (component_keys.empty()) continue; const Key component_key = *component_keys.begin(); @@ -196,13 +196,13 @@ void ManifoldOptimizer::constructManifoldValues( } // Construct manifold value auto constraint_manifold = ConstraintManifold( - component, component_values, p_.cc_params, p_.retract_init); + component, component_values, p_.constraintManifoldParams, p_.retractInitialValues); // check if the manifold is fully constrained if (constraint_manifold.dim() > 0) { - mopt_problem.values_.insert(component_key, constraint_manifold); - mopt_problem.manifold_keys_.insert(component_key); + mopt_problem.values.insert(component_key, constraint_manifold); + mopt_problem.manifoldKeys.insert(component_key); } else { - mopt_problem.fixed_manifolds_.insert(component_key, constraint_manifold); + mopt_problem.fixedManifolds.insert(component_key, constraint_manifold); } } } @@ -210,39 +210,38 @@ void ManifoldOptimizer::constructManifoldValues( /* ************************************************************************* */ void ManifoldOptimizer::constructUnconstrainedValues( const EConsOptProblem& equalityConstrainedProblem, - ManifoldOptProblem& mopt_problem) { + ManifoldOptimizationProblem& mopt_problem) { // Find out which variables are unconstrained - mopt_problem.unconstrained_keys_ = equalityConstrainedProblem.costs_.keys(); - KeySet& unconstrained_keys = mopt_problem.unconstrained_keys_; - for (const auto& component : mopt_problem.components_) { + mopt_problem.unconstrainedKeys = equalityConstrainedProblem.costs_.keys(); + KeySet& unconstrainedKeys = mopt_problem.unconstrainedKeys; + for (const auto& component : mopt_problem.components) { for (const Key& key : component->keys()) { - if (unconstrained_keys.find(key) != unconstrained_keys.end()) { - unconstrained_keys.erase(key); + if (unconstrainedKeys.find(key) != unconstrainedKeys.end()) { + unconstrainedKeys.erase(key); } } } // Copy the values of unconstrained variables - for (const Key& key : unconstrained_keys) { - mopt_problem.values_.insert(key, - equalityConstrainedProblem.values_.at(key)); + for (const Key& key : unconstrainedKeys) { + mopt_problem.values.insert(key, equalityConstrainedProblem.values_.at(key)); } } /* ************************************************************************* */ -void ManifoldOptimizer::constructMoptGraph( +void ManifoldOptimizer::constructManifoldOptimizationGraph( const EConsOptProblem& equalityConstrainedProblem, - ManifoldOptProblem& mopt_problem) { + ManifoldOptimizationProblem& mopt_problem) { // Construct base key to component map. std::map key_component_map; - for (const Key& cm_key : mopt_problem.manifold_keys_) { - const auto& cm = mopt_problem.values_.at(cm_key).cast(); + for (const Key& cm_key : mopt_problem.manifoldKeys) { + const auto& cm = mopt_problem.values.at(cm_key).cast(); 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(); + mopt_problem.fixedManifolds.at(cm_key).cast(); for (const Key& base_key : cm.values().keys()) { key_component_map[base_key] = cm_key; } @@ -260,30 +259,30 @@ void ManifoldOptimizer::constructMoptGraph( NoiseModelFactor::shared_ptr noise_factor = std::dynamic_pointer_cast(factor); auto subs_factor = std::make_shared( - noise_factor, replacement_map, mopt_problem.fixed_manifolds_); + noise_factor, replacement_map, mopt_problem.fixedManifolds); if (subs_factor->checkActive()) { - mopt_problem.graph_.add(subs_factor); + mopt_problem.graph.add(subs_factor); } } else { - mopt_problem.graph_.add(factor); + mopt_problem.graph.add(factor); } } } /* ************************************************************************* */ -Values ManifoldOptimizer::baseValues(const ManifoldOptProblem& mopt_problem, +Values ManifoldOptimizer::baseValues(const ManifoldOptimizationProblem& mopt_problem, const Values& nopt_values) const { Values base_values; - for (const Key& key : mopt_problem.fixed_manifolds_.keys()) { - base_values.insert(mopt_problem.fixed_manifolds_.at(key) + for (const Key& key : mopt_problem.fixedManifolds.keys()) { + base_values.insert(mopt_problem.fixedManifolds.at(key) .cast() .values()); } - for (const Key& key : mopt_problem.unconstrained_keys_) { + for (const Key& key : mopt_problem.unconstrainedKeys) { base_values.insert(key, nopt_values.at(key)); } - for (const Key& key : mopt_problem.manifold_keys_) { - if (p_.retract_final) { + for (const Key& key : mopt_problem.manifoldKeys) { + if (p_.retractFinalValues) { base_values.insert( nopt_values.at(key).cast().feasibleValues()); } else { @@ -296,27 +295,27 @@ Values ManifoldOptimizer::baseValues(const ManifoldOptProblem& mopt_problem, /* ************************************************************************* */ VectorValues ManifoldOptimizer::baseTangentVector( - const ManifoldOptProblem& mopt_problem, const Values& values, + const ManifoldOptimizationProblem& mopt_problem, const Values& values, const VectorValues& delta) const { - VectorValues tangent_vector; - for (const Key& key : mopt_problem.unconstrained_keys_) { - tangent_vector.insert(key, delta.at(key)); + VectorValues tangentVector; + for (const Key& key : mopt_problem.unconstrainedKeys) { + tangentVector.insert(key, delta.at(key)); } - for (const Key& key : mopt_problem.manifold_keys_) { - tangent_vector.insert( + for (const Key& key : mopt_problem.manifoldKeys) { + tangentVector.insert( values.at(key).cast().basis()->computeTangentVector( delta.at(key))); } - return tangent_vector; + return tangentVector; } /* ************************************************************************* */ -ManifoldOptProblem ManifoldOptimizer::initializeMoptProblem( +ManifoldOptimizationProblem ManifoldOptimizer::initializeManifoldOptimizationProblem( const gtsam::NonlinearFactorGraph& costs, const NonlinearEqualityConstraints& constraints, const gtsam::Values& init_values) const { EConsOptProblem equalityConstrainedProblem(costs, constraints, init_values); - return problemTransform(equalityConstrainedProblem); + return transformProblem(equalityConstrainedProblem); } } // namespace gtdynamics diff --git a/gtdynamics/cmopt/ManifoldOptimizer.h b/gtdynamics/cmopt/ManifoldOptimizer.h index 514c79013..a8796710a 100644 --- a/gtdynamics/cmopt/ManifoldOptimizer.h +++ b/gtdynamics/cmopt/ManifoldOptimizer.h @@ -43,15 +43,15 @@ using gtsam::NonlinearEqualityConstraints; * @see README.md#ccc-transformation * @see README.md#equivalent-factors */ -struct ManifoldOptProblem { - NonlinearFactorGraph graph_; // cost function on constraint manifolds +struct ManifoldOptimizationProblem { + NonlinearFactorGraph graph; // cost function on constraint manifolds std::vector - components_; // All the constraint-connected components + components; // All the constraint-connected components Values - values_; // values for constraint manifolds and unconstrained variables - Values fixed_manifolds_; // fully constrained variables - KeySet unconstrained_keys_; // keys for unconstrained variables - KeySet manifold_keys_; // keys for constraint manifold variables + values; // values for constraint manifolds and unconstrained variables + Values fixedManifolds; // fully constrained variables + KeySet unconstrainedKeys; // keys for unconstrained variables + KeySet manifoldKeys; // keys for constraint manifold variables /** Dimension of the manifold optimization problem, as factor dimension x * variable dimension. @@ -86,10 +86,10 @@ struct ManifoldOptProblem { struct ManifoldOptimizerParameters : public ConstrainedOptimizationParameters { using Base = ConstrainedOptimizationParameters; ConstraintManifold::Params::shared_ptr - cc_params; // Parameter for constraint-connected components - bool retract_init = true; // Perform retraction on constructing values for + constraintManifoldParams; // Parameter for constraint-connected components + bool retractInitialValues = true; // Perform retraction on constructing values for // connected component. - bool retract_final = false; // Perform retraction on manifolds after + bool retractFinalValues = false; // Perform retraction on manifolds after // optimization, used for infeasible methods. /// Default Constructor. ManifoldOptimizerParameters(); @@ -130,13 +130,13 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param var_index Variable index over constraints. * @return Constraint subset corresponding to the connected component. */ - static NonlinearEqualityConstraints::shared_ptr IdentifyConnectedComponent( + static NonlinearEqualityConstraints::shared_ptr identifyConnectedComponent( const NonlinearEqualityConstraints &constraints, const Key start_key, KeySet &keys, const VariableIndex &var_index); /// Identify the connected components by constraints. static std::vector - IdentifyConnectedComponents(const NonlinearEqualityConstraints &constraints); + identifyConnectedComponents(const NonlinearEqualityConstraints &constraints); /** * Create equivalent cost graph on manifold variables. @@ -145,7 +145,7 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param fc_manifolds Values of fully constrained manifolds. * @return Transformed manifold graph. */ - static NonlinearFactorGraph ManifoldGraph( + static NonlinearFactorGraph manifoldGraph( const NonlinearFactorGraph &graph, const std::map &var2man_keymap, const Values &fc_manifolds = Values()); @@ -157,8 +157,8 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param equalityConstrainedProblem Input constrained problem. * @param mopt_problem Output manifold optimization problem. */ - void constructMoptValues(const EConsOptProblem &equalityConstrainedProblem, - ManifoldOptProblem &mopt_problem) const; + void constructManifoldOptimizationValues(const EConsOptProblem &equalityConstrainedProblem, + ManifoldOptimizationProblem &mopt_problem) const; /** * Create initial values for constraint manifold variables. @@ -167,7 +167,7 @@ class ManifoldOptimizer : public ConstrainedOptimizer { */ void constructManifoldValues( const EConsOptProblem &equalityConstrainedProblem, - ManifoldOptProblem &mopt_problem) const; + ManifoldOptimizationProblem &mopt_problem) const; /** * Collect initial values for unconstrained variables. @@ -176,23 +176,23 @@ class ManifoldOptimizer : public ConstrainedOptimizer { */ static void constructUnconstrainedValues( const EConsOptProblem &equalityConstrainedProblem, - ManifoldOptProblem &mopt_problem); + ManifoldOptimizationProblem &mopt_problem); /** Create a factor graph of cost function with the constraint manifold * variables. * @param equalityConstrainedProblem Input constrained problem. * @param mopt_problem Output manifold optimization problem. */ - static void constructMoptGraph( + static void constructManifoldOptimizationGraph( const EConsOptProblem &equalityConstrainedProblem, - ManifoldOptProblem &mopt_problem); + ManifoldOptimizationProblem &mopt_problem); /** Transform an equality-constrained optimization problem into a manifold * optimization problem by creating constraint manifolds. * @param equalityConstrainedProblem Input constrained problem. * @return Transformed manifold optimization problem. */ - ManifoldOptProblem problemTransform( + ManifoldOptimizationProblem transformProblem( const EConsOptProblem &equalityConstrainedProblem) const; /** @@ -202,7 +202,7 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param init_values Initial values. * @return Initialized manifold optimization problem. */ - ManifoldOptProblem initializeMoptProblem( + ManifoldOptimizationProblem initializeManifoldOptimizationProblem( const NonlinearFactorGraph &costs, const NonlinearEqualityConstraints &constraints, const Values &init_values) const; @@ -212,7 +212,7 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param nopt_values Optimized values in transformed variable space. * @return Values in original base variable space. */ - Values baseValues(const ManifoldOptProblem &mopt_problem, + Values baseValues(const ManifoldOptimizationProblem &mopt_problem, const Values &nopt_values) const; /** @@ -222,7 +222,7 @@ class ManifoldOptimizer : public ConstrainedOptimizer { * @param delta Delta in transformed variable space. * @return Tangent vectors in base variable space. */ - VectorValues baseTangentVector(const ManifoldOptProblem &mopt_problem, + VectorValues baseTangentVector(const ManifoldOptimizationProblem &mopt_problem, const Values &values, const VectorValues &delta) const; }; diff --git a/gtdynamics/cmopt/MultiJacobian.cpp b/gtdynamics/cmopt/MultiJacobian.cpp index fb31e40c3..ae964a056 100644 --- a/gtdynamics/cmopt/MultiJacobian.cpp +++ b/gtdynamics/cmopt/MultiJacobian.cpp @@ -22,7 +22,7 @@ MultiJacobian::MultiJacobian(const Key &key, const Matrix &matrix) } /* ************************************************************************* */ -MultiJacobian MultiJacobian::Identity(const Key &key, const size_t &dim) { +MultiJacobian MultiJacobian::identity(const Key &key, const size_t &dim) { MultiJacobian jacobian; jacobian[key] = Matrix::Identity(dim, dim); jacobian.dim_ = dim; @@ -30,7 +30,7 @@ MultiJacobian MultiJacobian::Identity(const Key &key, const size_t &dim) { } /* ************************************************************************* */ -MultiJacobian MultiJacobian::VerticalStack(const MultiJacobian &jac1, +MultiJacobian MultiJacobian::verticalStack(const MultiJacobian &jac1, const MultiJacobian &jac2) { size_t dim1 = jac1.numRows(); size_t dim2 = jac2.numRows(); @@ -137,13 +137,13 @@ KeySet MultiJacobian::keys() const { } /* ************************************************************************* */ -void ComputeBayesNetJacobian(const GaussianBayesNet &bn, +void computeBayesNetJacobian(const GaussianBayesNet &bn, const KeyVector &basis_keys, const std::map &var_dim, MultiJacobians &jacobians) { // set jacobian of basis variables to identity for (const Key &key : basis_keys) { - jacobians.emplace(key, MultiJacobian::Identity(key, var_dim.at(key))); + jacobians.emplace(key, MultiJacobian::identity(key, var_dim.at(key))); } GaussianBayesNet reversed_bn = bn; @@ -212,7 +212,7 @@ MultiJacobian operator+(const MultiJacobian &jac1, const MultiJacobian &jac2) { return sum_jac; } -MultiJacobians JacobiansMultiply(const MultiJacobians &jacs1, +MultiJacobians multiplyJacobians(const MultiJacobians &jacs1, const MultiJacobians &jacs2) { MultiJacobians result_jacs; for (const auto &it : jacs1) { diff --git a/gtdynamics/cmopt/MultiJacobian.h b/gtdynamics/cmopt/MultiJacobian.h index 36ee2f2d2..c9d8d1044 100644 --- a/gtdynamics/cmopt/MultiJacobian.h +++ b/gtdynamics/cmopt/MultiJacobian.h @@ -64,7 +64,7 @@ class MultiJacobian : public std::unordered_map { * @param dim Variable dimension. * @return Identity multi-variable Jacobian. */ - static MultiJacobian Identity(const Key& key, const size_t& dim); + static MultiJacobian identity(const Key& key, const size_t& dim); /** * Vertically stack two multi-variable Jacobians. @@ -72,7 +72,7 @@ class MultiJacobian : public std::unordered_map { * @param jac2 Second Jacobian block. * @return Vertically stacked Jacobian. */ - static MultiJacobian VerticalStack(const MultiJacobian& jac1, + static MultiJacobian verticalStack(const MultiJacobian& jac1, const MultiJacobian& jac2); /** @@ -142,7 +142,7 @@ typedef std::unordered_map MultiJacobians; * @param multi_jac2 Inner Jacobian map. * @return Composed Jacobian map. */ -MultiJacobians JacobiansMultiply(const MultiJacobians& multi_jac1, +MultiJacobians multiplyJacobians(const MultiJacobians& multi_jac1, const MultiJacobians& multi_jac2); /** Given a bayes net, compute the jacobians of all variables w.r.t. basis @@ -152,7 +152,7 @@ MultiJacobians JacobiansMultiply(const MultiJacobians& multi_jac1, * @param var_dim dimension of variables * @param jacobians output, jacobians of all variables */ -void ComputeBayesNetJacobian(const GaussianBayesNet& bn, +void computeBayesNetJacobian(const GaussianBayesNet& bn, const KeyVector& basis_keys, const std::map& var_dim, MultiJacobians& jacobians); diff --git a/gtdynamics/cmopt/NonlinearMOptimizer.cpp b/gtdynamics/cmopt/NonlinearManifoldOptimizer.cpp similarity index 61% rename from gtdynamics/cmopt/NonlinearMOptimizer.cpp rename to gtdynamics/cmopt/NonlinearManifoldOptimizer.cpp index 4cb0c0186..f3ff3d4d4 100644 --- a/gtdynamics/cmopt/NonlinearMOptimizer.cpp +++ b/gtdynamics/cmopt/NonlinearManifoldOptimizer.cpp @@ -6,29 +6,29 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearMOptimizer.cpp + * @file NonlinearManifoldOptimizer.cpp * @brief Manifold optimizer implementations. * @author: Yetong Zhang */ #include -#include +#include #include #include namespace gtdynamics { /* ************************************************************************* */ -Values NonlinearMOptimizer::optimize(const NonlinearFactorGraph& costs, +Values NonlinearManifoldOptimizer::optimize(const NonlinearFactorGraph& costs, const NonlinearEqualityConstraints& constraints, const Values& init_values) const { - auto mopt_problem = initializeMoptProblem(costs, constraints, init_values); - return optimizeMOpt(mopt_problem); + auto mopt_problem = initializeManifoldOptimizationProblem(costs, constraints, init_values); + return optimizeManifoldProblem(mopt_problem); } /* ************************************************************************* */ -Values NonlinearMOptimizer::optimizeMOpt( - const ManifoldOptProblem& mopt_problem) const { +Values NonlinearManifoldOptimizer::optimizeManifoldProblem( + const ManifoldOptimizationProblem& mopt_problem) const { auto nonlinear_optimizer = constructNonlinearOptimizer(mopt_problem); auto nopt_values = nonlinear_optimizer->optimize(); // if (intermediate_result) { @@ -42,23 +42,23 @@ Values NonlinearMOptimizer::optimizeMOpt( /* ************************************************************************* */ std::shared_ptr -NonlinearMOptimizer::constructNonlinearOptimizer( - const ManifoldOptProblem& mopt_problem) const { - if (std::holds_alternative(nopt_params_)) { +NonlinearManifoldOptimizer::constructNonlinearOptimizer( + const ManifoldOptimizationProblem& mopt_problem) const { + if (std::holds_alternative(nonlinearOptimizerParams_)) { return std::make_shared( - mopt_problem.graph_, mopt_problem.values_, - std::get(nopt_params_)); - } else if (std::holds_alternative(nopt_params_)) { + mopt_problem.graph, mopt_problem.values, + std::get(nonlinearOptimizerParams_)); + } else if (std::holds_alternative(nonlinearOptimizerParams_)) { return std::make_shared( - mopt_problem.graph_, mopt_problem.values_, - std::get(nopt_params_)); - } else if (std::holds_alternative(nopt_params_)) { + mopt_problem.graph, mopt_problem.values, + std::get(nonlinearOptimizerParams_)); + } else if (std::holds_alternative(nonlinearOptimizerParams_)) { return std::make_shared( - mopt_problem.graph_, mopt_problem.values_, - std::get(nopt_params_)); + mopt_problem.graph, mopt_problem.values, + std::get(nonlinearOptimizerParams_)); } else { return std::make_shared( - mopt_problem.graph_, mopt_problem.values_); + mopt_problem.graph, mopt_problem.values); } } diff --git a/gtdynamics/cmopt/NonlinearMOptimizer.h b/gtdynamics/cmopt/NonlinearManifoldOptimizer.h similarity index 76% rename from gtdynamics/cmopt/NonlinearMOptimizer.h rename to gtdynamics/cmopt/NonlinearManifoldOptimizer.h index 2694a4df8..89a18942e 100644 --- a/gtdynamics/cmopt/NonlinearMOptimizer.h +++ b/gtdynamics/cmopt/NonlinearManifoldOptimizer.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearMOptimizer.h + * @file NonlinearManifoldOptimizer.h * @brief Manifold optimizer that internally calls nonlinear optimizer. * @author: Yetong Zhang */ @@ -41,30 +41,30 @@ using gtsam::Values; * @see README.md#solvers * @see README.md#ccc-transformation */ -class NonlinearMOptimizer : public ManifoldOptimizer { +class NonlinearManifoldOptimizer : public ManifoldOptimizer { public: - using shared_ptr = std::shared_ptr; + using shared_ptr = std::shared_ptr; typedef std::variant - NonlinearOptParamsVariant; + NonlinearOptimizerParamsVariant; protected: - NonlinearOptParamsVariant nopt_params_; + NonlinearOptimizerParamsVariant nonlinearOptimizerParams_; public: /** * Construct from manifold and nonlinear optimizer parameters. * @param mopt_params Parameters for manifold construction/transformation. * @param nopt_params Parameters for the underlying nonlinear optimizer. - * @param basis_key_func Optional basis-key selector. + * @param basisKeyFunction Optional basis-key selector. */ - NonlinearMOptimizer(const ManifoldOptimizerParameters& mopt_params, - const NonlinearOptParamsVariant& nopt_params, - std::optional basis_key_func = {}) - : ManifoldOptimizer(mopt_params), nopt_params_(nopt_params) {} + NonlinearManifoldOptimizer(const ManifoldOptimizerParameters& mopt_params, + const NonlinearOptimizerParamsVariant& nopt_params, + std::optional basisKeyFunction = {}) + : ManifoldOptimizer(mopt_params), nonlinearOptimizerParams_(nopt_params) {} /// Virtual destructor. - virtual ~NonlinearMOptimizer() {} + virtual ~NonlinearManifoldOptimizer() {} /** * Run manifold optimization from a constrained problem definition. @@ -82,7 +82,7 @@ class NonlinearMOptimizer : public ManifoldOptimizer { * @param mopt_problem Transformed manifold optimization problem. * @return Optimized base-variable values. */ - Values optimizeMOpt(const ManifoldOptProblem& mopt_problem) const; + Values optimizeManifoldProblem(const ManifoldOptimizationProblem& mopt_problem) const; /** * Create the underlying nonlinear optimizer instance. @@ -90,7 +90,7 @@ class NonlinearMOptimizer : public ManifoldOptimizer { * @return Nonlinear optimizer configured for transformed variables. */ std::shared_ptr constructNonlinearOptimizer( - const ManifoldOptProblem& mopt_problem) const; + const ManifoldOptimizationProblem& mopt_problem) const; protected: }; diff --git a/gtdynamics/cmopt/README.md b/gtdynamics/cmopt/README.md index 514f2356c..5d71fce29 100644 --- a/gtdynamics/cmopt/README.md +++ b/gtdynamics/cmopt/README.md @@ -31,9 +31,9 @@ constraint-manifold variable, and cost factors touching constrained variables are replaced by equivalent factors on those manifold variables. In code, this transformation happens in three stages: -- Find CCCs from equality constraints: [`ManifoldOptimizer::IdentifyConnectedComponents`](ManifoldOptimizer.cpp#L120) +- Find CCCs from equality constraints: [`ManifoldOptimizer::identifyConnectedComponents`](ManifoldOptimizer.cpp#L120) - Build one `ConstraintManifold` per CCC: [`ManifoldOptimizer::constructManifoldValues`](ManifoldOptimizer.cpp#L184) -- Build equivalent cost factors on manifold variables: [`ManifoldOptimizer::constructMoptGraph`](ManifoldOptimizer.cpp#L232), using [`SubstituteFactor`](../factors/SubstituteFactor.h#L39) +- Build equivalent cost factors on manifold variables: [`ManifoldOptimizer::constructManifoldOptimizationGraph`](ManifoldOptimizer.cpp#L232), using [`SubstituteFactor`](../factors/SubstituteFactor.h#L39) Useful cross-reference: @@ -71,17 +71,17 @@ For each CCC, CM-Opt constructs: - the transformed unconstrained problem, paper Eq. (16). Code mapping: -- DFS over the constraint-variable bipartite graph: [`IdentifyConnectedComponent`](ManifoldOptimizer.cpp#L88) -- All components: [`IdentifyConnectedComponents`](ManifoldOptimizer.cpp#L120) -- Full transformation pipeline: [`problemTransform`](ManifoldOptimizer.cpp#L165) -- Map from base keys to manifold keys: [`constructMoptGraph`](ManifoldOptimizer.cpp#L232) +- DFS over the constraint-variable bipartite graph: [`identifyConnectedComponent`](ManifoldOptimizer.cpp#L88) +- All components: [`identifyConnectedComponents`](ManifoldOptimizer.cpp#L120) +- Full transformation pipeline: [`transformProblem`](ManifoldOptimizer.cpp#L165) +- Map from base keys to manifold keys: [`constructManifoldOptimizationGraph`](ManifoldOptimizer.cpp#L232) - Equivalent factor construction: [`SubstituteFactor`](../factors/SubstituteFactor.h#L39) Implementation detail: - A CCC whose manifold dimension is positive becomes a free manifold-valued - variable in `mopt_problem.values_`. + variable in `mopt_problem.values`. - A fully constrained CCC becomes a fixed manifold in - `mopt_problem.fixed_manifolds_`; equivalent factors can still recover values + `mopt_problem.fixedManifolds`; equivalent factors can still recover values from it, but it is not optimized as a free variable. - Variables untouched by equality constraints are copied as ordinary unconstrained variables. @@ -138,22 +138,22 @@ B_{\theta_c}\mathcal{M}_c = B_{X_c^C}\tilde{\mathcal{M}}_c \, N, paper Eq. (20), where \(N\) is a null-space basis of the constraint Jacobian. Code mapping: -- Abstract basis API: [`TspaceBasis`](TspaceBasis.h#L90) -- Orthonormal/null-space basis: [`OrthonormalBasis`](TspaceBasis.h#L164) -- Constraint Jacobian: [`OrthonormalBasis::computeConstraintJacobian`](TspaceBasis.cpp#L57) -- Dense null-space basis: [`OrthonormalBasis::constructDense`](TspaceBasis.cpp#L108) -- Sparse null-space basis: [`OrthonormalBasis::constructSparse`](TspaceBasis.cpp#L115) -- Map reduced coordinates \(\xi\) to ambient `VectorValues`: [`OrthonormalBasis::computeTangentVector`](TspaceBasis.cpp#L223) +- Abstract basis API: [`TangentSpaceBasis`](TangentSpaceBasis.h#L90) +- Orthonormal/null-space basis: [`OrthonormalBasis`](TangentSpaceBasis.h#L164) +- Constraint Jacobian: [`OrthonormalBasis::computeConstraintJacobian`](TangentSpaceBasis.cpp#L57) +- Dense null-space basis: [`OrthonormalBasis::constructDense`](TangentSpaceBasis.cpp#L108) +- Sparse null-space basis: [`OrthonormalBasis::constructSparse`](TangentSpaceBasis.cpp#L115) +- Map reduced coordinates \(\xi\) to ambient `VectorValues`: [`OrthonormalBasis::computeTangentVector`](TangentSpaceBasis.cpp#L223) Parameterized/basis-variable path: -- Eliminate non-basis variables: [`EliminationBasis::construct`](TspaceBasis.cpp#L492) -- Propagate Bayes-net Jacobians: [`ComputeBayesNetJacobian`](MultiJacobian.cpp#L140) -- Lift reduced coordinates to full tangent values: [`EliminationBasis::computeTangentVector`](TspaceBasis.cpp#L572) +- Eliminate non-basis variables: [`EliminationBasis::construct`](TangentSpaceBasis.cpp#L492) +- Propagate Bayes-net Jacobians: [`computeBayesNetJacobian`](MultiJacobian.cpp#L140) +- Lift reduced coordinates to full tangent values: [`EliminationBasis::computeTangentVector`](TangentSpaceBasis.cpp#L572) The thesis is useful here because it separates the two basis choices. Eqs. (3.7)-(3.11) describe the parameterized/elimination basis; Eqs. (3.12)-(3.14) describe the orthonormal basis. The code supports both through -`TspaceBasisCreator`. +`TangentSpaceBasisCreator`. ## 4) Retraction on Constraint Manifolds (Paper Sec. IV-B, Eqs. 21-24) @@ -165,8 +165,8 @@ The paper gives three practical retraction schemes: | Scheme | Paper equation | Code | | --- | --- | --- | -| Metric projection | Eq. (22) | [`ProjRetractor::retract`](Retractor.cpp#L88) | -| Approximate metric projection | Eq. (23) | [`UoptRetractor::retractConstraints`](Retractor.cpp#L56) | +| Metric projection | Eq. (22) | [`ProjectionRetractor::retract`](Retractor.cpp#L88) | +| Approximate metric projection | Eq. (23) | [`UnconstrainedOptimizationRetractor::retractConstraints`](Retractor.cpp#L56) | | Retract basis variables | Eq. (24) | [`BasisRetractor::retractConstraints`](Retractor.cpp#L154) | Shared flow: @@ -176,9 +176,9 @@ Shared flow: - The retractor then enforces the equality constraints: [`Retractor::retractConstraints`](Retractor.h#L118) How the implementations differ: -- `UoptRetractor` runs unconstrained LM on the penalty graph, matching the +- `UnconstrainedOptimizationRetractor` runs unconstrained LM on the penalty graph, matching the approximate projection idea in paper Eq. (23). -- `ProjRetractor` adds priors around the product-manifold trial point, then can +- `ProjectionRetractor` adds priors around the product-manifold trial point, then can run a cleanup solve without priors. This is the code path closest to the metric-projection objective in paper Eq. (22). - `BasisRetractor` fixes the chosen basis variables at their retracted values @@ -228,14 +228,14 @@ implementation. The paper's optimization target is the transformed unconstrained manifold problem in Eq. (16). In GTDynamics there are two optimizer paths: -- Generic path: [`NonlinearMOptimizer`](NonlinearMOptimizer.cpp#L22) transforms +- Generic path: [`NonlinearManifoldOptimizer`](NonlinearManifoldOptimizer.cpp#L22) transforms the equality-constrained problem, then reuses standard GTSAM nonlinear optimizers on the manifold-valued variables. - Custom LM path: [`LMManifoldOptimizer`](LMManifoldOptimizer.cpp#L43) keeps explicit state/trial bookkeeping for CM-Opt updates. Benchmark helpers: -- `ConstrainedOptBenchmark::OptimizeCmOpt` uses `NonlinearMOptimizer`: [`ConstrainedOptBenchmark.cpp#L514`](../constrained_optimizer/ConstrainedOptBenchmark.cpp#L514) +- `ConstrainedOptBenchmark::OptimizeCmOpt` uses `NonlinearManifoldOptimizer`: [`ConstrainedOptBenchmark.cpp#L514`](../constrained_optimizer/ConstrainedOptBenchmark.cpp#L514) - Dense/null-space plus approximate projection defaults: [`DefaultMoptParams`](../constrained_optimizer/ConstrainedOptBenchmark.cpp#L487) - Elimination basis plus basis-variable retraction defaults: [`DefaultMoptParamsSV`](../constrained_optimizer/ConstrainedOptBenchmark.cpp#L499) @@ -254,7 +254,7 @@ constraint manifold in Eq. (26). In code, approximate behavior appears through retractor settings rather than a separate mathematical type: - limiting inner LM iterations in examples, -- using `UoptRetractor` for approximate metric projection, +- using `UnconstrainedOptimizationRetractor` for approximate metric projection, - using `BasisRetractor` for sparse/basis-variable updates, - fast-pathing already feasible basis-retraction solves in [`BasisRetractor::retractConstraints`](Retractor.cpp#L154). @@ -277,10 +277,10 @@ For a concrete end-to-end run, `examples/example_constraint_manifold/main_connec Read the core implementation in this order: 1. [`ManifoldOptimizer.h`](ManifoldOptimizer.h) and [`ManifoldOptimizer.cpp`](ManifoldOptimizer.cpp) 2. [`ConstraintManifold.h`](ConstraintManifold.h) and [`ConstraintManifold.cpp`](ConstraintManifold.cpp) -3. [`TspaceBasis.h`](TspaceBasis.h), [`TspaceBasis.cpp`](TspaceBasis.cpp), and [`MultiJacobian.cpp`](MultiJacobian.cpp) +3. [`TangentSpaceBasis.h`](TangentSpaceBasis.h), [`TangentSpaceBasis.cpp`](TangentSpaceBasis.cpp), and [`MultiJacobian.cpp`](MultiJacobian.cpp) 4. [`Retractor.h`](Retractor.h) and [`Retractor.cpp`](Retractor.cpp) 5. [`SubstituteFactor.h`](../factors/SubstituteFactor.h) and [`SubstituteFactor.cpp`](../factors/SubstituteFactor.cpp) -6. [`NonlinearMOptimizer.h`](NonlinearMOptimizer.h), [`NonlinearMOptimizer.cpp`](NonlinearMOptimizer.cpp), and optionally [`LMManifoldOptimizer.cpp`](LMManifoldOptimizer.cpp) +6. [`NonlinearManifoldOptimizer.h`](NonlinearManifoldOptimizer.h), [`NonlinearManifoldOptimizer.cpp`](NonlinearManifoldOptimizer.cpp), and optionally [`LMManifoldOptimizer.cpp`](LMManifoldOptimizer.cpp) For inequality boundaries and corners, see the separate CMC-Opt documentation in [`../cmcopt/README.md`](../cmcopt/README.md). diff --git a/gtdynamics/cmopt/Retractor.cpp b/gtdynamics/cmopt/Retractor.cpp index 670f5b922..7b0ff8cca 100644 --- a/gtdynamics/cmopt/Retractor.cpp +++ b/gtdynamics/cmopt/Retractor.cpp @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TspaceBasis.cpp + * @file TangentSpaceBasis.cpp * @brief Tagent space basis implementations. * @author: Yetong Zhang */ @@ -39,21 +39,22 @@ namespace gtdynamics { /* ************************************************************************* */ void Retractor::checkFeasible(const NonlinearFactorGraph &graph, const Values &values) const { - if (params_->check_feasible) { - if (graph.error(values) > params_->feasible_threshold) { + if (params_->checkFeasible) { + if (graph.error(values) > params_->feasibleThreshold) { std::cout << "fail: " << graph.error(values) << "\n"; } } } /* ************************************************************************* */ -UoptRetractor::UoptRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms) +UnconstrainedOptimizationRetractor::UnconstrainedOptimizationRetractor( + const NonlinearEqualityConstraints::shared_ptr &constraints, + const RetractionParams::shared_ptr ¶ms) : Retractor(params), - optimizer_(constraints->penaltyGraph(), params->lm_params) {} + optimizer_(constraints->penaltyGraph(), params->lmParams) {} /* ************************************************************************* */ -Values UoptRetractor::retractConstraints(const Values &values) { +Values UnconstrainedOptimizationRetractor::retractConstraints(const Values &values) { optimizer_.setValues(values); const Values &result = optimizer_.optimize(); checkFeasible(optimizer_.mutableGraph(), result); @@ -61,7 +62,7 @@ Values UoptRetractor::retractConstraints(const Values &values) { } /* ************************************************************************* */ -Values UoptRetractor::retractConstraints(Values &&values) { +Values UnconstrainedOptimizationRetractor::retractConstraints(Values &&values) { optimizer_.setValues(values); auto result = optimizer_.optimize(); checkFeasible(optimizer_.mutableGraph(), result); @@ -69,45 +70,46 @@ Values UoptRetractor::retractConstraints(Values &&values) { } /* ************************************************************************* */ -ProjRetractor::ProjRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, - std::optional basis_keys) +ProjectionRetractor::ProjectionRetractor( + const NonlinearEqualityConstraints::shared_ptr &constraints, + const RetractionParams::shared_ptr ¶ms, + std::optional basis_keys) : Retractor(params), merit_graph_(constraints->penaltyGraph()) { - if (params->use_basis_keys) { + if (params->useBasisKeys) { basis_keys_ = *basis_keys; } } /* ************************************************************************* */ -Values ProjRetractor::retractConstraints(const Values &values) { +Values ProjectionRetractor::retractConstraints(const Values &values) { gtsam::LevenbergMarquardtOptimizer optimizer(merit_graph_, values); return optimizer.optimize(); } /* ************************************************************************* */ -Values ProjRetractor::retract(const Values &values, const VectorValues &delta) { +Values ProjectionRetractor::retract(const Values &values, const VectorValues &delta) { // optimize with priors NonlinearFactorGraph graph = merit_graph_; Values values_retract_base = retractBaseVariables(values, delta); - if (params_->use_basis_keys) { + if (params_->useBasisKeys) { AddGeneralPriors(values_retract_base, basis_keys_, params_->sigma, graph); } else { AddGeneralPriors(values_retract_base, params_->sigma, graph); } // const Values &init_values = - // params_->apply_base_retraction ? values_retract_base : values; + // params_->applyBaseRetraction ? values_retract_base : values; const Values &init_values = values_retract_base; gtsam::LevenbergMarquardtOptimizer optimizer_with_priors(graph, init_values, - params_->lm_params); + params_->lmParams); const Values &result = optimizer_with_priors.optimize(); - if (params_->use_basis_keys && - optimizer_with_priors.error() < params_->feasible_threshold) { + if (params_->useBasisKeys && + optimizer_with_priors.error() < params_->feasibleThreshold) { return result; } // optimize without priors gtsam::LevenbergMarquardtOptimizer optimizer_without_priors( - merit_graph_, result, params_->lm_params); + merit_graph_, result, params_->lmParams); const Values &final_result = optimizer_without_priors.optimize(); checkFeasible(merit_graph_, final_result); return final_result; @@ -116,11 +118,11 @@ Values ProjRetractor::retract(const Values &values, const VectorValues &delta) { /* ************************************************************************* */ BasisRetractor::BasisRetractor( const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, const KeyVector &basis_keys) + const RetractionParams::shared_ptr ¶ms, const KeyVector &basis_keys) : Retractor(params), merit_graph_(constraints->penaltyGraph()), basis_keys_(basis_keys), - optimizer_(params->lm_params) { + optimizer_(params->lmParams) { NonlinearFactorGraph graph; KeySet fixed_keys(basis_keys.begin(), basis_keys.end()); for (const auto &factor : merit_graph_) { @@ -153,7 +155,7 @@ BasisRetractor::BasisRetractor( /* ************************************************************************* */ Values BasisRetractor::retractConstraints(const Values &values) { static const bool debug_retractor_stats = []() { - const char* env = std::getenv("GTDYN_DEBUG_RETRACTOR_STATS"); + const char *env = std::getenv("GTDYN_DEBUG_RETRACTOR_STATS"); return env && std::string(env) != "0"; }(); static size_t total_calls = 0; @@ -176,7 +178,7 @@ Values BasisRetractor::retractConstraints(const Values &values) { // Fast path: if constraints are already satisfied to threshold, avoid // repeatedly running tiny LM solves. const double initial_error = optimizer_.graph().error(opt_values); - if (initial_error <= params_->feasible_threshold) { + if (initial_error <= params_->feasibleThreshold) { ++fast_path_calls; Values result = opt_values; for (const Key &key : basis_keys_) { @@ -213,15 +215,14 @@ Values BasisRetractor::retractConstraints(const Values &values) { } } - // add fixed varaibles to the result + // add fixed variables to the result for (const Key &key : basis_keys_) { result.insert(key, values.at(key)); } const auto end = std::chrono::steady_clock::now(); - total_ms += - std::chrono::duration_cast(end - start) - .count() * - 1e-3; + total_ms += std::chrono::duration_cast(end - start) + .count() * + 1e-3; if (debug_retractor_stats && total_calls % 200 == 0) { std::cout << "[RETRACTOR] calls=" << total_calls << ", fast_path=" << fast_path_calls @@ -284,16 +285,16 @@ void DynamicsRetractor::classifyKeys(const CONTAINER &keys, KeySet &q_keys, /* ************************************************************************* */ DynamicsRetractor::DynamicsRetractor( const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, + const RetractionParams::shared_ptr ¶ms, std::optional basis_keys) : Retractor(params), merit_graph_(constraints->penaltyGraph()), - optimizer_wp_q_(params->lm_params), - optimizer_wp_v_(params->lm_params), - optimizer_wp_ad_(params->lm_params), - optimizer_np_q_(params->lm_params), - optimizer_np_v_(params->lm_params), - optimizer_np_ad_(params->lm_params) { + optimizer_wp_q_(params->lmParams), + optimizer_wp_v_(params->lmParams), + optimizer_wp_ad_(params->lmParams), + optimizer_np_q_(params->lmParams), + optimizer_np_v_(params->lmParams), + optimizer_np_ad_(params->lmParams) { /// classify keys KeySet q_keys, v_keys, ad_keys, qv_keys; classifyKeys(merit_graph_.keys(), q_keys, v_keys, ad_keys); @@ -301,7 +302,7 @@ DynamicsRetractor::DynamicsRetractor( qv_keys.merge(v_keys); /// classify basis keys - if (params_->use_basis_keys) { + if (params_->useBasisKeys) { if (!basis_keys) { throw std::runtime_error("basis keys not provided for retractor."); } @@ -405,13 +406,13 @@ Values DynamicsRetractor::retractConstraints(const Values &values) { // all_basis_keys.merge(basis_ad_keys_); // AddGeneralPriors(values, all_basis_keys, params_->sigma, graph_wp_all); - // LevenbergMarquardtParams lm_params = params_->lm_params; - // lm_params.setMaxIterations(10); + // LevenbergMarquardtParams lmParams = params_->lmParams; + // lmParams.setMaxIterations(10); // LevenbergMarquardtOptimizer optimizer_wp_all(graph_wp_all, known_values, - // params_->lm_params); auto result = optimizer_wp_all.optimize(); + // params_->lmParams); auto result = optimizer_wp_all.optimize(); // LevenbergMarquardtOptimizer optimizer_np_all(cc_->merit_graph_, result, - // params_->lm_params); result = optimizer_np_all.optimize(); + // params_->lmParams); result = optimizer_np_all.optimize(); // checkFeasible(cc_->merit_graph_, result); return known_values; diff --git a/gtdynamics/cmopt/Retractor.h b/gtdynamics/cmopt/Retractor.h index dcefa125a..4c8c6e9bd 100644 --- a/gtdynamics/cmopt/Retractor.h +++ b/gtdynamics/cmopt/Retractor.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TspaceBasis.h + * @file TangentSpaceBasis.h * @brief Basis for tangent space of constraint manifold. Detailed definition of * tangent space and basis are available at Boumal20book Sec.8.4. * @author: Yetong Zhang @@ -15,7 +15,7 @@ #pragma once #include -#include +#include #include #include #include @@ -47,21 +47,21 @@ using gtsam::VectorValues; * * @see README.md#retraction */ -struct RetractParams { +struct RetractionParams { public: - using shared_ptr = std::shared_ptr; + using shared_ptr = std::shared_ptr; // Member variables - bool check_feasible = false; - double feasible_threshold = 1e-5; - LevenbergMarquardtParams lm_params; - bool use_basis_keys = false; + bool checkFeasible = false; + double feasibleThreshold = 1e-5; + LevenbergMarquardtParams lmParams; + bool useBasisKeys = false; double sigma = 1.0; - bool apply_base_retraction = false; + bool applyBaseRetraction = false; bool recompute = false; // Constructor - RetractParams() = default; + RetractionParams() = default; }; /** @@ -77,14 +77,14 @@ struct RetractParams { */ class Retractor { protected: - RetractParams::shared_ptr params_; + RetractionParams::shared_ptr params_; public: using shared_ptr = std::shared_ptr; /// Default constructor. - Retractor(const RetractParams::shared_ptr ¶ms = - std::make_shared()) + Retractor(const RetractionParams::shared_ptr ¶ms = + std::make_shared()) : params_(params) {} virtual ~Retractor() {} @@ -149,7 +149,7 @@ class Retractor { * * @see README.md#retraction */ -class UoptRetractor : public Retractor { +class UnconstrainedOptimizationRetractor : public Retractor { protected: MutableLMOptimizer optimizer_; @@ -159,9 +159,9 @@ class UoptRetractor : public Retractor { * @param constraints Equality constraints for the component. * @param params Retraction parameters. */ - UoptRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms = - std::make_shared()); + UnconstrainedOptimizationRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, + const RetractionParams::shared_ptr ¶ms = + std::make_shared()); /// Retraction operation. Values retractConstraints(const Values &values) override; @@ -175,7 +175,7 @@ class UoptRetractor : public Retractor { * * @see README.md#retraction */ -class ProjRetractor : public Retractor { +class ProjectionRetractor : public Retractor { protected: NonlinearFactorGraph merit_graph_; KeyVector basis_keys_; @@ -185,10 +185,10 @@ class ProjRetractor : public Retractor { * Constructor. * @param constraints Equality constraints for the component. * @param params Retraction parameters. - * @param basis_keys Optional basis keys used when `use_basis_keys` is true. + * @param basis_keys Optional basis keys used when `useBasisKeys` is true. */ - ProjRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, + ProjectionRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, + const RetractionParams::shared_ptr ¶ms, std::optional basis_keys = {}); /** @@ -225,7 +225,7 @@ class BasisRetractor : public Retractor { * @param basis_keys Basis keys held fixed during inner solve. */ BasisRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, + const RetractionParams::shared_ptr ¶ms, const KeyVector &basis_keys); virtual ~BasisRetractor() {} @@ -268,7 +268,7 @@ class DynamicsRetractor : public Retractor { * @param basis_keys Optional basis keys. */ DynamicsRetractor(const NonlinearEqualityConstraints::shared_ptr &constraints, - const RetractParams::shared_ptr ¶ms, + const RetractionParams::shared_ptr ¶ms, std::optional basis_keys = {}); /// Retraction operation. @@ -303,13 +303,13 @@ class DynamicsRetractor : public Retractor { */ class RetractorCreator { protected: - RetractParams::shared_ptr params_; + RetractionParams::shared_ptr params_; public: using shared_ptr = std::shared_ptr; - RetractorCreator(RetractParams::shared_ptr params) : params_(params) {} + RetractorCreator(RetractionParams::shared_ptr params) : params_(params) {} - RetractParams::shared_ptr params() { return params_; } + RetractionParams::shared_ptr params() { return params_; } virtual ~RetractorCreator() {} @@ -318,40 +318,40 @@ class RetractorCreator { }; /** - * Factory for `UoptRetractor`. + * Factory for `UnconstrainedOptimizationRetractor`. * * @see README.md#retraction */ -class UoptRetractorCreator : public RetractorCreator { +class UnconstrainedOptimizationRetractorCreator : public RetractorCreator { public: - UoptRetractorCreator( - RetractParams::shared_ptr params = std::make_shared()) + UnconstrainedOptimizationRetractorCreator( + RetractionParams::shared_ptr params = std::make_shared()) : RetractorCreator(params) {} - virtual ~UoptRetractorCreator() {} + virtual ~UnconstrainedOptimizationRetractorCreator() {} Retractor::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints) const override { - return std::make_shared(constraints, params_); + return std::make_shared(constraints, params_); } }; /** - * Factory for `ProjRetractor`. + * Factory for `ProjectionRetractor`. * * @see README.md#retraction */ -class ProjRetractorCreator : public RetractorCreator { +class ProjectionRetractorCreator : public RetractorCreator { public: - ProjRetractorCreator( - RetractParams::shared_ptr params = std::make_shared()) + ProjectionRetractorCreator( + RetractionParams::shared_ptr params = std::make_shared()) : RetractorCreator(params) {} - virtual ~ProjRetractorCreator() {} + virtual ~ProjectionRetractorCreator() {} Retractor::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints) const override { - return std::make_shared(constraints, params_); + return std::make_shared(constraints, params_); } }; @@ -363,28 +363,28 @@ class ProjRetractorCreator : public RetractorCreator { */ class BasisRetractorCreator : public RetractorCreator { public: - BasisKeyFunc basis_key_func_; + BasisKeyFunction basisKeyFunction; public: BasisRetractorCreator( - RetractParams::shared_ptr params = std::make_shared()) + RetractionParams::shared_ptr params = std::make_shared()) : RetractorCreator(params) {} /** * Constructor with custom basis-key selector. - * @param basis_key_func Callback selecting basis keys. + * @param basisKeyFunction Callback selecting basis keys. * @param params Retraction parameters. */ BasisRetractorCreator( - BasisKeyFunc basis_key_func, - RetractParams::shared_ptr params = std::make_shared()) - : RetractorCreator(params), basis_key_func_(basis_key_func) {} + BasisKeyFunction basisKeyFunction, + RetractionParams::shared_ptr params = std::make_shared()) + : RetractorCreator(params), basisKeyFunction(basisKeyFunction) {} virtual ~BasisRetractorCreator() {} Retractor::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints) const override { - KeyVector basis_keys = basis_key_func_(constraints->keyVector()); + KeyVector basis_keys = basisKeyFunction(constraints->keyVector()); return std::make_shared(constraints, params_, basis_keys); } }; @@ -396,27 +396,27 @@ class BasisRetractorCreator : public RetractorCreator { */ class DynamicsRetractorCreator : public RetractorCreator { protected: - BasisKeyFunc basis_key_func_; + BasisKeyFunction basisKeyFunction; public: - DynamicsRetractorCreator(RetractParams::shared_ptr params) + DynamicsRetractorCreator(RetractionParams::shared_ptr params) : RetractorCreator(params) {} /** * Constructor with custom basis-key selector. * @param params Retraction parameters. - * @param basis_key_func Callback selecting basis keys. + * @param basisKeyFunction Callback selecting basis keys. */ - DynamicsRetractorCreator(RetractParams::shared_ptr params, - BasisKeyFunc basis_key_func) - : RetractorCreator(params), basis_key_func_(basis_key_func) {} + DynamicsRetractorCreator(RetractionParams::shared_ptr params, + BasisKeyFunction basisKeyFunction) + : RetractorCreator(params), basisKeyFunction(basisKeyFunction) {} virtual ~DynamicsRetractorCreator() {} Retractor::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints) const override { - if (params_->use_basis_keys) { - KeyVector basis_keys = basis_key_func_(constraints->keyVector()); + if (params_->useBasisKeys) { + KeyVector basis_keys = basisKeyFunction(constraints->keyVector()); return std::make_shared(constraints, params_, basis_keys); } else { diff --git a/gtdynamics/cmopt/TspaceBasis.cpp b/gtdynamics/cmopt/TangentSpaceBasis.cpp similarity index 92% rename from gtdynamics/cmopt/TspaceBasis.cpp rename to gtdynamics/cmopt/TangentSpaceBasis.cpp index bcf7828b3..f11e66341 100644 --- a/gtdynamics/cmopt/TspaceBasis.cpp +++ b/gtdynamics/cmopt/TangentSpaceBasis.cpp @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TspaceBasis.cpp + * @file TangentSpaceBasis.cpp * @brief Tagent space basis implementations. * @author: Yetong Zhang */ @@ -17,7 +17,7 @@ #include #endif -#include +#include #include #include #include @@ -30,7 +30,7 @@ namespace gtdynamics { /* ************************************************************************* */ -std::vector TspaceBasis::basisVectors() const { +std::vector TangentSpaceBasis::basisVectors() const { std::vector basis_vectors; basis_vectors.reserve(dim()); for (size_t i = 0; i < dim(); i++) { @@ -71,8 +71,8 @@ Matrix OrthonormalBasis::computeConstraintJacobian(const Values &values) const { /* ************************************************************************* */ OrthonormalBasis::OrthonormalBasis( const NonlinearEqualityConstraints::shared_ptr &constraints, - const Values &values, const TspaceBasisParams::shared_ptr ¶ms) - : TspaceBasis(params), attributes_(std::make_shared()) { + const Values &values, const TangentSpaceBasisParams::shared_ptr ¶ms) + : TangentSpaceBasis(params), attributes_(std::make_shared()) { // set attributes attributes_->total_constraint_dim = constraints->dim(); attributes_->total_var_dim = values.dim(); @@ -86,7 +86,7 @@ OrthonormalBasis::OrthonormalBasis( attributes_->var_location[key] = position; position += var_dim; } - if (params_->always_construct_basis) { + if (params_->alwaysConstructBasis) { construct(values); } } @@ -97,7 +97,7 @@ void OrthonormalBasis::construct(const Values &values) { basis_ = Matrix::Identity(attributes_->total_basis_dim, attributes_->total_basis_dim); } else { - if (params_->use_sparse) { + if (params_->useSparse) { constructSparse(values); } else { constructDense(values); @@ -126,17 +126,17 @@ void OrthonormalBasis::constructSparse(const Values &values) { cholmod_common *cc = &common; cholmod_l_start(cc); - cholmod_sparse *A_t = SparseJacobianTranspose(nrows, ncols, triplets, cc); + cholmod_sparse *A_t = sparseJacobianTranspose(nrows, ncols, triplets, cc); SuiteSparseQR_factorization *QR = SuiteSparseQR_factorize( SPQR_ORDERING_DEFAULT, SPQR_DEFAULT_TOL, A_t, cc); - cholmod_sparse *selection_mat = LastColsSelectionMat( + cholmod_sparse *selection_mat = lastColsSelectionMatrix( attributes_->total_var_dim, attributes_->total_basis_dim, cc); cholmod_sparse *basis_cholmod = SuiteSparseQR_qmult(SPQR_QX, QR, selection_mat, cc); - basis_ = CholmodToEigen(basis_cholmod, cc); + basis_ = cholmodToEigen(basis_cholmod, cc); cholmod_l_free_sparse(&A_t, cc); SuiteSparseQR_free(&QR, cc); @@ -144,21 +144,21 @@ void OrthonormalBasis::constructSparse(const Values &values) { cholmod_l_free_sparse(&basis_cholmod, cc); cholmod_l_finish(cc); #else - SpMatrix A_t = SparseJacobianTranspose(nrows, ncols, triplets); + SpMatrix A_t = sparseJacobianTranspose(nrows, ncols, triplets); Eigen::SparseQR> qr; qr.compute(A_t); if (qr.info() != Eigen::Success) { throw std::runtime_error("Eigen SparseQR failed for tangent basis."); } - SpMatrix selection_mat = LastColsSelectionMat(attributes_->total_var_dim, + SpMatrix selection_mat = lastColsSelectionMatrix(attributes_->total_var_dim, attributes_->total_basis_dim); basis_ = qr.matrixQ() * Matrix(selection_mat); #endif } /* ************************************************************************* */ -TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraints( +TangentSpaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraints( const NonlinearEqualityConstraints &constraints, const Values &values, bool create_from_scratch) const { // attributes @@ -186,7 +186,7 @@ TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraints( } auto new_linear_graph = new_merit_graph.linearize(values); - if (params_->use_sparse) { + if (params_->useSparse) { return createWithAdditionalConstraintsSparse(*new_linear_graph, new_attributes); } else { @@ -196,7 +196,7 @@ TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraints( } /* ************************************************************************* */ -TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsDense( +TangentSpaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsDense( const GaussianFactorGraph &graph, const Attributes::shared_ptr &new_attributes) const { gtsam::JacobianFactor combined(graph); @@ -210,7 +210,7 @@ TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsDense( } /* ************************************************************************* */ -TspaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsSparse( +TangentSpaceBasis::shared_ptr OrthonormalBasis::createWithAdditionalConstraintsSparse( const GaussianFactorGraph &graph, const Attributes::shared_ptr &new_attributes) const { SpMatrix A_new = eigenSparseJacobian(graph); @@ -283,7 +283,7 @@ void PrintSparse(cholmod_sparse *A, cholmod_common *cc) { /* ************************************************************************* */ #if defined(GTDYNAMICS_WITH_SUITESPARSE) -cholmod_sparse *OrthonormalBasis::SparseJacobianTranspose( +cholmod_sparse *OrthonormalBasis::sparseJacobianTranspose( const size_t nrows, const size_t ncols, const std::vector> &triplets, cholmod_common *cc) { @@ -317,7 +317,7 @@ cholmod_sparse *OrthonormalBasis::SparseJacobianTranspose( } /* ************************************************************************* */ -cholmod_sparse *OrthonormalBasis::LastColsSelectionMat(const size_t nrows, +cholmod_sparse *OrthonormalBasis::lastColsSelectionMatrix(const size_t nrows, const size_t ncols, cholmod_common *cc) { int A_stype = 0; @@ -339,7 +339,7 @@ cholmod_sparse *OrthonormalBasis::LastColsSelectionMat(const size_t nrows, } /* ************************************************************************* */ -OrthonormalBasis::SpMatrix OrthonormalBasis::CholmodToEigen( +OrthonormalBasis::SpMatrix OrthonormalBasis::cholmodToEigen( cholmod_sparse *A, cholmod_common *cc) { cholmod_triplet *T = cholmod_l_sparse_to_triplet(A, cc); std::vector> triplets; @@ -355,7 +355,7 @@ OrthonormalBasis::SpMatrix OrthonormalBasis::CholmodToEigen( return A_eigen; } #else -OrthonormalBasis::SpMatrix OrthonormalBasis::SparseJacobianTranspose( +OrthonormalBasis::SpMatrix OrthonormalBasis::sparseJacobianTranspose( const size_t nrows, const size_t ncols, const std::vector> &triplets) { const size_t last_col = ncols - 1; @@ -373,7 +373,7 @@ OrthonormalBasis::SpMatrix OrthonormalBasis::SparseJacobianTranspose( return A_t; } -OrthonormalBasis::SpMatrix OrthonormalBasis::LastColsSelectionMat( +OrthonormalBasis::SpMatrix OrthonormalBasis::lastColsSelectionMatrix( const size_t nrows, const size_t ncols) { std::vector> entries; entries.reserve(ncols); @@ -389,7 +389,7 @@ OrthonormalBasis::SpMatrix OrthonormalBasis::LastColsSelectionMat( #endif /* ************************************************************************* */ -OrthonormalBasis::SpMatrix OrthonormalBasis::EigenSparseJacobian( +OrthonormalBasis::SpMatrix OrthonormalBasis::eigenSparseJacobianFromGraph( const GaussianFactorGraph &graph) { Ordering ordering(graph.keys()); size_t rows, cols; @@ -436,9 +436,9 @@ OrthonormalBasis::SpMatrix OrthonormalBasis::eigenSparseJacobian( /* ************************************************************************* */ EliminationBasis::EliminationBasis( const NonlinearEqualityConstraints::shared_ptr &constraints, - const Values &values, const TspaceBasisParams::shared_ptr ¶ms, + const Values &values, const TangentSpaceBasisParams::shared_ptr ¶ms, std::optional basis_keys) - : TspaceBasis(params), attributes_(std::make_shared()) { + : TangentSpaceBasis(params), attributes_(std::make_shared()) { // Structural setup happens once for this basis object. It records the // component dimension, per-variable dimensions, and the constraint merit // graph used later for numeric construction. This part is not repeated by @@ -467,7 +467,7 @@ EliminationBasis::EliminationBasis( } else { // The no-basis-key path would require automatically choosing independent // variables. That symbolic choice is not implemented; all production uses - // route through EliminationBasisCreator with use_basis_keys enabled. + // route through EliminationBasisCreator with useBasisKeys enabled. throw std::runtime_error( "elimination basis without keys not implemented."); } @@ -488,7 +488,7 @@ EliminationBasis::EliminationBasis( // flag is false, ConstraintManifold::makeSureBasisConstructed will call // construct lazily on first recover-with-Jacobian, localCoordinates, or // retract use. - if (params_->always_construct_basis) { + if (params_->alwaysConstructBasis) { construct(values); } } @@ -496,11 +496,11 @@ EliminationBasis::EliminationBasis( /* ************************************************************************* */ EliminationBasis::EliminationBasis(const Values &values, const EliminationBasis &other) - : TspaceBasis(other.params_), attributes_(other.attributes_) { + : TangentSpaceBasis(other.params_), attributes_(other.attributes_) { // This path is used after a manifold moves to new values. The symbolic // structure is reused, but the numeric Jacobians are value-dependent and are // rebuilt here only for eager-construction configurations. - if (params_->always_construct_basis) { + if (params_->alwaysConstructBasis) { construct(values); } } @@ -524,13 +524,13 @@ void EliminationBasis::construct(const Values &values) { // Convert Bayes-net conditionals into recovery Jacobians d(non-basis)/d(basis) // used by computeTangentVector and recoverJacobian. After this cache is // filled, per-retraction tangent lifting is just block multiplication. - ComputeBayesNetJacobian(*bayes_net, attributes_->basis_keys, + computeBayesNetJacobian(*bayes_net, attributes_->basis_keys, attributes_->var_dim, jacobians_); is_constructed_ = true; } /* ************************************************************************* */ -TspaceBasis::shared_ptr EliminationBasis::createWithAdditionalConstraints( +TangentSpaceBasis::shared_ptr EliminationBasis::createWithAdditionalConstraints( const NonlinearEqualityConstraints &constraints, const Values &values, bool create_from_scratch) const { // This path is used when active inequality constraints are temporarily added @@ -565,7 +565,7 @@ TspaceBasis::shared_ptr EliminationBasis::createWithAdditionalConstraints( // linear_graph.eliminatePartialSequential(new_ordering, EliminateQR); // auto bayes_net = elim_result.first; // MultiJacobians new_jacobians; - // ComputeBayesNetJacobian(*bayes_net, new_basis_keys, var_dim_, + // computeBayesNetJacobian(*bayes_net, new_basis_keys, var_dim_, // new_jacobians); Ordering new_ordering(new_constraint_keys.begin(), new_constraint_keys.end()); @@ -598,7 +598,7 @@ TspaceBasis::shared_ptr EliminationBasis::createWithAdditionalConstraints( for (const Key &key : new_constraint_keys) { new_jacobians.insert({key, MultiJacobian()}); } - new_basis->jacobians_ = JacobiansMultiply(jacobians_, new_jacobians); + new_basis->jacobians_ = multiplyJacobians(jacobians_, new_jacobians); new_basis->is_constructed_ = true; } return new_basis; diff --git a/gtdynamics/cmopt/TspaceBasis.h b/gtdynamics/cmopt/TangentSpaceBasis.h similarity index 82% rename from gtdynamics/cmopt/TspaceBasis.h rename to gtdynamics/cmopt/TangentSpaceBasis.h index f1a4d687f..ca83f60c5 100644 --- a/gtdynamics/cmopt/TspaceBasis.h +++ b/gtdynamics/cmopt/TangentSpaceBasis.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file TspaceBasis.h + * @file TangentSpaceBasis.h * @brief Basis for tangent space of constraint manifold. Detailed definition of * tangent space and basis are available at Boumal20book Sec.8.4. * @author: Yetong Zhang @@ -49,7 +49,7 @@ using gtsam::Values; using gtsam::Vector; using gtsam::VectorValues; -typedef std::function BasisKeyFunc; +typedef std::function BasisKeyFunction; /** * Parameters for tangent-space basis construction. @@ -59,24 +59,24 @@ typedef std::function BasisKeyFunc; * * @see README.md#tangent-basis */ -struct TspaceBasisParams { +struct TangentSpaceBasisParams { public: - using shared_ptr = std::shared_ptr; + using shared_ptr = std::shared_ptr; /// Member variables. - bool always_construct_basis = true; - bool use_basis_keys = false; - bool use_sparse = false; + bool alwaysConstructBasis = true; + bool useBasisKeys = false; + bool useSparse = false; /** * Constructor. - * @param _use_basis_keys If true, basis keys are explicitly selected. - * @param _always_construct_basis If true, build basis immediately. + * @param _useBasisKeys If true, basis keys are explicitly selected. + * @param _alwaysConstructBasis If true, build basis immediately. */ - TspaceBasisParams(bool _use_basis_keys = false, - bool _always_construct_basis = true) - : always_construct_basis(_always_construct_basis), - use_basis_keys(_use_basis_keys) {} + TangentSpaceBasisParams(bool _useBasisKeys = false, + bool _alwaysConstructBasis = true) + : alwaysConstructBasis(_alwaysConstructBasis), + useBasisKeys(_useBasisKeys) {} }; /** @@ -87,21 +87,21 @@ struct TspaceBasisParams { * * @see README.md#tangent-basis */ -class TspaceBasis { +class TangentSpaceBasis { protected: - TspaceBasisParams::shared_ptr params_; + TangentSpaceBasisParams::shared_ptr params_; bool is_constructed_; public: - using shared_ptr = std::shared_ptr; + using shared_ptr = std::shared_ptr; /// Default constructor. - TspaceBasis(TspaceBasisParams::shared_ptr params = - std::make_shared()) + TangentSpaceBasis(TangentSpaceBasisParams::shared_ptr params = + std::make_shared()) : params_(params), is_constructed_(false) {} /// Default destructor. - virtual ~TspaceBasis() {} + virtual ~TangentSpaceBasis() {} /// Construct new basis by using new values virtual shared_ptr createWithNewValues(const Values &values) const = 0; @@ -123,7 +123,7 @@ class TspaceBasis { /** Given a tangent vector, compute a vector xi representing the magnitude of * basis components. */ - virtual Vector computeXi(const VectorValues &tangent_vector) const = 0; + virtual Vector computeXi(const VectorValues &tangentVector) const = 0; /** Compute the jacobian of recover function. i.e., function that recovers an * original variable from the constraint manifold. */ @@ -161,7 +161,7 @@ class TspaceBasis { * * @see README.md#tangent-basis */ -class OrthonormalBasis : public TspaceBasis { +class OrthonormalBasis : public TangentSpaceBasis { protected: // Structural properties that do not change with values. struct Attributes { @@ -187,16 +187,16 @@ class OrthonormalBasis : public TspaceBasis { */ OrthonormalBasis(const NonlinearEqualityConstraints::shared_ptr &constraints, const Values &values, - const TspaceBasisParams::shared_ptr ¶ms); + const TangentSpaceBasisParams::shared_ptr ¶ms); /** * Constructor from precomputed attributes. * @param params Basis construction parameters. * @param attributes Precomputed structural attributes. */ - OrthonormalBasis(const TspaceBasisParams::shared_ptr ¶ms, + OrthonormalBasis(const TangentSpaceBasisParams::shared_ptr ¶ms, const Attributes::shared_ptr &attributes) - : TspaceBasis(params), attributes_(attributes) {} + : TangentSpaceBasis(params), attributes_(attributes) {} /** * Constructor from precomputed attributes and matrix. @@ -204,9 +204,9 @@ class OrthonormalBasis : public TspaceBasis { * @param attributes Precomputed structural attributes. * @param mat Basis matrix. */ - OrthonormalBasis(const TspaceBasisParams::shared_ptr ¶ms, + OrthonormalBasis(const TangentSpaceBasisParams::shared_ptr ¶ms, const Attributes::shared_ptr &attributes, const Matrix &mat) - : TspaceBasis(params), attributes_(attributes), basis_(mat) {} + : TangentSpaceBasis(params), attributes_(attributes), basis_(mat) {} /** * Constructor from another basis, reusing structural attributes. @@ -214,14 +214,14 @@ class OrthonormalBasis : public TspaceBasis { * @param other Source basis. */ OrthonormalBasis(const Values &values, const OrthonormalBasis &other) - : TspaceBasis(other.params_), attributes_(other.attributes_) { - if (params_->always_construct_basis) { + : TangentSpaceBasis(other.params_), attributes_(other.attributes_) { + if (params_->alwaysConstructBasis) { construct(values); } } /// Create basis with new values. - TspaceBasis::shared_ptr createWithNewValues( + TangentSpaceBasis::shared_ptr createWithNewValues( const Values &values) const override { return std::make_shared(values, *this); } @@ -233,7 +233,7 @@ class OrthonormalBasis : public TspaceBasis { * @param create_from_scratch If true, rebuild basis from scratch. * @return New basis object. */ - TspaceBasis::shared_ptr createWithAdditionalConstraints( + TangentSpaceBasis::shared_ptr createWithAdditionalConstraints( const NonlinearEqualityConstraints &constraints, const Values &values, bool create_from_scratch = false) const override; @@ -244,7 +244,7 @@ class OrthonormalBasis : public TspaceBasis { VectorValues computeTangentVector(const Vector &xi) const override; /// Compute xi of basis components. - Vector computeXi(const VectorValues &tangent_vector) const override; + Vector computeXi(const VectorValues &tangentVector) const override; /// Jacobian of recover function. Matrix recoverJacobian(const Key &key) const override; @@ -280,7 +280,7 @@ class OrthonormalBasis : public TspaceBasis { * @param new_attributes Updated structural attributes. * @return New orthonormal basis. */ - TspaceBasis::shared_ptr createWithAdditionalConstraintsDense( + TangentSpaceBasis::shared_ptr createWithAdditionalConstraintsDense( const GaussianFactorGraph &graph, const Attributes::shared_ptr &new_attributes) const; @@ -290,7 +290,7 @@ class OrthonormalBasis : public TspaceBasis { * @param new_attributes Updated structural attributes. * @return New orthonormal basis. */ - TspaceBasis::shared_ptr createWithAdditionalConstraintsSparse( + TangentSpaceBasis::shared_ptr createWithAdditionalConstraintsSparse( const GaussianFactorGraph &graph, const Attributes::shared_ptr &new_attributes) const; @@ -313,7 +313,7 @@ class OrthonormalBasis : public TspaceBasis { * @return Transposed sparse Jacobian matrix. */ #if defined(GTDYNAMICS_WITH_SUITESPARSE) - static cholmod_sparse *SparseJacobianTranspose( + static cholmod_sparse *sparseJacobianTranspose( const size_t nrows, const size_t ncols, const std::vector> &triplets, cholmod_common *cc); @@ -325,7 +325,7 @@ class OrthonormalBasis : public TspaceBasis { * @param cc CHOLMOD context. * @return Sparse selector matrix. */ - static cholmod_sparse *LastColsSelectionMat(const size_t nrows, + static cholmod_sparse *lastColsSelectionMatrix(const size_t nrows, const size_t ncols, cholmod_common *cc); @@ -335,7 +335,7 @@ class OrthonormalBasis : public TspaceBasis { * @param cc CHOLMOD context. * @return Eigen sparse matrix. */ - static SpMatrix CholmodToEigen(cholmod_sparse *A, cholmod_common *cc); + static SpMatrix cholmodToEigen(cholmod_sparse *A, cholmod_common *cc); #else /** * Construct transposed sparse Jacobian from sparse triplets. @@ -344,7 +344,7 @@ class OrthonormalBasis : public TspaceBasis { * @param triplets Sparse Jacobian triplets. * @return Transposed sparse Jacobian matrix. */ - static SpMatrix SparseJacobianTranspose( + static SpMatrix sparseJacobianTranspose( const size_t nrows, const size_t ncols, const std::vector> &triplets); @@ -354,7 +354,7 @@ class OrthonormalBasis : public TspaceBasis { * @param ncols Number of selected columns. * @return Sparse selector matrix. */ - static SpMatrix LastColsSelectionMat(const size_t nrows, const size_t ncols); + static SpMatrix lastColsSelectionMatrix(const size_t nrows, const size_t ncols); #endif SpMatrix eigenSparseJacobian(const GaussianFactorGraph &graph) const; @@ -363,7 +363,7 @@ class OrthonormalBasis : public TspaceBasis { /** Return jacobian of a Gaussian factor graph represented as an Eigen sparse * matrix. */ - static SpMatrix EigenSparseJacobian(const GaussianFactorGraph &graph); + static SpMatrix eigenSparseJacobianFromGraph(const GaussianFactorGraph &graph); }; /** @@ -382,14 +382,14 @@ class OrthonormalBasis : public TspaceBasis { * - The constructor builds structural data once for a component: variable * dimensions, basis-key offsets, the constraint merit graph, and a symbolic * elimination ordering. - * - If `params->always_construct_basis` is true, the constructor also calls + * - If `params->alwaysConstructBasis` is true, the constructor also calls * `construct(values)`. Otherwise construction is lazy and happens when a * caller such as `ConstraintManifold::retract`, `recover` with Jacobians, or * `localCoordinates` first needs the basis. * - `createWithNewValues` is used whenever a manifold is rebuilt at new values, * including after retractions and across nonlinear optimization iterations. * It reuses the structural attributes but may reconstruct the numeric - * Jacobians depending on `always_construct_basis`. + * Jacobians depending on `alwaysConstructBasis`. * * Cost model: * - Structural setup is dominated by basis-key bookkeeping and the constrained @@ -406,7 +406,7 @@ class OrthonormalBasis : public TspaceBasis { * @see README.md#tangent-basis * @see README.md#retraction */ -class EliminationBasis : public TspaceBasis { +class EliminationBasis : public TangentSpaceBasis { protected: struct Attributes { NonlinearFactorGraph merit_graph; ///< Constraint penalty graph. @@ -426,7 +426,7 @@ class EliminationBasis : public TspaceBasis { * Construct an elimination basis for one constraint-connected component. * * This constructor always builds the structural attributes and, when - * `params->always_construct_basis` is true, immediately calls + * `params->alwaysConstructBasis` is true, immediately calls * `construct(values)`. Explicit `basis_keys` are required by the current * implementation; omitting them throws at runtime. The selected keys must * have total dimension `values.dim() - constraints->dim()`. @@ -442,7 +442,7 @@ class EliminationBasis : public TspaceBasis { */ EliminationBasis(const NonlinearEqualityConstraints::shared_ptr &constraints, const Values &values, - const TspaceBasisParams::shared_ptr ¶ms, + const TangentSpaceBasisParams::shared_ptr ¶ms, std::optional basis_keys = {}); /** @@ -452,7 +452,7 @@ class EliminationBasis : public TspaceBasis { * when `ConstraintManifold` or `IEConstraintManifold` creates a new manifold * state after retraction or between nonlinear iterations. It avoids * recomputing basis-key offsets and the symbolic ordering. If - * `always_construct_basis` is true, it still recomputes the numeric recovery + * `alwaysConstructBasis` is true, it still recomputes the numeric recovery * Jacobians at `values`. * * @param values Current values for the same component structure. @@ -471,22 +471,22 @@ class EliminationBasis : public TspaceBasis { * @param params Basis construction parameters. * @param attributes Shared structural data for the new basis. */ - EliminationBasis(const TspaceBasisParams::shared_ptr ¶ms, + EliminationBasis(const TangentSpaceBasisParams::shared_ptr ¶ms, const Attributes::shared_ptr &attributes) - : TspaceBasis(params), attributes_(attributes) {} + : TangentSpaceBasis(params), attributes_(attributes) {} /** * Create a basis at new values with the same structural attributes. * * This is called when a manifold is rebuilt after a retraction or nonlinear - * trial/update. It is cheap when `always_construct_basis` is false, and + * trial/update. It is cheap when `alwaysConstructBasis` is false, and * otherwise costs one call to `construct(values)`. * * @param values Current values for the same component structure. * @return Basis sharing structure with this basis and updated numerics if * requested by the parameters. */ - TspaceBasis::shared_ptr createWithNewValues( + TangentSpaceBasis::shared_ptr createWithNewValues( const Values &values) const override { return std::make_shared(values, *this); } @@ -510,7 +510,7 @@ class EliminationBasis : public TspaceBasis { * @param create_from_scratch If true, rebuild basis from scratch. * @return New basis object. */ - TspaceBasis::shared_ptr createWithAdditionalConstraints( + TangentSpaceBasis::shared_ptr createWithAdditionalConstraints( const NonlinearEqualityConstraints &constraints, const Values &values, bool create_from_scratch = false) const override; @@ -538,10 +538,10 @@ class EliminationBasis : public TspaceBasis { * they are dependent coordinates. Cost is linear in the total basis * dimension. * - * @param tangent_vector Ambient tangent vector. + * @param tangentVector Ambient tangent vector. * @return Reduced coordinates in basis-key order. */ - Vector computeXi(const VectorValues &tangent_vector) const override; + Vector computeXi(const VectorValues &tangentVector) const override; /** * Return the recover Jacobian for a variable. @@ -582,7 +582,7 @@ class EliminationBasis : public TspaceBasis { * non-basis variables with QR, and extracts Bayes-net Jacobians. It is the * expensive numeric step and may run at basis construction time, lazily on * first use, or when a new manifold state is created, depending on - * `always_construct_basis`. + * `alwaysConstructBasis`. * * @param values Current linearization point. */ @@ -608,15 +608,15 @@ class EliminationBasis : public TspaceBasis { * * @see README.md#tangent-basis */ -class TspaceBasisCreator { +class TangentSpaceBasisCreator { protected: - TspaceBasisParams::shared_ptr params_; + TangentSpaceBasisParams::shared_ptr params_; public: - using shared_ptr = std::shared_ptr; - TspaceBasisCreator(TspaceBasisParams::shared_ptr params) : params_(params) {} + using shared_ptr = std::shared_ptr; + TangentSpaceBasisCreator(TangentSpaceBasisParams::shared_ptr params) : params_(params) {} - virtual ~TspaceBasisCreator() {} + virtual ~TangentSpaceBasisCreator() {} /** * Create a basis object for a component. @@ -624,7 +624,7 @@ class TspaceBasisCreator { * @param values Current values for variables in the component. * @return Created basis object. */ - virtual TspaceBasis::shared_ptr create( + virtual TangentSpaceBasis::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints, const Values &values) const = 0; }; @@ -634,21 +634,21 @@ class TspaceBasisCreator { * * @see README.md#tangent-basis */ -class OrthonormalBasisCreator : public TspaceBasisCreator { +class OrthonormalBasisCreator : public TangentSpaceBasisCreator { public: - OrthonormalBasisCreator(TspaceBasisParams::shared_ptr params = - std::make_shared()) - : TspaceBasisCreator(params) {} + OrthonormalBasisCreator(TangentSpaceBasisParams::shared_ptr params = + std::make_shared()) + : TangentSpaceBasisCreator(params) {} - static TspaceBasisCreator::shared_ptr CreateSparse() { - auto params = std::make_shared(); - params->use_sparse = true; + static TangentSpaceBasisCreator::shared_ptr createSparse() { + auto params = std::make_shared(); + params->useSparse = true; return std::make_shared(params); } virtual ~OrthonormalBasisCreator() {} - TspaceBasis::shared_ptr create( + TangentSpaceBasis::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints, const Values &values) const override { return std::make_shared(constraints, values, params_); @@ -660,42 +660,42 @@ class OrthonormalBasisCreator : public TspaceBasisCreator { * * This is the normal construction path used by `ConstraintManifold` params. At * manifold construction time, `create` receives the keys in one - * constraint-connected component, applies `basis_key_func_`, and creates an + * constraint-connected component, applies `basisKeyFunction`, and creates an * `EliminationBasis` with those independent coordinates. The factory itself is * cheap; all graph-ordering and numeric construction costs are paid by the * basis object it creates. * * @see README.md#tangent-basis */ -class EliminationBasisCreator : public TspaceBasisCreator { +class EliminationBasisCreator : public TangentSpaceBasisCreator { protected: - BasisKeyFunc basis_key_func_; + BasisKeyFunction basisKeyFunction; public: /** * Construct a factory without an explicit key selector. * * This constructor is retained for API compatibility, but normal callers - * should use the constructor that accepts a `BasisKeyFunc`. With the default - * parameters, `create` needs a key selector; with `use_basis_keys=false`, the + * should use the constructor that accepts a `BasisKeyFunction`. With the default + * parameters, `create` needs a key selector; with `useBasisKeys=false`, the * current `EliminationBasis` implementation reaches the no-key path, which * is not implemented. * * @param params Basis construction parameters. */ - EliminationBasisCreator(TspaceBasisParams::shared_ptr params = - std::make_shared(true)) - : TspaceBasisCreator(params) {} + EliminationBasisCreator(TangentSpaceBasisParams::shared_ptr params = + std::make_shared(true)) + : TangentSpaceBasisCreator(params) {} /** * Create an elimination-basis factory with custom key selector. - * @param basis_key_func Callback selecting basis keys. + * @param basisKeyFunction Callback selecting basis keys. * @param params Basis construction parameters. */ - EliminationBasisCreator(BasisKeyFunc basis_key_func, - TspaceBasisParams::shared_ptr params = - std::make_shared(true)) - : TspaceBasisCreator(params), basis_key_func_(basis_key_func) {} + EliminationBasisCreator(BasisKeyFunction basisKeyFunction, + TangentSpaceBasisParams::shared_ptr params = + std::make_shared(true)) + : TangentSpaceBasisCreator(params), basisKeyFunction(basisKeyFunction) {} virtual ~EliminationBasisCreator() {} @@ -707,17 +707,17 @@ class EliminationBasisCreator : public TspaceBasisCreator { * tangent lift, but it may be called again when a new manifold object is * created for updated values or active constraints. The returned basis * controls whether numeric construction happens immediately or lazily through - * `TspaceBasisParams::always_construct_basis`. + * `TangentSpaceBasisParams::alwaysConstructBasis`. * * @param constraints Equality constraints for the component. * @param values Current values for variables in the component. * @return Created elimination basis. */ - TspaceBasis::shared_ptr create( + TangentSpaceBasis::shared_ptr create( const NonlinearEqualityConstraints::shared_ptr constraints, const Values &values) const override { - if (params_->use_basis_keys) { - KeyVector basis_keys = basis_key_func_(values.keys()); + if (params_->useBasisKeys) { + KeyVector basis_keys = basisKeyFunction(values.keys()); return std::make_shared(constraints, values, params_, basis_keys); } diff --git a/gtdynamics/cmopt/retraction_choices.html b/gtdynamics/cmopt/retraction_choices.html new file mode 100644 index 000000000..300b2dfdb --- /dev/null +++ b/gtdynamics/cmopt/retraction_choices.html @@ -0,0 +1,817 @@ + + + + + + CM-OPT Retraction Choices in GTDynamics + + + +
+
+

CM-OPT Retraction Choices in GTDynamics

+

+ This note explains how CM-OPT chooses a retraction in GTDynamics, what each + of the four implemented retraction policies solves, and where the mechanics + live in the code. It is written for the equality-constrained CM-OPT path in + gtdynamics/cmopt. +

+
+ +
+

Where Retraction Fits in CM-OPT

+

+ CM-OPT rewrites an equality-constrained problem + min f(X) subject to h(X) = 0 as an unconstrained optimization + over constraint-connected components. Each component becomes one + ConstraintManifold value. Ordinary cost factors are rewritten + as factors over those manifold values, and GTSAM's nonlinear optimizers can + then update the transformed problem. +

+

+ The retraction policy is not selected inside the outer nonlinear optimizer. + It is selected when each ConstraintManifold is constructed: + ConstraintManifold::Params::retractor_creator creates the + concrete Retractor. The default parameter object chooses + UoptRetractorCreator. +

+ +
+
+ 1. Reduced step + Optimizer computes xi in the component tangent coordinates. +
+
+ 2. Lift + TspaceBasis maps xi to ambient + VectorValues. +
+
+ 3. Base retract + Each original variable applies its own GTSAM retraction. +
+
+ 4. Restore constraints + The chosen Retractor solves a small constraint-restoration + problem. +
+
+ 5. Rebuild manifold + A new ConstraintManifold is created at the updated values. +
+
+ +

Primary Code Path

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
MechanicCode locationWhat to look for
Select basis and retractorConstraintManifold.h:70ConstraintManifold::Params contains basis_creator and retractor_creator.
Default retractorConstraintManifold.h:76The default is UoptRetractorCreator.
Create concrete retractorConstraintManifold.h:113The factory is called with the component's equality constraints.
Initialize feasible valuesConstraintManifold.cpp:23If retract_init is true, construction calls retractConstraints.
Retract during optimizationConstraintManifold.cpp:45xi is lifted, then retractor_->retract(values_, delta) is called.
Common base retractionRetractor.h:98values.retract(delta) applies each base variable's own manifold retraction.
Common constraint restoration hookRetractor.h:117The base-retracted trial point is passed to retractConstraints.
+
+ +
+

The Four Retraction Choices

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ChoiceWhat it solvesBest fitMain code
UoptRetractorUnconstrained LM on the equality-constraint penalty graph.Simple default, approximate projection, small problems where all constrained variables may move.Retractor.cpp:56
ProjRetractorMetric-projection style solve using priors around the base-retracted trial point, followed by optional cleanup.Small problems where the closest feasible point to the proposed step matters.Retractor.cpp:88
BasisRetractorFix selected basis variables at their trial values and solve constraints for the remaining variables.Sparse or structured problems with known independent coordinates.Retractor.cpp:154
DynamicsRetractorHierarchical kinodynamic solve: positions, then velocities, then acceleration/dynamics variables.Mechanics problems with q/v/acceleration structure and level-wise dependencies.Retractor.cpp:364
+
+ +
+

1. UoptRetractor: Unconstrained Penalty Projection

+
+ default + all variables free + approximate projection +
+

+ UoptRetractor is the default CM-OPT retraction. It takes the + base-retracted trial values and runs LM on the equality constraint penalty + graph. There are no priors tying the result to the trial point, so all + variables in the connected component may move as long as the constraint + residual is reduced. +

+

+ The constructor creates a reusable MutableLMOptimizer from + constraints->penaltyGraph(). Each retraction resets the + optimizer values to the current trial point and calls optimize(). +

+ +

Mechanics

+
    +
  1. Retractor::retract first computes values.retract(delta).
    2. +
  2. UoptRetractor::retractConstraints sets those values into the mutable LM optimizer.
    3. +
  3. The optimizer minimizes only the penalty graph generated from the equality constraints.
    4. +
  4. The returned values replace the component values for the next manifold state.
    5. +
+ +

Code Locations

+ + +

Choosing It

+

+ Choose this for the simplest CM-OPT setup, especially with the orthonormal + null-space basis. It is also a reasonable first choice for a small + constrained mechanics problem when you care more about getting back near + feasibility than preserving a specific independent coordinate exactly. +

+

+ Do not choose it when the proposed step has a specific physical meaning + that must be retained, for example "this joint coordinate is the independent + generalized coordinate." Because there are no anchoring priors, the solve + can redistribute motion across the component. +

+ + 
auto params = std::make_shared<RetractParams>();
+params->lm_params.setMaxIterations(10);
+params->check_feasible = true;
+
+moptParams.cc_params->retractor_creator =
+    std::make_shared<UoptRetractorCreator>(params);
+
+ +
+

2. ProjRetractor: Metric-Projection Style Retraction

+
+ closest feasible trial + uses priors + small-problem friendly +
+

+ ProjRetractor is the code path closest to the metric projection + described in CM-OPT. It starts from the product-manifold trial point, + appends Gaussian priors centered at that trial point, and solves a graph + containing both equality-constraint penalties and those priors. The priors + define the projection metric: smaller sigma means stronger + pressure to stay near the trial point. +

+

+ After the solve with priors, the implementation usually runs a second LM + solve without priors. That cleanup step improves final feasibility. If + use_basis_keys is enabled and the prior solve is already below + feasible_threshold, the implementation returns early. +

+ +

Mechanics

+
    +
  1. Copy the component merit graph from the equality constraint penalty graph.
    2. +
  2. Compute values_retract_base = retractBaseVariables(values, delta).
    3. +
  3. Add GeneralPriorFactor priors around values_retract_base.
    4. +
  4. Run LM on constraints plus priors, initialized at values_retract_base.
    5. +
  5. Run a cleanup LM solve on constraints only, unless the basis-key fast return condition applies.
    6. +
+ +

Code Locations

+ + +
+ Current factory caveat. + ProjRetractor can accept optional basis keys in its constructor, + but ProjRetractorCreator currently creates it without passing + basis keys. Do not set params->use_basis_keys = true through + the current creator unless you also extend the creator or instantiate the + retractor directly with basis keys. +
+ +

Choosing It

+

+ Choose this for a small constrained problem when "retraction" should mean + "find the feasible point closest to the base-retracted trial." That is often + the most literal mechanical interpretation if the trial point is meaningful + and the metric is acceptable. The cost is that it builds an augmented graph + per call and runs one or two LM solves. +

+ + 
auto params = std::make_shared<RetractParams>();
+params->sigma = 1.0;
+params->lm_params.setMaxIterations(20);
+params->check_feasible = true;
+
+moptParams.cc_params->retractor_creator =
+    std::make_shared<ProjRetractorCreator>(params);
+
+ +
+

3. BasisRetractor: Fix Independent Variables and Recover the Rest

+
+ basis variables fixed + sparse structure + SV default +
+

+ BasisRetractor implements the basis-variable retraction. The + independent coordinates are selected as concrete base variables. Retraction + applies the proposed update to those basis variables, treats them as fixed, + and solves the equality constraints for all remaining variables. +

+

+ This is tightly paired with EliminationBasis. The same + BasisKeyFunc should select variables whose total tangent + dimension equals the component manifold dimension. If the selected basis + variables do not span the manifold dimension, EliminationBasis + throws during construction. +

+ +

Mechanics

+
    +
  1. The constructor receives basis_keys and builds a graph where factors touching those keys are wrapped as ConstVarFactor.
    2. +
  2. On every retraction, each wrapped factor receives the trial values for the fixed basis variables.
    3. +
  3. The optimizer receives only the non-basis variables present in its graph.
    4. +
  4. If the reduced graph is already below feasible_threshold, the fast path returns without LM.
    5. +
  5. Otherwise LM solves for the non-basis variables, and the fixed basis variables are inserted back into the result.
    6. +
+ +

Code Locations

+ + +

Choosing It

+

+ Choose this when your mechanics model has natural independent generalized + coordinates and dependent variables that should be recovered from + constraints. This is usually the right choice for larger sparse mechanics + problems because the symbolic structure can be reused and the retraction + solve touches only dependent variables. +

+

+ The main responsibility is selecting good basis keys. For example, in the + connected-pose test, fixing x3 means the retractor preserves + the trial value of x3 and solves x1 and + x2 to satisfy the between constraints. +

+ + 
BasisKeyFunc basisKeyFunc = [](const KeyVector& keys) {
+  return KeyVector{/* independent coordinate keys */};
+};
+
+auto moptParams =
+    ConstrainedOptBenchmark::DefaultMoptParamsSV(basisKeyFunc);
+
+moptParams.cc_params->retractor_creator->params()
+    ->lm_params.setMaxIterations(10);
+
+ +
+

4. DynamicsRetractor: Hierarchical Kinodynamic Retraction

+
+ q/v/a staging + kinodynamics + specialized +
+

+ DynamicsRetractor is a mechanics-specific specialization. It + assumes variables can be classified into position-level, velocity-level, + and acceleration/dynamics-level groups from their DynamicsSymbol + labels. It then solves the retraction in stages: first positions, then + velocities with positions fixed, then acceleration and dynamics variables + with positions and velocities fixed. +

+

+ Each stage has two optimizers: one with priors around selected basis keys + and one without priors for cleanup. The flow mirrors the idea that lower + mechanical levels should be settled before higher-level dynamic quantities + are recovered. +

+ +

Mechanics

+
    +
  1. Classify keys with isQLevel and isVLevel; other keys go to acceleration/dynamics.
    2. +
  2. Split factors by the highest level of any key they touch.
    3. +
  3. Wrap velocity factors so q-level values are fixed.
    4. +
  4. Wrap acceleration/dynamics factors so q and v values are fixed.
    5. +
  5. For q, v, and acceleration/dynamics stages: update priors, solve with priors, then solve without priors.
    6. +
  6. Insert each stage's result into known_values before solving the next stage.
    7. +
+ +

Code Locations

+ + +
+ Implementation detail. + In the current constructor, basis_q_keys_ = q_keys is assigned + after optional basis-key classification. That means q-level basis selection + is effectively all q keys in the current implementation, even when + use_basis_keys is true. +
+ +

Choosing It

+

+ Choose this when the constrained component is a kinodynamic mechanics + component and the q/v/acceleration ordering is meaningful. It is not the + generic projection choice for arbitrary constraints; it encodes a modeling + assumption about how the physics variables should be recovered. +

+ + 
auto params = std::make_shared<RetractParams>();
+params->use_basis_keys = true;
+params->sigma = 1.0;
+params->check_feasible = true;
+
+moptParams.cc_params->retractor_creator =
+    std::make_shared<DynamicsRetractorCreator>(params, basisKeyFunc);
+
+ +
+

Basis Choice and Retraction Choice Are Related

+

+ CM-OPT has two separate but coupled choices: the tangent-space basis and + the retractor. The basis maps the optimizer's reduced step into ambient + base-variable tangent vectors; the retractor decides how to restore + feasibility after applying that ambient update. +

+ + + + + + + + + + + + + + + + + + + + + + + + +
BasisMechanicsTypical retractor pairingCode
OrthonormalBasisLinearize constraints, compute a null-space basis, and lift xi through that dense or sparse null space.UoptRetractor or ProjRetractor.TspaceBasis.cpp:95, TspaceBasis.cpp:225
EliminationBasisSelect explicit basis keys, eliminate non-basis variables, and cache recovery Jacobians.BasisRetractor.TspaceBasis.cpp:437, TspaceBasis.cpp:608
+ +

+ The benchmark defaults make this relationship explicit: + DefaultMoptParams() uses OrthonormalBasisCreator + and UoptRetractorCreator, while + DefaultMoptParamsSV() uses EliminationBasisCreator + and BasisRetractorCreator. +

+
+ +
+

CM(F) and CM(I) Are Accuracy Settings, Not New Retractors

+

+ In the benchmark runner, CM(F) and CM(I) do not inherently select different + retractor classes. The retractor still comes from the example's + moptFactory. The difference is the inner retractor LM budget + and optional final cleanup. +

+ +
+ +
+

Practical Decision Guide

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
SituationChooseReason
You want the default and can allow all constrained variables to move during feasibility restoration.UoptRetractorLowest setup burden. It only needs the equality constraints and LM parameters.
You are solving a small constrained problem and want the feasible point closest to the trial step.ProjRetractorThe priors around the trial point express the projection metric directly.
You know the independent mechanical coordinates and want them preserved by retraction.BasisRetractorIt fixes those variables and recovers the dependent variables through the constraints.
Your component has position, velocity, and acceleration/dynamics layers with natural staged recovery.DynamicsRetractorIt encodes the q, v, acceleration/dynamics hierarchy explicitly.
+ +
+ Recommendation for a small mechanics example. + Start with ProjRetractor if your question is geometric: + "which feasible point is closest to this proposed motion?" Start with + BasisRetractor if your question is coordinate-mechanical: + "these generalized coordinates are independent; recover everything else." + Use DynamicsRetractor only when the q/v/acceleration hierarchy + is part of the model. +
+
+ +
+

Parameter Map

+

+ The shared options live in Retractor.h:50 + as RetractParams. +

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ParameterUsed byEffect
lm_paramsAll fourControls inner LM iterations, linear solver, damping limits, and verbosity.
check_feasible, feasible_thresholdAll via checkFeasible; threshold also affects fast pathsReports failed feasibility checks and controls early return conditions.
sigmaProjRetractor, DynamicsRetractorStandard deviation for priors around the trial point. Smaller values hold closer to the trial.
use_basis_keysBasisRetractor, DynamicsRetractor, partial support in ProjRetractorIndicates that a selected basis-key set should define independent coordinates or priors.
recomputeBasisRetractorIf the reduced solve is still infeasible, optionally inserts basis variables and reruns full LM.
apply_base_retractionPresent in params, not active in current generic retractor flowThe relevant conditional in ProjRetractor is currently commented out; the code initializes from the base-retracted values.
+
+ +
+

End-to-End Places to Read

+ +
+
+ + diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp index 0a3b28eaf..b20e999d4 100644 --- a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.cpp @@ -8,7 +8,7 @@ /* Constrained optimization benchmark implementations. */ #include -#include +#include #include #include @@ -433,7 +433,7 @@ void ConstrainedOptBenchmark::run(std::ostream& latexOs) { auto moptParams = moptFactory_(method); auto* retractLm = - &moptParams.cc_params->retractor_creator->params()->lm_params; + &moptParams.constraintManifoldParams->retractorCreator->params()->lmParams; if (options_.verboseRetractor) { retractLm->setVerbosityLM("SUMMARY"); } @@ -443,7 +443,7 @@ void ConstrainedOptBenchmark::run(std::ostream& latexOs) { : options_.cmIRetractorMaxIterations); if (!feasible && options_.cmIRetractFinal) { - moptParams.retract_final = true; + moptParams.retractFinalValues = true; } result = OptimizeCmOpt(problem, latexOs, moptParams, lmParams, @@ -486,28 +486,28 @@ Values ConstrainedOptBenchmark::OptimizeSoftConstraints( ManifoldOptimizerParameters ConstrainedOptBenchmark::DefaultMoptParams() { ManifoldOptimizerParameters moptParams; - auto retractorParams = std::make_shared(); - moptParams.cc_params->retractor_creator = - std::make_shared(retractorParams); - auto basisParams = std::make_shared(); - basisParams->always_construct_basis = false; - moptParams.cc_params->basis_creator = + auto retractorParams = std::make_shared(); + moptParams.constraintManifoldParams->retractorCreator = + std::make_shared(retractorParams); + auto basisParams = std::make_shared(); + basisParams->alwaysConstructBasis = false; + moptParams.constraintManifoldParams->basisCreator = std::make_shared(basisParams); return moptParams; } ManifoldOptimizerParameters ConstrainedOptBenchmark::DefaultMoptParamsSV( - const BasisKeyFunc& basisKeyFunc) { + const BasisKeyFunction& basisKeyFunction) { ManifoldOptimizerParameters moptParams; - auto retractorParams = std::make_shared(); - retractorParams->use_basis_keys = true; - moptParams.cc_params->retractor_creator = - std::make_shared(basisKeyFunc, retractorParams); - auto basisParams = std::make_shared(); - basisParams->use_basis_keys = true; - basisParams->always_construct_basis = false; - moptParams.cc_params->basis_creator = - std::make_shared(basisKeyFunc, basisParams); + auto retractorParams = std::make_shared(); + retractorParams->useBasisKeys = true; + moptParams.constraintManifoldParams->retractorCreator = + std::make_shared(basisKeyFunction, retractorParams); + auto basisParams = std::make_shared(); + basisParams->useBasisKeys = true; + basisParams->alwaysConstructBasis = false; + moptParams.constraintManifoldParams->basisCreator = + std::make_shared(basisKeyFunction, basisParams); return moptParams; } @@ -515,12 +515,12 @@ Values ConstrainedOptBenchmark::OptimizeCmOpt( const EConsOptProblem& problem, std::ostream& latexOs, ManifoldOptimizerParameters moptParams, LevenbergMarquardtParams lmParams, const std::string& expName, double constraintUnitScale) { - NonlinearMOptimizer optimizer(moptParams, lmParams); - auto moptProblem = optimizer.initializeMoptProblem( + NonlinearManifoldOptimizer optimizer(moptParams, lmParams); + auto moptProblem = optimizer.initializeManifoldOptimizationProblem( problem.costs(), problem.constraints(), problem.initValues()); auto optimizationStart = std::chrono::system_clock::now(); - auto result = optimizer.optimizeMOpt(moptProblem); + auto result = optimizer.optimizeManifoldProblem(moptProblem); auto optimizationEnd = std::chrono::system_clock::now(); const auto optimizationTimeMs = std::chrono::duration_cast(optimizationEnd - diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h index e408453e5..267f9bb91 100644 --- a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmark.h @@ -15,7 +15,7 @@ #pragma once #include -#include +#include #include #include #include @@ -125,7 +125,7 @@ class ConstrainedOptBenchmark { /// Create default CM parameters using basis-key elimination + basis /// retraction. static ManifoldOptimizerParameters DefaultMoptParamsSV( - const BasisKeyFunc& basisKeyFunc); + const BasisKeyFunction& basisKeyFunction); /// Print common CLI usage flags for benchmark examples. static void PrintUsage(std::ostream& os, const char* programName, diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.cpp b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.cpp index c7a35cbad..ece5cba13 100644 --- a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.cpp +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.cpp @@ -43,21 +43,21 @@ Values ProjectValues(const IEConsOptProblem &problem, const Values &values, NonlinearFactorGraph graph = problem.eConstraints().penaltyGraph(); graph.add(problem.iConstraints().penaltyGraph()); - LevenbergMarquardtParams lm_params; - lm_params.setlambdaUpperBound(1e10); - lm_params.setErrorTol(1e-30); - lm_params.setAbsoluteErrorTol(1e-30); - lm_params.setRelativeErrorTol(1e-30); - // lm_params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtParams lmParams; + lmParams.setlambdaUpperBound(1e10); + lmParams.setErrorTol(1e-30); + lmParams.setAbsoluteErrorTol(1e-30); + lmParams.setRelativeErrorTol(1e-30); + // lmParams.setVerbosityLM("SUMMARY"); // First stage, optimize with priors NonlinearFactorGraph graph_wp = graph; AddGeneralPriors(values, sigma, graph_wp); - LevenbergMarquardtOptimizer optimizer_wp(graph, values, lm_params); + LevenbergMarquardtOptimizer optimizer_wp(graph, values, lmParams); auto result_wp = optimizer_wp.optimize(); // Second stage, optimize without priors - LevenbergMarquardtOptimizer optimizer(graph, result_wp, lm_params); + LevenbergMarquardtOptimizer optimizer(graph, result_wp, lmParams); Values projected_values = optimizer.optimize(); return projected_values; } @@ -173,13 +173,13 @@ void IEResultSummary::exportFile(const std::string &file_path) const { /* ************************************************************************* */ std::pair OptimizeIE_Soft( - const IEConsOptProblem &problem, LevenbergMarquardtParams lm_params, + const IEConsOptProblem &problem, LevenbergMarquardtParams lmParams, double mu, bool eval_projected_cost) { /// Run optimization. NonlinearFactorGraph graph = problem.costs_; graph.add(problem.constraints().penaltyGraph(mu)); graph.add(problem.iConstraints().penaltyGraph(mu)); - HistoryLMOptimizer optimizer(graph, problem.initValues(), lm_params); + HistoryLMOptimizer optimizer(graph, problem.initValues(), lmParams); Timer timer; timer.start(); @@ -375,8 +375,8 @@ std::pair OptimizeIE_IPOPT( } /* ************************************************************************* */ -std::pair OptimizeIE_CMCOptGD( - const IEConsOptProblem &problem, const GDParams ¶ms, +std::pair OptimizeIE_CMCOptGD( + const IEConsOptProblem &problem, const GradientDescentParams ¶ms, const IEConstraintManifold::Params::shared_ptr &iecm_params, bool eval_projected_cost) { /// Run optimization. @@ -401,14 +401,14 @@ std::pair OptimizeIE_CMCOptGD( /// Summary of each iteration. const auto &iters_details = optimizer.details(); summary.total_inner_iters = - iters_details.back().state.totalNumberInnerIterations; + iters_details.back().state.totalInnerIterations; summary.total_iters = iters_details.back().state.iterations; for (const auto &iter_detail : iters_details) { const auto &state = iter_detail.state; Values state_values = state.manifolds.baseValues(); IEIterSummary iter_summary; iter_summary.accum_iters = state.iterations; - iter_summary.accum_inner_iters = state.totalNumberInnerIterations; + iter_summary.accum_inner_iters = state.totalInnerIterations; iter_summary.evaluate(problem, state_values, eval_projected_cost); summary.iters_summary.emplace_back(iter_summary); } @@ -416,7 +416,7 @@ std::pair OptimizeIE_CMCOptGD( } /* ************************************************************************* */ -std::pair OptimizeIE_CMOpt( +std::pair OptimizeIE_CMOpt( const IEConsOptProblem &problem, const IELMParams &ielm_params, const IEConstraintManifold::Params::shared_ptr &iecm_params, double mu, bool eval_projected_cost) { @@ -443,13 +443,13 @@ std::pair OptimizeIE_CMOpt( /// Summary of each iteration. const auto &iters_details = optimizer.details(); summary.total_inner_iters = - iters_details.back().state.totalNumberInnerIterations; + iters_details.back().state.totalInnerIterations; summary.total_iters = iters_details.back().state.iterations; for (const auto &iter_detail : iters_details) { const auto &state = iter_detail.state; IEIterSummary iter_summary; iter_summary.accum_iters = state.iterations; - iter_summary.accum_inner_iters = state.totalNumberInnerIterations; + iter_summary.accum_inner_iters = state.totalInnerIterations; iter_summary.evaluate(problem, state.baseValues(), eval_projected_cost); summary.iters_summary.emplace_back(iter_summary); } @@ -457,7 +457,7 @@ std::pair OptimizeIE_CMOpt( } /* ************************************************************************* */ -std::pair OptimizeIE_CMCOptLM( +std::pair OptimizeIE_CMCOptLM( const IEConsOptProblem &problem, const IELMParams &ielm_params, const IEConstraintManifold::Params::shared_ptr &iecm_params, std::string exp_name, bool eval_projected_cost) { @@ -482,13 +482,13 @@ std::pair OptimizeIE_CMCOptLM( /// Summary of each iteration. const auto &iters_details = optimizer.details(); summary.total_inner_iters = - iters_details.back().state.totalNumberInnerIterations; + iters_details.back().state.totalInnerIterations; summary.total_iters = iters_details.back().state.iterations; for (const auto &iter_detail : iters_details) { const auto &state = iter_detail.state; IEIterSummary iter_summary; iter_summary.accum_iters = state.iterations; - iter_summary.accum_inner_iters = state.totalNumberInnerIterations; + iter_summary.accum_inner_iters = state.totalInnerIterations; iter_summary.evaluate(problem, state.baseValues(), eval_projected_cost); summary.iters_summary.emplace_back(iter_summary); } diff --git a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.h b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.h index 048755109..5c34dcc76 100644 --- a/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.h +++ b/gtdynamics/constrained_optimizer/ConstrainedOptBenchmarkIE.h @@ -50,15 +50,13 @@ struct PenaltyParameters : public gtsam::PenaltyOptimizerParams { double &initial_mu; double &mu_increase_rate; size_t &num_iterations; - LevenbergMarquardtParams &lm_params; std::vector iters_lm_params; bool store_iter_details = false; bool store_lm_details = false; PenaltyParameters() : gtsam::PenaltyOptimizerParams(), initial_mu(initialMuEq), - mu_increase_rate(muEqIncreaseRate), num_iterations(maxIterations), - lm_params(lmParams) {} + mu_increase_rate(muEqIncreaseRate), num_iterations(maxIterations) {} }; struct AugmentedLagrangianParameters : public gtsam::AugmentedLagrangianParams { @@ -71,7 +69,6 @@ struct AugmentedLagrangianParameters : public gtsam::AugmentedLagrangianParams { double &dual_step_size_factor_i; double &max_dual_step_size_e; double &max_dual_step_size_i; - LevenbergMarquardtParams &lm_params; bool store_iter_details = false; bool store_lm_details = false; @@ -84,7 +81,7 @@ struct AugmentedLagrangianParameters : public gtsam::AugmentedLagrangianParams { dual_step_size_factor_e(dualStepSizeFactorEq), dual_step_size_factor_i(dualStepSizeFactorIneq), max_dual_step_size_e(maxDualStepSizeEq), - max_dual_step_size_i(maxDualStepSizeIneq), lm_params(lmParams) {} + max_dual_step_size_i(maxDualStepSizeIneq) {} }; using PenaltyItersDetails = gtsam::PenaltyOptimizer::Progress; @@ -168,7 +165,7 @@ typedef std::vector LMItersDetail; */ std::pair OptimizeIE_Soft( const IEConsOptProblem &problem, - LevenbergMarquardtParams lm_params = LevenbergMarquardtParams(), + LevenbergMarquardtParams lmParams = LevenbergMarquardtParams(), double mu = 100, bool eval_projected_cost = true); /** Run constrained optimization using the penalty method. */ @@ -193,19 +190,19 @@ std::pair OptimizeIE_IPOPT( const IEConsOptProblem &problem, bool eval_projected_cost = true); /** Run e-manifold optimization, with added penalty for i-constraints. */ -std::pair OptimizeIE_CMOpt( +std::pair OptimizeIE_CMOpt( const IEConsOptProblem &problem, const IELMParams &ielm_params, const IEConstraintManifold::Params::shared_ptr &iecm_params, double mu = 100, bool eval_projected_cost = true); /** Run constrained optimization using the Augmented Lagrangian method. */ -std::pair OptimizeIE_CMCOptGD( - const IEConsOptProblem &problem, const GDParams ¶ms, +std::pair OptimizeIE_CMCOptGD( + const IEConsOptProblem &problem, const GradientDescentParams ¶ms, const IEConstraintManifold::Params::shared_ptr &iecm_params, bool eval_projected_cost = true); /** Run constrained optimization using the Augmented Lagrangian method. */ -std::pair OptimizeIE_CMCOptLM( +std::pair OptimizeIE_CMCOptLM( const IEConsOptProblem &problem, const IELMParams &ielm_params, const IEConstraintManifold::Params::shared_ptr &iecm_params, std::string exp_name = "CMOpt(IE)", bool eval_projected_cost = true); diff --git a/gtdynamics/factors/SubstituteFactor.h b/gtdynamics/factors/SubstituteFactor.h index bf161b2c3..30744394a 100644 --- a/gtdynamics/factors/SubstituteFactor.h +++ b/gtdynamics/factors/SubstituteFactor.h @@ -9,7 +9,7 @@ * @file SubstituteFactor.h * @brief Factor that substitute certain variables with its corresponding * recover function from the constraint manifold. It is used to represent the - * equivalent new cost factors on manifold varaibles. + * equivalent new cost factors on manifold variables. * @author: Yetong Zhang */ diff --git a/gtdynamics/scenarios/IECartPoleWithFriction.cpp b/gtdynamics/scenarios/IECartPoleWithFriction.cpp index 693c58f10..a46e080e0 100644 --- a/gtdynamics/scenarios/IECartPoleWithFriction.cpp +++ b/gtdynamics/scenarios/IECartPoleWithFriction.cpp @@ -276,7 +276,7 @@ void UpdateBoundary(const double &a, const double &b, const size_t &index, IEConstraintManifold CartPoleWithFrictionRetractor::retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { Values new_values = manifold->values().retract(delta); int k = Symbol(new_values.keys().front()).index(); double q = new_values.atDouble(QKey(k)); @@ -437,7 +437,7 @@ IEConstraintManifold CartPoleWithFrictionRetractor::retract1( IEConstraintManifold CPBarrierRetractor::retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo *retract_info) const { + IERetractionInfo *retract_info) const { const NonlinearInequalityConstraints &i_constraints = *manifold->iConstraints(); const NonlinearEqualityConstraints &e_constraints = *manifold->eConstraints(); diff --git a/gtdynamics/scenarios/IECartPoleWithFriction.h b/gtdynamics/scenarios/IECartPoleWithFriction.h index 70a1be19c..a37322ff8 100644 --- a/gtdynamics/scenarios/IECartPoleWithFriction.h +++ b/gtdynamics/scenarios/IECartPoleWithFriction.h @@ -154,7 +154,7 @@ class CartPoleWithFrictionRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; /** Fallback penalty-based retraction for boundary corner cases. */ IEConstraintManifold retract1(const IEConstraintManifold *manifold, @@ -176,7 +176,7 @@ class CPBarrierRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; }; } // namespace gtdynamics diff --git a/gtdynamics/scenarios/IECartPoleWithLimits.cpp b/gtdynamics/scenarios/IECartPoleWithLimits.cpp index 2fe13b92b..8ab940b6e 100644 --- a/gtdynamics/scenarios/IECartPoleWithLimits.cpp +++ b/gtdynamics/scenarios/IECartPoleWithLimits.cpp @@ -258,8 +258,8 @@ Values IECartPoleWithLimits::getInitValuesInterp(size_t num_steps) const { return init_values; } -BasisKeyFunc IECartPoleWithLimits::getBasisKeyFunc() const { - BasisKeyFunc basis_key_func = [=](const KeyVector& keys) -> KeyVector { +BasisKeyFunction IECartPoleWithLimits::getBasisKeyFunction() const { + BasisKeyFunction basisKeyFunction = [=](const KeyVector& keys) -> KeyVector { KeyVector basis_keys; size_t k = DynamicsSymbol(*keys.begin()).time(); if (k == 0) { @@ -287,7 +287,7 @@ BasisKeyFunc IECartPoleWithLimits::getBasisKeyFunc() const { } return basis_keys; }; - return basis_key_func; + return basisKeyFunction; } @@ -296,7 +296,7 @@ BasisKeyFunc IECartPoleWithLimits::getBasisKeyFunc() const { IEConstraintManifold CartPoleWithLimitsRetractor::retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices, - IERetractInfo* retract_info) const { + IERetractionInfo* retract_info) const { Values new_values = manifold->values().retract(delta); // delta.print("==============================delta============================\n", diff --git a/gtdynamics/scenarios/IECartPoleWithLimits.h b/gtdynamics/scenarios/IECartPoleWithLimits.h index 7e80c268e..9b9f656a7 100644 --- a/gtdynamics/scenarios/IECartPoleWithLimits.h +++ b/gtdynamics/scenarios/IECartPoleWithLimits.h @@ -175,7 +175,7 @@ class IECartPoleWithLimits { Values getInitValuesInterp(size_t num_steps) const; /** Return a function that selects basis keys for constraint manifolds. */ - BasisKeyFunc getBasisKeyFunc() const; + BasisKeyFunction getBasisKeyFunction() const; }; /** Retraction rule that clamps cart position and force to their limits. */ @@ -193,7 +193,7 @@ class CartPoleWithLimitsRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override; + IERetractionInfo *retract_info = nullptr) const override; }; } // namespace gtdynamics diff --git a/gtdynamics/scenarios/IEHalfSphere.h b/gtdynamics/scenarios/IEHalfSphere.h index 0e0a3a2a7..37a4d269f 100644 --- a/gtdynamics/scenarios/IEHalfSphere.h +++ b/gtdynamics/scenarios/IEHalfSphere.h @@ -162,7 +162,7 @@ class HalfSphereRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override { + IERetractionInfo *retract_info = nullptr) const override { Key key = manifold->values().keys().front(); Point3 p = manifold->values().at(key); Vector3 v = delta.at(key); @@ -194,7 +194,7 @@ class SphereRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override { + IERetractionInfo *retract_info = nullptr) const override { Key key = manifold->values().keys().front(); Point3 p = manifold->values().at(key); Vector3 v = delta.at(key); @@ -221,7 +221,7 @@ class DomeRetractor : public IERetractor { IEConstraintManifold retract( const IEConstraintManifold *manifold, const VectorValues &delta, const std::optional &blocking_indices = {}, - IERetractInfo *retract_info = nullptr) const override { + IERetractionInfo *retract_info = nullptr) const override { Key key = manifold->values().keys().front(); Point3 p = manifold->values().at(key); Vector3 v = delta.at(key); @@ -240,7 +240,7 @@ class DomeRetractor : public IERetractor { /** Move the current point onto active dome boundary constraints. */ IEConstraintManifold moveToBoundary( const IEConstraintManifold *manifold, const IndexSet &blocking_indices, - IERetractInfo *retract_info = nullptr) const override { + IERetractionInfo *retract_info = nullptr) const override { Key key = manifold->values().keys().front(); Point3 p = manifold->values().at(key); diff --git a/gtdynamics/scenarios/IEQuadrupedUtils.cpp b/gtdynamics/scenarios/IEQuadrupedUtils.cpp index ad720e170..49d7b939e 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtils.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtils.cpp @@ -434,13 +434,13 @@ IEVision60Robot::basisKeys(const size_t k, } /* ************************************************************************* */ -BasisKeyFunc IEVision60Robot::getBasisKeyFunc() const { +BasisKeyFunction IEVision60Robot::getBasisKeyFunction() const { bool express_redundancy = params->express_redundancy; bool ad_basis_using_torques = params->ad_basis_using_torques; IndexSet contact_indices = phase_info->contact_indices; IndexSet leaving_indices = phase_info->leaving_indices; - BasisKeyFunc basis_key_func = + BasisKeyFunction basisKeyFunction = [express_redundancy, ad_basis_using_torques, contact_indices, leaving_indices](const KeyVector &keys) -> KeyVector { if (keys.size() == 1) { @@ -491,7 +491,7 @@ BasisKeyFunc IEVision60Robot::getBasisKeyFunc() const { } return basis_keys; }; - return basis_key_func; + return basisKeyFunction; } /* ************************************************************************* */ diff --git a/gtdynamics/scenarios/IEQuadrupedUtils.h b/gtdynamics/scenarios/IEQuadrupedUtils.h index cc9971627..b2ed20fb5 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtils.h +++ b/gtdynamics/scenarios/IEQuadrupedUtils.h @@ -505,24 +505,24 @@ class IEVision60Robot { Values stepValuesQ(const size_t k, const Values &init_values, const KeyVector &known_q_keys = KeyVector(), bool satisfy_i_constriants = false, - bool ensure_feasible = true) const; + bool ensureFeasible = true) const; Values stepValuesV(const size_t k, const Values &init_values, const Values &known_q_values, const KeyVector &known_v_keys = KeyVector(), bool satisfy_i_constriants = false, - bool ensure_feasible = true) const; + bool ensureFeasible = true) const; Values stepValuesAD(const size_t k, const Values &init_values, const Values &known_qv_values, const KeyVector &known_ad_keys = KeyVector(), bool satisfy_i_constriants = false, - bool ensure_feasible = true) const; + bool ensureFeasible = true) const; Values stepValuesQV(const size_t k, const Values &init_values, const std::optional &known_keys = {}, bool satisfy_i_constriants = false, - bool ensure_feasible = true) const; + bool ensureFeasible = true) const; /** Return values of one step satisfying kinodynamic constraints. The * variables specified by known_keys will remain unchanged. If known_keys not @@ -531,13 +531,13 @@ class IEVision60Robot { * @param init_values init estimate of variables of the step * @param known_keys variables that can be used as priors * @param satisfy_i_constraints include i-constraints - * @param ensure_feasible run a 2nd phase optimization without priors to + * @param ensureFeasible run a 2nd phase optimization without priors to * ensure constraint satisfaction */ Values stepValues(const size_t k, const Values &init_values, const std::optional &known_keys = {}, bool satisfy_i_constriants = false, - bool ensure_feasible = true) const; + bool ensureFeasible = true) const; /// Return values of one step by integration from previous step. The variables /// specified with integration_keys will be used for integration. Variables of @@ -595,7 +595,7 @@ class IEVision60Robot { } /// Return function that select basis keys for constraint manifolds. - BasisKeyFunc getBasisKeyFunc() const; + BasisKeyFunction getBasisKeyFunction() const; KeyVector basisKeys(const size_t k, bool include_init_state_constraints = false) const; @@ -696,13 +696,13 @@ class IEVision60RobotMultiPhase { /// nominal values for q, 0 for v. ad satsify constraints. Values trajectoryValuesNominal(const std::vector &phases_dt, bool include_i_constriants, - bool ensure_feasible) const; + bool ensureFeasible) const; /// Interpolated values for q, v. ad satisfy constraints. Values trajectoryValuesByInterpolation( const std::vector &phases_dt, const std::vector> &boundary_poses, - bool include_i_constriants, bool ensure_feasible) const; + bool include_i_constriants, bool ensureFeasible) const; }; /* ************************************************************************* */ @@ -718,9 +718,9 @@ class Vision60HierarchicalRetractorCreator : public IERetractorCreator { public: Vision60HierarchicalRetractorCreator( const IEVision60Robot &robot, const IERetractorParams::shared_ptr ¶ms, - bool use_basis_keys) + bool useBasisKeys) : IERetractorCreator(params), robot_(robot), - use_basis_keys_(use_basis_keys) {} + use_basis_keys_(useBasisKeys) {} virtual ~Vision60HierarchicalRetractorCreator() {} @@ -737,9 +737,9 @@ class Vision60BarrierRetractorCreator : public IERetractorCreator { public: Vision60BarrierRetractorCreator(const IEVision60Robot &robot, const IERetractorParams::shared_ptr ¶ms, - bool use_basis_keys) + bool useBasisKeys) : IERetractorCreator(params), robot_(robot), - use_basis_keys_(use_basis_keys) {} + use_basis_keys_(useBasisKeys) {} virtual ~Vision60BarrierRetractorCreator() {} @@ -757,9 +757,9 @@ class Vision60MultiPhaseHierarchicalRetractorCreator public: Vision60MultiPhaseHierarchicalRetractorCreator( const IEVision60RobotMultiPhase::shared_ptr &vision60_multi_phase, - const IERetractorParams::shared_ptr ¶ms, bool use_basis_keys) + const IERetractorParams::shared_ptr ¶ms, bool useBasisKeys) : IERetractorCreator(params), vision60_multi_phase_(vision60_multi_phase), - use_basis_keys_(use_basis_keys) {} + use_basis_keys_(useBasisKeys) {} virtual ~Vision60MultiPhaseHierarchicalRetractorCreator() {} @@ -776,9 +776,9 @@ class Vision60MultiPhaseBarrierRetractorCreator : public IERetractorCreator { public: Vision60MultiPhaseBarrierRetractorCreator( const IEVision60RobotMultiPhase::shared_ptr &vision60_multi_phase, - const IERetractorParams::shared_ptr ¶ms, bool use_basis_keys) + const IERetractorParams::shared_ptr ¶ms, bool useBasisKeys) : IERetractorCreator(params), vision60_multi_phase_(vision60_multi_phase), - use_basis_keys_(use_basis_keys) {} + use_basis_keys_(useBasisKeys) {} virtual ~Vision60MultiPhaseBarrierRetractorCreator() {} @@ -787,19 +787,19 @@ class Vision60MultiPhaseBarrierRetractorCreator : public IERetractorCreator { }; /** Tangent space creator for multiple phases. */ -class Vision60MultiPhaseTspaceBasisCreator : public TspaceBasisCreator { +class Vision60MultiPhaseTangentSpaceBasisCreator : public TangentSpaceBasisCreator { protected: IEVision60RobotMultiPhase::shared_ptr vision60_multi_phase_; public: - Vision60MultiPhaseTspaceBasisCreator( + Vision60MultiPhaseTangentSpaceBasisCreator( const IEVision60RobotMultiPhase::shared_ptr &vision60_multi_phase, - const TspaceBasisParams::shared_ptr params = - std::make_shared(true)) - : TspaceBasisCreator(params), + const TangentSpaceBasisParams::shared_ptr params = + std::make_shared(true)) + : TangentSpaceBasisCreator(params), vision60_multi_phase_(vision60_multi_phase) {} - TspaceBasis::shared_ptr + TangentSpaceBasis::shared_ptr create(const NonlinearEqualityConstraints::shared_ptr constraints, const Values &values) const override; }; @@ -825,7 +825,7 @@ struct JumpParams { void EvaluateAndExportIELMResult( const IEConsOptProblem &problem, const IEVision60RobotMultiPhase &vision60_multi_phase, - const IELMItersDetails &ielm_iters, + const IELMOptimizationDetails &ielm_iters, const std::string &scenario_folder, bool print_values = false, bool print_iter_details = false); @@ -842,7 +842,7 @@ void EvaluateAndExportInitValues( void ExportOptimizationProgress( const IEVision60RobotMultiPhase &vision60_multi_phase, - const std::string &scenario_folder, const IELMItersDetails &iters_details); + const std::string &scenario_folder, const IELMOptimizationDetails &iters_details); } // namespace gtdynamics @@ -898,7 +898,7 @@ GetVision60MultiPhase(const IEVision60Robot::Params::shared_ptr ¶ms, Values InitValuesTrajectory( const IEVision60RobotMultiPhase &vision60_multi_phase, const std::vector &phases_dt, bool include_i_constriants, - bool ensure_feasible, bool ignore_last_step = false); + bool ensureFeasible, bool ignore_last_step = false); } // namespace quadruped_forward_jump namespace quadruped_forward_jump_land { @@ -922,7 +922,7 @@ GetVision60MultiPhase(const IEVision60Robot::Params::shared_ptr ¶ms, Values InitValuesTrajectory( const IEVision60RobotMultiPhase &vision60_multi_phase, const std::vector &phases_dt, bool include_i_constriants, - bool ensure_feasible); + bool ensureFeasible); } // namespace quadruped_forward_jump_land } // namespace gtdynamics diff --git a/gtdynamics/scenarios/IEQuadrupedUtilsExp.cpp b/gtdynamics/scenarios/IEQuadrupedUtilsExp.cpp index a36c7707f..557592287 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtilsExp.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtilsExp.cpp @@ -15,7 +15,7 @@ using namespace gtsam; void EvaluateAndExportIELMResult( const IEConsOptProblem &problem, const IEVision60RobotMultiPhase &vision60_multi_phase, - const IELMItersDetails &ielm_iters, + const IELMOptimizationDetails &ielm_iters, const std::string &scenario_folder, bool print_values, bool print_iter_details) { size_t num_steps = vision60_multi_phase.numSteps(); @@ -24,7 +24,7 @@ void EvaluateAndExportIELMResult( Values result_values = ielm_iters.back().state.baseValues(); if (print_iter_details) { for (const auto &iter_details : ielm_iters) { - IEOptimizer::PrintIterDetails( + IEOptimizer::printIterationDetails( iter_details, num_steps, false, IEVision60Robot::PrintValues, IEVision60Robot::PrintDelta, GTDKeyFormatter); } @@ -153,7 +153,7 @@ std::vector ActiveNames(const IEManifoldValues &manifolds) { /* ************************************************************************* */ void ExportOptimizationProgress( const IEVision60RobotMultiPhase &vision60_multi_phase, - const std::string &scenario_folder, const IELMItersDetails &iters_details) { + const std::string &scenario_folder, const IELMOptimizationDetails &iters_details) { std::string progress_folder = scenario_folder + "progress/"; std::filesystem::create_directory(progress_folder); diff --git a/gtdynamics/scenarios/IEQuadrupedUtilsRetractor.cpp b/gtdynamics/scenarios/IEQuadrupedUtilsRetractor.cpp index ec5406939..3802c98f3 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtilsRetractor.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtilsRetractor.cpp @@ -23,7 +23,7 @@ using namespace gtsam; IERetractor::shared_ptr Vision60HierarchicalRetractorCreator::create( const IEConstraintManifold &manifold) const { if (use_basis_keys_) { - KeyVector basis_keys = robot_.getBasisKeyFunc()(manifold.values().keys()); + KeyVector basis_keys = robot_.getBasisKeyFunction()(manifold.values().keys()); return std::make_shared(manifold, params_, basis_keys); } @@ -34,7 +34,7 @@ IERetractor::shared_ptr Vision60HierarchicalRetractorCreator::create( IERetractor::shared_ptr Vision60BarrierRetractorCreator::create( const IEConstraintManifold &manifold) const { if (use_basis_keys_) { - KeyVector basis_keys = robot_.getBasisKeyFunc()(manifold.values().keys()); + KeyVector basis_keys = robot_.getBasisKeyFunction()(manifold.values().keys()); return std::make_shared(params_, basis_keys); } return std::make_shared(params_); @@ -50,7 +50,7 @@ IERetractor::shared_ptr Vision60MultiPhaseHierarchicalRetractorCreator::create( size_t k = DynamicsSymbol(*manifold.values().keys().begin()).time(); KeyVector basis_keys = - vision60_multi_phase_->robotAtStep(k).getBasisKeyFunc()(manifold.values().keys()); + vision60_multi_phase_->robotAtStep(k).getBasisKeyFunction()(manifold.values().keys()); return std::make_shared(manifold, params_, basis_keys); } @@ -64,21 +64,21 @@ IERetractor::shared_ptr Vision60MultiPhaseBarrierRetractorCreator::create( size_t k = DynamicsSymbol(*manifold.values().keys().begin()).time(); KeyVector basis_keys = - vision60_multi_phase_->robotAtStep(k).getBasisKeyFunc()(manifold.values().keys()); + vision60_multi_phase_->robotAtStep(k).getBasisKeyFunction()(manifold.values().keys()); return std::make_shared(params_, basis_keys); } return std::make_shared(params_); } /* ************************************************************************* */ -TspaceBasis::shared_ptr Vision60MultiPhaseTspaceBasisCreator::create( +TangentSpaceBasis::shared_ptr Vision60MultiPhaseTangentSpaceBasisCreator::create( const NonlinearEqualityConstraints::shared_ptr constraints, const Values &values) const { if (values.size() == 1) { return std::make_shared(constraints, values, params_); } size_t k = DynamicsSymbol(*values.keys().begin()).time(); KeyVector basis_keys = - vision60_multi_phase_->robotAtStep(k).getBasisKeyFunc()(values.keys()); + vision60_multi_phase_->robotAtStep(k).getBasisKeyFunction()(values.keys()); return std::make_shared(constraints, values, params_, basis_keys); } diff --git a/gtdynamics/scenarios/IEQuadrupedUtilsValues.cpp b/gtdynamics/scenarios/IEQuadrupedUtilsValues.cpp index b81e4142b..4b1f59def 100644 --- a/gtdynamics/scenarios/IEQuadrupedUtilsValues.cpp +++ b/gtdynamics/scenarios/IEQuadrupedUtilsValues.cpp @@ -17,20 +17,20 @@ using namespace gtsam; Values OptimizeWithConstraints(const NonlinearFactorGraph &merit_graph, const Values &values, const KeyVector &known_keys, const double sigma, - bool ensure_feasible, + bool ensureFeasible, const std::string &info_str) { NonlinearFactorGraph graph_wp = merit_graph; Values init_values = SubValues(values, merit_graph.keys()); AddGeneralPriors(init_values, known_keys, sigma, graph_wp); - LevenbergMarquardtParams lm_params; - LevenbergMarquardtOptimizer optimizer(graph_wp, init_values, lm_params); + LevenbergMarquardtParams lmParams; + LevenbergMarquardtOptimizer optimizer(graph_wp, init_values, lmParams); auto results = optimizer.optimize(); if (!CheckFeasible(merit_graph, results, info_str)) { - lm_params.absoluteErrorTol = 1e-12; - lm_params.errorTol = 1e-12; + lmParams.absoluteErrorTol = 1e-12; + lmParams.errorTol = 1e-12; LevenbergMarquardtOptimizer optimizer_ad_np(merit_graph, results, - lm_params); + lmParams); results = optimizer_ad_np.optimize(); CheckFeasible(merit_graph, results, info_str + "_2nd"); }; @@ -41,14 +41,14 @@ Values OptimizeWithConstraints(const NonlinearFactorGraph &merit_graph, Values IEVision60Robot::stepValuesQ(const size_t k, const Values &init_values, const KeyVector &known_q_keys, bool satisfy_i_constriants, - bool ensure_feasible) const { + bool ensureFeasible) const { NonlinearFactorGraph graph_q = getConstraintsGraphStepQ(k); if (satisfy_i_constriants) { graph_q.add(stepIConstraintsQ(k).penaltyGraph()); } return OptimizeWithConstraints(graph_q, init_values, known_q_keys, - params->tol_q, ensure_feasible, "q-level"); + params->tol_q, ensureFeasible, "q-level"); } /* ************************************************************************* */ @@ -56,7 +56,7 @@ Values IEVision60Robot::stepValuesV(const size_t k, const Values &init_values, const Values &known_q_values, const KeyVector &known_v_keys, bool satisfy_i_constriants, - bool ensure_feasible) const { + bool ensureFeasible) const { NonlinearFactorGraph graph_v = getConstraintsGraphStepV(k); if (satisfy_i_constriants) { @@ -64,7 +64,7 @@ Values IEVision60Robot::stepValuesV(const size_t k, const Values &init_values, } graph_v = ConstVarGraph(graph_v, known_q_values); return OptimizeWithConstraints(graph_v, init_values, known_v_keys, - params->tol_v, ensure_feasible, "v-level"); + params->tol_v, ensureFeasible, "v-level"); } /* ************************************************************************* */ @@ -72,7 +72,7 @@ Values IEVision60Robot::stepValuesAD(const size_t k, const Values &init_values, const Values &known_qv_values, const KeyVector &known_ad_keys, bool satisfy_i_constriants, - bool ensure_feasible) const { + bool ensureFeasible) const { NonlinearFactorGraph graph_ad = getConstraintsGraphStepAD(k); if (satisfy_i_constriants) { @@ -80,7 +80,7 @@ Values IEVision60Robot::stepValuesAD(const size_t k, const Values &init_values, } graph_ad = ConstVarGraph(graph_ad, known_qv_values); return OptimizeWithConstraints(graph_ad, init_values, known_ad_keys, - params->tol_dynamics, ensure_feasible, + params->tol_dynamics, ensureFeasible, "ad-level"); } @@ -88,16 +88,16 @@ Values IEVision60Robot::stepValuesAD(const size_t k, const Values &init_values, Values IEVision60Robot::stepValuesQV( const size_t k, const Values &init_values, const std::optional &optional_known_keys, - bool satisfy_i_constriants, bool ensure_feasible) const { + bool satisfy_i_constriants, bool ensureFeasible) const { KeyVector known_keys = optional_known_keys ? *optional_known_keys : basisKeys(k); KeyVector known_q_keys, known_v_keys, known_ad_keys; ClassifyKeysByLevel(known_keys, known_q_keys, known_v_keys, known_ad_keys); - // LevenbergMarquardtParams lm_params; + // LevenbergMarquardtParams lmParams; Values known_values = stepValuesQ(k, init_values, known_q_keys, - satisfy_i_constriants, ensure_feasible); + satisfy_i_constriants, ensureFeasible); known_values.insert(stepValuesV(k, init_values, known_values, known_v_keys, - satisfy_i_constriants, ensure_feasible)); + satisfy_i_constriants, ensureFeasible)); return known_values; } @@ -106,17 +106,17 @@ Values IEVision60Robot::stepValues(const size_t k, const Values &init_values, const std::optional &optional_known_keys, bool satisfy_i_constriants, - bool ensure_feasible) const { + bool ensureFeasible) const { KeyVector known_keys = optional_known_keys ? *optional_known_keys : basisKeys(k); KeyVector known_q_keys, known_v_keys, known_ad_keys; ClassifyKeysByLevel(known_keys, known_q_keys, known_v_keys, known_ad_keys); Values known_values = stepValuesQ(k, init_values, known_q_keys, - satisfy_i_constriants, ensure_feasible); + satisfy_i_constriants, ensureFeasible); known_values.insert(stepValuesV(k, init_values, known_values, known_v_keys, - satisfy_i_constriants, ensure_feasible)); + satisfy_i_constriants, ensureFeasible)); known_values.insert(stepValuesAD(k, init_values, known_values, known_ad_keys, - satisfy_i_constriants, ensure_feasible)); + satisfy_i_constriants, ensureFeasible)); return known_values; } @@ -202,8 +202,8 @@ Values IEVision60Robot::getInitValuesStep(const size_t k, } Values known_values; - LevenbergMarquardtParams lm_params; - // lm_params->setVerbosityLM("SUMMARY"); + LevenbergMarquardtParams lmParams; + // lmParams->setVerbosityLM("SUMMARY"); // solve q level NonlinearFactorGraph graph_q = getConstraintsGraphStepQ(k); @@ -227,7 +227,7 @@ Values IEVision60Robot::getInitValuesStep(const size_t k, } 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"; @@ -242,7 +242,7 @@ Values IEVision60Robot::getInitValuesStep(const size_t k, 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"; @@ -266,7 +266,7 @@ Values IEVision60Robot::getInitValuesStep(const size_t k, } 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) @@ -395,16 +395,16 @@ TrajectoryWithTrapezoidal(const IEVision60RobotMultiPhase &vision60_multi_phase, dt_model); } - LevenbergMarquardtParams lm_params; - // lm_params->setVerbosityLM("SUMMARY"); - LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + LevenbergMarquardtParams lmParams; + // lmParams->setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); return optimizer.optimize(); } /* ************************************************************************* */ Values IEVision60RobotMultiPhase::trajectoryValuesNominal( const std::vector &phases_dt, bool include_i_constriants, - bool ensure_feasible) const { + bool ensureFeasible) const { Values values; size_t num_steps = numSteps(); for (size_t k = 0; k <= num_steps; k++) { @@ -414,11 +414,11 @@ Values IEVision60RobotMultiPhase::trajectoryValuesNominal( TwistKey(IEVision60Robot::base_id, k)}; Values step_values_qv = robot.stepValuesQV(k, init_values_k, known_qv_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); KeyVector known_ad_keys; Values step_values_ad = robot.stepValuesAD(k, init_values_k, step_values_qv, known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); Values step_values_k = step_values_qv; step_values_k.insert(step_values_ad); values.insert(step_values_k); @@ -434,7 +434,7 @@ Values IEVision60RobotMultiPhase::trajectoryValuesNominal( Values IEVision60RobotMultiPhase::trajectoryValuesByInterpolation( const std::vector &phases_dt, const std::vector> &prior_poses, - bool include_i_constriants, bool ensure_feasible) const { + bool include_i_constriants, bool ensureFeasible) const { Values values; size_t num_steps = numSteps(); @@ -475,11 +475,11 @@ Values IEVision60RobotMultiPhase::trajectoryValuesByInterpolation( torso_twists.at(k)); Values step_values_qv = robot.stepValuesQV(k, init_values_k, known_qv_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); KeyVector known_ad_keys; Values step_values_ad = robot.stepValuesAD(k, init_values_k, step_values_qv, known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); Values step_values_k = step_values_qv; step_values_k.insert(step_values_ad); values.insert(step_values_k); @@ -787,7 +787,7 @@ namespace quadruped_forward_jump { Values InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, const std::vector &phases_dt, - bool include_i_constriants, bool ensure_feasible, + bool include_i_constriants, bool ensureFeasible, bool ignore_last_step) { const auto &vision60_ground = vision60_multi_phase.phase_robots_[0]; @@ -826,7 +826,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, KeyVector known_keys{PoseKey(IEVision60Robot::base_id, k), TwistKey(IEVision60Robot::base_id, k)}; step_values[k] = vision60_ground.stepValuesQV( - k, init_values_k, known_keys, include_i_constriants, ensure_feasible); + k, init_values_k, known_keys, include_i_constriants, ensureFeasible); // set a,v,q level of base_link for next step Vector torso_twistaccel_w = k < num_steps_ground ? torso_twistaccel_ground_w @@ -870,7 +870,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, } } step_values[k] = vision60_back.stepValuesQV( - k, init_values_k, known_keys, include_i_constriants, ensure_feasible); + k, init_values_k, known_keys, include_i_constriants, ensureFeasible); // set a,v,q level of base_link for next step Vector torso_twistaccel_w = k < num_steps_ground + num_steps_back @@ -912,7 +912,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, known_keys.push_back(JointVelKey(j, k)); } step_values[k] = vision60_air.stepValuesQV( - k, init_values_k, known_keys, include_i_constriants, ensure_feasible); + k, init_values_k, known_keys, include_i_constriants, ensureFeasible); // set a,v,q level of base_link for next step Vector torso_twistaccel_w = torso_twistaccel_air_w; @@ -939,7 +939,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, // } Values ad_values = vision60_ground.stepValuesAD( k, init_values_k, step_values.at(k), known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); step_values[k].insert(ad_values); } // bound_gb @@ -954,7 +954,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, } Values ad_values = vision60_ground.stepValuesAD( k, init_values_k, step_values.at(k), known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); step_values[k].insert(ad_values); } // back @@ -972,7 +972,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, // } Values ad_values = vision60_back.stepValuesAD( k, init_values_k, step_values.at(k), known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); step_values[k].insert(ad_values); } // bound_ba @@ -987,7 +987,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, } Values ad_values = vision60_back.stepValuesAD( k, init_values_k, step_values.at(k), known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); step_values[k].insert(ad_values); } // air @@ -1005,7 +1005,7 @@ InitValuesTrajectory(const IEVision60RobotMultiPhase &vision60_multi_phase, // step_torso_twistaccels[k]); Values ad_values = vision60_air.stepValuesAD( k, init_values_k, step_values.at(k), known_ad_keys, - include_i_constriants, ensure_feasible); + include_i_constriants, ensureFeasible); step_values[k].insert(ad_values); } @@ -1085,7 +1085,7 @@ namespace quadruped_forward_jump_land { Values InitValuesTrajectory( const IEVision60RobotMultiPhase &vision60_multi_phase, const std::vector &phases_dt, bool include_i_constriants, - bool ensure_feasible) { + bool ensureFeasible) { const IEVision60Robot &vision60_ground_forward = vision60_multi_phase.phase_robots_.back(); @@ -1110,7 +1110,7 @@ Values InitValuesTrajectory( phase_robots, boundary_robots, phase_num_steps); Values values = quadruped_forward_jump::InitValuesTrajectory( vision60_multi_phase_jump, phases_dt_jump, include_i_constriants, - ensure_feasible, true); + ensureFeasible, true); // initialize landing phase size_t jumping_steps = vision60_multi_phase_jump.numSteps(); @@ -1138,7 +1138,7 @@ Values InitValuesTrajectory( init_values_k.update(JointAccelKey(joint->id(), k), 0.0); } step_values[landing_k] = vision60_ground_forward.stepValues( - k, init_values_k, known_keys, include_i_constriants, ensure_feasible); + k, init_values_k, known_keys, include_i_constriants, ensureFeasible); values.insert(step_values[landing_k]); } values.insert(PhaseKey(phases_dt.size() - 1), phases_dt.back()); diff --git a/tests/testConstraintManifold.cpp b/tests/testConstraintManifold.cpp index 364fab813..3b8de35fd 100644 --- a/tests/testConstraintManifold.cpp +++ b/tests/testConstraintManifold.cpp @@ -58,21 +58,21 @@ TEST_UNSAFE(ConstraintManifold, connected_poses) { cm_base_values.insert(x3_key, Pose3(Rot3(), Point3(0, 1, 1))); // Create constraint manifold with various tspacebasis and retractors - BasisKeyFunc basis_key_func = [=](const KeyVector& keys) -> KeyVector { + BasisKeyFunction basisKeyFunction = [=](const KeyVector& keys) -> KeyVector { return KeyVector{x3_key}; }; - std::vector basis_creators{ + std::vector basis_creators{ std::make_shared(), - std::make_shared(basis_key_func)}; + std::make_shared(basisKeyFunction)}; std::vector retractor_creators{ - std::make_shared(), - std::make_shared(basis_key_func)}; + std::make_shared(), + std::make_shared(basisKeyFunction)}; - for (const auto& basis_creator : basis_creators) { - for (const auto& retractor_creator : retractor_creators) { + for (const auto& basisCreator : basis_creators) { + for (const auto& retractorCreator : retractor_creators) { auto params = std::make_shared(); - params->basis_creator = basis_creator; - params->retractor_creator = retractor_creator; + params->basisCreator = basisCreator; + params->retractorCreator = retractorCreator; ConstraintManifold manifold(constraints, cm_base_values, params, true); // Check recover @@ -331,13 +331,13 @@ TEST(ConstraintManifold, connected_poses_many_components_manifold_keys_stable) { EConsOptProblem problem(costs, constraints, initValues); auto moptParams = ConstrainedOptBenchmark::DefaultMoptParams(); LevenbergMarquardtParams lmParams; - NonlinearMOptimizer optimizer(moptParams, lmParams); + NonlinearManifoldOptimizer optimizer(moptParams, lmParams); bool success = true; std::string errorMessage; - ManifoldOptProblem moptProblem; + ManifoldOptimizationProblem moptProblem; try { - moptProblem = optimizer.initializeMoptProblem( + moptProblem = optimizer.initializeManifoldOptimizationProblem( problem.costs(), problem.constraints(), problem.initValues()); } catch (const std::exception& e) { success = false; @@ -350,11 +350,11 @@ TEST(ConstraintManifold, connected_poses_many_components_manifold_keys_stable) { } EXPECT(success); if (success) { - EXPECT(moptProblem.components_.size() == (kNumSteps + 1)); - EXPECT(moptProblem.manifold_keys_.size() == (kNumSteps + 1)); + EXPECT(moptProblem.components.size() == (kNumSteps + 1)); + EXPECT(moptProblem.manifoldKeys.size() == (kNumSteps + 1)); for (size_t k = 0; k <= kNumSteps; ++k) { - EXPECT(moptProblem.manifold_keys_.find(A(k)) != - moptProblem.manifold_keys_.end()); + EXPECT(moptProblem.manifoldKeys.find(A(k)) != + moptProblem.manifoldKeys.end()); } } } @@ -383,16 +383,16 @@ TEST(ConstraintManifold, sv_mode_fully_constrained_component_no_throw) { initValues.insert(x2Key, Pose3(Rot3(), Point3(0.0, 0.2, 0.8))); EConsOptProblem problem(costs, constraints, initValues); - BasisKeyFunc basisKeyFunc = [](const KeyVector& keys) { return keys; }; - auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV(basisKeyFunc); + BasisKeyFunction basisKeyFunction = [](const KeyVector& keys) { return keys; }; + auto moptParams = ConstrainedOptBenchmark::DefaultMoptParamsSV(basisKeyFunction); LevenbergMarquardtParams lmParams; - NonlinearMOptimizer optimizer(moptParams, lmParams); + NonlinearManifoldOptimizer optimizer(moptParams, lmParams); bool success = true; std::string errorMessage; - ManifoldOptProblem moptProblem; + ManifoldOptimizationProblem moptProblem; try { - moptProblem = optimizer.initializeMoptProblem( + moptProblem = optimizer.initializeManifoldOptimizationProblem( problem.costs(), problem.constraints(), problem.initValues()); } catch (const std::exception& e) { success = false; @@ -405,9 +405,9 @@ TEST(ConstraintManifold, sv_mode_fully_constrained_component_no_throw) { } EXPECT(success); if (success) { - EXPECT(moptProblem.components_.size() == 1); - EXPECT(moptProblem.manifold_keys_.size() == 0); - EXPECT(moptProblem.fixed_manifolds_.size() == 1); + EXPECT(moptProblem.components.size() == 1); + EXPECT(moptProblem.manifoldKeys.size() == 0); + EXPECT(moptProblem.fixedManifolds.size() == 1); } } @@ -452,19 +452,19 @@ TEST(ConstraintManifold_retract, cart_pole_dynamics) { basis_keys.push_back(JointVelKey(j1_id, 0)); basis_keys.push_back(JointAccelKey(j0_id, 0)); basis_keys.push_back(JointAccelKey(j1_id, 0)); - BasisKeyFunc basis_key_func = [=](const KeyVector& keys) -> KeyVector { + BasisKeyFunction basisKeyFunction = [=](const KeyVector& keys) -> KeyVector { return basis_keys; }; // constraint manifold auto constraints = std::make_shared( gtsam::NonlinearEqualityConstraints::FromCostGraph(constraints_graph)); - auto cc_params = std::make_shared(); - cc_params->retractor_creator = - std::make_shared(basis_key_func); - cc_params->basis_creator = - std::make_shared(basis_key_func); - auto cm = ConstraintManifold(constraints, init_values, cc_params, true); + auto constraintManifoldParams = std::make_shared(); + constraintManifoldParams->retractorCreator = + std::make_shared(basisKeyFunction); + constraintManifoldParams->basisCreator = + std::make_shared(basisKeyFunction); + auto cm = ConstraintManifold(constraints, init_values, constraintManifoldParams, true); // retract Vector xi = (Vector(6) << 1, 0, 0, 0, 0, 0).finished(); @@ -558,13 +558,13 @@ TEST(ConstraintManifold, lynch_park_four_bar_loop_closure_cmopt) { EConsOptProblem problem(costs, constraints, init_values); auto mopt_params = ConstrainedOptBenchmark::DefaultMoptParams(); - LevenbergMarquardtParams lm_params; - NonlinearMOptimizer optimizer(mopt_params, lm_params); + LevenbergMarquardtParams lmParams; + NonlinearManifoldOptimizer optimizer(mopt_params, lmParams); - const auto mopt_problem = optimizer.initializeMoptProblem( + const auto mopt_problem = optimizer.initializeManifoldOptimizationProblem( problem.costs(), problem.constraints(), problem.initValues()); - EXPECT_LONGS_EQUAL(1, mopt_problem.components_.size()); - EXPECT_LONGS_EQUAL(1, mopt_problem.manifold_keys_.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.components.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.manifoldKeys.size()); EXPECT_LONGS_EQUAL(1, mopt_problem.manifolds().begin()->second.dim()); const Values result = diff --git a/tests/testIEConstraintManifold.cpp b/tests/testIEConstraintManifold.cpp index cefa151f0..d38958fd1 100644 --- a/tests/testIEConstraintManifold.cpp +++ b/tests/testIEConstraintManifold.cpp @@ -41,9 +41,9 @@ TEST(IEConstraintManifold, HalfSphere) { ScalarExpressionInequalityConstraint::LeqZero(z_expr, 1.0)); auto params = std::make_shared(); - params->ecm_params = std::make_shared(); - params->retractor_creator = std::make_shared(); - params->e_basis_creator = std::make_shared(); + params->equalityManifoldParams = std::make_shared(); + params->retractorCreator = std::make_shared(); + params->equalityBasisCreator = std::make_shared(); { Values values; @@ -110,17 +110,17 @@ TEST(IEConstraintManifold, HalfSphere) { // Test linear i-constraints Key manifold_key = 3; - VectorValues tangent_vector; - tangent_vector.insert(point_key, Vector3(8, -6, 2)); - Vector xi = manifold.eBasis()->computeXi(tangent_vector); + VectorValues tangentVector; + tangentVector.insert(point_key, Vector3(8, -6, 2)); + Vector xi = manifold.eBasis()->computeXi(tangentVector); VectorValues delta; delta.insert(manifold_key, xi); - auto linear_base_i_constraints = manifold.linearActiveBaseIConstraints(); - auto linear_manifold_i_constraints = manifold.linearActiveManIConstraints(manifold_key); + auto linearBaseInequalityConstraints = manifold.linearActiveBaseInequalityConstraints(); + auto linearManifoldInequalityConstraints = manifold.linearActiveManifoldInequalityConstraints(manifold_key); - for (const auto&[idx, base_constraint]: linear_base_i_constraints) { - auto manifold_constraint = linear_manifold_i_constraints.at(idx); - auto base_constraint_eval = (*base_constraint)(tangent_vector); + for (const auto&[idx, base_constraint]: linearBaseInequalityConstraints) { + auto manifold_constraint = linearManifoldInequalityConstraints.at(idx); + auto base_constraint_eval = (*base_constraint)(tangentVector); auto manifold_constraint_eval = (*manifold_constraint)(delta); EXPECT(assert_equal(Vector1(2.0), base_constraint_eval)); EXPECT(assert_equal(Vector1(2.0), manifold_constraint_eval)); diff --git a/tests/testIEHalfSphere.cpp b/tests/testIEHalfSphere.cpp index c6a92aa7c..4e50be249 100644 --- a/tests/testIEHalfSphere.cpp +++ b/tests/testIEHalfSphere.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include #include #include #include @@ -64,9 +64,9 @@ TEST(HalfSphereRetractor, retract) { auto i_constraints = std::make_shared( half_sphere.iConstraints(0)); auto params = std::make_shared(); - params->retractor_creator = + params->retractorCreator = std::make_shared(retractor); - params->e_basis_creator = std::make_shared(); + params->equalityBasisCreator = std::make_shared(); Values values1; values1.insert(PointKey(0), Point3(3, 0, 0)); IEConstraintManifold manifold1(params, e_constraints, i_constraints, values1); @@ -100,9 +100,9 @@ TEST(HalfSphere, Dome) { auto i_constraints = std::make_shared( half_sphere.iDomeConstraints(0)); auto params = std::make_shared(); - params->retractor_creator = + params->retractorCreator = std::make_shared(retractor); - params->e_basis_creator = std::make_shared(); + params->equalityBasisCreator = std::make_shared(); IEConstraintManifold manifold(params, e_constraints, i_constraints, values); diff --git a/tests/testIEManifoldOptimizer.cpp b/tests/testIEManifoldOptimizer.cpp index 93cccf1a5..7a774e0ef 100644 --- a/tests/testIEManifoldOptimizer.cpp +++ b/tests/testIEManifoldOptimizer.cpp @@ -1,6 +1,6 @@ #include "gtdynamics/cmcopt/IERetractor.h" -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" #include #include #include @@ -23,7 +23,7 @@ using namespace gtdynamics; using namespace gtsam; -TEST(IdentifyManifolds, HalfSphere) { +TEST(identifyManifolds, HalfSphere) { IEHalfSphere half_sphere; size_t num_steps = 2; @@ -40,12 +40,12 @@ TEST(IdentifyManifolds, HalfSphere) { } auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); - auto manifolds = IEOptimizer::IdentifyManifolds(e_constraints, i_constraints, + auto manifolds = IEOptimizer::identifyManifolds(e_constraints, i_constraints, values, iecm_params); for (const auto &it : manifolds) { const auto &manifold = it.second; @@ -56,7 +56,7 @@ TEST(IdentifyManifolds, HalfSphere) { EXPECT_LONGS_EQUAL(num_steps + 1, manifolds.size()); } -TEST(IdentifyManifolds, CartPoleWithFriction) { +TEST(identifyManifolds, CartPoleWithFriction) { IECartPoleWithFriction cp; size_t num_steps = 2; @@ -73,12 +73,12 @@ TEST(IdentifyManifolds, CartPoleWithFriction) { } auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(cp)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); - auto manifolds = IEOptimizer::IdentifyManifolds(e_constraints, i_constraints, + auto manifolds = IEOptimizer::identifyManifolds(e_constraints, i_constraints, values, iecm_params); for (const auto &it : manifolds) { const auto &manifold = it.second; @@ -89,7 +89,7 @@ TEST(IdentifyManifolds, CartPoleWithFriction) { EXPECT_LONGS_EQUAL(num_steps + 1, manifolds.size()); } -TEST(IdentifyManifolds, Dome) { +TEST(identifyManifolds, Dome) { IEHalfSphere half_sphere; size_t num_steps = 2; @@ -100,17 +100,17 @@ TEST(IdentifyManifolds, Dome) { } auto iecm_params = std::make_shared(); - iecm_params->retractor_creator = + iecm_params->retractorCreator = std::make_shared( std::make_shared(half_sphere)); - iecm_params->e_basis_creator = std::make_shared(); + iecm_params->equalityBasisCreator = std::make_shared(); Values values; for (size_t k = 0; k <= num_steps; k++) { values.insert(PointKey(k), Point3(0, 1, 0)); } - auto manifolds = IEOptimizer::IdentifyManifolds(e_constraints, i_constraints, + auto manifolds = IEOptimizer::identifyManifolds(e_constraints, i_constraints, values, iecm_params); for (const auto &it : manifolds) { const auto &manifold = it.second; diff --git a/tests/testIEQuadrupedUtilsManifold.cpp b/tests/testIEQuadrupedUtilsManifold.cpp index bdcc18977..e6efa24b8 100644 --- a/tests/testIEQuadrupedUtilsManifold.cpp +++ b/tests/testIEQuadrupedUtilsManifold.cpp @@ -392,22 +392,22 @@ TEST(IEVision60Robot_ground_air_boundary, constraints_and_values) { TEST(IEVision60Robot_4c, manifold) { using namespace vision60_4c_single_step; auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(vision60.getBasisKeyFunc()); - iecm_params->ecm_params->retractor_creator = - std::make_shared(vision60.getBasisKeyFunc()); + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(vision60.getBasisKeyFunction()); + iecm_params->equalityManifoldParams->retractorCreator = + std::make_shared(vision60.getBasisKeyFunction()); auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - // retractor_params->lm_params.setVerbosityLM("SUMMARY"); - // retractor_params->lm_params.minModelFidelity = 0.5; - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + // retractor_params->lmParams.setVerbosityLM("SUMMARY"); + // retractor_params->lmParams.minModelFidelity = 0.5; + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + iecm_params->retractorCreator = std::make_shared( vision60, retractor_params, true); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; IEConstraintManifold manifold(iecm_params, e_constraints, i_constraints, values); @@ -498,24 +498,24 @@ TEST(IEVision60Robot_4c, manifold) { TEST(IEVision60Robot_back_on_ground, manifold) { using namespace vision60_back_on_ground_single_step; auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(vision60.getBasisKeyFunc()); - iecm_params->ecm_params->retractor_creator = - std::make_shared(vision60.getBasisKeyFunc()); + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(vision60.getBasisKeyFunction()); + iecm_params->equalityManifoldParams->retractorCreator = + std::make_shared(vision60.getBasisKeyFunction()); auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - // retractor_params->lm_params.setVerbosityLM("SUMMARY"); - // retractor_params->lm_params.minModelFidelity = 0.5; - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + // retractor_params->lmParams.setVerbosityLM("SUMMARY"); + // retractor_params->lmParams.minModelFidelity = 0.5; + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + iecm_params->retractorCreator = std::make_shared( vision60, retractor_params, true); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; - // KeyVector basis_keys = iecm_params->ecm_params->basis_key_func(e_cc); + // KeyVector basis_keys = iecm_params->equalityManifoldParams->basisKeyFunction(e_cc); // PrintKeyVector(basis_keys, "", GTDKeyFormatter); IEConstraintManifold manifold(iecm_params, e_constraints, i_constraints, @@ -607,24 +607,24 @@ TEST(IEVision60Robot_back_on_ground, manifold) { TEST(IEVision60Robot_in_air, manifold) { using namespace vision60_in_air_single_step; auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(vision60.getBasisKeyFunc()); - iecm_params->ecm_params->retractor_creator = - std::make_shared(vision60.getBasisKeyFunc()); + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(vision60.getBasisKeyFunction()); + iecm_params->equalityManifoldParams->retractorCreator = + std::make_shared(vision60.getBasisKeyFunction()); auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - // retractor_params->lm_params.setVerbosityLM("SUMMARY"); - // retractor_params->lm_params.minModelFidelity = 0.5; - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + // retractor_params->lmParams.setVerbosityLM("SUMMARY"); + // retractor_params->lmParams.minModelFidelity = 0.5; + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + iecm_params->retractorCreator = std::make_shared( vision60, retractor_params, true); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; - // KeyVector basis_keys = iecm_params->ecm_params->basis_key_func(e_cc); + // KeyVector basis_keys = iecm_params->equalityManifoldParams->basisKeyFunction(e_cc); // PrintKeyVector(basis_keys, "", GTDKeyFormatter); IEConstraintManifold manifold(iecm_params, e_constraints, i_constraints, @@ -720,24 +720,24 @@ TEST(IEVision60Robot_in_air, manifold) { TEST(IEVision60Robot_ground_air_boundary, manifold) { using namespace vision60_ground_air_boundary_step; auto iecm_params = std::make_shared(); - iecm_params->ecm_params->basis_creator = - std::make_shared(vision60.getBasisKeyFunc()); - iecm_params->ecm_params->retractor_creator = - std::make_shared(vision60.getBasisKeyFunc()); + iecm_params->equalityManifoldParams->basisCreator = + std::make_shared(vision60.getBasisKeyFunction()); + iecm_params->equalityManifoldParams->retractorCreator = + std::make_shared(vision60.getBasisKeyFunction()); auto retractor_params = std::make_shared(); - retractor_params->lm_params = LevenbergMarquardtParams(); - // retractor_params->lm_params.setVerbosityLM("SUMMARY"); - // retractor_params->lm_params.minModelFidelity = 0.5; - retractor_params->check_feasible = true; - retractor_params->feasible_threshold = 1e-3; - retractor_params->prior_sigma = 0.1; - iecm_params->retractor_creator = + retractor_params->lmParams = LevenbergMarquardtParams(); + // retractor_params->lmParams.setVerbosityLM("SUMMARY"); + // retractor_params->lmParams.minModelFidelity = 0.5; + retractor_params->checkFeasible = true; + retractor_params->feasibleThreshold = 1e-3; + retractor_params->priorSigma = 0.1; + iecm_params->retractorCreator = std::make_shared( vision60, retractor_params, true); - iecm_params->e_basis_creator = iecm_params->ecm_params->basis_creator; + iecm_params->equalityBasisCreator = iecm_params->equalityManifoldParams->basisCreator; - // KeyVector basis_keys = iecm_params->ecm_params->basis_key_func(e_cc); + // KeyVector basis_keys = iecm_params->equalityManifoldParams->basisKeyFunction(e_cc); // PrintKeyVector(basis_keys, "", GTDKeyFormatter); IEConstraintManifold manifold(iecm_params, e_constraints, i_constraints, diff --git a/tests/testIERetractor.cpp b/tests/testIERetractor.cpp index 091b94ccf..351f2aa71 100644 --- a/tests/testIERetractor.cpp +++ b/tests/testIERetractor.cpp @@ -26,9 +26,9 @@ TEST(IECartPoleWithFrictionCone, BarrierRetractor) { e_constraints->add(cp.eConstraints(k)); auto params = std::make_shared(); - params->ecm_params = std::make_shared(); - params->retractor_creator = std::make_shared(); - params->e_basis_creator = std::make_shared(); + params->equalityManifoldParams = std::make_shared(); + params->retractorCreator = std::make_shared(); + params->equalityBasisCreator = std::make_shared(); Values values; double q = M_PI_2, v = 0, a = 0; @@ -71,10 +71,10 @@ TEST(IECartPoleWithFrictionCone, BarrierRetractor) { // auto e_cc = std::make_shared(e_constraints); // auto params = std::make_shared(); -// params->ecm_params = std::make_shared(); -// params->retractor_creator = std::make_shared(std::make_shared()); -// params->e_basis_creator = -// std::make_shared(params->ecm_params->basis_params); +// params->equalityManifoldParams = std::make_shared(); +// params->retractorCreator = std::make_shared(std::make_shared()); +// params->equalityBasisCreator = +// std::make_shared(params->equalityManifoldParams->basis_params); // Values values; // double q = M_PI_2, v = 0, a = 0; @@ -159,10 +159,10 @@ TEST(IECartPoleWithFrictionCone, BarrierRetractor) { // auto e_cc = std::make_shared(e_constraints); // auto params = std::make_shared(); -// params->ecm_params = std::make_shared(); -// params->retractor_creator = std::make_shared(std::make_shared()); -// params->e_basis_creator = -// std::make_shared(params->ecm_params->basis_params); +// params->equalityManifoldParams = std::make_shared(); +// params->retractorCreator = std::make_shared(std::make_shared()); +// params->equalityBasisCreator = +// std::make_shared(params->equalityManifoldParams->basis_params); // Values values; // double q = M_PI_2, v = 0, a = 0; diff --git a/tests/testManifoldOptimizer_rot2.cpp b/tests/testManifoldOptimizer_rot2.cpp index 4468321fa..30717f320 100644 --- a/tests/testManifoldOptimizer_rot2.cpp +++ b/tests/testManifoldOptimizer_rot2.cpp @@ -12,7 +12,7 @@ */ #include -#include +#include #include #include #include @@ -174,7 +174,7 @@ using namespace gtsam; using namespace gtdynamics; /** Check creation of manifold optimization problem. */ -TEST(ManifoldOptProblem, SO2) { +TEST(ManifoldOptimizationProblem, SO2) { using namespace so2_scenario; auto costs = get_graph(-2, 0); auto constraints = get_constraints(); @@ -185,15 +185,15 @@ TEST(ManifoldOptProblem, SO2) { LevenbergMarquardtParams nopt_params; ManifoldOptimizerParameters mopt_params; - NonlinearMOptimizer optimizer(mopt_params, nopt_params); + NonlinearManifoldOptimizer optimizer(mopt_params, nopt_params); auto mopt_problem = - optimizer.initializeMoptProblem(*costs, *constraints, init_values); + optimizer.initializeManifoldOptimizationProblem(*costs, *constraints, init_values); - EXPECT_LONGS_EQUAL(1, mopt_problem.components_.size()); - EXPECT_LONGS_EQUAL(0, mopt_problem.unconstrained_keys_.size()); - EXPECT_LONGS_EQUAL(1, mopt_problem.manifold_keys_.size()); - EXPECT_LONGS_EQUAL(1, mopt_problem.values_.size()); - EXPECT_LONGS_EQUAL(2, mopt_problem.graph_.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.components.size()); + EXPECT_LONGS_EQUAL(0, mopt_problem.unconstrainedKeys.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.manifoldKeys.size()); + EXPECT_LONGS_EQUAL(1, mopt_problem.values.size()); + EXPECT_LONGS_EQUAL(2, mopt_problem.graph.size()); EXPECT_LONGS_EQUAL(2, mopt_problem.problemDimension().first); EXPECT_LONGS_EQUAL(1, mopt_problem.problemDimension().second); } @@ -225,7 +225,7 @@ TEST(ManifoldOptimization, SO2) { } /** Optimization using Type1 manifold optimizer. */ -TEST(NonlinearMOptimizer, SO2) { +TEST(NonlinearManifoldOptimizer, SO2) { using namespace so2_scenario; auto costs = get_graph(-2, 0); auto constraints = get_constraints(); @@ -238,7 +238,7 @@ TEST(NonlinearMOptimizer, SO2) { nopt_params.minModelFidelity = 0.5; // nopt_params.setVerbosityLM("SUMMARY"); ManifoldOptimizerParameters mopt_params; - NonlinearMOptimizer optimizer(mopt_params, nopt_params); + NonlinearManifoldOptimizer optimizer(mopt_params, nopt_params); auto result = optimizer.optimize(*costs, *constraints, init_values); // result.print(); @@ -247,7 +247,7 @@ TEST(NonlinearMOptimizer, SO2) { } /** Optimization using Type1 manifold optimizer, infeasible. */ -TEST(NonlinearMOptimizer_infeasible, SO2) { +TEST(NonlinearManifoldOptimizer_infeasible, SO2) { using namespace so2_scenario; auto costs = get_graph(-2, 0); auto constraints = get_constraints(); @@ -260,8 +260,8 @@ TEST(NonlinearMOptimizer_infeasible, SO2) { nopt_params.minModelFidelity = 0.5; // nopt_params.setVerbosityLM("SUMMARY"); ManifoldOptimizerParameters mopt_params; - mopt_params.cc_params->retractor_creator->params()->lm_params.setMaxIterations(4); - NonlinearMOptimizer optimizer(mopt_params, nopt_params); + mopt_params.constraintManifoldParams->retractorCreator->params()->lmParams.setMaxIterations(4); + NonlinearManifoldOptimizer optimizer(mopt_params, nopt_params); auto result = optimizer.optimize(*costs, *constraints, init_values); // result.print(); @@ -305,8 +305,8 @@ TEST(ManifoldOptimizer, GaussNewtonEquality) { GaussNewtonParams nopt_params; GaussNewtonOptimizer optimizer_m(graph_rot2, init_values_rot2, nopt_params); ManifoldOptimizerParameters mopt_params; - NonlinearMOptimizer optimizer_type1(mopt_params, nopt_params); - auto mopt_problem = optimizer_type1.initializeMoptProblem( + NonlinearManifoldOptimizer optimizer_type1(mopt_params, nopt_params); + auto mopt_problem = optimizer_type1.initializeManifoldOptimizationProblem( *costs_cm, *constraints_cm, init_values_cm); auto mopt_noptimizer = optimizer_type1.constructNonlinearOptimizer(mopt_problem); diff --git a/tests/testMultiJacobian.cpp b/tests/testMultiJacobian.cpp index 56a28f264..581a660b3 100644 --- a/tests/testMultiJacobian.cpp +++ b/tests/testMultiJacobian.cpp @@ -61,7 +61,7 @@ TEST(MultiJacobian, Add_Mult) { expected_mult1.insert({x2, (Matrix(2, 1) << 6, 4).finished()}); EXPECT(expected_mult1.equals(m * jac1)); - MultiJacobian jac12_stack = MultiJacobian::VerticalStack(jac1, jac2); + MultiJacobian jac12_stack = MultiJacobian::verticalStack(jac1, jac2); MultiJacobian expected_stack12; expected_stack12.insert( {x1, (Matrix(4, 2) << 1, 2, 3, 4, 2, 2, 3, 3).finished()}); @@ -103,7 +103,7 @@ TEST(MultiJacobians, Mult) { expected_jac_x4.insert({x1, (Matrix(1,2)<<1+2,2-2).finished()}); expected_jac_x4.insert({x2, (Matrix(1,2)<<2-1,1+1).finished()}); - MultiJacobians jacs_mult = JacobiansMultiply(jacs1, jacs2); + MultiJacobians jacs_mult = multiplyJacobians(jacs1, jacs2); EXPECT(expected_jac_x1.equals(jacs_mult.at(x1))); EXPECT(expected_jac_x2.equals(jacs_mult.at(x2))); EXPECT(expected_jac_x3.equals(jacs_mult.at(x3))); @@ -111,7 +111,7 @@ TEST(MultiJacobians, Mult) { } /// Test computing jacobians from a bayes net. -TEST(MultiJacobian, ComputeBayesNetJacobian) { +TEST(MultiJacobian, computeBayesNetJacobian) { /// Construct a bayes net Key x1 = 1; Key x2 = 2; @@ -140,7 +140,7 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { var_dim.emplace(x1, 1); var_dim.emplace(x2, 1); var_dim.emplace(x4, 1); - ComputeBayesNetJacobian(*bayes_net, basis_keys, var_dim, jacobians); + computeBayesNetJacobian(*bayes_net, basis_keys, var_dim, jacobians); MultiJacobian jacobian_x3, jacobian_x5; jacobian_x3.addJacobian(x1, I_1x1); @@ -148,9 +148,9 @@ TEST(MultiJacobian, ComputeBayesNetJacobian) { jacobian_x5.addJacobian(x1, H_3); jacobian_x5.addJacobian(x2, H_3); jacobian_x5.addJacobian(x4, H_4); - EXPECT(jacobians.at(x1).equals(MultiJacobian::Identity(x1, 1))); - EXPECT(jacobians.at(x2).equals(MultiJacobian::Identity(x2, 1))); - EXPECT(jacobians.at(x4).equals(MultiJacobian::Identity(x4, 1))); + EXPECT(jacobians.at(x1).equals(MultiJacobian::identity(x1, 1))); + EXPECT(jacobians.at(x2).equals(MultiJacobian::identity(x2, 1))); + EXPECT(jacobians.at(x4).equals(MultiJacobian::identity(x4, 1))); EXPECT(jacobians.at(x3).equals(jacobian_x3)); EXPECT(jacobians.at(x5).equals(jacobian_x5)); } diff --git a/tests/testRetractor.cpp b/tests/testRetractor.cpp index 2823e4483..7e35005f5 100644 --- a/tests/testRetractor.cpp +++ b/tests/testRetractor.cpp @@ -25,13 +25,13 @@ #include #include -#include "gtdynamics/cmopt/TspaceBasis.h" +#include "gtdynamics/cmopt/TangentSpaceBasis.h" using namespace gtsam; using namespace gtdynamics; /** Simple example Pose3 with between constraints. */ -TEST(TspaceBasis, connected_poses) { +TEST(TangentSpaceBasis, connected_poses) { Key x1_key = 1; Key x2_key = 2; Key x3_key = 3; @@ -54,13 +54,13 @@ TEST(TspaceBasis, connected_poses) { // Construct retractor. - auto params_uopt = std::make_shared(); - auto params_proj = std::make_shared(); - auto params_fix_vars = std::make_shared(); - params_fix_vars->use_basis_keys = true; + auto params_uopt = std::make_shared(); + auto params_proj = std::make_shared(); + auto params_fix_vars = std::make_shared(); + params_fix_vars->useBasisKeys = true; KeyVector basis_keys{x3_key}; - UoptRetractor retractor_uopt(constraints, params_uopt); - ProjRetractor retractor_proj(constraints, params_proj); + UnconstrainedOptimizationRetractor retractor_uopt(constraints, params_uopt); + ProjectionRetractor retractor_proj(constraints, params_proj); BasisRetractor retractor_basis(constraints, params_fix_vars, basis_keys); Values values_uopt = retractor_uopt.retractConstraints(base_values); diff --git a/tests/testTspaceBasis.cpp b/tests/testTspaceBasis.cpp index 170d99cdd..65fafdb36 100644 --- a/tests/testTspaceBasis.cpp +++ b/tests/testTspaceBasis.cpp @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testTspaceBasis.cpp + * @file testTangentSpaceBasis.cpp * @brief Test tangent space basis for constraint manifold. * @author Yetong Zhang */ @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include #include #include @@ -32,7 +32,7 @@ using namespace gtsam; using namespace gtdynamics; /** Simple example Pose3 with between constraints. */ -TEST(TspaceBasis, connected_poses) { +TEST(TangentSpaceBasis, connected_poses) { Key x1_key = 1; Key x2_key = 2; Key x3_key = 3; @@ -55,16 +55,16 @@ TEST(TspaceBasis, connected_poses) { // Construct basis. KeyVector basis_keys{x3_key}; - auto basis_params = std::make_shared(); + auto basis_params = std::make_shared(); auto basis_m = std::make_shared(constraints, cm_base_values, basis_params); auto basis_e = std::make_shared(constraints, cm_base_values, basis_params, basis_keys); - auto sparse_creator = OrthonormalBasisCreator::CreateSparse(); + auto sparse_creator = OrthonormalBasisCreator::createSparse(); auto basis_sm = sparse_creator->create(constraints, cm_base_values); auto linear_graph = constraints->penaltyGraph().linearize(cm_base_values); - std::vector basis_vec{basis_m, basis_e, basis_sm}; + std::vector basis_vec{basis_m, basis_e, basis_sm}; // Check dimension. for (const auto& basis : basis_vec) { @@ -124,7 +124,7 @@ TEST(TspaceBasis, connected_poses) { } /** Simple example Pose3 with between constraints. */ -TEST(TspaceBasis, linear_system) { +TEST(TangentSpaceBasis, linear_system) { Key x1_key = 1; Key x2_key = 2; Key x3_key = 3; @@ -146,12 +146,12 @@ TEST(TspaceBasis, linear_system) { values.insertDouble(x3_key, 0.0); values.insertDouble(x4_key, 0.0); KeyVector basis_keys{x1_key, x2_key}; - auto basis_params = std::make_shared(); + auto basis_params = std::make_shared(); // auto basis_m = std::make_shared(basis_params, cc, values); auto basis_e = std::make_shared(constraints, values, basis_params, basis_keys); auto basis_m = std::make_shared(constraints, values, basis_params); - auto sparse_creator = OrthonormalBasisCreator::CreateSparse(); + auto sparse_creator = OrthonormalBasisCreator::createSparse(); auto basis_sm = sparse_creator->create(constraints, values); // Construct new basis by adding additional constraints