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

[REFACTOR] Replace BOOST_FOREACH with c++ foreach loop 

[BUGFIX] Fixed failing unit test due to assert.
release/4.3a0
ivan 2016-06-13 20:35:17 -04:00
parent 6bafe9932e
commit bf68fc6b4b
12 changed files with 68 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
gtsam_unstable/linear/ActiveSetSolver.cpp
View File

@ -45,23 +45,24 @@ namespace gtsam {
*
*/
int ActiveSetSolver::identifyLeavingConstraint(
const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const {
int worstFactorIx = -1;
const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const {
int worstFactorIx = -1;
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is
// either
// inactive or a good inequality constraint, so we don't care!
double maxLambda = 0.0;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
if (factor->active()) {
double lambda = lambdas.at(factor->dualKey())[0];
if (lambda > maxLambda) {
worstFactorIx = factorIx;
maxLambda = lambda;
double maxLambda = 0.0;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
if (factor->active()) {
double lambda = lambdas.at(factor->dualKey())[0];
if (lambda > maxLambda) {
worstFactorIx = factorIx;
maxLambda = lambda;
}
}
}
}
return worstFactorIx;
return worstFactorIx;
}
/* This function will create a dual graph that solves for the
@ -76,15 +77,16 @@ return worstFactorIx;
* if lambda > 0 you are violating the constraint.
*/
GaussianFactorGraph::shared_ptr ActiveSetSolver::buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
BOOST_FOREACH (Key key, constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor =
createDualFactor(key, workingSet, delta);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
}
return dualGraph;
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
for (Key key : constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet,
delta);
if (!dualFactor->empty())
dualGraph->push_back(dualFactor);
}
return dualGraph;
}
/*
@ -96,35 +98,35 @@ return dualGraph;
* in the next iteration.
*/
boost::tuple<double, int> ActiveSetSolver::computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const {
double minAlpha = startAlpha;
int closestFactorIx = -1;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double b = factor->getb()[0];
// only check inactive factors
if (!factor->active()) {
// Compute a'*p
double aTp = factor->dotProductRow(p);
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const {
double minAlpha = startAlpha;
int closestFactorIx = -1;
for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
const LinearInequality::shared_ptr& factor = workingSet.at(factorIx);
double b = factor->getb()[0];
// only check inactive factors
if (!factor->active()) {
// Compute a'*p
double aTp = factor->dotProductRow(p);
// Check if a'*p >0. Don't care if it's not.
if (aTp <= 0)
continue;
// Check if a'*p >0. Don't care if it's not.
if (aTp <= 0)
continue;
// Compute a'*xk
double aTx = factor->dotProductRow(xk);
// Compute a'*xk
double aTx = factor->dotProductRow(xk);
// alpha = (b - a'*xk) / (a'*p)
double alpha = (b - aTx) / aTp;
// We want the minimum of all those max alphas
if (alpha < minAlpha) {
closestFactorIx = factorIx;
minAlpha = alpha;
// alpha = (b - a'*xk) / (a'*p)
double alpha = (b - aTx) / aTp;
// We want the minimum of all those max alphas
if (alpha < minAlpha) {
closestFactorIx = factorIx;
minAlpha = alpha;
}
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx);
return boost::make_tuple(minAlpha, closestFactorIx);
}
}

2
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -53,7 +53,7 @@ public:
*/
TermsContainer Aterms;
if (variableIndex.find(key) != variableIndex.end()) {
BOOST_FOREACH (size_t factorIx, variableIndex[key]) {
for(size_t factorIx: variableIndex[key]) {
typename FACTOR::shared_ptr factor = graph.at(factorIx);
if (!factor->active()) continue;
Matrix Ai = factor->getA(factor->find(key)).transpose();

2
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -36,7 +36,7 @@ public:
*/
double error(const VectorValues& x) const {
double total_error = 0.;
BOOST_FOREACH(const sharedFactor& factor, *this){
for(const sharedFactor& factor: *this){
if(factor)
total_error += factor->error(x);
}

2
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -47,7 +47,7 @@ public:
* Compute error of a guess.
* Infinity error if it violates an inequality; zero otherwise. */
double error(const VectorValues& x) const {
BOOST_FOREACH(const sharedFactor& factor, *this) {
for(const sharedFactor& factor: *this) {
if(factor)
if (factor->error(x) > 0)
return std::numeric_limits<double>::infinity();

3
gtsam_unstable/linear/LPSolver.cpp
View File

@ -161,7 +161,8 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr &factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
GTSAM_PRINT(*workingFactor);
GTSAM_PRINT(initialValues);
double error = workingFactor->error(initialValues);
// TODO: find a feasible initial point for LPSolver.
// For now, we just throw an exception

9
gtsam_unstable/linear/LPSolver.h
View File

@ -43,10 +43,11 @@ public:
template<class LinearGraph>
KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const {
KeyDimMap keysDim;
BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) {
if (!factor) continue;
BOOST_FOREACH(Key key, factor->keys())
keysDim[key] = factor->getDim(factor->find(key));
for (const typename LinearGraph::sharedFactor &factor : linearGraph) {
if (!factor)
continue;
for (Key key : factor->keys())
keysDim[key] = factor->getDim(factor->find(key));
}
return keysDim;
}

6
gtsam_unstable/linear/QPSolver.cpp
View File

@ -184,9 +184,9 @@ 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
BOOST_FOREACH(Key key, qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
if(newKey < key)
newKey = key;
for (Key key : qp_.cost.getKeyDimMap() | boost::adaptors::map_keys)
if (newKey < key)
newKey = key;
newKey++;
initProblem.cost = LinearCost(newKey, Vector::Ones(1));
initProblem.equalities = qp_.equalities;

2
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -94,7 +94,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
VectorValues starter;
starter.insert(X, kZero);
starter.insert(Y, kZero);
starter.insert(Z, Vector::Constant(2, 2.0));
starter.insert(Z, Vector::Constant(1, 2.0));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;

1
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -149,7 +149,6 @@ TEST(QPSolver, indentifyActiveConstraints) {
expectedSolution.insert(X(1), kZero);
expectedSolution.insert(X(2), kZero);
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
/* ************************************************************************* */

6
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -37,7 +37,7 @@ bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state
/* ************************************************************************* */
bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearInequalities) {
ConstrainedFactor::shared_ptr inequality
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
@ -66,7 +66,7 @@ bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state
/* ************************************************************************* */
VectorValues LinearConstraintSQP::initializeDuals() const {
VectorValues duals;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){
for(const NonlinearFactor::shared_ptr& factor: lcnlp_.linearEqualities){
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
duals.insert(constraint->dualKey(), Vector::Zero(factor->dim()));
@ -92,7 +92,7 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
VectorValues delta, duals;
QPSolver qpSolver(qp);
VectorValues zeroInitialValues;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values)
for(const Values::ConstKeyValuePair& key_value: state.values)
zeroInitialValues.insert(key_value.key, Vector::Zero(key_value.value.dim()));
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,

4
gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp
View File

@ -27,7 +27,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize(
const Values& linearizationPoint) const {
EqualityFactorGraph::shared_ptr linearGraph(
new EqualityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
for(const NonlinearFactor::shared_ptr& factor: *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
@ -38,7 +38,7 @@ EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize(
/* ************************************************************************* */
bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
for(const NonlinearFactor::shared_ptr& factor: *this){
NoiseModelFactor::shared_ptr noiseModelFactor
= boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);

4
gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize(
const Values& linearizationPoint) const {
InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
for(const NonlinearFactor::shared_ptr& factor: *this) {
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
ConstrainedFactor::shared_ptr constraint
@ -40,7 +40,7 @@ InequalityFactorGraph::shared_ptr LinearInequalityFactorGraph::linearize(
bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary(
const Values& values, const VectorValues& dualValues, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
for(const NonlinearFactor::shared_ptr& factor: *this) {
NoiseModelFactor::shared_ptr noiseModelFactor
= boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);