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

Removed constraint variations on graphs/factors

release/4.3a0
Alex Cunningham 2009-11-10 04:33:39 +00:00
parent 2c37c94b5d
commit a7b711db37
12 changed files with 0 additions and 2042 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
cpp/ConstrainedConditionalGaussian.cpp
View File

@ -1,80 +0,0 @@
/**
* @file ConstrainedConditionalGaussian.cpp
* @brief Implements the constrained version of the conditional gaussian class,
* primarily handling the possible solutions
* @author Alex Cunningham
*/
#include <boost/numeric/ublas/lu.hpp>
#include "ConstrainedConditionalGaussian.h"
#include "Matrix.h"
using namespace gtsam;
using namespace std;
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key) :
ConditionalGaussian(key) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& v) :
ConditionalGaussian(key, v, eye(v.size()), ones(v.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A) :
ConditionalGaussian(key, b, A, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parent, const Matrix& A2) :
ConditionalGaussian(key, b, A1, parent, A2, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Vector& b, const Matrix& A1,
const std::string& parentY, const Matrix& A2, const std::string& parentZ,
const Matrix& A3) :
ConditionalGaussian(key, b, A1, parentY, A2, parentZ, A3, ones(b.size())*INFINITY) {
}
ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(
const string& key, const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b) :
ConditionalGaussian(key, b, A1, parents, ones(b.size())*INFINITY) {
}
Vector ConstrainedConditionalGaussian::solve(const VectorConfig& x) const {
// sum the RHS
Vector rhs = d_;
for (map<string, Matrix>::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) {
const string& j = it->first;
const Matrix& Aj = it->second;
rhs -= Aj * x[j];
}
// verify invertibility of A matrix
Matrix A = R_;
Matrix b = Matrix_(rhs.size(), 1, rhs);
if (A.size1() != A.size2())
throw invalid_argument("Matrix A is not invertible - non-square matrix");
using namespace boost::numeric::ublas;
if (lu_factorize(A))
throw invalid_argument("Matrix A is singular");
// get the actual solution
//FIXME: This is just the Matrix::solve() function, but due to name conflicts
// the compiler won't find the real version in Matrix.h
lu_substitute<const Matrix, Matrix> (A, b);
return Vector_(b);
//TODO: Handle overdetermined case
//TODO: Handle underdetermined case
}

102
cpp/ConstrainedConditionalGaussian.h
View File

@ -1,102 +0,0 @@
/**
* @file ConstrainedConditionalGaussian.h
* @brief Class which implements a conditional gaussian that handles situations
* that occur when linear constraints appear in the system. A constrained
* conditional gaussian is the result of eliminating a linear equality
* constraint.
*
* @author Alex Cunningham
*/
#pragma once
#include "ConditionalGaussian.h"
namespace gtsam {
/**
* Implements a more generalized conditional gaussian to represent
* the result of eliminating an equality constraint. All of the
* forms of an equality constraint will be of the form
* A1x = b - sum(Aixi from i=2 to i=N)
* If A1 is triangular, then it can be solved using backsubstitution
* If A1 is invertible, then it can be solved with the Matrix::solve() command
* If A1 overconstrains the system, then ???
* If A1 underconstrains the system, then ???
*/
class ConstrainedConditionalGaussian: public ConditionalGaussian {
public:
typedef boost::shared_ptr<ConstrainedConditionalGaussian> shared_ptr;
/**
* Default Constructor
* Don't use this
*/
ConstrainedConditionalGaussian(const std::string& key);
/**
* Used for unary factors that simply associate a name with a particular value
* Can use backsubstitution to solve trivially
* @param value is a fixed value for x in the form x = value
*/
ConstrainedConditionalGaussian(const std::string& key, const Vector& value);
/**
* Used for unary factors of the form Ax=b
* Invertability of A is significant
* @param b is the RHS of the equation
* @param A is the A matrix
*/
ConstrainedConditionalGaussian(const std::string& key, const Vector& value, const Matrix& A);
/**
* Binary constructor of the form A1*x = b - A2*y
* for node with one parent
* @param b is the RHS of the equation
* @param A1 is the A1 matrix
* @param parent is the string identifier for the parent node
* @param A2 is the A2 matrix
*/
ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
const std::string& parent, const Matrix& A2);
/**
* Ternary constructor of the form A1*x = b - A2*y - A3*z
* @param b is the RHS of the equation
* @param A1 is the A1 matrix
* @param parentY string id for y
* @param A2 is the A2 matrix
* @param parentZ string id for z
* @param A3 is the A3 matrix
*/
ConstrainedConditionalGaussian(const std::string& key, const Vector& b, const Matrix& A1,
const std::string& parentY, const Matrix& A2,
const std::string& parentZ, const Matrix& A3);
/**
* Construct with arbitrary number of parents of form
* A1*x = b - sum(Ai*xi)
* @param A1 is the matrix associated with the variable that was eliminated
* @param parents is the map of parents (Ai and xi from above)
* @param b is the rhs vector
*/
ConstrainedConditionalGaussian(const std::string& key, const Matrix& A1,
const std::map<std::string, Matrix>& parents, const Vector& b);
virtual ~ConstrainedConditionalGaussian() {
}
/**
* Solve for the value of the node given the parents
* If A1 (R from the conditional gaussian) is triangular, then backsubstitution
* If A1 invertible, Matrix::solve()
* If A1 under/over constrains the system, TODO
* @param config contains the values for all of the parents
* @return the value for this node
*/
Vector solve(const VectorConfig& x) const;
};
}

