Linear/EqualityConstraint

[Duy-Nguyen Ta]

Add linear/EqualityConstraint, which is result of linearizing nonlinear/EqualityConstraint, also does general Ax==b

[Duy-Nguyen Ta]

Question for @dellaert : I think it is handled in the current JacobianFactor already. What is the motivation for another special equality constraint factor at the linear level?

[Frank Dellaert]

Actually, it's so that users could find it. It can be a trivial derived class of JacobianFactor, one that takes and A and a b and implements Ax==b

[Frank Dellaert]

Actually, would make your constraint stuff in QP much more readable, compare

  // Special constrained noise model denoting <= inequalities with negative sigmas
  noiseModel::Constrained::shared_ptr noise =
      noiseModel::Constrained::MixedSigmas((Vector(4) << -1, -1, -1, -1));
  graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise));

with desired

  graph.push_back(InequalityConstraint(X(1), A1, X(2), A2, b, noise));

(realizing you could create both an EqualityConstraint and an InequalityConstraint. Adding the latter in QP branch would allow us to easily change the implementation, if we decide to abandon the -1 hack.

[Frank Dellaert]

Is this done, @thduynguyen ?

[Duy-Nguyen Ta]

Yes. A simple QP without LP is now in develop. We will need to implement the simplex method to find a feasible initial point in the feature. It is important for SQP to solve nonlinear constraints.

[Frank Dellaert]

Then close this issue?

[Duy-Nguyen Ta]

LinearEquality and LinearInequality constraints, inherited from JacobianFactor, are added, together with a QPSolver.