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

[LP] Remove all zero priors

release/4.3a0
ivan 2016-05-06 11:24:15 -04:00
parent 8d49d13516
commit 7769455e63
1 changed files with 3 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam_unstable/linear/LPSolver.cpp
View File

@ -25,14 +25,6 @@ LPSolver::LPSolver(const LP &lp) :
KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities);
keysDim_.insert(keysDim2.begin(), keysDim2.end());
// Create and push zero priors of constrained variables that do not exist in
// the cost function
auto addedZeroPriorsGraph = *createZeroPriors(lp_.cost.keys(), keysDim_);
//before we add them we fill an array with the indeces of the added zero priors
addedZeroPriorsIndex_.resize(addedZeroPriorsGraph.size());
std::iota(addedZeroPriorsIndex_.begin(), addedZeroPriorsIndex_.end(), baseGraph_.size());
baseGraph_.push_back(addedZeroPriorsGraph);
// Variable index
equalityVariableIndex_ = VariableIndex(lp_.equalities);
inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
@ -40,17 +32,6 @@ LPSolver::LPSolver(const LP &lp) :
constrainedKeys_.merge(lp_.inequalities.keys());
}
GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors(
const KeyVector &costKeys, const KeyDimMap &keysDim) const {
GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
for (Key key : keysDim | boost::adaptors::map_keys) {
if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) {
size_t dim = keysDim.at(key);
graph->push_back(JacobianFactor(key, eye(dim), zero(dim)));
}
}
return graph;
}
LPState LPSolver::iterate(const LPState &state) const {
// Solve with the current working set
@ -140,9 +121,9 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
// We remove the old zero priors from the base graph we are going to use to solve
//This iteration's problem
for (size_t index : addedZeroPriorsIndex_) {
workingGraph.remove(index);
}
// for (size_t index : addedZeroPriorsIndex_) {
// workingGraph.remove(index);
// }
workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
for (const LinearInequality::shared_ptr &factor : workingSet) {