268
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -1,268 +0,0 @@
/**
* @file ConstrainedLinearFactorGraph.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include "Ordering.h"
#include "ConstrainedLinearFactorGraph.h"
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
namespace gtsam {
/* ************************************************************************* */
ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph() {
}
/* ************************************************************************* */
ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph(
const LinearFactorGraph& lfg) {
BOOST_FOREACH(LinearFactor::shared_ptr s, lfg)
{ push_back(s);
}
}
/* ************************************************************************* */
ConstrainedLinearFactorGraph::~ConstrainedLinearFactorGraph() {
}
/* ************************************************************************* */
void ConstrainedLinearFactorGraph::push_back_constraint(LinearConstraint::shared_ptr factor)
{
constraints_.push_back(factor);
}
/* ************************************************************************* */
bool ConstrainedLinearFactorGraph::is_constrained(const std::string& key) const
{
BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_)
{
if (e->involves(key)) return true;
}
return false;
}
/* ************************************************************************* */
bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double tol) const
{
const ConstrainedLinearFactorGraph* p = (const ConstrainedLinearFactorGraph *) &fg;
if (p == NULL) return false;
/** check equality factors */
if (constraints_.size() != p->constraints_.size()) return false;
BOOST_FOREACH(LinearConstraint::shared_ptr ef1, constraints_)
{
bool contains = false;
BOOST_FOREACH(LinearConstraint::shared_ptr ef2, p->constraints_)
if (ef1->equals(*ef2))
contains = true;
if (!contains) return false;
}
/** default equality check */
return LinearFactorGraph::equals(fg, tol);
}
/* ************************************************************************* */
GaussianBayesNet ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) {
GaussianBayesNet cbn;
BOOST_FOREACH(string key, ordering) {
// constraints take higher priority in elimination, so check if
// there are constraints first
if (is_constrained(key))
{
ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key);
cbn.push_back(ccg);
}
else
{
ConditionalGaussian::shared_ptr cg = eliminateOne(key);
cbn.push_back(cg);
}
}
return cbn;
}
/* ************************************************************************* */
ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::eliminate_constraint(const string& key)
{
// extract all adjacent constraints
vector<LinearConstraint::shared_ptr> constraint_separator = find_constraints_and_remove(key);
if (constraint_separator.size() == 0)
throw invalid_argument("No constraints on node " + key);
// split out a constraint to apply
LinearConstraint::shared_ptr constraint = pick_constraint(constraint_separator);
// perform change of variables on the remaining constraints and reinsert
update_constraints(key, constraint_separator, constraint);
// perform elimination on the constraint itself to get the constrained conditional gaussian
ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
// perform a change of variables on the linear factors in the separator
vector<LinearFactor::shared_ptr> separator = findAndRemoveFactors(key);
BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) {
// store the block matrices
map<string, Matrix> blocks;
// get the A1 term and reconstruct new_factor without the eliminated block
Matrix A1 = factor->get_A(key);
list<string> factor_keys = factor->keys();
BOOST_FOREACH(string cur_key, factor_keys) {
if (cur_key != key)
blocks.insert(make_pair(cur_key, factor->get_A(cur_key)));
}
// get T = A1*inv(C1) term
Matrix invC1 = inverse(constraint->get_A(key));
Matrix T = A1*invC1;
// loop over all nodes in separator of constraint
list<string> constraint_keys = constraint->keys(key);
BOOST_FOREACH(string cur_key, constraint_keys) {
Matrix Ci = constraint->get_A(cur_key);
if (cur_key != key && !factor->involves(cur_key)) {
Matrix Ai = T*Ci;
blocks.insert(make_pair(cur_key, -1 *Ai));
} else if (cur_key != key) {
Matrix Ai = factor->get_A(cur_key) - T*Ci;
blocks.insert(make_pair(cur_key, Ai));
}
}
// construct the updated factor
boost::shared_ptr<LinearFactor> new_factor(new LinearFactor);
string cur_key; Matrix M;
FOREACH_PAIR(cur_key, M, blocks) {
new_factor->insert(cur_key, M);
}
// update RHS of updated factor
Vector new_b(A1.size1());
if (factor->get_b().size() == 0)
new_b = -1 * (T * constraint->get_b());
else
new_b = factor->get_b() - T * constraint->get_b();
new_factor->set_b(new_b);
// insert the new factor into the graph
push_back(new_factor);
}
return ccg;
}
/* ************************************************************************* */
LinearConstraint::shared_ptr ConstrainedLinearFactorGraph::pick_constraint(
const std::vector<LinearConstraint::shared_ptr>& constraints) const {
if (constraints.size() == 0)
throw invalid_argument("Constraint set is empty!");
return constraints[0];
}
/* ************************************************************************* */
void ConstrainedLinearFactorGraph::update_constraints(const std::string& key,
const std::vector<LinearConstraint::shared_ptr>& separator,
const LinearConstraint::shared_ptr& primary) {
// Implements update for each constraint, where primary is
// C1*x1 = d - C2*x2 - ...
// and each constraint is
// A1*x1 + A2*x2 + ... = b;
// extract components from primary
Matrix invC1 = inverse(primary->get_A(key));
Vector d = primary->get_b();
// perform transform on each constraint
// constraint c is the one being updated
BOOST_FOREACH(LinearConstraint::shared_ptr updatee, separator) {
if (!updatee->equals(*primary)) {
// build transform matrix
Matrix A1 = updatee->get_A(key);
Matrix T = A1 * invC1;
// copy existing nodes into new factor without the eliminated variable
list<string> updatee_keys = updatee->keys(key);
map<string, Matrix> blocks;
BOOST_FOREACH(string cur_key, updatee_keys) {
blocks[cur_key] = updatee->get_A(cur_key);
}
// loop over all nodes in separator of constraint
list<string> primary_keys = primary->keys(key); // keys that are not key
BOOST_FOREACH(string cur_key, primary_keys) {
Matrix Ci = primary->get_A(cur_key);
if (cur_key != key && !updatee->involves(cur_key)) {
Matrix Ai = T*Ci;
blocks[cur_key] = -1 * Ai;
} else if (cur_key != key) {
Matrix Ai = updatee->get_A(cur_key) - T*Ci;
blocks[cur_key] = Ai;
}
}
Vector rhs = updatee->get_b() - T * d;
LinearConstraint::shared_ptr new_constraint(new LinearConstraint(blocks, rhs));
// reinsert into graph
push_back_constraint(new_constraint);
}
}
}
/* ************************************************************************* */
VectorConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) {
GaussianBayesNet cbn = eliminate(ordering);
VectorConfig newConfig = gtsam::optimize(cbn);
return newConfig;
}
/* ************************************************************************* */
std::vector<LinearConstraint::shared_ptr>
ConstrainedLinearFactorGraph::find_constraints_and_remove(const string& key)
{
vector<LinearConstraint::shared_ptr> found, uninvolved;
BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_) {
if (constraint->involves(key)) {
found.push_back(constraint);
} else {
uninvolved.push_back(constraint);
}
}
constraints_ = uninvolved;
return found;
}
/* ************************************************************************* */
void ConstrainedLinearFactorGraph::print(const std::string& s) const
{
cout << "ConstrainedFactorGraph: " << s << endl;
BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
{
f->print();
}
BOOST_FOREACH(LinearConstraint::shared_ptr f, constraints_)
{
f->print();
}
}
/* ************************************************************************* */
Ordering ConstrainedLinearFactorGraph::getOrdering() const
{
Ordering ord = LinearFactorGraph::getOrdering();
cout << "ConstrainedLinearFactorGraph::getOrdering() - Not Implemented!" << endl;
// BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_)
// ord.push_back(e->get_key());
return ord;
}
}

147
cpp/ConstrainedLinearFactorGraph.h
View File

