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

move code to cpp and some small clean up

release/4.3a0
thduynguyen 2015-03-02 08:27:24 -05:00
parent b9dbde14f2
commit bdd00d8b49
9 changed files with 151 additions and 226 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -18,7 +18,7 @@
#pragma once #pragma once
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph.h>
#include <gtsam_unstable/linear/LinearInequality.h> #include <gtsam_unstable/linear/LinearInequality.h>
namespace gtsam { namespace gtsam {

18
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -17,8 +17,10 @@
* @date Dec 15, 2014 * @date Dec 15, 2014
*/ */
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h> #include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
@ -38,12 +40,12 @@ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
ConstrainedFactor::shared_ptr inequality ConstrainedFactor::shared_ptr inequality
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor); = boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
Key dualKey = inequality->dualKey(); Key dualKey = inequality->dualKey();
if (!duals.exists(dualKey)) continue; // should be inactive constraint! if (!duals.exists(dualKey)) continue; // should be inactive constraint!
double dual = duals.at(dualKey)[0];// because we only support single-valued inequalities double dual = duals.at(dualKey)[0];// because we only support single-valued inequalities
if (dual > 0.0) { // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? if (dual > 0.0) // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ?
return false; return false;
}
} }
return true; return true;
} }
@ -90,9 +92,9 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
VectorValues delta, duals; VectorValues delta, duals;
QPSolver qpSolver(qp); QPSolver qpSolver(qp);
VectorValues zeroInitialValues; VectorValues zeroInitialValues;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values)
zeroInitialValues.insert(key_value.key, zero(key_value.value.dim())); zeroInitialValues.insert(key_value.key, zero(key_value.value.dim()));
}
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
params_.warmStart); params_.warmStart);
@ -106,10 +108,8 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
newState.converged = checkConvergence(newState, delta); newState.converged = checkConvergence(newState, delta);
newState.iterations = state.iterations + 1; newState.iterations = state.iterations + 1;
if(params_.verbosity >= NonlinearOptimizerParams::VALUES) { if(params_.verbosity >= NonlinearOptimizerParams::VALUES)
newState.values.print("Values"); newState.print("Values");
newState.duals.print("Duals");
}
return newState; return newState;
} }

52
gtsam_unstable/nonlinear/LinearEqualityFactorGraph.cpp Normal file
View File

@ -0,0 +1,52 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LinearEqualityFactorGraph.cpp
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
/* ************************************************************************* */
EqualityFactorGraph::shared_ptr LinearEqualityFactorGraph::linearize(
const Values& linearizationPoint) const {
EqualityFactorGraph::shared_ptr linearGraph(
new EqualityFactorGraph());
BOOST_FOREACH(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);
linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/* ************************************************************************* */
bool LinearEqualityFactorGraph::checkFeasibility(const Values& values, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
NoiseModelFactor::shared_ptr noiseModelFactor
= boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);
if (error.lpNorm<Eigen::Infinity>() > tol)
return false;
}
return true;
}
}

33
gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h
View File

@ -18,8 +18,10 @@
*/ */
#pragma once #pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h> #include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam { namespace gtsam {
@ -33,33 +35,10 @@ public:
} }
/// Linearize to a EqualityFactorGraph /// Linearize to a EqualityFactorGraph
EqualityFactorGraph::shared_ptr linearize( EqualityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const;
const Values& linearizationPoint) const {
EqualityFactorGraph::shared_ptr linearGraph(
new EqualityFactorGraph());
BOOST_FOREACH(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);
linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/** /// Return true if the max absolute error all factors is less than a tolerance
* Return true if the max absolute error all factors is less than a tolerance bool checkFeasibility(const Values& values, double tol) const;
*/
bool checkFeasibility(const Values& values, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast<NoiseModelFactor>(
factor);
Vector error = noiseModelFactor->unwhitenedError(values);
if (error.lpNorm<Eigen::Infinity>() > tol) {
return false;
}
}
return true;
}
}; };

69
gtsam_unstable/nonlinear/LinearInequalityFactorGraph.cpp Normal file
View File

@ -0,0 +1,69 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LinearInequalityFactorGraph.cpp
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
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) {
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/* ************************************************************************* */
bool LinearInequalityFactorGraph::checkFeasibilityAndComplimentary(
const Values& values, const VectorValues& dualValues, double tol) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
NoiseModelFactor::shared_ptr noiseModelFactor
= boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);
// Primal feasibility condition: all constraints need to be <= 0.0
if (error[0] > tol)
return false;
// Complimentary condition: errors of active constraints need to be 0.0
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
Key dualKey = constraint->dualKey();
// if dualKey doesn't exist in dualValues, it must be an inactive constraint!
if (!dualValues.exists(dualKey))
continue;
// for active constraint, the error should be 0.0
if (fabs(error[0]) > tol)
return false;
}
return true;
}
}

52
gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h
View File

