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

Small formatting changes and removal of test header

release/4.3a0
Frank Dellaert 2020-09-30 15:38:08 -04:00
parent b89478c0fa
commit c0fb3a271b
3 changed files with 12 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam_unstable/linear/ActiveSetSolver-inl.h
View File

@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
// to compute the least-square approximation of dual variables // to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b); return boost::make_shared<JacobianFactor>(Aterms, b);
} else { } else {
return boost::make_shared<JacobianFactor>(); return nullptr;
} }
} }
@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
* if lambda = 0 you are on the constraint * if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint. * if lambda > 0 you are violating the constraint.
*/ */
Template GaussianFactorGraph::shared_ptr This::buildDualGraph( Template GaussianFactorGraph This::buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); GaussianFactorGraph dualGraph;
for (Key key : constrainedKeys_) { for (Key key : constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph // Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor = auto dualFactor = createDualFactor(key, workingSet, delta);
createDualFactor(key, workingSet, delta); if (dualFactor) dualGraph.push_back(dualFactor);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
} }
return dualGraph; return dualGraph;
} }
@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
Template typename This::State This::iterate( Template typename This::State This::iterate(
const typename This::State& state) const { const typename This::State& state) const {
// Algorithm 16.3 from Nocedal06book. // Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p // Solve with the current working set eqn 16.39, but solve for x not p
// solve for x auto workingGraph = buildWorkingGraph(state.workingSet, state.values);
GaussianFactorGraph workingGraph =
buildWorkingGraph(state.workingSet, state.values);
VectorValues newValues = workingGraph.optimize(); VectorValues newValues = workingGraph.optimize();
// If we CAN'T move further // If we CAN'T move further
// if p_k = 0 is the original condition, modified by Duy to say that the state // if p_k = 0 is the original condition, modified by Duy to say that the state
// update is zero. // update is zero.
if (newValues.equals(state.values, 1e-7)) { if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph // Compute lambda from the dual graph
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, auto dualGraph = buildDualGraph(state.workingSet, newValues);
newValues); VectorValues duals = dualGraph.optimize();
VectorValues duals = dualGraph->optimize();
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!! // If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) { if (leavingFactor < 0) {

4
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -154,8 +154,8 @@ protected:
public: /// Just for testing... public: /// Just for testing...
/// Builds a dual graph from the current working set. /// Builds a dual graph from the current working set.
GaussianFactorGraph::shared_ptr buildDualGraph( GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const; const VectorValues &delta) const;
/** /**
* Build a working graph of cost, equality and active inequality constraints * Build a working graph of cost, equality and active inequality constraints

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -21,7 +21,6 @@
#include <gtsam_unstable/linear/LP.h> #include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/Test.h>
namespace gtsam { namespace gtsam {
/** /**
@ -83,7 +82,7 @@ private:
const InequalityFactorGraph& inequalities) const; const InequalityFactorGraph& inequalities) const;
// friend class for unit-testing private methods // friend class for unit-testing private methods
FRIEND_TEST(LPInitSolver, initialization); friend class LPInitSolverInitializationTest;
}; };
} }