@ -1,147 +0,0 @@
/**
* @file ConstrainedLinearFactorGraph.h
* @brief A modified version of LinearFactorGraph that can handle
* linear constraints.
* @author Alex Cunningham
*/
#ifndef CONSTRAINEDLINEARFACTORGRAPH_H_
#define CONSTRAINEDLINEARFACTORGRAPH_H_
#include "LinearFactorGraph.h"
#include "GaussianBayesNet.h"
#include "LinearConstraint.h"
namespace gtsam {
class ConstrainedLinearFactorGraph: public LinearFactorGraph {
protected:
std::vector<LinearConstraint::shared_ptr> constraints_; /// collection of equality factors
public:
// iterators for equality constraints - same interface as linear factors
typedef std::vector<LinearConstraint::shared_ptr>::const_iterator constraint_const_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::iterator constraint_iterator;
public:
/**
* Default constructor
*/
ConstrainedLinearFactorGraph();
/**
* Copy from linear factor graph
*/
ConstrainedLinearFactorGraph(const LinearFactorGraph& lfg);
virtual ~ConstrainedLinearFactorGraph();
/**
* Add a constraint to the graph
* @param constraint is a shared pointer to a linear constraint between nodes in the graph
*/
void push_back_constraint(LinearConstraint::shared_ptr constraint);
/**
* STL-like indexing into the constraint vector
* @param i index of the target constraint
* @return the constraint to be returned
*/
LinearConstraint::shared_ptr constraint_at(const size_t i) const {return constraints_.at(i);}
/** return the iterator pointing to the first equality factor */
constraint_const_iterator constraint_begin() const {
return constraints_.begin();
}
/** return the iterator pointing to the last factor */
constraint_const_iterator constraint_end() const {
return constraints_.end();
}
/** clear the factor graph - re-implemented to include equality factors */
void clear(){
factors_.clear();
constraints_.clear();
}
/** size - reimplemented to include the equality factors_ */
inline size_t size() const { return factors_.size() + constraints_.size(); }
/** Check equality - checks equality constraints as well*/
bool equals(const LinearFactorGraph& fg, double tol=1e-9) const;
/**
* eliminate factor graph in place(!) in the given order, yielding
* a chordal Bayes net. Note that even with constraints,
* a constrained factor graph can produce a CBN, because
* constrained conditional gaussian is a subclass of conditional
* gaussian, with a different solving procedure.
* @param ordering is the order to eliminate the variables
*/
GaussianBayesNet eliminate(const Ordering& ordering);
/**
* Picks one of the contraints in a set of constraints to eliminate
* Currently just picks the first one - should probably be optimized
* @param constraints is a set of constraints of which one will be eliminated
* @return one of the constraints to use
*/
LinearConstraint::shared_ptr pick_constraint(
const std::vector<LinearConstraint::shared_ptr>& constraints) const;
/**
* Eliminates the specified constraint for the selected variable,
* and then performs a change of variables on all the constraints in the separator
* and reinserts them into the graph.
* @param key is the identifier for the node to eliminate
* @param separator is the set of constraints to transform and reinsert
* @param constraint is the primary constraint to eliminate
*/
void update_constraints(const std::string& key,
const std::vector<LinearConstraint::shared_ptr>& separator,
const LinearConstraint::shared_ptr& constraint);
/**
* Eliminates a node with a constraint on it
* Other factors have a change of variables performed via Schur complement to remove the
* eliminated node.
*/
ConstrainedConditionalGaussian::shared_ptr eliminate_constraint(const std::string& key);
/**
* optimize a linear factor graph
* @param ordering fg in order
*/
VectorConfig optimize(const Ordering& ordering);
/**
* Determines if a node has any constraints attached to it
*/
bool is_constrained(const std::string& key) const;
/**
* Prints the contents of the factor graph with optional name string
*/
void print(const std::string& s="") const;
/**
* Pulls out all constraints attached to a particular node
* Note: this removes the constraints in place
* @param key of the node to pull constraints on
* @return a set of constraints
*/
std::vector<LinearConstraint::shared_ptr> find_constraints_and_remove(const std::string& key);
/**
* This function returns the best ordering for this linear factor
* graph, computed using colamd for the linear factors with all
* of the equality factors eliminated first
*/
Ordering getOrdering() const;
};
}
#endif /* CONSTRAINEDLINEARFACTORGRAPH_H_ */

9
cpp/ConstrainedNonlinearFactorGraph.cpp
View File

@ -1,9 +0,0 @@
/*
* ConstrainedNonlinearFactorGraph.cpp
*
* Created on: Aug 10, 2009
* Author: alexgc
*/
#include "ConstrainedNonlinearFactorGraph.h"

95
cpp/ConstrainedNonlinearFactorGraph.h
View File

@ -1,95 +0,0 @@
/*
* ConstrainedNonlinearFactorGraph.h
*
* Created on: Aug 10, 2009
* Author: alexgc
*/
#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_
#define CONSTRAINEDNONLINEARFACTORGRAPH_H_
#include <boost/shared_ptr.hpp>
#include "NonlinearFactorGraph.h"
#include "LinearConstraint.h"
#include "ConstrainedLinearFactorGraph.h"
namespace gtsam {
/**
* A nonlinear factor graph with the addition of equality constraints.
*
* Templated on factor and configuration type.
* TODO FD: this is totally wrong: it can only work if Config==VectorConfig
* as LinearConstraint is only defined for VectorConfig
* To fix it, we need to think more deeply about this.
*/
template<class Factor, class Config>
class ConstrainedNonlinearFactorGraph: public FactorGraph<Factor> {
protected:
/** collection of equality factors */
std::vector<LinearConstraint::shared_ptr> eq_factors;
public:
// iterators over equality factors
typedef std::vector<LinearConstraint::shared_ptr>::const_iterator eq_const_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::iterator eq_iterator;
/**
* Default constructor
*/
ConstrainedNonlinearFactorGraph() {
}
/**
* Copy constructor from regular NLFGs
*/
ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph<Config>& nfg) :
FactorGraph<Factor> (nfg) {
}
typedef typename boost::shared_ptr<Factor> shared_factor;
typedef typename std::vector<shared_factor>::const_iterator const_iterator;
/**
* Linearize a nonlinear graph to produce a linear graph
* Note that equality constraints will just pass through
*/
ConstrainedLinearFactorGraph linearize(const Config& config) const {
ConstrainedLinearFactorGraph ret;
// linearize all nonlinear factors
// TODO uncomment
for (const_iterator factor = this->factors_.begin(); factor < this->factors_.end(); factor++) {
LinearFactor::shared_ptr lf = (*factor)->linearize(config);
ret.push_back(lf);
}
// linearize the equality factors (set to zero because they are now in delta space)
for (eq_const_iterator e_factor = eq_factors.begin(); e_factor
< eq_factors.end(); e_factor++) {
// LinearConstraint::shared_ptr eq = (*e_factor)->linearize();
// ret.push_back_eq(eq);
}
return ret;
}
/**
* Insert a factor into the graph
*/
void push_back(const shared_factor& f) {
FactorGraph<Factor>::push_back(f);
}
/**
* Insert a equality factor into the graph
*/
void push_back_eq(const LinearConstraint::shared_ptr& eq) {
eq_factors.push_back(eq);
}
};
}
#endif /* CONSTRAINEDNONLINEARFACTORGRAPH_H_ */

129
cpp/LinearConstraint.cpp
View File

