[TEST] QP Now correctly handles negative constant values on hessian factors.

examples/Data/HS21.QPS

@@ -0,0 +1,20 @@
+NAME          HS21
+ROWS
+  N OBJ.FUNC
+  G R------1
+COLUMNS
+    C------1  R------1  0.100000e+02
+    C------2  R------1  -.100000e+01
+RHS
+    RHS       OBJ.FUNC  0.100000e+03
+    RHS       R------1  0.100000e+02
+RANGES
+BOUNDS
+ LO BOUNDS    C------1  0.200000e+01
+ UP BOUNDS    C------1  0.500000e+02
+ LO BOUNDS    C------2  -.500000e+02
+ UP BOUNDS    C------2  0.500000e+02
+QUADOBJ
+    C------1  C------1  0.200000e-01
+    C------2  C------2  0.200000e+01
+ENDATA

gtsam_unstable/linear/QPSolver.h

@@ -32,9 +32,17 @@ struct QPPolicy {
   static constexpr double maxAlpha = 1.0;
 
   /// Simply the cost of the QP problem
-  static const GaussianFactorGraph& buildCostFunction(
+  static const GaussianFactorGraph buildCostFunction(const QP& qp,
-      const QP& qp, const VectorValues& xk = VectorValues()) {
+      const VectorValues& xk = VectorValues()) {
-    return qp.cost;
+    GaussianFactorGraph no_constant_factor;
+    for (auto factor : qp.cost) {
+      HessianFactor hf = static_cast<HessianFactor>(*factor);
+      if (hf.constantTerm() < 0) // Hessian Factors cannot deal
+        // with negative constant terms replace with zero in this case
+        hf.constantTerm() = 0.0;
+      no_constant_factor.push_back(hf);
+    }
+    return no_constant_factor;
   }
 };
 

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -218,7 +218,7 @@ pair<QP, QP> testParser(QPSParser parser) {
   // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
   expectedqp.cost.push_back(
       HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1,
-          2.0 * kOne, 4.0));
+          2.0 * kOne, 8.0));
   // 2x + y >= 2
   // -x + 2y <= 6
   expectedqp.inequalities.push_back(
@@ -269,6 +269,18 @@ TEST(QPSolver, QPExampleTest){
   CHECK(assert_equal(error_expected, error_actual))
 }
 
+TEST(QPSolver, HS21) {
+  QP problem = QPSParser("HS21.QPS").Parse();
+  VectorValues actualSolution;
+  VectorValues expectedSolution;
+  expectedSolution.insert(Symbol('X',1), 2.0*I_1x1);
+  expectedSolution.insert(Symbol('X',2), 0.0*I_1x1);
+  boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
+  double error_actual = problem.cost.error(actualSolution);
+  CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
+  CHECK(assert_equal(expectedSolution, actualSolution))
+}
+
 /* ************************************************************************* */
 // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
 QP createTestMatlabQPEx() {