add definition of constraints

[yetongumich]

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

1. Introducing factors representing equality and inequality constraints; refactor existing constraint factors to inherit from it.
2. 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.
3. Constraint manifold optimization that solves problems with equality constraints.
4. Constraint manifold with corners optimization that solves problems with both equality and inequality constraints.
5. (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]

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

[yetongumich]

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.

[dellaert]

@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

[ProfFan]

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

[yetongumich]

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

Seems still not working

[dellaert]

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

[dellaert]

@yetongumich , checks passed, will you address comments (and resolve them when addressed) ?