@ -1,129 +0,0 @@
/**
* @file LinearConstraint.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "LinearConstraint.h"
#include "Matrix.h"
namespace gtsam {
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
LinearConstraint::LinearConstraint() {
}
LinearConstraint::LinearConstraint(const Vector& constraint,
const std::string& id) :
b(constraint) {
int size = constraint.size();
Matrix A = eye(size);
As.insert(make_pair(id, A));
}
LinearConstraint::LinearConstraint(const std::string& node1, const Matrix& A1,
const std::string& node2, const Matrix& A2, const Vector& rhs)
: b(rhs) {
As.insert(make_pair(node1, A1));
As.insert(make_pair(node2, A2));
}
LinearConstraint::LinearConstraint(const std::map<std::string, Matrix>& matrices, const Vector& rhs)
: As(matrices), b(rhs)
{
}
void LinearConstraint::print(const string& s) const {
cout << s << ": " << endl;
string key; Matrix A;
FOREACH_PAIR(key, A, As) {
gtsam::print(A, key);
}
gtsam::print(b, "rhs= ");
}
bool LinearConstraint::equals(const LinearConstraint& f, double tol) const {
// check sizes
if (size() != f.size()) return false;
// check rhs
if (!equal_with_abs_tol(b, f.b, tol)) return false;
// check all matrices
pair<string, Matrix> p;
BOOST_FOREACH(p, As) {
// check for key existence
const_iterator it = f.As.find(p.first);
if (it == f.As.end()) return false;
Matrix f_mat = it->second;
// check matrix
if (!equal_with_abs_tol(f_mat,p.second,tol)) return false;
}
return true;
}
ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std::string& key) {
// check to ensure key is one of constraint nodes
const_iterator it = As.find(key);
if (it == As.end())
throw invalid_argument("Node " + key + " is not in LinearConstraint");
// extract the leading matrix
Matrix A1 = it->second;
// assemble parents
map<string, Matrix> parents = As;
parents.erase(key);
// construct resulting CCG with parts
ConstrainedConditionalGaussian::shared_ptr ccg(new ConstrainedConditionalGaussian(key, A1, parents, b));
return ccg;
}
bool LinearConstraint::involves(const std::string& key) const {
return As.find(key) != As.end();
}
list<string> LinearConstraint::keys(const std::string& key) const {
list<string> ret;
pair<string, Matrix> p;
BOOST_FOREACH(p, As) {
if (p.first != key)
ret.push_back(p.first);
}
return ret;
}
LinearConstraint::shared_ptr combineConstraints(
const set<LinearConstraint::shared_ptr>& constraints) {
map<string, Matrix> blocks;
Vector rhs;
BOOST_FOREACH(LinearConstraint::shared_ptr c, constraints) {
string key; Matrix A;
FOREACH_PAIR( key, A, c->As) {
if (blocks.find(key) == blocks.end())
blocks[key] = A;
else {
Matrix Aold = blocks[key];
if (A.size1() != Aold.size1() || A.size2() != Aold.size2())
throw invalid_argument("Dimension mismatch");
blocks[key] = A + Aold;
}
}
// assemble rhs
if (rhs.size() == 0)
rhs = c->get_b();
else
rhs = rhs + c->get_b();
}
LinearConstraint::shared_ptr result(new LinearConstraint(blocks, rhs));
return result;
}
}

138
cpp/LinearConstraint.h
View File

@ -1,138 +0,0 @@
/**
* @file LinearConstraint.h
* @brief Class that implements linear equality constraints
* @author Alex Cunningham
*/
#ifndef EQUALITYFACTOR_H_
#define EQUALITYFACTOR_H_
#include <list>
#include <set>
#include "Matrix.h"
#include "ConstrainedConditionalGaussian.h"
namespace gtsam {
/**
* Linear constraints are similar to factors in structure, but represent
* a different problem
*/
class LinearConstraint: Testable<LinearConstraint> {
public:
typedef boost::shared_ptr<LinearConstraint> shared_ptr;
typedef std::map<std::string, Matrix>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator;
protected:
std::map<std::string, Matrix> As; // linear matrices
Vector b; // right-hand-side
public:
/**
* Default constructor
*/
LinearConstraint();
/**
* Constructor with initialization of a unary equality factor
* Creates an identity matrix for the underlying implementation and the constraint
* value becomes the RHS value - use for setting a variable to a fixed value
* @param constraint the value that the variable node is defined as equal to
* @param key identifier for the variable node
*/
LinearConstraint(const Vector& constraint, const std::string& key);
/**
* Constructor for binary constraint
* @param key for first node
* @param A Matrix for first node
* @param key for second node
* @param A Matrix for second node
* @param RHS b vector
*/
LinearConstraint(const std::string& node1, const Matrix& A1,
const std::string& node2, const Matrix& A2, const Vector& b);
/**
* Constructor for arbitrary numbers of nodes
* @param matrices is the full map of A matrices
* @param b is the RHS vector
*/
LinearConstraint(const std::map<std::string, Matrix>& matrices,
const Vector& b);
/**
* Default Destructor
*/
~LinearConstraint() {
}
/**
* print
* @param s optional string naming the factor
*/
void print(const std::string& s = "") const;
/**
* equality up to tolerance
*/
bool equals(const LinearConstraint& f, double tol = 1e-9) const;
/**
* Eliminates the constraint
* Note: Assumes that the constraint will be completely eliminated
* and that the matrix associated with the key is invertible
* @param key is the variable to eliminate
* @return a constrained conditional gaussian for the variable that is a
* function of its parents
*/
ConstrainedConditionalGaussian::shared_ptr
eliminate(const std::string& key);
/** get a copy of b */
const Vector& get_b() const {
return b;
}
/** check if the constraint is connected to a particular node */
bool involves(const std::string& key) const;
/**
* get a copy of the A matrix from a specific node
* O(log n)
*/
const Matrix& get_A(const std::string& key) const {
const_iterator it = As.find(key);
if (it == As.end())
throw(std::invalid_argument("LinearFactor::[] invalid key: " + key));
return it->second;
}
/**
* Gets all of the keys connected in a constraint
* @param key is a key to leave out of the final set
* @return a list of the keys for nodes connected to the constraint
*/
std::list<std::string> keys(const std::string& key = "") const;
/**
* @return the number of nodes the constraint connects
*/
std::size_t size() const {
return As.size();
}
// friends
friend LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
};
/**
* Combines constraints into one constraint
*/
LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
}
#endif /* EQUALITYFACTOR_H_ */

130
cpp/testConstrainedConditionalGaussian.cpp
View File

