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
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam_unstable/linear/LinearInequality.h>
namespace gtsam {

18
gtsam_unstable/nonlinear/LinearConstraintSQP.cpp
View File

@ -17,8 +17,10 @@
* @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/nonlinear/LinearConstraintSQP.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
#include <iostream>
namespace gtsam {
@ -38,12 +40,12 @@ bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) {
ConstrainedFactor::shared_ptr inequality
= boost::dynamic_pointer_cast<ConstrainedFactor>(factor);
Key dualKey = inequality->dualKey();
if (!duals.exists(dualKey)) continue; // should be inactive constraint!
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 true;
}
@ -90,9 +92,9 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
VectorValues delta, duals;
QPSolver qpSolver(qp);
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()));
}
boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals,
params_.warmStart);
@ -106,10 +108,8 @@ LinearConstraintNLPState LinearConstraintSQP::iterate(
newState.converged = checkConvergence(newState, delta);
newState.iterations = state.iterations + 1;
if(params_.verbosity >= NonlinearOptimizerParams::VALUES) {
newState.values.print("Values");
newState.duals.print("Duals");
}
if(params_.verbosity >= NonlinearOptimizerParams::VALUES)
newState.print("Values");
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
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
@ -33,33 +35,10 @@ public:
}
/// Linearize to a EqualityFactorGraph
EqualityFactorGraph::shared_ptr 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;
}
EqualityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const;
/**
* Return true if the max absolute error all factors is less than a tolerance
*/
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;
}
/// Return true if the max absolute error all factors is less than a tolerance
bool checkFeasibility(const Values& values, double tol) const;
};

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
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/ConstrainedFactor.h>
namespace gtsam {
@ -31,52 +32,15 @@ class LinearInequalityFactorGraph: public FactorGraph<NonlinearFactor> {
public:
/// Default constructor
LinearInequalityFactorGraph() {
}
LinearInequalityFactorGraph() {}
/// Linearize to a InequalityFactorGraph
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;
}
InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const;
/**
* Return true if the all errors are <= 0.0
*/
/// Return true if the all errors are <= 0.0
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
* @brief Unit tests for testQPSimple
* @file testLinearInequalityFactorGraph.cpp
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
@ -19,7 +18,7 @@
*/
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h>
#include <gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
@ -29,9 +28,9 @@ using namespace gtsam;
const double tol = 1e-10;
//******************************************************************************
TEST(NonlinearInequalityFactorGraph, constructor) {
NonlinearInequalityFactorGraph nonlinearInequalities;
CHECK(nonlinearInequalities.empty());
TEST(LinearInequalityFactorGraph, constructor) {
LinearInequalityFactorGraph linearInequalities;
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);
}
//******************************************************************************
//