diff --git a/cpp/ConstrainedConditionalGaussian.cpp b/cpp/ConstrainedConditionalGaussian.cpp deleted file mode 100644 index ff7e90c9b..000000000 --- a/cpp/ConstrainedConditionalGaussian.cpp +++ /dev/null @@ -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 -#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& 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::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 (A, b); - return Vector_(b); - - //TODO: Handle overdetermined case - - //TODO: Handle underdetermined case - -} diff --git a/cpp/ConstrainedConditionalGaussian.h b/cpp/ConstrainedConditionalGaussian.h deleted file mode 100644 index 02f3ac1f2..000000000 --- a/cpp/ConstrainedConditionalGaussian.h +++ /dev/null @@ -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 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& 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; - -}; - -} diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp deleted file mode 100644 index 2b2d78e37..000000000 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ /dev/null @@ -1,268 +0,0 @@ -/** - * @file ConstrainedLinearFactorGraph.cpp - * @author Alex Cunningham - */ - -#include -#include -#include -#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 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 separator = findAndRemoveFactors(key); - BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) { - // store the block matrices - map blocks; - - // get the A1 term and reconstruct new_factor without the eliminated block - Matrix A1 = factor->get_A(key); - list 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 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 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& 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& 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 updatee_keys = updatee->keys(key); - map 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 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 -ConstrainedLinearFactorGraph::find_constraints_and_remove(const string& key) -{ - vector 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; -} - -} diff --git a/cpp/ConstrainedLinearFactorGraph.h b/cpp/ConstrainedLinearFactorGraph.h deleted file mode 100644 index 057bd03a1..000000000 --- a/cpp/ConstrainedLinearFactorGraph.h +++ /dev/null @@ -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 constraints_; /// collection of equality factors - -public: - // iterators for equality constraints - same interface as linear factors - typedef std::vector::const_iterator constraint_const_iterator; - typedef std::vector::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& 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& 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 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_ */ diff --git a/cpp/ConstrainedNonlinearFactorGraph.cpp b/cpp/ConstrainedNonlinearFactorGraph.cpp deleted file mode 100644 index e68470523..000000000 --- a/cpp/ConstrainedNonlinearFactorGraph.cpp +++ /dev/null @@ -1,9 +0,0 @@ -/* - * ConstrainedNonlinearFactorGraph.cpp - * - * Created on: Aug 10, 2009 - * Author: alexgc - */ - -#include "ConstrainedNonlinearFactorGraph.h" - diff --git a/cpp/ConstrainedNonlinearFactorGraph.h b/cpp/ConstrainedNonlinearFactorGraph.h deleted file mode 100644 index 4c7468057..000000000 --- a/cpp/ConstrainedNonlinearFactorGraph.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * ConstrainedNonlinearFactorGraph.h - * - * Created on: Aug 10, 2009 - * Author: alexgc - */ - -#ifndef CONSTRAINEDNONLINEARFACTORGRAPH_H_ -#define CONSTRAINEDNONLINEARFACTORGRAPH_H_ - -#include -#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 ConstrainedNonlinearFactorGraph: public FactorGraph { -protected: - /** collection of equality factors */ - std::vector eq_factors; - -public: - // iterators over equality factors - typedef std::vector::const_iterator eq_const_iterator; - typedef std::vector::iterator eq_iterator; - - /** - * Default constructor - */ - ConstrainedNonlinearFactorGraph() { - } - - /** - * Copy constructor from regular NLFGs - */ - ConstrainedNonlinearFactorGraph(const NonlinearFactorGraph& nfg) : - FactorGraph (nfg) { - } - - typedef typename boost::shared_ptr shared_factor; - typedef typename std::vector::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::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_ */ diff --git a/cpp/LinearConstraint.cpp b/cpp/LinearConstraint.cpp deleted file mode 100644 index 08908415c..000000000 --- a/cpp/LinearConstraint.cpp +++ /dev/null @@ -1,129 +0,0 @@ -/** - * @file LinearConstraint.cpp - * @author Alex Cunningham - */ - -#include -#include -#include -#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& 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 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 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 LinearConstraint::keys(const std::string& key) const { - list ret; - pair p; - BOOST_FOREACH(p, As) { - if (p.first != key) - ret.push_back(p.first); - } - return ret; -} - -LinearConstraint::shared_ptr combineConstraints( - const set& constraints) { - map 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; -} - -} diff --git a/cpp/LinearConstraint.h b/cpp/LinearConstraint.h deleted file mode 100644 index ea5b411a8..000000000 --- a/cpp/LinearConstraint.h +++ /dev/null @@ -1,138 +0,0 @@ -/** - * @file LinearConstraint.h - * @brief Class that implements linear equality constraints - * @author Alex Cunningham - */ - -#ifndef EQUALITYFACTOR_H_ -#define EQUALITYFACTOR_H_ - -#include -#include -#include "Matrix.h" -#include "ConstrainedConditionalGaussian.h" - -namespace gtsam { - -/** - * Linear constraints are similar to factors in structure, but represent - * a different problem - */ -class LinearConstraint: Testable { -public: - - typedef boost::shared_ptr shared_ptr; - typedef std::map::iterator iterator; - typedef std::map::const_iterator const_iterator; - -protected: - std::map 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& 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 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& constraints); -}; - -/** - * Combines constraints into one constraint - */ -LinearConstraint::shared_ptr combineConstraints(const std::set& constraints); - -} - -#endif /* EQUALITYFACTOR_H_ */ diff --git a/cpp/testConstrainedConditionalGaussian.cpp b/cpp/testConstrainedConditionalGaussian.cpp deleted file mode 100644 index 4396d708a..000000000 --- a/cpp/testConstrainedConditionalGaussian.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/** - * @file testConstrainedConditionalGaussian.cpp - * @brief tests for constrained conditional gaussians - * @author Alex Cunningham - */ - - -#include -#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);} -/* ************************************************************************* */ - diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp deleted file mode 100644 index 306657c20..000000000 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ /dev/null @@ -1,561 +0,0 @@ -/** - * @file testConstrainedLinearFactorGraph.cpp - * @author Alex Cunningham - */ - -#include -#include -#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 actual1 = fg1.find_constraints_and_remove("y"); -// CHECK(fg1.size() == 2); -// CHECK(actual1.size() == 1); -// CHECK((*actual1.begin())->equals(*lc1)); -// -// vector 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 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 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::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 f1_set; -// f1_set.insert(f1); -// boost::shared_ptr 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); -} -/* ************************************************************************* */ - diff --git a/cpp/testConstrainedNonlinearFactorGraph.cpp b/cpp/testConstrainedNonlinearFactorGraph.cpp deleted file mode 100644 index feddfc447..000000000 --- a/cpp/testConstrainedNonlinearFactorGraph.cpp +++ /dev/null @@ -1,83 +0,0 @@ -/* - * testConstrainedNonlinearFactorGraph.cpp - * - * Created on: Aug 10, 2009 - * Author: Alex Cunningham - */ - - -#include -#include "FactorGraph-inl.h" -#include "ConstrainedNonlinearFactorGraph.h" -#include "smallExample.h" - -using namespace gtsam; - -typedef boost::shared_ptr > shared; -typedef ConstrainedNonlinearFactorGraph,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);} -/* ************************************************************************* */ - diff --git a/cpp/testLinearConstraint.cpp b/cpp/testLinearConstraint.cpp deleted file mode 100644 index c828f088c..000000000 --- a/cpp/testLinearConstraint.cpp +++ /dev/null @@ -1,300 +0,0 @@ -/** - * @file testLinearConstraint.cpp - * @brief Tests for linear constraints - * @author Alex Cunningham - */ - - -#include -#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 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 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 actual = lc.keys(); - - list 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 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 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);} -/* ************************************************************************* */ -