@ -1,130 +0,0 @@
/**
* @file testConstrainedConditionalGaussian.cpp
* @brief tests for constrained conditional gaussians
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "ConstrainedConditionalGaussian.h"
using namespace gtsam;
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_unary1 )
{
Vector v(2); v(0)=1.0; v(1)=2.0;
// check unary constructor that doesn't require an R matrix
// assumed identity matrix
ConstrainedConditionalGaussian unary("x1",v);
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
}
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_unary2 )
{
Vector v(2); v(0)=1.0; v(1)=2.0;
// check unary constructor that makes use of a A matrix
Matrix A = eye(2) * 10;
ConstrainedConditionalGaussian unary("x1",10*v, A);
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
}
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_unary3 )
{
Vector v(2); v(0)=1.0; v(1)=2.0;
// check unary constructor that makes use of a A matrix
// where A^(-1) exists, but A is not triangular
Matrix A(2,2);
A(0,0) = 1.0 ; A(0,1) = 2.0;
A(1,0) = 2.0 ; A(1,1) = 1.0;
Vector rhs = A*v;
ConstrainedConditionalGaussian unary("x1",rhs, A);
VectorConfig fg;
fg.insert("x1", v);
CHECK(assert_equal(v, unary.solve(fg)));
}
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_binary1 )
{
// tests x = (A1^(-1))*(b - A2y) case, where A1 is invertible
// A1 is not already triangular, however
// RHS
Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible - should still work
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
Vector y = Vector_(2, 1.0, 2.0);
VectorConfig fg;
fg.insert("x1", y);
Vector expected = Vector_(2, -3.3333, 0.6667);
ConstrainedConditionalGaussian binary("x2",b, A1, "x1", A2);
CHECK(assert_equal(expected, binary.solve(fg), 1e-4));
}
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_ternary1 )
{
// tests x = (A1^(-1))*(b - A2*y - A3*z) case, where A1 is invertible
// A1 is not already triangular, however
// RHS
Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible - should still work
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
Matrix A3 = eye(2)*10;
Vector y = Vector_(2, 1.0, 2.0);
Vector z = Vector_(2, 1.0, -1.0);
VectorConfig fg;
fg.insert("x1", y);
fg.insert("x2", z);
Vector expected = Vector_(2, 6.6667, -9.3333);
ConstrainedConditionalGaussian ternary("x3",b, A1, "x1", A2, "x2", A3);
CHECK(assert_equal(expected, ternary.solve(fg), 1e-4));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

561
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -1,561 +0,0 @@
/**
* @file testConstrainedLinearFactorGraph.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include "ConstrainedLinearFactorGraph.h"
#include "LinearFactorGraph.h"
#include "Ordering.h"
#include "smallExample.h"
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, elimination1 )
{
// get the graph
// *-X-x-Y
ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
// verify construction of the graph
CHECK(fg.size() == 2);
// eliminate x
Ordering ord;
ord.push_back("x");
GaussianBayesNet cbn = fg.eliminate(ord);
// verify result of elimination
// CBN of size 1, as we only eliminated X now
CHECK(fg.nrFactors() == 1);
CHECK(cbn.size() == 1);
// We will have a "delta function" on X as a function of Y
// |1 2||x_1| = |1| - |10 0||y_1|
// |2 1||x_2| |2| |0 10||y_2|
Matrix Ax1(2, 2);
Ax1(0, 0) = 1.0; Ax1(0, 1) = 2.0;
Ax1(1, 0) = 2.0; Ax1(1, 1) = 1.0;
Matrix Ay1 = eye(2) * 10;
Vector b2 = Vector_(2, 1.0, 2.0);
ConstrainedConditionalGaussian expectedCCG1("x",b2, Ax1, "y", Ay1);
CHECK(expectedCCG1.equals(*(cbn["x"])));
// // verify remaining factor on y
// // Gaussian factor on X becomes different Gaussian factor on Y
// Matrix Ap(2,2);
// Ap(0, 0) = 1.0; Ap(0, 1) = -2.0;
// Ap(1, 0) = -2.0; Ap(1, 1) = 1.0;
// Ap = 33.3333 * Ap;
// Vector bp = Vector_(2, 0.0, -10.0);
// double sigma1 = 1;
// LinearFactor expectedLF("y", Ap, bp,sigma1);
// CHECK(expectedLF.equals(*(fg[0]), 1e-4));
//
// // eliminate y
// Ordering ord2;
// ord2.push_back("y");
// cbn = fg.eliminate(ord2);
//
// // Check result
// CHECK(fg.size() == 0);
// Matrix R(2,2);
// R(0, 0) = 74.5356; R(0, 1) = -59.6285;
// R(1, 0) = 0.0; R(1, 1) = 44.7214;
// Vector br = Vector_(2, 8.9443, 4.4721);
// Vector tau(2);
// tau(0) = R(0,0);
// tau(1) = R(1,1);
//
// // normalize the existing matrices
// Matrix N = eye(2,2);
// N(0,0) = 1/tau(0);
// N(1,1) = 1/tau(1);
// R = N*R;
// ConditionalGaussian expected2("y",br, R, tau);
// CHECK(expected2.equals(*((*cbn)["y"])));
}
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, optimize )
//{
// // create graph
// ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
//
// // perform optimization
// Ordering ord;
// ord.push_back("y");
// ord.push_back("x");
// VectorConfig actual = fg.optimize(ord);
//
// VectorConfig expected;
// expected.insert("x", Vector_(2, 1.0, -1.0));
// expected.insert("y", Vector_(2, 0.2, 0.1));
//
// CHECK(expected.size() == actual.size());
// CHECK(assert_equal(expected["x"], actual["x"], 1e-4));
// CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, optimize2 )
//{
// // create graph
// ConstrainedLinearFactorGraph fg = createSingleConstraintGraph();
//
// // perform optimization
// Ordering ord;
// ord.push_back("x");
// ord.push_back("y");
// VectorConfig actual = fg.optimize(ord);
//
// VectorConfig expected;
// expected.insert("x", Vector_(2, 1.0, -1.0));
// expected.insert("y", Vector_(2, 0.2, 0.1));
//
// CHECK(expected.size() == actual.size());
// CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); // Fails here: gets x = (-3, 1)
// CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, is_constrained )
//{
// // very simple check
// ConstrainedLinearFactorGraph fg;
// CHECK(!fg.is_constrained("x"));
//
// // create simple graph
// Vector b = Vector_(2, 0.0, 0.0);
// LinearFactor::shared_ptr f1(new LinearFactor("x", eye(2), "y", eye(2), b,1));
// LinearFactor::shared_ptr f2(new LinearFactor("z", eye(2), "w", eye(2), b,1));
// LinearConstraint::shared_ptr f3(new LinearConstraint("y", eye(2), "z", eye(2), b));
// fg.push_back(f1);
// fg.push_back(f2);
// fg.push_back_constraint(f3);
//
// CHECK(fg.is_constrained("y"));
// CHECK(fg.is_constrained("z"));
// CHECK(!fg.is_constrained("x"));
// CHECK(!fg.is_constrained("w"));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, get_constraint_separator )
//{
// ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
// ConstrainedLinearFactorGraph fg2 = createMultiConstraintGraph();
// LinearConstraint::shared_ptr lc1 = fg1.constraint_at(0);
// LinearConstraint::shared_ptr lc2 = fg1.constraint_at(1);
//
// vector<LinearConstraint::shared_ptr> actual1 = fg1.find_constraints_and_remove("y");
// CHECK(fg1.size() == 2);
// CHECK(actual1.size() == 1);
// CHECK((*actual1.begin())->equals(*lc1));
//
// vector<LinearConstraint::shared_ptr> actual2 = fg2.find_constraints_and_remove("x");
// CHECK(fg2.size() == 1);
// CHECK(actual2.size() == 2);
// CHECK((*actual1.begin())->equals(*lc1));
// LinearConstraint::shared_ptr act = *(++actual2.begin());
// CHECK(act->equals(*lc2));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, update_constraints )
//{
// // create a graph
// ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
//
// // process constraints - picking first constraint on x
// vector<LinearConstraint::shared_ptr> constraints = fg1.find_constraints_and_remove("x");
// CHECK(constraints.size() == 2);
// CHECK(fg1.size() == 1); // both constraints removed
// LinearConstraint::shared_ptr primary = constraints[0];
// LinearConstraint::shared_ptr updatee = constraints[1];
// fg1.update_constraints("x", constraints, primary);
// CHECK(fg1.size() == 2); // induced constraint added back
//
// // expected induced constraint
// Matrix Ar(2,2);
// Ar(0, 0) = -16.6666; Ar(0, 1) = -6.6666;
// Ar(1, 0) = 10.0; Ar(1, 1) = 0.0;
// Matrix A22(2,2);
// A22(0,0) = 1.0 ; A22(0,1) = 1.0;
// A22(1,0) = 1.0 ; A22(1,1) = 2.0;
// Vector br = Vector_(2, 0.0, 5.0);
// LinearConstraint::shared_ptr exp(new LinearConstraint("y", Ar, "z", A22, br));
//
// // evaluate
// CHECK(assert_equal(*(fg1.constraint_at(0)), *exp, 1e-4));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, find_constraints_and_remove )
//{
// // constraint 1
// Matrix A11(2,2);
// A11(0,0) = 1.0 ; A11(0,1) = 2.0;
// A11(1,0) = 2.0 ; A11(1,1) = 1.0;
//
// Matrix A12(2,2);
// A12(0,0) = 10.0 ; A12(0,1) = 0.0;
// A12(1,0) = 0.0 ; A12(1,1) = 10.0;
//
// Vector b1(2);
// b1(0) = 1.0; b1(1) = 2.0;
// LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
//
// // constraint 2
// Matrix A21(2,2);
// A21(0,0) = 3.0 ; A21(0,1) = 4.0;
// A21(1,0) = -1.0 ; A21(1,1) = -2.0;
//
// Matrix A22(2,2);
// A22(0,0) = 1.0 ; A22(0,1) = 1.0;
// A22(1,0) = 1.0 ; A22(1,1) = 2.0;
//
// Vector b2(2);
// b2(0) = 3.0; b2(1) = 4.0;
// LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
//
// // construct the graph
// ConstrainedLinearFactorGraph fg1;
// fg1.push_back_constraint(lc1);
// fg1.push_back_constraint(lc2);
//
// // constraints on x
// vector<LinearConstraint::shared_ptr> expected1, actual1;
// expected1.push_back(lc1);
// expected1.push_back(lc2);
// actual1 = fg1.find_constraints_and_remove("x");
// CHECK(fg1.size() == 0);
// CHECK(expected1.size() == actual1.size());
// vector<LinearConstraint::shared_ptr>::const_iterator exp1, act1;
// for(exp1=expected1.begin(), act1=actual1.begin(); act1 != actual1.end(); ++act1, ++exp1) {
// CHECK((*exp1)->equals(**act1));
// }
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
//{
// ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
//
// // eliminate the constraint
// ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
// CHECK(cg1->nrParents() == 1);
// CHECK(fg.nrFactors() == 1);
//
// // eliminate the induced constraint
// ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y");
// CHECK(cg2->nrParents() == 1);
// CHECK(fg.nrFactors() == 0);
//
// // eliminate the linear factor
// ConditionalGaussian::shared_ptr cg3 = fg.eliminateOne("z");
// CHECK(cg3->nrParents() == 0);
// CHECK(fg.size() == 0);
//
// // solve piecewise
// VectorConfig actual;
// Vector act_z = cg3->solve(actual);
// actual.insert("z", act_z);
// CHECK(assert_equal(act_z, Vector_(2, -4.0, 5.0), 1e-4));
// Vector act_y = cg2->solve(actual);
// actual.insert("y", act_y);
// CHECK(assert_equal(act_y, Vector_(2, -0.1, 0.4), 1e-4));
// Vector act_x = cg1->solve(actual);
// CHECK(assert_equal(act_x, Vector_(2, -2.0, 2.0), 1e-4));
//}
//
///* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, optimize_multi_constraint )
//{
// ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
// // solve the graph
// Ordering ord;
// ord.push_back("x");
// ord.push_back("y");
// ord.push_back("z");
//
// VectorConfig actual = fg.optimize(ord);
//
// // verify
// VectorConfig expected;
// expected.insert("x", Vector_(2, -2.0, 2.0));
// expected.insert("y", Vector_(2, -0.1, 0.4));
// expected.insert("z", Vector_(2, -4.0, 5.0));
// CHECK(expected.size() == actual.size());
// CHECK(assert_equal(expected["x"], actual["x"], 1e-4));
// CHECK(assert_equal(expected["y"], actual["y"], 1e-4));
// CHECK(assert_equal(expected["z"], actual["z"], 1e-4));
//}
/* ************************************************************************* */
// OLD TESTS - should be ported into the new structure when possible
/* ************************************************************************* */
/* ************************************************************************* */
//TEST( ConstrainedLinearFactorGraph, basic )
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
//
// // expected equality factor
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0"));
//
// // expected normal linear factor
// Matrix A21(2,2);
// A21(0,0) = -10 ; A21(0,1) = 0;
// A21(1,0) = 0 ; A21(1,1) = -10;
//
// Matrix A22(2,2);
// A22(0,0) = 10 ; A22(0,1) = 0;
// A22(1,0) = 0 ; A22(1,1) = 10;
//
// Vector b(2);
// b(0) = 20 ; b(1) = 30;
//
// LinearFactor::shared_ptr f2(new LinearFactor("x0", A21, "x1", A22, b));
//
// CHECK(f2->equals(*(fg[0])));
// CHECK(f1->equals(*(fg.eq_at(0))));
//}
//TEST ( ConstrainedLinearFactorGraph, copy )
//{
// LinearFactorGraph lfg = createLinearFactorGraph();
// LinearFactor::shared_ptr f1 = lfg[0];
// LinearFactor::shared_ptr f2 = lfg[1];
// LinearFactor::shared_ptr f3 = lfg[2];
// LinearFactor::shared_ptr f4 = lfg[3];
//
// ConstrainedLinearFactorGraph actual(lfg);
//
// ConstrainedLinearFactorGraph expected;
// expected.push_back(f1);
// expected.push_back(f2);
// expected.push_back(f3);
// expected.push_back(f4);
//
// CHECK(actual.equals(expected));
//}
//
//TEST( ConstrainedLinearFactorGraph, equals )
//{
// // basic equality test
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph();
// CHECK( fg.equals(fg2) );
//
// // ensuring that equality factors are compared
// LinearFactor::shared_ptr f2 = fg[0]; // get a linear factor from existing graph
// ConstrainedLinearFactorGraph fg3;
// fg3.push_back(f2);
// CHECK( !fg3.equals(fg) );
//}
//
//TEST( ConstrainedLinearFactorGraph, size )
//{
// LinearFactorGraph lfg = createLinearFactorGraph();
// ConstrainedLinearFactorGraph fg1(lfg);
//
// CHECK(fg1.size() == lfg.size());
//
// ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph();
//
// CHECK(fg2.size() == 2);
//}
//
//TEST( ConstrainedLinearFactorGraph, is_constrained )
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
//
// CHECK(fg.is_constrained("x0"));
// CHECK(!fg.is_constrained("x1"));
//}
//
//TEST( ConstrainedLinearFactorGraph, optimize )
//{
// ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph();
// ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph();
//
// VectorConfig expected = createConstrainedConfig();
//
// Ordering ord1;
// ord1.push_back("x0");
// ord1.push_back("x1");
//
// Ordering ord2;
// ord2.push_back("x1");
// ord2.push_back("x0");
//
// VectorConfig actual1 = fg1.optimize(ord1);
// VectorConfig actual2 = fg2.optimize(ord2);
//
// CHECK(actual1.equals(expected));
// CHECK(actual1.equals(actual2));
//}
//
//TEST (ConstrainedLinearFactorGraph, eliminate )
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// VectorConfig c = createConstrainedConfig();
//
// Ordering ord1;
// ord1.push_back("x0");
// ord1.push_back("x1");
//
// ConstrainedGaussianBayesNet::shared_ptr actual = fg.eliminate(ord1);
//
// // create an expected bayes net
// ConstrainedGaussianBayesNet::shared_ptr expected(new ConstrainedGaussianBayesNet);
//
// ConstrainedConditionalGaussian::shared_ptr d(new ConstrainedConditionalGaussian);//(c["x0"], "x0"));
// expected->insert_df("x0", d);
//
// Matrix A = eye(2);
// double sigma = 0.1;
// Vector dv = c["x1"];
// ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(dv/sigma, A/sigma));
// expected->insert("x1", cg);
//
// CHECK(actual->equals(*expected));
//}
//
//TEST (ConstrainedLinearFactorGraph, baseline_optimize)
//{
// // tests performance when there are no equality factors in the graph
// LinearFactorGraph lfg = createLinearFactorGraph();
// ConstrainedLinearFactorGraph clfg(lfg); // copy in the linear factor graph
//
// Ordering ord;
// ord.push_back("l1");
// ord.push_back("x1");
// ord.push_back("x2");
//
// VectorConfig actual = clfg.optimize(ord);
//
// VectorConfig expected = lfg.optimize(ord); // should be identical to regular lfg optimize
//
// CHECK(actual.equals(expected));
//}
//
//TEST (ConstrainedLinearFactorGraph, baseline_eliminate_one )
//{
// LinearFactorGraph fg = createLinearFactorGraph();
// ConstrainedLinearFactorGraph cfg(fg);
//
// ConditionalGaussian::shared_ptr actual = cfg.eliminate_one("x1");
//
// // create expected Conditional Gaussian
// Matrix R11 = Matrix_(2,2,
// 15.0, 00.0,
// 00.0, 15.0
// );
// Matrix S12 = Matrix_(2,2,
// -1.66667, 0.00,
// +0.00,-1.66667
// );
// Matrix S13 = Matrix_(2,2,
// -6.66667, 0.00,
// +0.00,-6.66667
// );
// Vector d(2); d(0) = -2; d(1) = -1.0/3.0;
// ConditionalGaussian expected(d,R11,"l1",S12,"x2",S13);
//
// CHECK( actual->equals(expected) );
//}
//
//TEST (ConstrainedLinearFactorGraph, eliminate_constraint)
//{
//// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
//// ConstrainedConditionalGaussian::shared_ptr actual = fg.eliminate_constraint("x0");
////
//// VectorConfig c = createConstrainedConfig();
//// ConstrainedConditionalGaussian::shared_ptr expected(new ConstrainedConditionalGaussian);//(c["x0"], "x0"));
////
//// CHECK(assert_equal(*actual, *expected)); // check output for correct delta function
////
//// CHECK(fg.size() == 1); // check size
////
//// ConstrainedLinearFactorGraph::eq_const_iterator eit = fg.eq_begin();
//// CHECK(eit == fg.eq_end()); // ensure no remaining equality factors
////
//// // verify the remaining factor - should be a unary factor on x1
//// ConstrainedLinearFactorGraph::const_iterator it = fg.begin();
//// LinearFactor::shared_ptr factor_actual = *it;
////
//// CHECK(factor_actual->size() == 1);
//}
//
//TEST (ConstrainedLinearFactorGraph, constraintCombineAndEliminate )
//{
// // create a set of factors
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// LinearConstraint::shared_ptr eq = fg.eq_at(0);
// LinearFactor::shared_ptr f1 = fg[0];
//
// // make a joint linear factor
// set<LinearFactor::shared_ptr> f1_set;
// f1_set.insert(f1);
// boost::shared_ptr<LinearFactor> joined(new LinearFactor(f1_set));
//
// // create a sample graph
// ConstrainedLinearFactorGraph graph;
//
// // combine linear factor and eliminate
// graph.constraintCombineAndEliminate(*eq, *joined);
//
// // verify structure
// CHECK(graph.size() == 1); // will have only one factor
// LinearFactor::shared_ptr actual = graph[0];
// CHECK(actual->size() == 1); // remaining factor will be unary
//
// // verify values
// VectorConfig c = createConstrainedConfig();
// Vector exp_v = c["x1"];
// Matrix A = actual->get_A("x1");
// Vector b = actual->get_b();
// Vector act_v = backsubstitution(A, b);
// CHECK(assert_equal(act_v, exp_v));
//}
//
//TEST (ConstrainedLinearFactorGraph, extract_eq)
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// LinearConstraint::shared_ptr actual = fg.extract_eq("x0");
//
// Vector v1(2); v1(0)=1.;v1(1)=2.;
// LinearConstraint::shared_ptr expected(new LinearConstraint(v1, "x0"));
//
// // verify output
// CHECK(assert_equal(*actual, *expected));
//
// // verify removal
// ConstrainedLinearFactorGraph::eq_const_iterator it = fg.eq_begin();
// CHECK(it == fg.eq_end());
//
// // verify full size
// CHECK(fg.size() == 1);
//}
//
//TEST( ConstrainedLinearFactorGraph, GET_ORDERING)
//{
// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph();
// Ordering ord = fg.getOrdering();
// CHECK(ord[0] == string("x0"));
// CHECK(ord[1] == string("x1"));
//}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

