make QPSolver throws an exception if the provided initial values are infeasible

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<VectorValues, VectorValues> 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);
 }
 

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<InfeasibleInitialValues> {
+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 */