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

Merge remote-tracking branch 'origin/feature/LPSolver' into feature/LPSolver

release/4.3a0
= 2016-06-15 12:00:44 -04:00
parent 01131e463a 3c85e2d625
commit 1102a0e425
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam_unstable/linear/QPSolver.cpp
View File

@ -183,11 +183,13 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
pair<VectorValues, VectorValues> QPSolver::optimize() const {
//Make an LP with any linear cost function. It doesn't matter for initialization.
LP initProblem;
Key newKey = 0; // make an unrelated key for a random variable cost
for (Key key : qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
if (newKey < key)
newKey = key;
newKey++;
// make an unrelated key for a random variable cost: max key + 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;