12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

addressing comments

release/4.3a0
yetongumich 2024-12-31 14:26:31 -08:00
parent 021d105428
commit 3c81405a01
18 changed files with 105 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/CMakeLists.txt
View File

@ -6,7 +6,7 @@ project(gtsam LANGUAGES CXX)
set (gtsam_subdirs
base
basis
constraint
constrained
geometry
inference
symbolic

2
gtsam/constraint/CMakeLists.txt → gtsam/constrained/CMakeLists.txt
View File

@ -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)

2
gtsam/constraint/InequalityPenaltyFunction.cpp → gtsam/constrained/InequalityPenaltyFunction.cpp
View File

@ -16,7 +16,7 @@
* @date Aug 3, 2024
*/
#include <gtsam/constraint/InequalityPenaltyFunction.h>
#include <gtsam/constrained/InequalityPenaltyFunction.h>
namespace gtsam {

9
gtsam/constraint/InequalityPenaltyFunction.h → gtsam/constrained/InequalityPenaltyFunction.h
View File

@ -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,

0
gtsam/constraint/NonlinearConstraint.h → gtsam/constrained/NonlinearConstraint.h
View File

2
gtsam/constraint/NonlinearEqualityConstraint-inl.h → gtsam/constrained/NonlinearEqualityConstraint-inl.h
View File

@ -17,7 +17,7 @@
#pragma once
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
namespace gtsam {

2
gtsam/constraint/NonlinearEqualityConstraint.cpp → gtsam/constrained/NonlinearEqualityConstraint.cpp
View File

@ -15,7 +15,7 @@
* @author Yetong Zhang, Frank Dellaert
* @date Aug 3, 2024 */
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/inference/FactorGraph-inst.h>
namespace gtsam {

10
gtsam/constraint/NonlinearEqualityConstraint.h → gtsam/constrained/NonlinearEqualityConstraint.h
View File

@ -17,7 +17,7 @@
#pragma once
#include <gtsam/constraint/NonlinearConstraint.h>
#include <gtsam/constrained/NonlinearConstraint.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -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<NonlinearEq
} // namespace gtsam
#include <gtsam/constraint/NonlinearEqualityConstraint-inl.h>
#include <gtsam/constrained/NonlinearEqualityConstraint-inl.h>

2
gtsam/constraint/NonlinearInequalityConstraint.cpp → gtsam/constrained/NonlinearInequalityConstraint.cpp
View File

@ -16,7 +16,7 @@
* @date Aug 3, 2024
*/
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
#include <gtsam/inference/FactorGraph-inst.h>
namespace gtsam {

4
gtsam/constraint/NonlinearInequalityConstraint.h → gtsam/constrained/NonlinearInequalityConstraint.h
View File

@ -18,8 +18,8 @@
#pragma once
#include <gtsam/constraint/InequalityPenaltyFunction.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/InequalityPenaltyFunction.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/nonlinear/expressions.h>
namespace gtsam {

0
gtsam/constraint/tests/CMakeLists.txt → gtsam/constrained/tests/CMakeLists.txt
View File

73
gtsam/constrained/tests/constrainedExample.h Normal file
View File

@ -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 <gtsam/constrained/ConstrainedOptProblem.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
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

2
gtsam/constraint/tests/testInequalityPenaltyFunction.cpp → gtsam/constrained/tests/testInequalityPenaltyFunction.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/InequalityPenaltyFunction.h>
#include <gtsam/constrained/InequalityPenaltyFunction.h>
using namespace gtsam;

2
gtsam/constraint/tests/testNonlinearEqualityConstraint.cpp → gtsam/constrained/tests/testNonlinearEqualityConstraint.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>

2
gtsam/constraint/tests/testNonlinearInequalityConstraint.cpp → gtsam/constrained/tests/testNonlinearInequalityConstraint.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
#include <gtsam/nonlinear/factorTesting.h>
#include "constrainedExample.h"

181
gtsam/constraint/tests/constrainedExample.h
View File

@ -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 <gtsam/constraint/ConstrainedOptProblem.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
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<double>(cost_noise, 0., f1));
// graph.add(ExpressionFactor<double>(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<double>(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<ConstrainedOptProblem>(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<ExpressionEqualityConstraint<double>>(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<ScalarExpressionInequalityConstraint>(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<ConstrainedOptProblem>(costs, e_constraints, i_constraints, init_values);
// Values optimal_values = OptimalValues();
// } // namespace constrained_example2

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -18,7 +18,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constraint/NonlinearEqualityConstraint.h>
#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

10
gtsam/slam/BoundingConstraint.h
View File

@ -19,7 +19,7 @@
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/constraint/NonlinearInequalityConstraint.h>
#include <gtsam/constrained/NonlinearInequalityConstraint.h>
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<X>(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<X1>(keys().front());
X2 x2 = x.at<X2>(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) {