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

Removed LinearEqualityManifoldFactorGraph.

release/4.3a0
krunalchande 2014-12-23 14:45:23 -05:00 committed by thduynguyen
parent e0e5e72020
commit 0fdd49ca4e
2 changed files with 39 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h
View File

@ -1,62 +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 LinearEqualityManifoldFactorGraph.h
* @author Duy-Nguyen Ta
* @author Krunal Chande
* @author Luca Carlone
* @date Dec 15, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.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;
}
};
}

43
gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h
View File

@ -18,16 +18,49 @@
*/ */
#pragma once #pragma once
#include <gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h> #include <gtsam_unstable/linear/LinearEqualityFactorGraph.h>
#include <gtsam_unstable/nonlinear/NonlinearConstraint.h>
namespace gtsam { namespace gtsam {
class NonlinearEqualityFactorGraph: public LinearEqualityManifoldFactorGraph {
class NonlinearEqualityFactorGraph: public FactorGraph<NonlinearFactor> {
public: public:
/// default constructor /// Default constructor
NonlinearEqualityFactorGraph() { NonlinearEqualityFactorGraph() {
} }
/// 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;
}
/**
* Additional cost for -lambda*ConstraintHessian for SQP
*/
GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const { GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const {
GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph());
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) {
@ -36,5 +69,7 @@ public:
} }
return constrainedHessians; return constrainedHessians;
} }
}; };
} }