diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index de28d8fe8..83984adc2 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -231,6 +231,11 @@ 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 + if (error > 0) + throw InfeasibleInitialValues(); + if (fabs(error)<1e-7) { workingFactor->activate(); } @@ -257,7 +262,7 @@ pair QPSolver::optimize( while (!state.converged) { state = iterate(state); } - std::cout << "Number of inner iterations: " << state.iterations << std::endl; + return make_pair(state.values, state.duals); } diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 90fea4f70..1c4cd1f15 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -197,4 +197,24 @@ public: }; +/* ************************************************************************* */ +/** An exception indicating that the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ +class InfeasibleInitialValues : public ThreadsafeException { +public: + InfeasibleInitialValues() {} + virtual ~InfeasibleInitialValues() throw() {} + + virtual const char* what() const throw() { + if(description_.empty()) + description_ = "An infeasible intial value was provided for the QPSolver.\n" + "This current version of QPSolver does not handle infeasible" + "initial point due to the lack of a LPSolver.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; + } /* namespace gtsam */