83
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

@ -1,83 +0,0 @@
/*
* testConstrainedNonlinearFactorGraph.cpp
*
* Created on: Aug 10, 2009
* Author: Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "FactorGraph-inl.h"
#include "ConstrainedNonlinearFactorGraph.h"
#include "smallExample.h"
using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<VectorConfig> > shared;
typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<VectorConfig>,VectorConfig> TestGraph;
//TEST( TestGraph, equals )
//{
// TestGraph fg = createConstrainedNonlinearFactorGraph();
// TestGraph fg2 = createConstrainedNonlinearFactorGraph();
// CHECK( fg.equals(fg2) );
//}
//
//TEST( TestGraph, copy )
//{
// ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
// TestGraph actual(nfg);
//
// shared f1 = nfg[0];
// shared f2 = nfg[1];
// shared f3 = nfg[2];
// shared f4 = nfg[3];
//
// TestGraph expected;
// expected.push_back(f1);
// expected.push_back(f2);
// expected.push_back(f3);
// expected.push_back(f4);
//
// CHECK(actual.equals(expected));
//}
//
//TEST( TestGraph, baseline )
//{
// // use existing examples
// ExampleNonlinearFactorGraph nfg = createNonlinearFactorGraph();
// TestGraph cfg(nfg);
//
// VectorConfig initial = createNoisyConfig();
// ConstrainedLinearFactorGraph linearized = cfg.linearize(initial);
// LinearFactorGraph lfg = createLinearFactorGraph();
// ConstrainedLinearFactorGraph expected(lfg);
//
// CHECK(expected.equals(linearized));
//}
//
///*
//TEST( TestGraph, convert )
//{
// ExampleNonlinearFactorGraph expected = createNonlinearFactorGraph();
// TestGraph cfg(expected);
// ExampleNonlinearFactorGraph actual = cfg.convert();
// CHECK(actual.equals(expected));
//}
//*/
//
//TEST( TestGraph, linearize_and_solve )
//{
// TestGraph nfg = createConstrainedNonlinearFactorGraph();
// VectorConfig lin = createConstrainedLinConfig();
// ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin);
// VectorConfig actual = actual_lfg.optimize(actual_lfg.getOrdering());
//
// VectorConfig expected = createConstrainedCorrectDelta();
// CHECK(actual.equals(expected));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

