12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix bad bugs when constrained graphs are empty

release/4.3a0
Duy-Nguyen Ta 2016-06-15 11:36:04 -04:00
parent 3b897950a6
commit 3c85e2d625
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/linear/QPSolver.cpp
View File

@ -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;