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
* @date: Dec 8, 2014
* @author: Duy-Nguyen Ta
@ -23,14 +23,14 @@
namespace gtsam {
class LinearEqualityFactorGraph : public FactorGraph<LinearEquality> {
class EqualityFactorGraph : public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<LinearEqualityFactorGraph> shared_ptr;
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
};
/// traits
template<> struct traits<LinearEqualityFactorGraph> : public Testable<
LinearEqualityFactorGraph> {
template<> struct traits<EqualityFactorGraph> : public Testable<
EqualityFactorGraph> {
};
} // \ 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
* @date: Dec 8, 2014
* @author: Duy-Nguyen Ta
@ -23,12 +23,12 @@
namespace gtsam {
class LinearInequalityFactorGraph: public FactorGraph<LinearInequality> {
class InequalityFactorGraph: public FactorGraph<LinearInequality> {
private:
typedef FactorGraph<LinearInequality> Base;
public:
typedef boost::shared_ptr<LinearInequalityFactorGraph> shared_ptr;
typedef boost::shared_ptr<InequalityFactorGraph> shared_ptr;
/** print */
void print(const std::string& str, const KeyFormatter& keyFormatter =
@ -37,15 +37,15 @@ public:
}
/** equals */
bool equals(const LinearInequalityFactorGraph& other,
bool equals(const InequalityFactorGraph& other,
double tol = 1e-9) const {
return Base::equals(other, tol);
}
};
/// traits
template<> struct traits<LinearInequalityFactorGraph> : public Testable<
LinearInequalityFactorGraph> {
template<> struct traits<InequalityFactorGraph> : public Testable<
InequalityFactorGraph> {
};
} // \ namespace gtsam

12
gtsam_unstable/linear/QP.h
View File

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

22
gtsam_unstable/linear/QPSolver.cpp
View File

@ -39,7 +39,7 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
//******************************************************************************
VectorValues QPSolver::solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const {
const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) {
if (factor->active())
@ -50,7 +50,7 @@ VectorValues QPSolver::solveWithCurrentWorkingSet(
//******************************************************************************
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
std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians
@ -78,7 +78,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
//******************************************************************************
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());
BOOST_FOREACH(Key key, constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
@ -91,7 +91,7 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph(
//******************************************************************************
int QPSolver::identifyLeavingConstraint(
const LinearInequalityFactorGraph& workingSet,
const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const {
int worstFactorIx = -1;
// 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.
*/
boost::tuple<double, int> QPSolver::computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const {
static bool debug = false;
@ -198,7 +198,7 @@ QPState QPSolver::iterate(const QPState& state) const {
}
else {
// Inactivate the leaving constraint
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false, state.iterations+1);
}
@ -216,7 +216,7 @@ QPState QPSolver::iterate(const QPState& state) const {
<< endl;
// also add to the working set the one that complains the most
LinearInequalityFactorGraph newWorkingSet = state.workingSet;
InequalityFactorGraph newWorkingSet = state.workingSet;
if (factorIx >= 0)
newWorkingSet.at(factorIx)->activate();
@ -228,10 +228,10 @@ QPState QPSolver::iterate(const QPState& state) const {
}
//******************************************************************************
LinearInequalityFactorGraph QPSolver::identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
InequalityFactorGraph QPSolver::identifyActiveConstraints(
const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const {
LinearInequalityFactorGraph workingSet;
InequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
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 {
// Initialize workingSet from the feasible initialValues
LinearInequalityFactorGraph workingSet =
InequalityFactorGraph workingSet =
identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0);

18
gtsam_unstable/linear/QPSolver.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
struct QPState {
VectorValues values;
VectorValues duals;
LinearInequalityFactorGraph workingSet;
InequalityFactorGraph workingSet;
bool converged;
size_t iterations;
@ -41,7 +41,7 @@ struct QPState {
/// constructor with initial values
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(
_converged), iterations(_iterations) {
}
@ -68,7 +68,7 @@ public:
/// Find solution with the current working set
VectorValues solveWithCurrentWorkingSet(
const LinearInequalityFactorGraph& workingSet) const;
const InequalityFactorGraph& workingSet) const;
/// @name Build the dual graph
/// @{
@ -92,7 +92,7 @@ public:
/// Create a dual factor
JacobianFactor::shared_ptr createDualFactor(Key key,
const LinearInequalityFactorGraph& workingSet,
const InequalityFactorGraph& workingSet,
const VectorValues& delta) const;
/**
@ -123,7 +123,7 @@ public:
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
*/
GaussianFactorGraph::shared_ptr buildDualGraph(
const LinearInequalityFactorGraph& workingSet,
const InequalityFactorGraph& workingSet,
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.
*
*/
int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet,
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const;
/**
@ -174,7 +174,7 @@ public:
* in the next iteration
*/
boost::tuple<double, int> computeStepSize(
const LinearInequalityFactorGraph& workingSet, const VectorValues& xk,
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const;
/** 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.
*/
LinearInequalityFactorGraph identifyActiveConstraints(
const LinearInequalityFactorGraph& inequalities,
InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph& inequalities,
const VectorValues& initialValues,
const VectorValues& duals = VectorValues(), bool useWarmStart = true) const;

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

@ -12,7 +12,7 @@
/**
* @file testLinearEquality.cpp
* @brief Unit tests for LinearEquality
* @author thduynguyen
* @author Duy-Nguyen Ta
**/
#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(2), zero(1));
LinearInequalityFactorGraph workingSet =
InequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
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(2), (Vector(1) << 0.5).finished());
LinearInequalityFactorGraph workingSet =
InequalityFactorGraph workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPState state(currentSolution, VectorValues(), workingSet, false, 100);