From 3c81405a019eddc398a89ba4df70deb3de806297 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Tue, 31 Dec 2024 14:26:31 -0800 Subject: [PATCH] addressing comments --- gtsam/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- .../InequalityPenaltyFunction.cpp | 2 +- .../InequalityPenaltyFunction.h | 9 + .../NonlinearConstraint.h | 0 .../NonlinearEqualityConstraint-inl.h | 2 +- .../NonlinearEqualityConstraint.cpp | 2 +- .../NonlinearEqualityConstraint.h | 10 +- .../NonlinearInequalityConstraint.cpp | 2 +- .../NonlinearInequalityConstraint.h | 4 +- .../tests/CMakeLists.txt | 0 gtsam/constrained/tests/constrainedExample.h | 73 +++++++ .../tests/testInequalityPenaltyFunction.cpp | 2 +- .../tests/testNonlinearEqualityConstraint.cpp | 2 +- .../testNonlinearInequalityConstraint.cpp | 2 +- gtsam/constraint/tests/constrainedExample.h | 181 ------------------ gtsam/nonlinear/NonlinearEquality.h | 2 +- gtsam/slam/BoundingConstraint.h | 10 +- 18 files changed, 105 insertions(+), 202 deletions(-) rename gtsam/{constraint => constrained}/CMakeLists.txt (93%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.cpp (97%) rename gtsam/{constraint => constrained}/InequalityPenaltyFunction.h (93%) rename gtsam/{constraint => constrained}/NonlinearConstraint.h (100%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint-inl.h (97%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.cpp (98%) rename gtsam/{constraint => constrained}/NonlinearEqualityConstraint.h (96%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/NonlinearInequalityConstraint.h (98%) rename gtsam/{constraint => constrained}/tests/CMakeLists.txt (100%) create mode 100644 gtsam/constrained/tests/constrainedExample.h rename gtsam/{constraint => constrained}/tests/testInequalityPenaltyFunction.cpp (98%) rename gtsam/{constraint => constrained}/tests/testNonlinearEqualityConstraint.cpp (99%) rename gtsam/{constraint => constrained}/tests/testNonlinearInequalityConstraint.cpp (98%) delete mode 100644 gtsam/constraint/tests/constrainedExample.h diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index dcc350fe8..0911ef550 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -6,7 +6,7 @@ project(gtsam LANGUAGES CXX) set (gtsam_subdirs base basis - constraint + constrained geometry inference symbolic diff --git a/gtsam/constraint/CMakeLists.txt b/gtsam/constrained/CMakeLists.txt similarity index 93% rename from gtsam/constraint/CMakeLists.txt rename to gtsam/constrained/CMakeLists.txt index e4bbd89dd..168744076 100644 --- a/gtsam/constraint/CMakeLists.txt +++ b/gtsam/constrained/CMakeLists.txt @@ -1,6 +1,6 @@ # Install headers file(GLOB constraint_headers "*.h") -install(FILES ${constraint_headers} DESTINATION include/gtsam/constraint) +install(FILES ${constraint_headers} DESTINATION include/gtsam/constrained) # Build tests add_subdirectory(tests) diff --git a/gtsam/constraint/InequalityPenaltyFunction.cpp b/gtsam/constrained/InequalityPenaltyFunction.cpp similarity index 97% rename from gtsam/constraint/InequalityPenaltyFunction.cpp rename to gtsam/constrained/InequalityPenaltyFunction.cpp index 8b048686f..2e331292c 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.cpp +++ b/gtsam/constrained/InequalityPenaltyFunction.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include namespace gtsam { diff --git a/gtsam/constraint/InequalityPenaltyFunction.h b/gtsam/constrained/InequalityPenaltyFunction.h similarity index 93% rename from gtsam/constraint/InequalityPenaltyFunction.h rename to gtsam/constrained/InequalityPenaltyFunction.h index baafaef0b..3aef099af 100644 --- a/gtsam/constraint/InequalityPenaltyFunction.h +++ b/gtsam/constrained/InequalityPenaltyFunction.h @@ -83,6 +83,9 @@ class GTSAM_EXPORT SmoothRampPoly2 : public InequalityPenaltyFunction { double a_; public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ SmoothRampPoly2(const double epsilon = 1) : Base(), epsilon_(epsilon), a_(0.5 / epsilon) {} @@ -110,6 +113,9 @@ class GTSAM_EXPORT SmoothRampPoly3 : public InequalityPenaltyFunction { double b_; public: + /** Constructor. + * @param epsilon parameter for adjusting the smoothness of the function. + */ SmoothRampPoly3(const double epsilon = 1) : Base(), epsilon_(epsilon), @@ -131,6 +137,9 @@ class GTSAM_EXPORT SoftPlusFunction : public InequalityPenaltyFunction { double k_; public: + /** Constructor. + * @param k parameter for adjusting the smoothness of the function. + */ SoftPlusFunction(const double k = 1) : Base(), k_(k) {} virtual double operator()(const double& x, diff --git a/gtsam/constraint/NonlinearConstraint.h b/gtsam/constrained/NonlinearConstraint.h similarity index 100% rename from gtsam/constraint/NonlinearConstraint.h rename to gtsam/constrained/NonlinearConstraint.h diff --git a/gtsam/constraint/NonlinearEqualityConstraint-inl.h b/gtsam/constrained/NonlinearEqualityConstraint-inl.h similarity index 97% rename from gtsam/constraint/NonlinearEqualityConstraint-inl.h rename to gtsam/constrained/NonlinearEqualityConstraint-inl.h index c228cf97d..14bc42939 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint-inl.h +++ b/gtsam/constrained/NonlinearEqualityConstraint-inl.h @@ -17,7 +17,7 @@ #pragma once -#include +#include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.cpp b/gtsam/constrained/NonlinearEqualityConstraint.cpp similarity index 98% rename from gtsam/constraint/NonlinearEqualityConstraint.cpp rename to gtsam/constrained/NonlinearEqualityConstraint.cpp index 170f2317c..effc8774c 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/NonlinearEqualityConstraint.cpp @@ -15,7 +15,7 @@ * @author Yetong Zhang, Frank Dellaert * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearEqualityConstraint.h b/gtsam/constrained/NonlinearEqualityConstraint.h similarity index 96% rename from gtsam/constraint/NonlinearEqualityConstraint.h rename to gtsam/constrained/NonlinearEqualityConstraint.h index 30e8e52d3..017ca1607 100644 --- a/gtsam/constraint/NonlinearEqualityConstraint.h +++ b/gtsam/constrained/NonlinearEqualityConstraint.h @@ -17,7 +17,7 @@ #pragma once -#include +#include #include #include @@ -99,7 +99,10 @@ class ExpressionEqualityConstraint : public NonlinearEqualityConstraint { #endif }; -/** Equality constraint that enforce the cost factor with zero error. */ +/** Equality constraint that enforce the cost factor with zero error. + * e.g., for a factor with unwhitened cost 2x-1, the constraint enforces the + * equlity 2x-1=0. + */ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { public: typedef NonlinearEqualityConstraint Base; @@ -114,7 +117,6 @@ class GTSAM_EXPORT ZeroCostConstraint : public NonlinearEqualityConstraint { * @brief Constructor. * * @param factor NoiseModel factor. - * @param tolerance vector representing tolerance in each dimension. */ ZeroCostConstraint(const NoiseModelFactor::shared_ptr& factor); @@ -177,4 +179,4 @@ class GTSAM_EXPORT NonlinearEqualityConstraints : public FactorGraph +#include diff --git a/gtsam/constraint/NonlinearInequalityConstraint.cpp b/gtsam/constrained/NonlinearInequalityConstraint.cpp similarity index 99% rename from gtsam/constraint/NonlinearInequalityConstraint.cpp rename to gtsam/constrained/NonlinearInequalityConstraint.cpp index 2f96f9a29..b1dcb7534 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/NonlinearInequalityConstraint.cpp @@ -16,7 +16,7 @@ * @date Aug 3, 2024 */ -#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/NonlinearInequalityConstraint.h b/gtsam/constrained/NonlinearInequalityConstraint.h similarity index 98% rename from gtsam/constraint/NonlinearInequalityConstraint.h rename to gtsam/constrained/NonlinearInequalityConstraint.h index 658a29f2f..9dbf1f008 100644 --- a/gtsam/constraint/NonlinearInequalityConstraint.h +++ b/gtsam/constrained/NonlinearInequalityConstraint.h @@ -18,8 +18,8 @@ #pragma once -#include -#include +#include +#include #include namespace gtsam { diff --git a/gtsam/constraint/tests/CMakeLists.txt b/gtsam/constrained/tests/CMakeLists.txt similarity index 100% rename from gtsam/constraint/tests/CMakeLists.txt rename to gtsam/constrained/tests/CMakeLists.txt diff --git a/gtsam/constrained/tests/constrainedExample.h b/gtsam/constrained/tests/constrainedExample.h new file mode 100644 index 000000000..0b708a41f --- /dev/null +++ b/gtsam/constrained/tests/constrainedExample.h @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file constrainedExample.h + * @brief Simple constrained optimization scenarios. + * @author Yetong Zhang + * @date Aug 3, 2024 + */ + +#pragma once + +// #include +#include +#include +#include +#include + +namespace constrained_example { + +using namespace gtsam; + +/// Exponential function e^x. +inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { + double result = exp(x); + if (H1) H1->setConstant(result); + return result; +} + +/// Exponential expression e^x. +inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } + +/// Pow functor used for pow function. +class PowFunctor { + private: + double c_; + + public: + PowFunctor(const double& c) : c_(c) {} + + double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { + if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); + return pow(x, c_); + } +}; + +/// Pow function. +inline Double_ pow(const Double_& x, const double& c) { + PowFunctor pow_functor(c); + return Double_(pow_functor, x); +} + +/// Plus between Double expression and double. +inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } + +/// Negative sign operator. +inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } + +/// Keys for creating expressions. +Symbol x1_key('x', 1); +Symbol x2_key('x', 2); +Double_ x1(x1_key), x2(x2_key); + +} // namespace constrained_example + diff --git a/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp similarity index 98% rename from gtsam/constraint/tests/testInequalityPenaltyFunction.cpp rename to gtsam/constrained/tests/testInequalityPenaltyFunction.cpp index 09cd17d5b..9b41dbe2c 100644 --- a/gtsam/constraint/tests/testInequalityPenaltyFunction.cpp +++ b/gtsam/constrained/tests/testInequalityPenaltyFunction.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include using namespace gtsam; diff --git a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp similarity index 99% rename from gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp index 70dabff45..e09c197a3 100644 --- a/gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include diff --git a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp similarity index 98% rename from gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp rename to gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp index 3950bfa03..4d9034daa 100644 --- a/gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp +++ b/gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include "constrainedExample.h" diff --git a/gtsam/constraint/tests/constrainedExample.h b/gtsam/constraint/tests/constrainedExample.h deleted file mode 100644 index 2cc3ba826..000000000 --- a/gtsam/constraint/tests/constrainedExample.h +++ /dev/null @@ -1,181 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file constrainedExample.h - * @brief Simple constrained optimization scenarios. - * @author Yetong Zhang - * @date Aug 3, 2024 - */ - -#pragma once - -// #include -#include -#include -#include -#include - -namespace constrained_example { - -using namespace gtsam; - -/// Exponential function e^x. -inline double exp_func(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) { - double result = exp(x); - if (H1) H1->setConstant(result); - return result; -} - -/// Exponential expression e^x. -inline Double_ exp(const Double_& x) { return Double_(exp_func, x); } - -/// Pow functor used for pow function. -class PowFunctor { - private: - double c_; - - public: - PowFunctor(const double& c) : c_(c) {} - - double operator()(const double& x, gtsam::OptionalJacobian<1, 1> H1 = {}) const { - if (H1) H1->setConstant(c_ * pow(x, c_ - 1)); - return pow(x, c_); - } -}; - -/// Pow function. -inline Double_ pow(const Double_& x, const double& c) { - PowFunctor pow_functor(c); - return Double_(pow_functor, x); -} - -/// Plus between Double expression and double. -inline Double_ operator+(const Double_& x, const double& d) { return x + Double_(d); } - -/// Negative sign operator. -inline Double_ operator-(const Double_& x) { return Double_(0.0) - x; } - -/// Keys for creating expressions. -Symbol x1_key('x', 1); -Symbol x2_key('x', 2); -Double_ x1(x1_key), x2(x2_key); - -} // namespace constrained_example - -/* ************************************************************************* */ -/** - * Constrained optimization example in L. Vandenberghe slides: - * https://www.seas.ucla.edu/~vandenbe/133B/lectures/nllseq.pdf - * f(x) = 0.5*||x1 + e^(-x2)||^2 + 0.5*||x1^2 + 2*x2 + 1||^2 - * h(x) = x1 + x1^3 + x2 + x2^2 = 0 - */ -// namespace constrained_example1 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// NonlinearFactorGraph graph; -// auto f1 = x1 + exp(-x2); -// auto f2 = pow(x1, 2.0) + 2.0 * x2 + 1.0; -// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); -// graph.add(ExpressionFactor(cost_noise, 0., f1)); -// graph.add(ExpressionFactor(cost_noise, 0., f2)); -// return graph; -// } - -// NonlinearEqualityConstraints EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector sigmas = Vector1(1.0); -// auto h1 = x1 + pow(x1, 3) + x2 + pow(x2, 2); -// constraints.push_back(NonlinearEqualityConstraint::shared_ptr( -// new ExpressionEqualityConstraint(h1, 0.0, sigmas))); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -0.2); -// values.insert(x2_key, -0.2); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 0.0); -// values.insert(x2_key, 0.0); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints; -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); -// } // namespace constrained_example1 - -// /* ************************************************************************* */ -// /** -// * Constrained optimization example with inequality constraints -// * Approach a point while staying on unit circle, and within an ellipse. -// * f(x) = 0.5 * ||x1-1||^2 + 0.5 * ||x2-1||^2 -// * h(x) = x1^2 + x2^2 - 1 = 0 -// * g(x) = 4*x1^2 + 0.25*x2^2 - 1 <= 0 -// */ -// namespace constrained_example2 { -// using namespace constrained_example; -// NonlinearFactorGraph Cost() { -// NonlinearFactorGraph graph; -// auto cost_noise = gtsam::noiseModel::Isotropic::Sigma(1, 1.0); -// graph.addPrior(x1_key, 1.0, cost_noise); -// graph.addPrior(x2_key, 1.0, cost_noise); -// return graph; -// } - -// NonlinearEqualityConstraints EqConstraints() { -// NonlinearEqualityConstraints constraints; -// Vector1 sigmas(1.0); -// Double_ h1 = x1 * x1 + x2 * x2; -// constraints.emplace_shared>(h1, 1.0, sigmas); -// return constraints; -// } - -// NonlinearInequalityConstraints IneqConstraints() { -// NonlinearInequalityConstraints constraints; -// Double_ g1 = 4 * x1 * x1 + 0.25 * x2 * x2 - Double_(1.0); -// double sigma = 1; -// constraints.emplace_shared(g1, sigma); -// return constraints; -// } - -// Values InitValues() { -// Values values; -// values.insert(x1_key, -1.0); -// values.insert(x2_key, 2.0); -// return values; -// } - -// Values OptimalValues() { -// Values values; -// values.insert(x1_key, 1.0 / sqrt(5)); -// values.insert(x2_key, 2.0 / sqrt(5)); -// return values; -// } - -// NonlinearFactorGraph costs = Cost(); -// NonlinearEqualityConstraints e_constraints = EqConstraints(); -// NonlinearInequalityConstraints i_constraints = IneqConstraints(); -// Values init_values = InitValues(); -// ConstrainedOptProblem::shared_ptr problem = -// std::make_shared(costs, e_constraints, i_constraints, init_values); -// Values optimal_values = OptimalValues(); - -// } // namespace constrained_example2 diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c91bddd9c..cac54f332 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include #include #include diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index d35152adf..4c6c775a0 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -19,7 +19,7 @@ #include #include -#include +#include namespace gtsam { @@ -59,7 +59,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { virtual double value(const X& x, OptionalMatrixType H = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { if (H) { double d = value(x.at(this->key()), &(H->front())); if (isGreaterThan_) { @@ -75,7 +75,7 @@ struct BoundingConstraint1: public NonlinearInequalityConstraint { } /// TODO: This should be deprecated. - Vector evaluateError(const X& x, OptionalMatrixType H = nullptr) const { + Vector evaluateError(const X& x, OptionalMatrixType H = {}) const { Matrix D; double error = value(x, &D) - threshold_; if (H) { @@ -138,7 +138,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const = 0; - Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override { + Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = {}) const override { X1 x1 = x.at(keys().front()); X2 x2 = x.at(keys().back()); if (H) { @@ -158,7 +158,7 @@ struct BoundingConstraint2: public NonlinearInequalityConstraint { /// TODO: This should be deprecated. Vector evaluateError(const X1& x1, const X2& x2, - OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const { + OptionalMatrixType H1 = {}, OptionalMatrixType H2 = {}) const { Matrix D1, D2; double error = value(x1, x2, &D1, &D2) - threshold_; if (H1) {