add definition of constraints #1789

yetongumich · 2024-08-06T14:26:59Z

First PR in the 4 PRs to implement nonlinear constrained optimization in GTSAM.

Introducing factors representing equality and inequality constraints; refactor existing constraint factors to inherit from it.
Implementation of existing constrained optimization methods that are compatible with the current gtsam framework, including penalty methods, augmented Lagrangian methods, SQP methods (potentially also interior point methods), with a document explaining the math of the implementations.
Constraint manifold optimization that solves problems with equality constraints.
Constraint manifold with corners optimization that solves problems with both equality and inequality constraints.
(potentially 1 additional PR) That solves an equality-constrained factor graph using nonlinear variable elimination (resulting in an behavior identical to iLQR or DDP).

This PR

Introduce NonlinearEqualityConstraint and NonlinearInequalityConstraint inheriting from NoiseModelFactor to represent constraints.
Refactor the existing classesNonlinearEquality to inherit from NonlinearEqualityConstraint, and BoundingConstraint to inherit from NonlinearInequalityConstraint.

dellaert · 2024-08-26T23:20:21Z

CI fails. Talking about CI, try to save on CO2 by not pushing every commit, but only after you think CI will pass.

yetongumich · 2024-08-27T02:08:00Z

CI fails. Talking about CI, try to save on CO2 by not pushing every commit, but only after you think CI will pass.

It seems pretty weird that only the Windows CI fails. Unfortunately I do not have a Windows environment to test this on. I was trying to tweak with different changes to see which causes the Windows CI to fail.

ProfFan · 2024-08-27T16:41:36Z

gtsam/constraint/NonlinearInequalityConstraint.cpp

+ */
+
+#include <gtsam/constraint/NonlinearInequalityConstraint.h>
+


Include "
#include <gtsam/inference/FactorGraph-inst.h>"

Added this line, CI still reports the same error in Windows environments.

dellaert · 2024-09-13T20:48:15Z

@yetongatech error is due to a argument type not being recognized:

D:\a\gtsam\gtsam\gtsam/inference/FactorGraph-inst.h(69): error C2664: 'double gtsam::NoiseModelFactor::error(const gtsam::Values &) const': cannot convert argument 1 from 'const gtsam::HybridValues' to 'const gtsam::Values &'

Try figuring out what type you want and somehow being explicit? I just looked at the error above but maybe you can dig deeper.

dellaert

Cool ! Some comments/questions.
Still need to pass CI

dellaert · 2024-09-20T19:54:22Z

gtsam/CMakeLists.txt

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


If this directory only contains constraint classes, this is ok. Otherwise (if optimization code will land here) could we rename this to constrained ?

dellaert · 2024-09-20T19:58:57Z

gtsam/constraint/InequalityPenaltyFunction.h

+  double k_;
+
+ public:
+  SoftPlusFunction(const double k = 1) : Base(), k_(k) {}


doxygen block describing argument

dellaert · 2024-09-20T19:59:10Z

gtsam/constraint/InequalityPenaltyFunction.h

+  double b_;
+
+ public:
+  SmoothRampPoly3(const double epsilon = 1)


doxygen block describing argument

dellaert · 2024-09-20T19:59:23Z

gtsam/constraint/InequalityPenaltyFunction.h

+  double a_;
+
+ public:
+  SmoothRampPoly2(const double epsilon = 1)


doxygen block describing argument

dellaert · 2024-09-20T20:03:37Z

gtsam/constraint/NonlinearEqualityConstraint.h

+   * @brief Constructor.
+   *
+   * @param factor  NoiseModel factor.
+   * @param tolerance   vector representing tolerance in each dimension.


dellaert · 2024-09-20T20:08:58Z

gtsam/nonlinear/NonlinearEquality.h

+  Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
+    VALUE x1 = x.at<VALUE>(key());
+    if (H) {
+      return evaluateError(x1, &(H->front()));


does the base class not have unwhitenedError? Why not? Would that not be easier?

dellaert · 2024-09-20T20:09:47Z

gtsam/slam/BoundingConstraint.h

@@ -60,14 +59,23 @@ struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
  virtual double value(const X& x, OptionalMatrixType H =
      OptionalNone) const = 0;

-  /** active when constraint *NOT* met */
-  bool active(const Values& c) const override {


is active not used? Or is it in base class? Backwards compat...

dellaert · 2024-09-20T20:10:07Z

gtsam/slam/BoundingConstraint.h

-    // note: still active at equality to avoid zigzagging
-    double x = value(c.at<X>(this->key()));
-    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {


same, base class unwhitenedError?

dellaert · 2024-09-20T20:11:02Z

gtsam/slam/BoundingConstraint.h

-    // note: still active at equality to avoid zigzagging
-    double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
-    return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
+  Vector unwhitenedExpr(const Values& x, OptionalMatrixVecType H = nullptr) const override {


same question about active
what is unwhitenedExpr? Not documented

dellaert · 2024-09-20T20:11:24Z

gtsam/slam/BoundingConstraint.h

  Vector evaluateError(const X1& x1, const X2& x2,
-      OptionalMatrixType H1, OptionalMatrixType H2) const override {
+      OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const {


You used {} elsewhere

ProfFan · 2024-09-26T17:54:32Z

I think if you rebase on current develop it should fix stuff

yetongumich · 2024-10-03T20:33:24Z

I think if you rebase on current develop it should fix stuff

Seems still not working

dellaert · 2024-10-23T16:29:01Z

@yetongatech you might have to investigate a bit to get past windows issue. None of us have the answers exactly :-)

add definition of constraints

59f6f08

yetongumich requested a review from dellaert August 6, 2024 14:27

yetongatech added 6 commits August 6, 2024 10:41

add serialization

ba50bd7

remove accidentally added file

80fcff1

clean up and refactor NonlinearEquality and BoundingConstraint

9b0fa12

add GTSAM_EXPORT and refactor

6caf0a3

adding constrained optimization problem

7bb76f5

rm constrainedOptProblem

8b09ef1

ProfFan reviewed Aug 27, 2024

View reviewed changes

yetongatech added 2 commits August 27, 2024 16:25

add FactorGraph include

e482c41

add FactorGraph-inst

2516a0a

dellaert reviewed Sep 20, 2024

View reviewed changes

Merge branch 'develop' into feature/constrained_optimization

90536c5

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

add definition of constraints #1789

add definition of constraints #1789

yetongumich commented Aug 6, 2024 •

edited

Loading

dellaert commented Aug 26, 2024

yetongumich commented Aug 27, 2024

ProfFan Aug 27, 2024

yetongumich Aug 27, 2024

dellaert commented Sep 13, 2024

dellaert left a comment

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

dellaert Sep 20, 2024

ProfFan commented Sep 26, 2024

yetongumich commented Oct 3, 2024

dellaert commented Oct 23, 2024

		*/

		#include <gtsam/constraint/NonlinearInequalityConstraint.h>

add definition of constraints #1789

Are you sure you want to change the base?

add definition of constraints #1789

Conversation

yetongumich commented Aug 6, 2024 • edited Loading

dellaert commented Aug 26, 2024

yetongumich commented Aug 27, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert commented Sep 13, 2024

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ProfFan commented Sep 26, 2024

yetongumich commented Oct 3, 2024

dellaert commented Oct 23, 2024

yetongumich commented Aug 6, 2024 •

edited

Loading