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

Linear[In]EqualityFactorGraph --> [In]EqualityFactorGraph

release/4.3a0
thduynguyen 2015-02-24 22:25:26 -05:00
parent ce50219f9d
commit d2f919e632
7 changed files with 40 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/linear/LinearEqualityFactorGraph.h → gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* LinearEqualityFactorGraph.h * EqualityFactorGraph.h
* @brief: Factor graph of all LinearEquality factors * @brief: Factor graph of all LinearEquality factors
* @date: Dec 8, 2014 * @date: Dec 8, 2014
* @author: Duy-Nguyen Ta * @author: Duy-Nguyen Ta
@ -23,14 +23,14 @@
namespace gtsam { namespace gtsam {
class LinearEqualityFactorGraph : public FactorGraph<LinearEquality> { class EqualityFactorGraph : public FactorGraph<LinearEquality> {
public: public:
typedef boost::shared_ptr<LinearEqualityFactorGraph> shared_ptr; typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
}; };
/// traits /// traits
template<> struct traits<LinearEqualityFactorGraph> : public Testable< template<> struct traits<EqualityFactorGraph> : public Testable<
LinearEqualityFactorGraph> { EqualityFactorGraph> {
}; };
} // \ namespace gtsam } // \ namespace gtsam

12
gtsam_unstable/linear/LinearInequalityFactorGraph.h → gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* LinearInequalityFactorGraph.h * InequalityFactorGraph.h
* @brief: Factor graph of all LinearInequality factors * @brief: Factor graph of all LinearInequality factors
* @date: Dec 8, 2014 * @date: Dec 8, 2014
* @author: Duy-Nguyen Ta * @author: Duy-Nguyen Ta
@ -23,12 +23,12 @@
namespace gtsam { namespace gtsam {
class LinearInequalityFactorGraph: public FactorGraph<LinearInequality> { class InequalityFactorGraph: public FactorGraph<LinearInequality> {
private: private:
typedef FactorGraph<LinearInequality> Base; typedef FactorGraph<LinearInequality> Base;
public: public:
typedef boost::shared_ptr<LinearInequalityFactorGraph> shared_ptr; typedef boost::shared_ptr<InequalityFactorGraph> shared_ptr;
/** print */ /** print */
void print(const std::string& str, const KeyFormatter& keyFormatter = void print(const std::string& str, const KeyFormatter& keyFormatter =
@ -37,15 +37,15 @@ public:
} }
/** equals */ /** equals */
bool equals(const LinearInequalityFactorGraph& other, bool equals(const InequalityFactorGraph& other,
double tol = 1e-9) const { double tol = 1e-9) const {
return Base::equals(other, tol); return Base::equals(other, tol);
} }
}; };
/// traits /// traits
template<> struct traits<LinearInequalityFactorGraph> : public Testable< template<> struct traits<InequalityFactorGraph> : public Testable<
LinearInequalityFactorGraph> { InequalityFactorGraph> {
}; };
} // \ namespace gtsam } // \ namespace gtsam

12
gtsam_unstable/linear/QP.h
View File

@ -19,8 +19,8 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/LinearEqualityFactorGraph.h> #include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/LinearInequalityFactorGraph.h> #include <gtsam_unstable/linear/InequalityFactorGraph.h>
namespace gtsam { namespace gtsam {
@ -29,8 +29,8 @@ namespace gtsam {
*/ */
struct QP { struct QP {
GaussianFactorGraph cost; //!< Quadratic cost factors GaussianFactorGraph cost; //!< Quadratic cost factors
LinearEqualityFactorGraph equalities; //!< linear equality constraints: f(x) = 0 EqualityFactorGraph equalities; //!< linear equality constraints: f(x) = 0
LinearInequalityFactorGraph inequalities; //!< linear inequality constraints: g(x) <= 0 InequalityFactorGraph inequalities; //!< linear inequality constraints: g(x) <= 0
/** default constructor */ /** default constructor */
QP() : QP() :
@ -39,8 +39,8 @@ struct QP {
/** constructor */ /** constructor */
QP(const GaussianFactorGraph& _cost, QP(const GaussianFactorGraph& _cost,
const LinearEqualityFactorGraph& _linearEqualities, const EqualityFactorGraph& _linearEqualities,
const LinearInequalityFactorGraph& _linearInequalities) : const InequalityFactorGraph& _linearInequalities) :
cost(_cost), equalities(_linearEqualities), inequalities( cost(_cost), equalities(_linearEqualities), inequalities(
_linearInequalities) { _linearInequalities) {
} }

22
gtsam_unstable/linear/QPSolver.cpp
View File

@ -39,7 +39,7 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
//****************************************************************************** //******************************************************************************
VectorValues QPSolver::solveWithCurrentWorkingSet( VectorValues QPSolver::solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const { const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; GaussianFactorGraph workingGraph = baseGraph_;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
if (factor->active()) if (factor->active())
@ -50,7 +50,7 @@ VectorValues QPSolver::solveWithCurrentWorkingSet(
//****************************************************************************** //******************************************************************************
JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the dual key // Transpose the A matrix of constrained factors to have the jacobian of the dual key
std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians
@ -78,7 +78,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
//****************************************************************************** //******************************************************************************
GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
BOOST_FOREACH(Key key, constrainedKeys_) { BOOST_FOREACH(Key key, constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph // Each constrained key becomes a factor in the dual graph
@ -91,7 +91,7 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
//****************************************************************************** //******************************************************************************
int QPSolver::identifyLeavingConstraint( int QPSolver::identifyLeavingConstraint(
const LinearInequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const { const VectorValues& lambdas) const {
int worstFactorIx = -1; int worstFactorIx = -1;
// preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
@ -127,7 +127,7 @@ int QPSolver::identifyLeavingConstraint(
* We want the minimum of all those alphas among all inactive inequality. * We want the minimum of all those alphas among all inactive inequality.
*/ */
boost::tuple<double, int> QPSolver::computeStepSize( boost::tuple<double, int> QPSolver::computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const { const VectorValues& p) const {
static bool debug = false; static bool debug = false;
@ -198,7 +198,7 @@ QPState QPSolver::iterate(const QPState& state) const {
} }
else { else {
// Inactivate the leaving constraint // Inactivate the leaving constraint
LinearInequalityFactorGraph newWorkingSet = state.workingSet; InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate(); newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false, state.iterations+1); return QPState(newValues, duals, newWorkingSet, false, state.iterations+1);
} }
@ -216,7 +216,7 @@ QPState QPSolver::iterate(const QPState& state) const {
<< endl; << endl;
// also add to the working set the one that complains the most // also add to the working set the one that complains the most
LinearInequalityFactorGraph newWorkingSet = state.workingSet; InequalityFactorGraph newWorkingSet = state.workingSet;
if (factorIx >= 0) if (factorIx >= 0)
newWorkingSet.at(factorIx)->activate(); newWorkingSet.at(factorIx)->activate();
@ -228,10 +228,10 @@ QPState QPSolver::iterate(const QPState& state) const {
} }
//****************************************************************************** //******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( InequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
LinearInequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
@ -265,7 +265,7 @@ pair<VectorValues, VectorValues> QPSolver::optimize(
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
// Initialize workingSet from the feasible initialValues // Initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet = InequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart); identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0); QPState state(initialValues, duals, workingSet, false, 0);

18
gtsam_unstable/linear/QPSolver.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
struct QPState { struct QPState {
VectorValues values; VectorValues values;
VectorValues duals; VectorValues duals;
LinearInequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
bool converged; bool converged;
size_t iterations; size_t iterations;
@ -41,7 +41,7 @@ struct QPState {
/// constructor with initial values /// constructor with initial values
QPState(const VectorValues& initialValues, const VectorValues& initialDuals, QPState(const VectorValues& initialValues, const VectorValues& initialDuals,
const LinearInequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) : const InequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) :
values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged(
_converged), iterations(_iterations) { _converged), iterations(_iterations) {
} }
@ -68,7 +68,7 @@ public:
/// Find solution with the current working set /// Find solution with the current working set
VectorValues solveWithCurrentWorkingSet( VectorValues solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const; const InequalityFactorGraph& workingSet) const;
/// @name Build the dual graph /// @name Build the dual graph
/// @{ /// @{
@ -92,7 +92,7 @@ public:
/// Create a dual factor /// Create a dual factor
JacobianFactor::shared_ptr createDualFactor(Key key, JacobianFactor::shared_ptr createDualFactor(Key key,
const LinearInequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const; const VectorValues& delta) const;
/** /**
@ -123,7 +123,7 @@ public:
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
*/ */
GaussianFactorGraph::shared_ptr buildDualGraph( GaussianFactorGraph::shared_ptr buildDualGraph(
const LinearInequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const; const VectorValues& delta) const;
/// @} /// @}
@ -162,7 +162,7 @@ public:
* And we want to remove the worst one with the largest lambda from the active set. * And we want to remove the worst one with the largest lambda from the active set.
* *
*/ */
int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const; const VectorValues& lambdas) const;
/** /**
@ -174,7 +174,7 @@ public:
* in the next iteration * in the next iteration
*/ */
boost::tuple<double, int> computeStepSize( boost::tuple<double, int> computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const; const VectorValues& p) const;
/** Iterate 1 step, return a new state with a new workingSet and values */ /** Iterate 1 step, return a new state with a new workingSet and values */
@ -183,8 +183,8 @@ public:
/** /**
* Identify active constraints based on initial values. * Identify active constraints based on initial values.
*/ */
LinearInequalityFactorGraph identifyActiveConstraints( InequalityFactorGraph identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& initialValues,
const VectorValues& duals = VectorValues(), bool useWarmStart = true) const; const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;

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

@ -12,7 +12,7 @@
/** /**
* @file testLinearEquality.cpp * @file testLinearEquality.cpp
* @brief Unit tests for LinearEquality * @brief Unit tests for LinearEquality
* @author thduynguyen * @author Duy-Nguyen Ta
**/ **/
#include <gtsam_unstable/linear/LinearEquality.h> #include <gtsam_unstable/linear/LinearEquality.h>

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

@ -132,7 +132,7 @@ TEST(QPSolver, indentifyActiveConstraints) {
currentSolution.insert(X(1), zero(1)); currentSolution.insert(X(1), zero(1));
currentSolution.insert(X(2), zero(1)); currentSolution.insert(X(2), zero(1));
LinearInequalityFactorGraph workingSet = InequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive CHECK(!workingSet.at(0)->active()); // inactive
@ -173,7 +173,7 @@ TEST(QPSolver, iterate) {
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
LinearInequalityFactorGraph workingSet = InequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution); solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPState state(currentSolution, VectorValues(), workingSet, false, 100); QPState state(currentSolution, VectorValues(), workingSet, false, 100);