diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 111ab506f..f1324e4d0 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * QP.h +/** + * @file: QP.h * @brief: Factor graphs of a Quadratic Programming problem * @date: Dec 8, 2014 - * @author: thduynguyen + * @author: Duy-Nguyen Ta */ #pragma once @@ -25,12 +25,12 @@ namespace gtsam { /** - * struct contains factor graphs of a Quadratic Programming problem + * Struct contains factor graphs of a Quadratic Programming problem */ struct QP { GaussianFactorGraph cost; //!< Quadratic cost factors - LinearEqualityFactorGraph equalities; //!< linear equality constraints - LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + LinearEqualityFactorGraph equalities; //!< linear equality constraints: f(x) = 0 + LinearInequalityFactorGraph inequalities; //!< linear inequality constraints: g(x) <= 0 /** default constructor */ QP() : diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 83984adc2..b7ff3780a 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -1,8 +1,19 @@ -/* - * QPSolver.cpp +/* ---------------------------------------------------------------------------- + + * 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: QPSolver.cpp * @brief: * @date: Apr 15, 2014 - * @author: thduynguyen + * @author: Duy-Nguyen Ta */ #include @@ -231,8 +242,8 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( workingFactor->inactivate(); } else { double error = workingFactor->error(initialValues); - // TODO: This version of QPSolver doesn't handle infeasible initial point - // since we haven't had an LPSolver yet + // TODO: find a feasible initial point for QPSolver. + // For now, we just throw an exception, since we don't have an LPSolver to do this yet if (error > 0) throw InfeasibleInitialValues(); diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 1c4cd1f15..b354dcccb 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -8,11 +8,12 @@ * See LICENSE for the license information * -------------------------------------------------------------------------- */ -/* - * QPSolver.h + +/** + * @file: QPSolver.h * @brief: A quadratic programming solver implements the active set method * @date: Apr 15, 2014 - * @author: thduynguyen + * @author: Duy-Nguyen Ta */ #pragma once @@ -47,19 +48,19 @@ struct QPState { }; /** - * This class implements the active set method to solve quadratic programming problems - * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which - * a negative sigma denotes an inequality <=0 constraint, - * a zero sigma denotes an equality =0 constraint, - * and a positive sigma denotes a normal Gaussian noise model. + * This QPSolver uses the active set method to solve a quadratic programming problem + * defined in the QP struct. + * Note: This version of QPSolver only works with a feasible initial value. */ class QPSolver { const QP& qp_; //!< factor graphs of the QP problem, can't be modified! - GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. + GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. + //!< used to initialize the working set factor graph, + //!< to which active inequalities will be added VariableIndex costVariableIndex_, equalityVariableIndex_, - inequalityVariableIndex_; - FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph + inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs + FastSet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs public: /// Constructor @@ -79,13 +80,13 @@ public: const VariableIndex& variableIndex) const { std::vector > Aterms; if (variableIndex.find(key) != variableIndex.end()) { - BOOST_FOREACH(size_t factorIx, variableIndex[key]){ - typename FACTOR::shared_ptr factor = graph.at(factorIx); - if (!factor->active()) continue; - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + BOOST_FOREACH(size_t factorIx, variableIndex[key]) { + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } } - } return Aterms; } @@ -189,7 +190,7 @@ public: /** Optimize with a provided initial values * For this version, it is the responsibility of the caller to provide - * a feasible initial value. + * a feasible initial value, otherwise, an exception will be thrown. * @return a pair of solutions */ std::pair optimize(