@ -19,8 +19,9 @@
#pragma once #pragma once
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h> #include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam { namespace gtsam {
@ -31,52 +32,15 @@ class LinearInequalityFactorGraph: public FactorGraph<NonlinearFactor> {
public: public:
/// Default constructor /// Default constructor
LinearInequalityFactorGraph() { LinearInequalityFactorGraph() {}
}
/// Linearize to a InequalityFactorGraph /// Linearize to a InequalityFactorGraph
InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const { InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const;
InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph());
BOOST_FOREACH(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);
linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/** /// Return true if the all errors are <= 0.0
* Return true if the all errors are <= 0.0
*/
bool checkFeasibilityAndComplimentary(const Values& values, bool checkFeasibilityAndComplimentary(const Values& values,
const VectorValues& dualValues, double tol) const { const VectorValues& dualValues, double tol) const;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
NoiseModelFactor::shared_ptr noiseModelFactor
= boost::dynamic_pointer_cast<NoiseModelFactor>(factor);
Vector error = noiseModelFactor->unwhitenedError(values);
// Primal feasibility condition: all constraints need to be <= 0.0
if (error[0] > tol)
return false;
// Complimentary condition: errors of active constraints need to be 0.0
ConstrainedFactor::shared_ptr constraint
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
Key dualKey = constraint->dualKey();
// if dualKey doesn't exist in dualValues, it must be an inactive constraint!
if (!dualValues.exists(dualKey))
continue;
// for active constraint, the error should be 0.0
if (fabs(error[0]) > tol)
return false;
}
return true;
}
}; };
}
} // namespace gtsam

2
gtsam_unstable/nonlinear/tests/CMakeLists.txt
View File

@ -1 +1 @@
gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp" "gtsam_unstable") gtsamAddTestsGlob(nonlinear_unstable "test*.cpp" "testCustomChartExpression.cpp;testLCNLPSolver2.cpp" "gtsam_unstable")

11
gtsam_unstable/nonlinear/tests/testNonlinearInequalityFactorGraph.cpp → gtsam_unstable/nonlinear/tests/testLinearInequalityFactorGraph.cpp
View File

@ -10,8 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testQPSimple.cpp * @file testLinearInequalityFactorGraph.cpp
* @brief Unit tests for testQPSimple
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
* @author Krunal Chande * @author Krunal Chande
* @author Luca Carlone * @author Luca Carlone
@ -19,7 +18,7 @@
*/ */
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h> #include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
@ -29,9 +28,9 @@ using namespace gtsam;
const double tol = 1e-10; const double tol = 1e-10;
//****************************************************************************** //******************************************************************************
TEST(NonlinearInequalityFactorGraph, constructor) { TEST(LinearInequalityFactorGraph, constructor) {
NonlinearInequalityFactorGraph nonlinearInequalities; LinearInequalityFactorGraph linearInequalities;
CHECK(nonlinearInequalities.empty()); CHECK(linearInequalities.empty());
} }

138
gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp
View File

@ -1,138 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testLinearlyConstrainedNonlinearOptimizer.cpp
* @brief Unit tests for LinearlyConstrainedNonlinearOptimizer
* @author Krunal Chande
* @author Duy-Nguyen Ta
* @author Luca Carlone
* @date Dec 15, 2014
*/
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
//namespace gtsam {
//struct LinearlyConstrainedNLP {
// NonlinearFactorGraph cost;
// LinearEqualityFactorGraph equalities;
// LinearInequalityFactorGraph inequalities;
//};
//
//struct LinearlyConstrainedNLPState {
// Values values;
// VectorValues duals;
// bool converged;
// LinearlyConstrainedNLPState(const Values& initialValues) :
// values(initialValues), duals(VectorValues()), converged(false) {
// }
//};
//class LinearlyConstrainedNonLinearOptimizer {
// LinearlyConstrainedNLP lcNLP_;
//public:
// LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {}
//
// LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const {
// QP qp;
// qp.cost = lcNLP_.cost.linearize(state.values);
// qp.equalities = lcNLP_.equalities;
// qp.inequalities = lcNLP_.inequalities;
// QPSolver qpSolver(qp);
// VectorValues delta, duals;
// boost::tie(delta, duals) = qpSolver.optimize();
// LinearlyConstrainedNLPState newState;
// newState.values = state.values.retract(delta);
// newState.duals = duals;
// newState.converged = checkConvergence(newState.values, newState.duals);
// return newState;
// }
//
// /**
// * Main optimization function.
// */
// std::pair<Values, VectorValues> optimize(const Values& initialValues) const {
// LinearlyConstrainedNLPState state(initialValues);
// while(!state.converged){
// state = iterate(state);
// }
//
// return std::make_pair(initialValues, VectorValues());
// }
//};
//}
//
//using namespace std;
//using namespace gtsam::symbol_shorthand;
//using namespace gtsam;
//const double tol = 1e-10;
////******************************************************************************
//TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) {
//
// // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5
// // Note the Hessian encodes:
// // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
// HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
// 2.0 * ones(1, 1), zero(1), 10.0);
//
// // build linear inequalities
// LinearInequalityFactorGraph inequalities;
// inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
// inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0
// inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0
// inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2
//
// // Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic
// // (LinearContainerFactor makes a linear factor behave like a nonlinear one)
// LinearlyConstrainedNLP lcNLP;
// lcNLP.cost.add(LinearContainerFactor(lf));
// lcNLP.inequalities = inequalities;
//
// // Compare against a QP
// QP qp;
// qp.cost.add(lf);
// qp.inequalities = inequalities;
//
// // instantiate QPsolver
// QPSolver qpSolver(qp);
// // create initial values for optimization
// VectorValues initialVectorValues;
// initialVectorValues.insert(X(1), zero(1));
// initialVectorValues.insert(X(2), zero(1));
// VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
//
// // instantiate LinearlyConstrainedNonLinearOptimizer
// LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP);
// // create initial values for optimization
// Values initialValues;
// initialValues.insert(X(1), 0.0);
// initialValues.insert(X(2), 0.0);
// Values actualSolution = lcNLPSolver.optimize(initialValues).first;
//
//
// DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at<double>(X(1)), tol);
// DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at<double>(X(2)), tol);
//}
//
//******************************************************************************
int main() {
std::cout<<"here"<<std::endl;
// TestResult tr;
// return TestRegistry::runAllTests(tr);
}
//******************************************************************************
//