300
cpp/testLinearConstraint.cpp
View File

@ -1,300 +0,0 @@
/**
* @file testLinearConstraint.cpp
* @brief Tests for linear constraints
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "LinearConstraint.h"
#include "smallExample.h"
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST ( LinearConstraint, basic_unary )
{
// create an initialized factor with a unary factor
Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0";
LinearConstraint factor(v, key);
// get the data back out of it
CHECK(assert_equal(v, factor.get_b()));
Matrix expected = eye(2);
CHECK(assert_equal(expected, factor.get_A("x0")));
}
/* ************************************************************************* */
TEST( LinearConstraint, basic_binary )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearConstraint lc("x1", A1, "x2", A2, b);
// verify contents
CHECK( assert_equal(A1, lc.get_A("x1")));
CHECK( assert_equal(A2, lc.get_A("x2")));
CHECK( assert_equal(b, lc.get_b()));
}
/* ************************************************************************* */
TEST( LinearConstraint, basic_arbitrary )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Matrix A3(2,2);
A3(0,0) = 10.0 ; A3(0,1) = 7.0;
A3(1,0) = 7.0 ; A3(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
// build a map
map<string, Matrix> matrices;
matrices.insert(make_pair("x1", A1));
matrices.insert(make_pair("x2", A2));
matrices.insert(make_pair("x3", A3));
LinearConstraint lc(matrices, b);
// verify contents
CHECK( assert_equal(A1, lc.get_A("x1")));
CHECK( assert_equal(A2, lc.get_A("x2")));
CHECK( assert_equal(A3, lc.get_A("x3")));
CHECK( assert_equal(b, lc.get_b()));
}
/* ************************************************************************* */
TEST ( LinearConstraint, size )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Matrix A3(2,2);
A3(0,0) = 10.0 ; A3(0,1) = 7.0;
A3(1,0) = 7.0 ; A3(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
// build some constraints
LinearConstraint lc1(b, "x1");
LinearConstraint lc2("x1", A1, "x2", A2, b);
map<string, Matrix> matrices;
matrices.insert(make_pair("x1", A1));
matrices.insert(make_pair("x2", A2));
matrices.insert(make_pair("x3", A3));
LinearConstraint lc3(matrices, b);
CHECK(lc1.size() == 1);
CHECK(lc2.size() == 2);
CHECK(lc3.size() == 3);
}
/* ************************************************************************* */
TEST ( LinearConstraint, equals )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
Vector b2 = Vector_(2, 0.0, 0.0);
LinearConstraint lc1("x1", A1, "x2", A2, b);
LinearConstraint lc2("x1", A1, "x2", A2, b);
LinearConstraint lc3("x1", A1, "x2", A2, b2);
// check for basic equality
CHECK(lc1.equals(lc2));
CHECK(lc2.equals(lc1));
CHECK(!lc1.equals(lc3));
}
/* ************************************************************************* */
TEST ( LinearConstraint, eliminate1 )
{
// construct a linear constraint
Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0";
LinearConstraint lc(v, key);
// eliminate it to get a constrained conditional gaussian
ConstrainedConditionalGaussian::shared_ptr ccg = lc.eliminate(key);
// solve the ccg to get a value
VectorConfig fg;
CHECK(assert_equal(ccg->solve(fg), v));
}
/* ************************************************************************* */
TEST ( LinearConstraint, eliminate2 )
{
// Construct a linear constraint
// RHS
Vector b(2); b(0)=3.0; b(1)=4.0;
// A1 - invertible
Matrix A1(2,2);
A1(0,0) = 1.0 ; A1(0,1) = 2.0;
A1(1,0) = 2.0 ; A1(1,1) = 1.0;
// A2 - not invertible - solve will throw an exception
Matrix A2(2,2);
A2(0,0) = 1.0 ; A2(0,1) = 2.0;
A2(1,0) = 2.0 ; A2(1,1) = 4.0;
LinearConstraint lc("x", A1, "y", A2, b);
Vector y = Vector_(2, 1.0, 2.0);
VectorConfig fg1;
fg1.insert("y", y);
Vector expected = Vector_(2, -3.3333, 0.6667);
// eliminate x for basic check
ConstrainedConditionalGaussian::shared_ptr actual = lc.eliminate("x");
CHECK(assert_equal(expected, actual->solve(fg1), 1e-4));
// eliminate y to test thrown error
VectorConfig fg2;
fg2.insert("x", expected);
actual = lc.eliminate("y");
try {
Vector output = actual->solve(fg2);
CHECK(false);
} catch (...) {
CHECK(true);
}
}
/* ************************************************************************* */
TEST ( LinearConstraint, involves )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearConstraint lc("x1", A1, "x2", A2, b);
CHECK(lc.involves("x1"));
CHECK(lc.involves("x2"));
CHECK(!lc.involves("x3"));
}
/* ************************************************************************* */
TEST ( LinearConstraint, keys )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearConstraint lc("x1", A1, "x2", A2, b);
list<string> actual = lc.keys();
list<string> expected;
expected.push_back("x1");
expected.push_back("x2");
CHECK(actual == expected);
}
/* ************************************************************************* */
TEST ( LinearConstraint, combine )
{
// constraint 1
Matrix A11(2,2);
A11(0,0) = 1.0 ; A11(0,1) = 2.0;
A11(1,0) = 2.0 ; A11(1,1) = 1.0;
Matrix A12(2,2);
A12(0,0) = 10.0 ; A12(0,1) = 0.0;
A12(1,0) = 0.0 ; A12(1,1) = 10.0;
Vector b1(2);
b1(0) = 1.0; b1(1) = 2.0;
LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
// constraint 2
Matrix A21(2,2);
A21(0,0) = 3.0 ; A21(0,1) = 4.0;
A21(1,0) = -1.0 ; A21(1,1) = -2.0;
Matrix A22(2,2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0;
Vector b2(2);
b2(0) = 3.0; b2(1) = 4.0;
LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
// combine
set<LinearConstraint::shared_ptr> constraints;
constraints.insert(lc1);
constraints.insert(lc2);
LinearConstraint::shared_ptr actual = combineConstraints(constraints);
// expected
Matrix A1 = A11 + A21;
Matrix A2 = A12;
Matrix A3 = A22;
Vector b = b1 + b2;
map<string, Matrix> blocks;
blocks.insert(make_pair("x", A1));
blocks.insert(make_pair("y", A2));
blocks.insert(make_pair("z", A3));
LinearConstraint expected(blocks, b);
// verify
CHECK(actual->equals(expected));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */