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

Refactoring.

release/4.3a0
krunalchande 2014-12-22 18:35:22 -05:00 committed by thduynguyen
parent d435fa72df
commit c99a848148
4 changed files with 138 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h Normal file
View File

@ -0,0 +1,50 @@
/**
* @file LinearEqualityManifoldFactorGraph.h
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
class LinearEqualityManifoldFactorGraph: public FactorGraph<NonlinearFactor> {
public:
/// default constructor
LinearEqualityManifoldFactorGraph() {
}
/// linearize to a LinearEqualityFactorGraph
LinearEqualityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearEqualityFactorGraph::shared_ptr linearGraph(
new LinearEqualityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/**
* 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;
}
};
}

27
gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h Normal file
View File

@ -0,0 +1,27 @@
/**
* @file NonlinearEqualityFactorGraph.h
* @author Krunal Chande
* @date Dec 22, 2014
*/
#pragma once
#include <gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h>
namespace gtsam {
class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph {
public:
/// default constructor
NonlinearEqualityFactorGraph() {
}
GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const {
GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
constrainedHessians->push_back(constraint->multipliedHessian(values, duals));
}
return constrainedHessians;
}
};
}

58
gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h Normal file
View File

@ -0,0 +1,58 @@
/**
* @file NonlinearInequalityFactorGraph.h
* @author Krunal Chande
* @date Dec 22, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {
class NonlinearInequalityFactorGraph : public FactorGraph<NonlinearFactor> {
public:
/// default constructor
NonlinearInequalityFactorGraph() {
}
/// linearize to a LinearEqualityFactorGraph
LinearInequalityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearInequalityFactorGraph::shared_ptr linearGraph(
new LinearInequalityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/**
* Return true if the all errors are <= 0.0
*/
bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, 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
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(
factor);
Key dualKey = constraint->dualKey();
if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint!
if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0
return false;
}
return true;
}
};
}

104
gtsam_unstable/nonlinear/tests/testSQPSimple.cpp
View File

@ -24,112 +24,14 @@
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
#include <gtsam_unstable/nonlinear/NonlinearInequality.h>
#include <gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
namespace gtsam {
class LinearEqualityManifoldFactorGraph: public FactorGraph<NonlinearFactor> {
public:
/// default constructor
LinearEqualityManifoldFactorGraph() {
}
/// linearize to a LinearEqualityFactorGraph
LinearEqualityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearEqualityFactorGraph::shared_ptr linearGraph(
new LinearEqualityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
linearGraph->add(LinearEquality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/**
* 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;
}
};
class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph {
public:
/// default constructor
NonlinearEqualityFactorGraph() {
}
GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const {
GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
constrainedHessians->push_back(constraint->multipliedHessian(values, duals));
}
return constrainedHessians;
}
};
class NonlinearInequalityFactorGraph : public FactorGraph<NonlinearFactor> {
public:
/// default constructor
NonlinearInequalityFactorGraph() {
}
/// linearize to a LinearEqualityFactorGraph
LinearInequalityFactorGraph::shared_ptr linearize(
const Values& linearizationPoint) const {
LinearInequalityFactorGraph::shared_ptr linearGraph(
new LinearInequalityFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){
JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast<JacobianFactor>(
factor->linearize(linearizationPoint));
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(factor);
linearGraph->add(LinearInequality(*jacobian, constraint->dualKey()));
}
return linearGraph;
}
/**
* Return true if the all errors are <= 0.0
*/
bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, 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
NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast<NonlinearConstraint>(
factor);
Key dualKey = constraint->dualKey();
if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint!
if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0
return false;
}
return true;
}
};
struct NLP {
NonlinearFactorGraph cost;
NonlinearEqualityFactorGraph linearEqualities;