From c9efb4bbda31c7ba52cffc223d1947cee14678a2 Mon Sep 17 00:00:00 2001
From: = <ijimenez3@gatech.edu>
Date: Tue, 28 Jun 2016 19:51:51 -0400
Subject: [PATCH] [TEST] QP Now correctly handles negative constant values on
 hessian factors.

---
 examples/Data/HS21.QPS                       | 20 ++++++++++++++++++++
 gtsam_unstable/linear/QPSolver.h             | 14 +++++++++++---
 gtsam_unstable/linear/tests/testQPSolver.cpp | 14 +++++++++++++-
 3 files changed, 44 insertions(+), 4 deletions(-)
 create mode 100644 examples/Data/HS21.QPS

diff --git a/examples/Data/HS21.QPS b/examples/Data/HS21.QPS
new file mode 100644
index 000000000..c71305c6e
--- /dev/null
+++ b/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
diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h
index 9efc23a67..2c59423fe 100644
--- a/gtsam_unstable/linear/QPSolver.h
+++ b/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(
-      const QP& qp, const VectorValues& xk = VectorValues()) {
-    return qp.cost;
+  static const GaussianFactorGraph buildCostFunction(const QP& qp,
+      const VectorValues& xk = VectorValues()) {
+    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;
   }
 };
 
diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp
index bc9dd1f98..0765cae44 100644
--- a/gtsam_unstable/linear/tests/testQPSolver.cpp
+++ b/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() {