From 3c85e2d625b20c3b5080fd3023e083d890c7b053 Mon Sep 17 00:00:00 2001
From: Duy-Nguyen Ta <thduynguyen@gmail.com>
Date: Wed, 15 Jun 2016 11:36:04 -0400
Subject: [PATCH] fix bad bugs when constrained graphs are empty

---
 gtsam_unstable/linear/QPSolver.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp
index d4436d141..eb2f40e05 100644
--- a/gtsam_unstable/linear/QPSolver.cpp
+++ b/gtsam_unstable/linear/QPSolver.cpp
@@ -184,10 +184,12 @@ pair<VectorValues, VectorValues> QPSolver::optimize() const {
   //Make an LP with any linear cost function. It doesn't matter for initialization.
   LP initProblem;
   // make an unrelated key for a random variable cost: max key + 1
-  std::array<Key, 3> maxKeys = {*qp_.cost.keys().rbegin(),
-                                *qp_.equalities.keys().rbegin(),
-                                *qp_.inequalities.keys().rbegin()};
-  Key newKey = *std::max_element(maxKeys.begin(), maxKeys.end()) + 1;
+  Key newKey = *qp_.cost.keys().rbegin();
+  if (!qp_.equalities.empty())
+    newKey = std::max(newKey, *qp_.equalities.keys().rbegin());
+  if (!qp_.inequalities.empty())
+    newKey = std::max(newKey, *qp_.inequalities.keys().rbegin());
+  ++newKey;
   initProblem.cost = LinearCost(newKey, Vector::Ones(1));
   initProblem.equalities = qp_.equalities;
   initProblem.inequalities = qp_.inequalities;