diff --git a/.cproject b/.cproject index bf6c64a8b..332430cf6 100644 --- a/.cproject +++ b/.cproject @@ -300,6 +300,7 @@ make + check true true @@ -307,7 +308,6 @@ make - testSimpleCamera.run true true @@ -323,6 +323,7 @@ make + testVSLAMFactor.run true true @@ -330,7 +331,6 @@ make - testCalibratedCamera.run true true @@ -338,6 +338,7 @@ make + testConditionalGaussian.run true true @@ -345,7 +346,6 @@ make - testPose2.run true true @@ -361,7 +361,6 @@ make - testRot3.run true true @@ -369,6 +368,7 @@ make + testNonlinearOptimizer.run true true @@ -376,7 +376,6 @@ make - testLinearFactor.run true true @@ -384,7 +383,6 @@ make - testConstrainedNonlinearFactorGraph.run true true @@ -392,14 +390,22 @@ make - testLinearFactorGraph.run true true true + +make + +testLinearConstraint.run +true +true +true + make + testNonlinearFactorGraph.run true true @@ -407,12 +413,27 @@ make - testPose3.run true true true + +make + +testConstrainedConditionalGaussian.run +true +true +true + + +make + +testConstrainedLinearFactorGraph.run +true +true +true + make install @@ -429,6 +450,7 @@ make + check true true diff --git a/cpp/ChordalBayesNet.h b/cpp/ChordalBayesNet.h index 310a49b1c..f1a3bde4c 100644 --- a/cpp/ChordalBayesNet.h +++ b/cpp/ChordalBayesNet.h @@ -81,6 +81,9 @@ public: /** check equality */ bool equals(const ChordalBayesNet& cbn) const; + /** size is the number of nodes */ + size_t size() const {return nodes.size();} + /** * Return (dense) upper-triangular matrix representation */ diff --git a/cpp/ConditionalGaussian.cpp b/cpp/ConditionalGaussian.cpp index e681ba500..e1647daed 100644 --- a/cpp/ConditionalGaussian.cpp +++ b/cpp/ConditionalGaussian.cpp @@ -40,6 +40,14 @@ ConditionalGaussian::ConditionalGaussian(Vector d, parents_.insert(make_pair(name2, T)); } +/* ************************************************************************* */ +ConditionalGaussian::ConditionalGaussian(const Vector& d, + const Matrix& R, + const map& parents) + : R_(R), d_(d), parents_(parents) + { + } + /* ************************************************************************* */ void ConditionalGaussian::print(const string &s) const { diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h index 977f4710d..bfa236f61 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/ConditionalGaussian.h @@ -77,6 +77,14 @@ namespace gtsam { Matrix T ); + /** + * constructor with number of arbitrary parents + * |Rx+sum(Ai*xi)-d| + */ + ConditionalGaussian(const Vector& d, + const Matrix& R, + const std::map& parents); + /** deconstructor */ virtual ~ConditionalGaussian() {}; diff --git a/cpp/ConstrainedConditionalGaussian.cpp b/cpp/ConstrainedConditionalGaussian.cpp new file mode 100644 index 000000000..33de46b25 --- /dev/null +++ b/cpp/ConstrainedConditionalGaussian.cpp @@ -0,0 +1,80 @@ +/** + * @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() { + +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& v) : + ConditionalGaussian(v, eye(v.size())) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, + const Matrix& A) : + ConditionalGaussian(b, A) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, + const Matrix& A1, const std::string& parent, const Matrix& A2) : + ConditionalGaussian(b, A1, parent, A2) { +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Vector& b, + const Matrix& A1, const std::string& parentY, const Matrix& A2, + const std::string& parentZ, const Matrix& A3) +: ConditionalGaussian(b, A1, parentY, A2, parentZ, A3) +{ +} + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian(const Matrix& A1, + const std::map& parents, const Vector& b) +: ConditionalGaussian(b, A1, parents) +{ +} + + +ConstrainedConditionalGaussian::ConstrainedConditionalGaussian( + const ConstrainedConditionalGaussian& df) { +} + +Vector ConstrainedConditionalGaussian::solve(const FGConfig& 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 new file mode 100644 index 000000000..2227fa267 --- /dev/null +++ b/cpp/ConstrainedConditionalGaussian.h @@ -0,0 +1,107 @@ +/** + * @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 underconstraints the system, then ??? + */ +class ConstrainedConditionalGaussian: public ConditionalGaussian { + +public: + typedef boost::shared_ptr shared_ptr; + + /** + * Default Constructor + * Don't use this + */ + ConstrainedConditionalGaussian(); + + /** + * 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 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 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 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 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 Matrix& A1, + const std::map& parents, const Vector& b); + + /** + * Copy constructor + */ + ConstrainedConditionalGaussian(const ConstrainedConditionalGaussian& df); + + 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 FGConfig& x) const; + +}; + +} diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index 54e1de877..ddb81b30a 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -1,14 +1,11 @@ -/* - * ConstrainedLinearFactorGraph.cpp - * - * Created on: Aug 10, 2009 - * Author: alexgc +/** + * @file ConstrainedLinearFactorGraph.cpp + * @author Alex Cunningham */ #include #include "ConstrainedLinearFactorGraph.h" #include "FactorGraph-inl.h" // for getOrdering - using namespace std; // trick from some reading group @@ -20,27 +17,26 @@ ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph() { } -ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph(const LinearFactorGraph& lfg) -{ +ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph( + const LinearFactorGraph& lfg) { BOOST_FOREACH(LinearFactor::shared_ptr s, lfg) - { - push_back(s); + { push_back(s); } } ConstrainedLinearFactorGraph::~ConstrainedLinearFactorGraph() { } -void ConstrainedLinearFactorGraph::push_back_eq(EqualityFactor::shared_ptr factor) +void ConstrainedLinearFactorGraph::push_back_constraint(LinearConstraint::shared_ptr factor) { - eq_factors.push_back(factor); + constraints_.push_back(factor); } -bool ConstrainedLinearFactorGraph::involves_equality(const std::string& key) const +bool ConstrainedLinearFactorGraph::is_constrained(const std::string& key) const { - BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors) + BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_) { - if (e->get_key() == key) return true; + if (e->involves(key)) return true; } return false; } @@ -51,11 +47,11 @@ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double to if (p == NULL) return false; /** check equality factors */ - if (eq_factors.size() != p->eq_factors.size()) return false; - BOOST_FOREACH(EqualityFactor::shared_ptr ef1, eq_factors) + if (constraints_.size() != p->constraints_.size()) return false; + BOOST_FOREACH(LinearConstraint::shared_ptr ef1, constraints_) { bool contains = false; - BOOST_FOREACH(EqualityFactor::shared_ptr ef2, p->eq_factors) + BOOST_FOREACH(LinearConstraint::shared_ptr ef2, p->constraints_) if (ef1->equals(*ef2)) contains = true; if (!contains) return false; @@ -65,15 +61,16 @@ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double to return LinearFactorGraph::equals(fg, tol); } -ConstrainedChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering){ - ConstrainedChordalBayesNet::shared_ptr cbn (new ConstrainedChordalBayesNet()); +ChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) { + ChordalBayesNet::shared_ptr cbn (new ChordalBayesNet()); BOOST_FOREACH(string key, ordering) { - if (involves_equality(key)) // check whether this is an existing equality factor + // constraints take higher priority in elimination, so check if + // there are constraints first + if (is_constrained(key)) { - // check if eliminating an equality factor - DeltaFunction::shared_ptr d = eliminate_one_eq(key); - cbn->insert_df(key,d); + ConditionalGaussian::shared_ptr ccg = eliminate_constraint(key); + cbn->insert(key,ccg); } else { @@ -85,61 +82,71 @@ ConstrainedChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(c return cbn; } -DeltaFunction::shared_ptr ConstrainedLinearFactorGraph::eliminate_one_eq(const string& key) +ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::eliminate_constraint(const string& key) { - // extract the equality factor - also removes from graph - EqualityFactor::shared_ptr eqf = extract_eq(key); + // extract the constraint - in-place remove from graph + LinearConstraint::shared_ptr constraint = extract_constraint(key); - // remove all unary linear factors on this node - vector newfactors; - BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) - { - if (f->size() != 1 || !f->involves(key)) - { - newfactors.push_back(f); + // 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 + LinearFactorSet separator = find_factors_and_remove(key); + BOOST_FOREACH(LinearFactor::shared_ptr factor, separator) { + // reconstruct with a mutable factor + boost::shared_ptr new_factor(new MutableLinearFactor); + + // get T = A1*inv(C1) term + Matrix A1 = factor->get_A(key); + 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; + new_factor->insert(cur_key, -1 *Ai); + } else if (cur_key != key) { + Matrix Ai = factor->get_A(cur_key) - T*Ci; + new_factor->insert(cur_key, Ai); + } } + + // update RHS of updated factor + Vector 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); } - factors_ = newfactors; - // combine the linear factors connected to equality node - boost::shared_ptr joint_factor = combine_factors(key); - - // combine the joint factor with the equality factor and add factors to graph - if (joint_factor->size() > 0) - eq_combine_and_eliminate(*eqf, *joint_factor); - - // create the delta function - all delta function information contained in the equality factor - DeltaFunction::shared_ptr d = eqf->getDeltaFunction(); - - return d; + return ccg; } -EqualityFactor::shared_ptr ConstrainedLinearFactorGraph::extract_eq(const string& key) +LinearConstraint::shared_ptr ConstrainedLinearFactorGraph::extract_constraint(const string& key) { - EqualityFactor::shared_ptr ret; - vector new_vec; - BOOST_FOREACH(EqualityFactor::shared_ptr eq, eq_factors) + LinearConstraint::shared_ptr ret; + bool found_key = false; + vector new_vec; + BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_) { - if (eq->get_key() == key) - ret = eq; + if (constraint->involves(key)) { + ret = constraint; + found_key = true; + } else - new_vec.push_back(eq); + new_vec.push_back(constraint); } - eq_factors = new_vec; + constraints_ = new_vec; + if (!found_key) + throw invalid_argument("No constraint connected to node: " + key); return ret; } -FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering){ - if (eq_factors.size() == 0) - { - // use default optimization - return LinearFactorGraph::optimize(ordering); - } - - // eliminate all nodes in the given ordering -> chordal Bayes net - ConstrainedChordalBayesNet::shared_ptr cbn = eliminate(ordering); - - // calculate new configuration (using backsubstitution) +FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { + ChordalBayesNet::shared_ptr cbn = eliminate(ordering); boost::shared_ptr newConfig = cbn->optimize(); return *newConfig; } @@ -151,63 +158,18 @@ void ConstrainedLinearFactorGraph::print(const std::string& s) const { f->print(); } - BOOST_FOREACH(EqualityFactor::shared_ptr f, eq_factors) + BOOST_FOREACH(LinearConstraint::shared_ptr f, constraints_) { f->print(); } } -void ConstrainedLinearFactorGraph::eq_combine_and_eliminate( - const EqualityFactor& eqf, const MutableLinearFactor& joint_factor) -{ - // start empty remaining factor to be returned - boost::shared_ptr lf(new MutableLinearFactor); - - // get the value of the target variable (x) - Vector x = eqf.get_value(); - - // get the RHS vector - Vector b = joint_factor.get_b(); - - // get key - string key = eqf.get_key(); - - // get the Ax matrix - Matrix Ax = joint_factor.get_A(key); - - // calculate new b - b -= Ax * x; - - // reassemble new factor - lf->set_b(b); - string j; Matrix A; - LinearFactor::const_iterator it = joint_factor.begin(); - for (; it != joint_factor.end(); it++) { - j = it->first; - A = it->second; - if (j != key) lf->insert(j, A); - } - - // insert factor - push_back(lf); -} - Ordering ConstrainedLinearFactorGraph::getOrdering() const { Ordering ord = LinearFactorGraph::getOrdering(); - BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors) - ord.push_back(e->get_key()); + // BOOST_FOREACH(LinearConstraint::shared_ptr e, constraints_) + // ord.push_back(e->get_key()); return ord; } -LinearFactorGraph ConstrainedLinearFactorGraph::convert() const -{ - LinearFactorGraph ret; - BOOST_FOREACH(LinearFactor::shared_ptr f, factors_) - ret.push_back(f); - return ret; -} - - - } diff --git a/cpp/ConstrainedLinearFactorGraph.h b/cpp/ConstrainedLinearFactorGraph.h index 518a89ffd..46e9fe02a 100644 --- a/cpp/ConstrainedLinearFactorGraph.h +++ b/cpp/ConstrainedLinearFactorGraph.h @@ -1,31 +1,28 @@ -/* - * ConstrainedLinearFactorGraph.h - * - * Created on: Aug 10, 2009 - * Author: alexgc +/** + * @file ConstrainedLinearFactorGraph.h + * @brief A modified version of LinearFactorGraph that can handle + * linear constraints. + * @author Alex Cunningham */ #ifndef CONSTRAINEDLINEARFACTORGRAPH_H_ #define CONSTRAINEDLINEARFACTORGRAPH_H_ -#include -#include - #include "LinearFactorGraph.h" -#include "EqualityFactor.h" -#include "ConstrainedChordalBayesNet.h" +#include "ChordalBayesNet.h" +#include "LinearConstraint.h" namespace gtsam { class ConstrainedLinearFactorGraph: public LinearFactorGraph { protected: - std::vector eq_factors; /// collection of equality factors + std::vector constraints_; /// collection of equality factors public: // iterators for equality constraints - same interface as linear factors - typedef std::vector::const_iterator eq_const_iterator; - typedef std::vector::iterator eq_iterator; + typedef std::vector::const_iterator constraint_const_iterator; + typedef std::vector::iterator constraint_iterator; public: /** @@ -40,39 +37,54 @@ public: virtual ~ConstrainedLinearFactorGraph(); - void push_back_eq(EqualityFactor::shared_ptr factor); + /** + * 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); // Additional STL-like functions for Equality Factors - - EqualityFactor::shared_ptr eq_at(const size_t i) const {return eq_factors.at(i);} + LinearConstraint::shared_ptr constraint_at(const size_t i) const {return constraints_.at(i);} /** return the iterator pointing to the first equality factor */ - eq_const_iterator eq_begin() const { - return eq_factors.begin(); + constraint_const_iterator constraint_begin() const { + return constraints_.begin(); } /** return the iterator pointing to the last factor */ - eq_const_iterator eq_end() const { - return eq_factors.end(); + constraint_const_iterator constraint_end() const { + return constraints_.end(); } /** clear the factor graph - re-implemented to include equality factors */ void clear(){ factors_.clear(); - eq_factors.clear(); + constraints_.clear(); } /** size - reimplemented to include the equality factors_ */ - inline size_t size() const { return factors_.size() + eq_factors.size(); } + 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 + * 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 */ - ConstrainedChordalBayesNet::shared_ptr eliminate(const Ordering& ordering); + ChordalBayesNet::shared_ptr eliminate(const Ordering& ordering); + + /** + * Eliminates a node with a constraint on it + * Other factors have a change of variables performed via Schur complement to remove the + * eliminated node. + * FIXME: currently will not handle multiple constraints on the same node + */ + ConstrainedConditionalGaussian::shared_ptr eliminate_constraint(const std::string& key); /** * optimize a linear factor graph @@ -81,18 +93,9 @@ public: FGConfig optimize(const Ordering& ordering); /** - * eliminate one node yielding a DeltaFunction - * Eliminates the factors from the factor graph through find_factors_and_remove - * and adds a new factor to the factor graph - * - * Only eliminates nodes *with* equality factors + * Determines if a node has any constraints attached to it */ - DeltaFunction::shared_ptr eliminate_one_eq(const std::string& key); - - /** - * Determines if a node has any equality factors connected to it - */ - bool involves_equality(const std::string& key) const; + bool is_constrained(const std::string& key) const; /** * Prints the contents of the factor graph with optional name string @@ -101,15 +104,9 @@ public: /** * Finds a matching equality constraint by key - assumed present - * Performs in-place removal of the equality constraint + * Performs in-place removal of the constraint */ - EqualityFactor::shared_ptr extract_eq(const std::string& key); - - /** - * Combines an equality factor with a joined linear factor - * Executes in place, and will add new factors back to the graph - */ - void eq_combine_and_eliminate(const EqualityFactor& eqf, const MutableLinearFactor& joint_factor); + LinearConstraint::shared_ptr extract_constraint(const std::string& key); /** * This function returns the best ordering for this linear factor @@ -117,13 +114,6 @@ public: * of the equality factors eliminated first */ Ordering getOrdering() const; - - /** - * Converts the graph into a linear factor graph - * Removes all equality constraints - */ - LinearFactorGraph convert() const; - }; } diff --git a/cpp/ConstrainedNonlinearFactorGraph.h b/cpp/ConstrainedNonlinearFactorGraph.h index b11c0ae02..86454fae0 100644 --- a/cpp/ConstrainedNonlinearFactorGraph.h +++ b/cpp/ConstrainedNonlinearFactorGraph.h @@ -10,7 +10,7 @@ #include #include "NonlinearFactorGraph.h" -#include "EqualityFactor.h" +#include "LinearConstraint.h" #include "ConstrainedLinearFactorGraph.h" namespace gtsam { @@ -20,19 +20,19 @@ namespace gtsam { * * Templated on factor and configuration type. * TODO FD: this is totally wrong: it can only work if Config==FGConfig - * as EqualityFactor is only defined for FGConfig + * as LinearConstraint is only defined for FGConfig * 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; + std::vector eq_factors; public: // iterators over equality factors - typedef std::vector::const_iterator eq_const_iterator; - typedef std::vector::iterator eq_iterator; + typedef std::vector::const_iterator eq_const_iterator; + typedef std::vector::iterator eq_iterator; /** * Default constructor @@ -67,8 +67,8 @@ public: // 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++) { - EqualityFactor::shared_ptr eq = (*e_factor)->linearize(); - ret.push_back_eq(eq); +// LinearConstraint::shared_ptr eq = (*e_factor)->linearize(); +// ret.push_back_eq(eq); } return ret; @@ -84,7 +84,7 @@ public: /** * Insert a equality factor into the graph */ - void push_back_eq(const EqualityFactor::shared_ptr& eq) { + void push_back_eq(const LinearConstraint::shared_ptr& eq) { eq_factors.push_back(eq); } diff --git a/cpp/DeltaFunction.cpp b/cpp/DeltaFunction.cpp deleted file mode 100644 index 233595083..000000000 --- a/cpp/DeltaFunction.cpp +++ /dev/null @@ -1,60 +0,0 @@ -/* - * DeltaFunction.cpp - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - -#include -#include "DeltaFunction.h" - -namespace gtsam { - -using namespace std; - -DeltaFunction::DeltaFunction() { - // TODO Auto-generated constructor stub - -} - -DeltaFunction::DeltaFunction(const Vector& v, const std::string& id) -: value_(v), key_(id) -{ -} - -DeltaFunction::DeltaFunction(const DeltaFunction& df) -: boost::noncopyable(), value_(df.value_), key_(df.key_) -{ -} - -DeltaFunction::~DeltaFunction() { - // TODO Auto-generated destructor stub -} - -bool DeltaFunction::equals(const DeltaFunction &df) const -{ - return equal_with_abs_tol(value_, df.value_) && key_ == df.key_; -} - -void DeltaFunction::print() const -{ - cout << "DeltaFunction: [" << key_ << "]"; - gtsam::print(value_); - cout << endl; -} - -bool assert_equal(const DeltaFunction& actual, const DeltaFunction& expected, double tol) -{ - bool ret = actual.equals(expected); - if (!ret) - { - cout << "Not Equal!" << endl; - cout << " Actual:" << endl; - actual.print(); - cout << " Expected:" << endl; - expected.print(); - } - return ret; -} - -} diff --git a/cpp/DeltaFunction.h b/cpp/DeltaFunction.h deleted file mode 100644 index 12abd6129..000000000 --- a/cpp/DeltaFunction.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * DeltaFunction.h - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - -#ifndef DELTAFUNCTION_H_ -#define DELTAFUNCTION_H_ - -#include -#include -#include -#include "Vector.h" - -namespace gtsam { - -class DeltaFunction : boost::noncopyable { -protected: - Vector value_; /// location of the delta function - std::string key_; /// id of node with delta function - -public: - typedef boost::shared_ptr shared_ptr; - - /** - * Default Constructor - */ - DeltaFunction(); - - /** - * Constructor with initialization - */ - DeltaFunction(const Vector& value, const std::string& key); - - /** - * Copy constructor - */ - DeltaFunction(const DeltaFunction& df); - - virtual ~DeltaFunction(); - - /** - * basic get functions - */ - Vector get_value() const {return value_;} - std::string get_key() const {return key_;} - - - /** equals function */ - bool equals(const DeltaFunction &cg) const; - - /** basic print */ - void print() const; - -}; - -/** equals function for testing - prints if not equal */ -bool assert_equal(const DeltaFunction& actual, const DeltaFunction& expected, double tol=1e-9); - -} - -#endif /* DELTAFUNCTION_H_ */ diff --git a/cpp/EqualityFactor.cpp b/cpp/EqualityFactor.cpp deleted file mode 100644 index dd0e7c073..000000000 --- a/cpp/EqualityFactor.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/* - * EqualityFactor.cpp - * - * Created on: Aug 10, 2009 - * Author: alexgc - */ - -#include "EqualityFactor.h" -#include - -namespace gtsam { -using namespace std; - -EqualityFactor::EqualityFactor() -: key_(""), value_(Vector(0)) -{ -} - -EqualityFactor::EqualityFactor(const Vector& constraint, const std::string& id) -: key_(id), value_(constraint) -{ -} - -list EqualityFactor::keys() const { - list keys; - keys.push_back(key_); - return keys; -} - -double EqualityFactor::error(const FGConfig& c) const -{ - return 0.0; -} - -void EqualityFactor::print(const string& s) const -{ - cout << s << ": " << dump() << endl; -} - -bool EqualityFactor::equals(const EqualityFactor& f, double tol) const -{ - return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key(); -} - -string EqualityFactor::dump() const -{ - string ret = "[" + key_ + "] " + gtsam::dump(value_); - return ret; -} - -DeltaFunction::shared_ptr EqualityFactor::getDeltaFunction() const -{ - DeltaFunction::shared_ptr ret(new DeltaFunction(value_, key_)); - return ret; -} - -EqualityFactor::shared_ptr EqualityFactor::linearize() const -{ - EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value_.size()), key_)); - return ret; -} - -bool assert_equal(const EqualityFactor& actual, const EqualityFactor& expected, double tol) -{ - bool ret = actual.equals(expected, tol); - if (!ret) - { - cout << "Not Equal:" << endl; - actual.print("Actual"); - expected.print("Expected"); - } - return ret; -} - -} diff --git a/cpp/EqualityFactor.h b/cpp/EqualityFactor.h deleted file mode 100644 index e1a5ba05a..000000000 --- a/cpp/EqualityFactor.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * EqualityFactor.h - * - * Created on: Aug 10, 2009 - * Author: alexgc - */ - -#ifndef EQUALITYFACTOR_H_ -#define EQUALITYFACTOR_H_ - -#include "Factor.h" -#include "FGConfig.h" -#include "DeltaFunction.h" - -namespace gtsam { - -class EqualityFactor: public Factor { -public: - typedef boost::shared_ptr shared_ptr; - - -protected: - Vector value_; /// forces a variable be equal to this value - std::string key_; /// name of variable factor is attached to - -public: - /** - * Default constructor - */ - EqualityFactor(); - - /** - * Constructor with initializiation - * @param constraint the value that the variable node is defined as equal to - * @param key identifier for the variable node - */ - EqualityFactor(const Vector& constraint, const std::string& key); - - /** - * Default Destructor - */ - ~EqualityFactor() {} - - /** - * negative log probability - */ - double error(const FGConfig& c) const; - - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s="") const; - - /** - * equality up to tolerance - */ - bool equals(const EqualityFactor& f, double tol=1e-9) const; - - /** - * linearize - */ - EqualityFactor::shared_ptr linearize() const; - - /** - * returns a version of the factor as a string - */ - std::string dump() const; - - // get functions - std::string get_key() const {return key_;} - Vector get_value() const {return value_;} - - /** - * return keys in preferred order - */ - std::list keys() const; - - /** - * @return the number of nodes the factor connects - */ - std::size_t size() const {return 1;} - - /** - * Returns the corresponding delta function for elimination - */ - DeltaFunction::shared_ptr getDeltaFunction() const; -}; - -/** assert equals for testing - prints when not equal */ -bool assert_equal(const EqualityFactor& actual, const EqualityFactor& expected, double tol=1e-9); - -} - -#endif /* EQUALITYFACTOR_H_ */ diff --git a/cpp/LinearConstraint.cpp b/cpp/LinearConstraint.cpp new file mode 100644 index 000000000..b56f143b3 --- /dev/null +++ b/cpp/LinearConstraint.cpp @@ -0,0 +1,111 @@ +/* + * LinearConstraint.cpp + * + * Created on: Aug 10, 2009 + * Author: alexgc + */ + +#include +#include +#include "LinearConstraint.h" +#include "Matrix.h" + +namespace gtsam { +using namespace std; + +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) +{ +} + +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(A1, parents, b)); + return ccg; +} + +void LinearConstraint::print(const string& s) const { + cout << s << ": " << dump() << endl; +} + +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 (!(f_mat == p.second)) return false; + } + return true; +} + +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; +} + +string LinearConstraint::dump() const { + string ret; + return ret; +} + +bool assert_equal(const LinearConstraint& actual, + const LinearConstraint& expected, double tol) { + bool ret = actual.equals(expected, tol); + if (!ret) { + cout << "Not Equal:" << endl; + actual.print("Actual"); + expected.print("Expected"); + } + return ret; +} + +} diff --git a/cpp/LinearConstraint.h b/cpp/LinearConstraint.h new file mode 100644 index 000000000..d386f149c --- /dev/null +++ b/cpp/LinearConstraint.h @@ -0,0 +1,131 @@ +/* + * LinearConstraint.h + * + * Created on: Aug 10, 2009 + * Author: alexgc + */ + +#ifndef EQUALITYFACTOR_H_ +#define EQUALITYFACTOR_H_ + +#include +#include "Matrix.h" +#include "ConstrainedConditionalGaussian.h" + +namespace gtsam { + +/** + * Linear constraints are similar to factors in structure, but represent + * a different problem + */ +class LinearConstraint { +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() {} + + /** + * 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); + + /** + * 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; + + /** + * returns a version of the factor as a string + */ + std::string dump() const; + + /** 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();} +}; + +/** assert equals for testing - prints when not equal */ +bool assert_equal(const LinearConstraint& actual, const LinearConstraint& expected, double tol=1e-9); + +} + +#endif /* EQUALITYFACTOR_H_ */ diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index 5c2a32830..ebac8f8bf 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -17,8 +17,8 @@ using namespace std; #include "Ordering.h" #include "Matrix.h" #include "NonlinearFactor.h" -#include "EqualityFactor.h" -#include "DeltaFunction.h" +#include "LinearConstraint.h" +#include "ConstrainedConditionalGaussian.h" #include "smallExample.h" #include "Point2Prior.h" #include "Simulated2DOdometry.h" @@ -66,47 +66,44 @@ ExampleNonlinearFactorGraph createNonlinearFactorGraph() { } /* ************************************************************************* */ -ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph() -{ - ConstrainedLinearFactorGraph graph; - - // add an equality factor - Vector v1(2); v1(0)=1.;v1(1)=2.; - EqualityFactor::shared_ptr f1(new EqualityFactor(v1, "x0")); - graph.push_back_eq(f1); - - // add a normal linear factor - Matrix A21 = -1 * eye(2); - - Matrix A22 = eye(2); - - Vector b(2); - b(0) = 2 ; b(1) = 3; - - double sigma = 0.1; - LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma)); - graph.push_back(f2); - - - return graph; -} +//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph() +//{ +// ConstrainedLinearFactorGraph graph; +// +// // add an equality factor +// Vector v1(2); v1(0)=1.;v1(1)=2.; +// LinearConstraint::shared_ptr f1(new LinearConstraint(v1, "x0")); +// graph.push_back_eq(f1); +// +// // add a normal linear factor +// Matrix A21 = -1 * eye(2); +// +// Matrix A22 = eye(2); +// +// Vector b(2); +// b(0) = 2 ; b(1) = 3; +// +// double sigma = 0.1; +// LinearFactor::shared_ptr f2(new LinearFactor("x0", A21/sigma, "x1", A22/sigma, b/sigma)); +// graph.push_back(f2); +// return graph; +//} /* ************************************************************************* */ - ConstrainedNonlinearFactorGraph , FGConfig> createConstrainedNonlinearFactorGraph() { - ConstrainedNonlinearFactorGraph , FGConfig> graph; - FGConfig c = createConstrainedConfig(); - - // equality constraint for initial pose - EqualityFactor::shared_ptr f1(new EqualityFactor(c["x0"], "x0")); - graph.push_back_eq(f1); - - // odometry between x0 and x1 - double sigma = 0.1; - shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1")); - graph.push_back(f2); // TODO - - return graph; - } +// ConstrainedNonlinearFactorGraph , FGConfig> createConstrainedNonlinearFactorGraph() { +// ConstrainedNonlinearFactorGraph , FGConfig> graph; +// FGConfig c = createConstrainedConfig(); +// +// // equality constraint for initial pose +// LinearConstraint::shared_ptr f1(new LinearConstraint(c["x0"], "x0")); +// graph.push_back_eq(f1); +// +// // odometry between x0 and x1 +// double sigma = 0.1; +// shared f2(new Simulated2DOdometry(c["x1"] - c["x0"], sigma, "x0", "x1")); +// graph.push_back(f2); // TODO +// return graph; +// } /* ************************************************************************* */ FGConfig createConfig() @@ -139,46 +136,46 @@ FGConfig createNoisyConfig() { } /* ************************************************************************* */ -FGConfig createConstrainedConfig() -{ - FGConfig config; - - Vector x0(2); x0(0)=1.0; x0(1)=2.0; - config.insert("x0", x0); - - Vector x1(2); x1(0)=3.0; x1(1)=5.0; - config.insert("x1", x1); - - return config; -} +//FGConfig createConstrainedConfig() +//{ +// FGConfig config; +// +// Vector x0(2); x0(0)=1.0; x0(1)=2.0; +// config.insert("x0", x0); +// +// Vector x1(2); x1(0)=3.0; x1(1)=5.0; +// config.insert("x1", x1); +// +// return config; +//} /* ************************************************************************* */ -FGConfig createConstrainedLinConfig() -{ - FGConfig config; - - Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter - config.insert("x0", x0); - - Vector x1(2); x1(0)=2.3; x1(1)=5.3; - config.insert("x1", x1); - - return config; -} +//FGConfig createConstrainedLinConfig() +//{ +// FGConfig config; +// +// Vector x0(2); x0(0)=1.0; x0(1)=2.0; // value doesn't actually matter +// config.insert("x0", x0); +// +// Vector x1(2); x1(0)=2.3; x1(1)=5.3; +// config.insert("x1", x1); +// +// return config; +//} /* ************************************************************************* */ -FGConfig createConstrainedCorrectDelta() -{ - FGConfig config; - - Vector x0(2); x0(0)=0.; x0(1)=0.; - config.insert("x0", x0); - - Vector x1(2); x1(0)= 0.7; x1(1)= -0.3; - config.insert("x1", x1); - - return config; -} +//FGConfig createConstrainedCorrectDelta() +//{ +// FGConfig config; +// +// Vector x0(2); x0(0)=0.; x0(1)=0.; +// config.insert("x0", x0); +// +// Vector x1(2); x1(0)= 0.7; x1(1)= -0.3; +// config.insert("x1", x1); +// +// return config; +//} /* ************************************************************************* */ FGConfig createCorrectDelta() { @@ -293,24 +290,24 @@ ChordalBayesNet createSmallChordalBayesNet() } /* ************************************************************************* */ -ConstrainedChordalBayesNet createConstrainedChordalBayesNet() -{ - ConstrainedChordalBayesNet cbn; - FGConfig c = createConstrainedConfig(); - - // add regular conditional gaussian - no parent - Matrix R = eye(2); - Vector d = c["x1"]; - double sigma = 0.1; - ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); - cbn.insert("x1", f1); - - // add a delta function to the cbn - DeltaFunction::shared_ptr f2(new DeltaFunction(c["x0"], "x0")); - cbn.insert_df("x0", f2); - - return cbn; -} +//ConstrainedChordalBayesNet createConstrainedChordalBayesNet() +//{ +// ConstrainedChordalBayesNet cbn; +// FGConfig c = createConstrainedConfig(); +// +// // add regular conditional gaussian - no parent +// Matrix R = eye(2); +// Vector d = c["x1"]; +// double sigma = 0.1; +// ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); +// cbn.insert("x1", f1); +// +// // add a delta function to the cbn +// ConstrainedConditionalGaussian::shared_ptr f2(new ConstrainedConditionalGaussian); //(c["x0"], "x0")); +// cbn.insert_df("x0", f2); +// +// return cbn; +//} /* ************************************************************************* */ // Some nonlinear functions to optimize diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 3a6604ea6..858dd98ad 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -11,10 +11,11 @@ #pragma once #include -#include "ConstrainedNonlinearFactorGraph.h" -#include "ConstrainedChordalBayesNet.h" -#include "ConstrainedLinearFactorGraph.h" +//#include "ConstrainedNonlinearFactorGraph.h" // will be added back once design is solidified +//#include "ConstrainedLinearFactorGraph.h" #include "NonlinearFactorGraph.h" +#include "ChordalBayesNet.h" +#include "LinearFactorGraph.h" #include "FGConfig.h" // \namespace @@ -32,13 +33,13 @@ namespace gtsam { /** * Create small example constrained factor graph */ - ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph(); + //ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph(); /** * Create small example constrained nonlinear factor graph */ - ConstrainedNonlinearFactorGraph,FGConfig> - createConstrainedNonlinearFactorGraph(); +// ConstrainedNonlinearFactorGraph,FGConfig> +// createConstrainedNonlinearFactorGraph(); /** * Create configuration to go with it @@ -50,7 +51,7 @@ namespace gtsam { * Create configuration for constrained example * This is the ground truth version */ - FGConfig createConstrainedConfig(); + //FGConfig createConstrainedConfig(); /** * create a noisy configuration for a nonlinear factor graph @@ -79,11 +80,6 @@ namespace gtsam { */ ChordalBayesNet createSmallChordalBayesNet(); - /** - * create small Constrained Chordal Bayes Net (from other constrained example) - */ - ConstrainedChordalBayesNet createConstrainedChordalBayesNet(); - /** * Create really non-linear factor graph (cos/sin) */ @@ -93,10 +89,10 @@ namespace gtsam { /** * Create a noisy configuration for linearization */ - FGConfig createConstrainedLinConfig(); + //FGConfig createConstrainedLinConfig(); /** * Create the correct delta configuration */ - FGConfig createConstrainedCorrectDelta(); + //FGConfig createConstrainedCorrectDelta(); } diff --git a/cpp/testConstrainedConditionalGaussian.cpp b/cpp/testConstrainedConditionalGaussian.cpp new file mode 100644 index 000000000..f74433272 --- /dev/null +++ b/cpp/testConstrainedConditionalGaussian.cpp @@ -0,0 +1,130 @@ +/** + * @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(v); + FGConfig 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(10*v, A); + FGConfig 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(rhs, A); + FGConfig 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); + + FGConfig fg; + fg.insert("x1", y); + + Vector expected = Vector_(2, -3.3333, 0.6667); + + ConstrainedConditionalGaussian binary(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); + + FGConfig fg; + fg.insert("x1", y); + fg.insert("x2", z); + + Vector expected = Vector_(2, 6.6667, -9.3333); + + ConstrainedConditionalGaussian ternary(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 index b8dcc5c1e..3fab95abd 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -1,11 +1,9 @@ -/* - * testConstrainedLinearFactorGraph.cpp - * - * Created on: Aug 10, 2009 - * Author: Alex Cunningham +/** + * @file testConstrainedLinearFactorGraph.cpp + * @author Alex Cunningham */ - +#include #include #include "ConstrainedLinearFactorGraph.h" #include "LinearFactorGraph.h" @@ -14,259 +12,384 @@ using namespace gtsam; using namespace std; -TEST( ConstrainedLinearFactorGraph, basic ) +/* ************************************************************************* */ +TEST( ConstrainedLinearFactorGraph, elimination1 ) { - ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); + // create unary factor + double sigma = 0.1; + Matrix Ax = eye(2) / sigma; + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma)); - // expected equality factor - Vector v1(2); v1(0)=1.;v1(1)=2.; - EqualityFactor::shared_ptr f1(new EqualityFactor(v1, "x0")); + // create binary constraint factor + 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); + LinearConstraint::shared_ptr f2( + new LinearConstraint("x", Ax1, "y", Ay1, b2)); - // expected normal linear factor - Matrix A21(2,2); - A21(0,0) = -10 ; A21(0,1) = 0; - A21(1,0) = 0 ; A21(1,1) = -10; + // construct the graph + ConstrainedLinearFactorGraph fg; + fg.push_back(f1); + fg.push_back_constraint(f2); - Matrix A22(2,2); - A22(0,0) = 10 ; A22(0,1) = 0; - A22(1,0) = 0 ; A22(1,1) = 10; + // verify construction of the graph + CHECK(fg.size() == 2); - Vector b(2); - b(0) = 20 ; b(1) = 30; + // eliminate x + Ordering ord; + ord.push_back("x"); + ChordalBayesNet::shared_ptr cbn = fg.eliminate(ord); - LinearFactor::shared_ptr f2(new LinearFactor("x0", A21, "x1", A22, b)); + //verify changes and output + CHECK(fg.size() == 1); + CHECK(cbn->size() == 1); + ConstrainedConditionalGaussian expectedCCG1(b2, Ax1, "y", Ay1); + CHECK(expectedCCG1.equals(*(cbn->get("x")))); + 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); + LinearFactor expectedLF("y", Ap, bp); + CHECK(expectedLF.equals(*(fg[0]), 1e-4)); - 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, involves_equality ) -{ - ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); - - CHECK(fg.involves_equality("x0")); - CHECK(!fg.involves_equality("x1")); + // eliminate y + Ordering ord2; + ord2.push_back("y"); + cbn = fg.eliminate(ord2); + CHECK(fg.size() == 0); + Matrix Ar(2,2); + Ar(0, 0) = 74.5356; Ar(0, 1) = -59.6285; + Ar(1, 0) = 0.0; Ar(1, 1) = 44.7214; + Vector br = Vector_(2, 8.9443, 4.4721); + ConditionalGaussian expected2(br, Ar); + CHECK(expected2.equals(*(cbn->get("y")))); } +/* ************************************************************************* */ TEST( ConstrainedLinearFactorGraph, optimize ) { - ConstrainedLinearFactorGraph fg1 = createConstrainedLinearFactorGraph(); - ConstrainedLinearFactorGraph fg2 = createConstrainedLinearFactorGraph(); - - FGConfig expected = createConstrainedConfig(); - - Ordering ord1; - ord1.push_back("x0"); - ord1.push_back("x1"); - - Ordering ord2; - ord2.push_back("x1"); - ord2.push_back("x0"); - - FGConfig actual1 = fg1.optimize(ord1); - FGConfig actual2 = fg2.optimize(ord2); - - CHECK(actual1.equals(expected)); - CHECK(actual1.equals(actual2)); -} - -TEST (ConstrainedLinearFactorGraph, eliminate ) -{ - ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); - FGConfig c = createConstrainedConfig(); - - Ordering ord1; - ord1.push_back("x0"); - ord1.push_back("x1"); - - ConstrainedChordalBayesNet::shared_ptr actual = fg.eliminate(ord1); - - // create an expected bayes net - ConstrainedChordalBayesNet::shared_ptr expected(new ConstrainedChordalBayesNet); - - DeltaFunction::shared_ptr d(new DeltaFunction(c["x0"], "x0")); - expected->insert_df("x0", d); - - Matrix A = eye(2); + // create unary factor double sigma = 0.1; - Vector dv = c["x1"]; - ConditionalGaussian::shared_ptr cg(new ConditionalGaussian(dv/sigma, A/sigma)); - expected->insert("x1", cg); + Matrix Ax = eye(2) / sigma; + Vector b1(2); + b1(0) = 1.0; + b1(1) = -1.0; + LinearFactor::shared_ptr f1(new LinearFactor("x", Ax, b1 / sigma)); - CHECK(actual->equals(*expected)); -} + // create binary constraint factor + 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); + LinearConstraint::shared_ptr f2( + new LinearConstraint("x", Ax1, "y", Ay1, b2)); -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 + // construct the graph + ConstrainedLinearFactorGraph fg; + fg.push_back(f1); + fg.push_back_constraint(f2); + // perform optimization Ordering ord; - ord.push_back("l1"); - ord.push_back("x1"); - ord.push_back("x2"); + ord.push_back("y"); + ord.push_back("x"); + FGConfig actual = fg.optimize(ord); - FGConfig actual = clfg.optimize(ord); + FGConfig expected; + expected.insert("x", Vector_(2, 1.0, -1.0)); + expected.insert("y", Vector_(2, 0.2, 0.1)); - FGConfig 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_one_eq) -{ - ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); - DeltaFunction::shared_ptr actual = fg.eliminate_one_eq("x0"); - - FGConfig c = createConstrainedConfig(); - DeltaFunction::shared_ptr expected(new DeltaFunction(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, eq_combine_and_eliminate ) -{ - // create a set of factors - ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); - EqualityFactor::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 MutableLinearFactor(f1_set)); - - // create a sample graph - ConstrainedLinearFactorGraph graph; - - // combine linear factor and eliminate - graph.eq_combine_and_eliminate(*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 - FGConfig 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(); - EqualityFactor::shared_ptr actual = fg.extract_eq("x0"); - - Vector v1(2); v1(0)=1.;v1(1)=2.; - EqualityFactor::shared_ptr expected(new EqualityFactor(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")); + CHECK(expected.size() == actual.size()); + CHECK(assert_equal(expected["x"], actual["x"], 1e-4)); + CHECK(assert_equal(expected["y"], actual["y"], 1e-4)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +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)); + LinearFactor::shared_ptr f2(new LinearFactor("z", eye(2), "w", eye(2), b)); + 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, 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(); +// +// FGConfig expected = createConstrainedConfig(); +// +// Ordering ord1; +// ord1.push_back("x0"); +// ord1.push_back("x1"); +// +// Ordering ord2; +// ord2.push_back("x1"); +// ord2.push_back("x0"); +// +// FGConfig actual1 = fg1.optimize(ord1); +// FGConfig actual2 = fg2.optimize(ord2); +// +// CHECK(actual1.equals(expected)); +// CHECK(actual1.equals(actual2)); +//} +// +//TEST (ConstrainedLinearFactorGraph, eliminate ) +//{ +// ConstrainedLinearFactorGraph fg = createConstrainedLinearFactorGraph(); +// FGConfig c = createConstrainedConfig(); +// +// Ordering ord1; +// ord1.push_back("x0"); +// ord1.push_back("x1"); +// +// ConstrainedChordalBayesNet::shared_ptr actual = fg.eliminate(ord1); +// +// // create an expected bayes net +// ConstrainedChordalBayesNet::shared_ptr expected(new ConstrainedChordalBayesNet); +// +// 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"); +// +// FGConfig actual = clfg.optimize(ord); +// +// FGConfig 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"); +//// +//// FGConfig 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 MutableLinearFactor(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 +// FGConfig 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 index cc73242e5..99caf1717 100644 --- a/cpp/testConstrainedNonlinearFactorGraph.cpp +++ b/cpp/testConstrainedNonlinearFactorGraph.cpp @@ -15,66 +15,66 @@ using namespace gtsam; typedef boost::shared_ptr > shared; typedef ConstrainedNonlinearFactorGraph,FGConfig> 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); - - FGConfig 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(); - FGConfig lin = createConstrainedLinConfig(); - ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); - FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); - - FGConfig expected = createConstrainedCorrectDelta(); - CHECK(actual.equals(expected)); -} +//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); +// +// FGConfig 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(); +// FGConfig lin = createConstrainedLinConfig(); +// ConstrainedLinearFactorGraph actual_lfg = nfg.linearize(lin); +// FGConfig actual = actual_lfg.optimize(actual_lfg.getOrdering()); +// +// FGConfig expected = createConstrainedCorrectDelta(); +// CHECK(actual.equals(expected)); +//} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/cpp/testDeltaFunction.cpp b/cpp/testDeltaFunction.cpp deleted file mode 100644 index b1e561d83..000000000 --- a/cpp/testDeltaFunction.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/* - * testDeltaFunction.cpp - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - - -#include -#include "DeltaFunction.h" - -using namespace gtsam; - -TEST (DeltaFunction, basic) -{ - Vector v(2); v(0)=1.0; v(1)=2.0; - - DeltaFunction delta(v, "x"); - - CHECK(assert_equal(v, delta.get_value())); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ - diff --git a/cpp/testEqualityFactor.cpp b/cpp/testEqualityFactor.cpp deleted file mode 100644 index d0fe0c316..000000000 --- a/cpp/testEqualityFactor.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/* - * testEqualityFactor.cpp - * - * Created on: Aug 10, 2009 - * Author: Alex Cunningham - */ - - -#include -#include "EqualityFactor.h" -#include "smallExample.h" - -using namespace gtsam; -using namespace std; - -TEST ( EqualityFactor, basic ) -{ - // create an initialized factor - Vector v(2); v(0)=1.2; v(1)=3.4; - string key = "x0"; - EqualityFactor factor(v, key); - - // get the data back out of it - CHECK(assert_equal(v, factor.get_value())); - CHECK(key == factor.get_key()); -} - -TEST ( EqualityFactor, equals ) -{ - Vector v(2); v(0)=1.2; v(1)=3.4; - string key = "x0"; - EqualityFactor factor1(v, key); - EqualityFactor factor2(v, key); - CHECK(factor1.equals(factor2)); -} - -TEST (EqualityFactor, getDeltaFunction ) -{ - Vector v(2); v(0)=1.2; v(1)=3.4; - string key = "x0"; - EqualityFactor factor(v, key); - - DeltaFunction::shared_ptr actual = factor.getDeltaFunction(); - - DeltaFunction::shared_ptr expected(new DeltaFunction(v, key)); - CHECK(assert_equal(*actual, *expected)); -} - -TEST (EqualityFactor, linearize ) -{ - FGConfig c = createConstrainedConfig(); - EqualityFactor init(c["x0"], "x0"); - EqualityFactor::shared_ptr actual = init.linearize(); - EqualityFactor::shared_ptr expected(new EqualityFactor(zero(2), "x0")); - CHECK(assert_equal(*actual, *expected)); -} - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ - diff --git a/cpp/testLinearConstraint.cpp b/cpp/testLinearConstraint.cpp new file mode 100644 index 000000000..51f18438d --- /dev/null +++ b/cpp/testLinearConstraint.cpp @@ -0,0 +1,246 @@ +/** + * @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; + + LinearConstraint lc1("x1", A1, "x2", A2, b); + LinearConstraint lc2("x1", A1, "x2", A2, b); + + // check for basic equality + CHECK(lc1.equals(lc2)); + CHECK(lc2.equals(lc1)); +} + +/* ************************************************************************* */ +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 + FGConfig 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); + + FGConfig 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 + FGConfig 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); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ +