diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 560414419..c174b8a01 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -100,7 +100,17 @@ public: return worstFactorIx; } - /// TODO: comment + /* This function will create a dual graph that solves for the + * lagrange multipliers for the current working set. + * You can use lagrange multipliers as a necessary condition for optimality. + * The factor graph that is being solved is f' = -lambda * g' + * where f is the optimized function and g is the function resulting from + * aggregating the working set. + * The lambdas give you information about the feasibility of a constraint. + * if lambda < 0 the constraint is Ok + * if lambda = 0 you are on the constraint + * if lambda > 0 you are violating the constraint. + */ GaussianFactorGraph::shared_ptr buildDualGraph( const InequalityFactorGraph& workingSet, const VectorValues& delta) const { diff --git a/gtsam_unstable/linear/LPInitSolverMatlab.h b/gtsam_unstable/linear/LPInitSolverMatlab.h index d6f28e563..e9b792a35 100644 --- a/gtsam_unstable/linear/LPInitSolverMatlab.h +++ b/gtsam_unstable/linear/LPInitSolverMatlab.h @@ -116,7 +116,7 @@ private: } - /// Collect all terms of a factor into a container. TODO: avoid memcpy? + /// Collect all terms of a factor into a container. std::vector > collectTerms(const LinearInequality& factor) const { std::vector > terms; for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 762431010..849be035d 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -35,7 +35,10 @@ public: return keysDim_; } - /// TODO(comment) + /* + * Iterates through every factor in a linear graph and generates a + * mapping between every factor key and it's corresponding dimensionality. + */ template KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { KeyDimMap keysDim; @@ -51,7 +54,12 @@ public: GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys, const KeyDimMap& keysDim) const; - /// TODO(comment) + /* + * This function performs an iteration of the Active Set Method for solving + * LP problems. At the end of this iteration the problem should either be found + * to be unfeasible, solved or the current state changed to reflect a new + * working set. + */ LPState iterate(const LPState& state) const; /** @@ -73,7 +81,12 @@ public: VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, const InequalityFactorGraph& workingSet) const; - /// TODO(comment) + /* + * A dual factor takes the objective function and a set of constraints. + * It then creates a least-square approximation of the lagrangian multipliers + * for the following problem: f' = - lambda * g' where f is the objection + * function g are dual factors and lambda is the lagrangian multiplier. + */ JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph& workingSet, const VectorValues& delta) const; @@ -82,7 +95,11 @@ public: const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; - /// TODO(comment) + /* + * Given an initial value this function determine which constraints are active + * which can be used to initialize the working set. + * A constraint Ax <= b is active if we have an x' s.t. Ax' = b + */ InequalityFactorGraph identifyActiveConstraints( const InequalityFactorGraph& inequalities, const VectorValues& initialValues, const VectorValues& duals) const; diff --git a/gtsam_unstable/linear/LPState.h b/gtsam_unstable/linear/LPState.h index fee5d3aa8..187ea2b51 100644 --- a/gtsam_unstable/linear/LPState.h +++ b/gtsam_unstable/linear/LPState.h @@ -10,13 +10,21 @@ namespace gtsam { -// TODO: comment +/* + * This struct contains the state information for a single iteration of an + * active set method iteration. + */ struct LPState { - // TODO: comment member variables + // A itermediate value for the value of the final solution. VectorValues values; + // Constains the set of duals computed during the iteration that retuned this + // state. VectorValues duals; + // An inequality Factor Graph that contains only the active constriants. InequalityFactorGraph workingSet; + // True if the algorithm has converged to a solution bool converged; + // counter for the number of iteration. Incremented at the end of each iter. size_t iterations; /// default constructor diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 7473994b0..bb47067cc 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -79,21 +79,6 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor( } //****************************************************************************** -/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints - * If some inactive inequality constraints complain about the full step (alpha = 1), - * we have to adjust alpha to stay within the inequality constraints' feasible regions. - * - * For each inactive inequality j: - * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints - * - We want: aj'*(xk + alpha*p) - bj <= 0 - * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 - * it's good! - * - We only care when aj'*p > 0. In this case, we need to choose alpha so that - * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) - * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) - * - * We want the minimum of all those alphas among all inactive inequality. - */ boost::tuple QPSolver::computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const { diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index fd30945e3..6ff47e580 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -34,7 +34,7 @@ namespace gtsam { * defined in the QP struct. * Note: This version of QPSolver only works with a feasible initial value. */ - //TODO: Remove Vector Values +//TODO: Remove Vector Values class QPSolver: public ActiveSetSolver { const QP& qp_; //!< factor graphs of the QP problem, can't be modified! @@ -51,7 +51,21 @@ public: JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph& workingSet, const VectorValues& delta) const; - /// TODO(comment) + /* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. + * + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints + * - We want: aj'*(xk + alpha*p) - bj <= 0 + * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 + * it's good! + * - We only care when aj'*p > 0. In this case, we need to choose alpha so that + * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) + * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) + * + * We want the minimum of all those alphas among all inactive inequality. + */ boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const;