From 8f20523e7f83f7ea49ca6ce4759c5564fe423167 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Wed, 14 Oct 2009 15:32:05 +0000 Subject: [PATCH] ConstrainedLinearFactorGraphs now handles multiple constraints on a node properly. smallExample was changed to include two of the examples used in testConstrainedLinearFactorGraph ConditionalGaussian was changed to make solve() virtual, as this is necessary for ConstrainedConditionalGaussian --- cpp/ConditionalGaussian.h | 2 +- cpp/ConstrainedLinearFactorGraph.cpp | 143 +++++++++--- cpp/ConstrainedLinearFactorGraph.h | 36 ++- cpp/LinearConstraint.cpp | 48 +++- cpp/LinearConstraint.h | 72 +++--- cpp/smallExample.cpp | 277 ++++++++++++++--------- cpp/smallExample.h | 56 +++-- cpp/testConstrainedLinearFactorGraph.cpp | 229 +++++++++++++++---- cpp/testLinearConstraint.cpp | 54 +++++ 9 files changed, 686 insertions(+), 231 deletions(-) diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h index bfa236f61..b31c9bd27 100644 --- a/cpp/ConditionalGaussian.h +++ b/cpp/ConditionalGaussian.h @@ -115,7 +115,7 @@ namespace gtsam { * @param x configuration in which the parents values (y,z,...) are known * @return solution x = R \ (d - Sy - Tz - ...) */ - Vector solve(const FGConfig& x) const; + virtual Vector solve(const FGConfig& x) const; /** * adds a parent diff --git a/cpp/ConstrainedLinearFactorGraph.cpp b/cpp/ConstrainedLinearFactorGraph.cpp index ddb81b30a..c360677fd 100644 --- a/cpp/ConstrainedLinearFactorGraph.cpp +++ b/cpp/ConstrainedLinearFactorGraph.cpp @@ -13,10 +13,12 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph() { } +/* ************************************************************************* */ ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph( const LinearFactorGraph& lfg) { BOOST_FOREACH(LinearFactor::shared_ptr s, lfg) @@ -24,14 +26,17 @@ ConstrainedLinearFactorGraph::ConstrainedLinearFactorGraph( } } +/* ************************************************************************* */ 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_) @@ -41,6 +46,7 @@ bool ConstrainedLinearFactorGraph::is_constrained(const std::string& key) const return false; } +/* ************************************************************************* */ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double tol) const { const ConstrainedLinearFactorGraph* p = (const ConstrainedLinearFactorGraph *) &fg; @@ -61,6 +67,7 @@ bool ConstrainedLinearFactorGraph::equals(const LinearFactorGraph& fg, double to return LinearFactorGraph::equals(fg, tol); } +/* ************************************************************************* */ ChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Ordering& ordering) { ChordalBayesNet::shared_ptr cbn (new ChordalBayesNet()); @@ -82,10 +89,19 @@ ChordalBayesNet::shared_ptr ConstrainedLinearFactorGraph::eliminate(const Orderi return cbn; } +/* ************************************************************************* */ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::eliminate_constraint(const string& key) { - // extract the constraint - in-place remove from graph - LinearConstraint::shared_ptr constraint = extract_constraint(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); @@ -93,11 +109,18 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina // 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); + // 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 A1 = factor->get_A(key); Matrix invC1 = inverse(constraint->get_A(key)); Matrix T = A1*invC1; @@ -107,15 +130,26 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina 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); + blocks.insert(make_pair(cur_key, -1 *Ai)); } else if (cur_key != key) { Matrix Ai = factor->get_A(cur_key) - T*Ci; - new_factor->insert(cur_key, Ai); + blocks.insert(make_pair(cur_key, Ai)); } } + // construct the updated factor + boost::shared_ptr new_factor(new MutableLinearFactor); + 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 = factor->get_b() - T*constraint->get_b(); + 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 @@ -125,32 +159,87 @@ ConstrainedConditionalGaussian::shared_ptr ConstrainedLinearFactorGraph::elimina return ccg; } -LinearConstraint::shared_ptr ConstrainedLinearFactorGraph::extract_constraint(const string& key) -{ - LinearConstraint::shared_ptr ret; - bool found_key = false; - vector new_vec; - BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_) - { - if (constraint->involves(key)) { - ret = constraint; - found_key = true; - } - else - new_vec.push_back(constraint); - } - constraints_ = new_vec; - if (!found_key) - throw invalid_argument("No constraint connected to node: " + key); - return ret; +/* ************************************************************************* */ +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); + } + } +} + +/* ************************************************************************* */ FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) { ChordalBayesNet::shared_ptr cbn = eliminate(ordering); boost::shared_ptr newConfig = cbn->optimize(); 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; @@ -164,9 +253,11 @@ void ConstrainedLinearFactorGraph::print(const std::string& s) const } } +/* ************************************************************************* */ 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 index 46e9fe02a..ce29cea69 100644 --- a/cpp/ConstrainedLinearFactorGraph.h +++ b/cpp/ConstrainedLinearFactorGraph.h @@ -43,7 +43,11 @@ public: */ void push_back_constraint(LinearConstraint::shared_ptr constraint); - // Additional STL-like functions for Equality Factors + /** + * 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 */ @@ -78,11 +82,31 @@ public: */ ChordalBayesNet::shared_ptr 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. - * FIXME: currently will not handle multiple constraints on the same node */ ConstrainedConditionalGaussian::shared_ptr eliminate_constraint(const std::string& key); @@ -103,10 +127,12 @@ public: void print(const std::string& s="") const; /** - * Finds a matching equality constraint by key - assumed present - * Performs in-place removal of the constraint + * 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 */ - LinearConstraint::shared_ptr extract_constraint(const std::string& key); + std::vector find_constraints_and_remove(const std::string& key); /** * This function returns the best ordering for this linear factor diff --git a/cpp/LinearConstraint.cpp b/cpp/LinearConstraint.cpp index b56f143b3..4a26c36c7 100644 --- a/cpp/LinearConstraint.cpp +++ b/cpp/LinearConstraint.cpp @@ -1,18 +1,20 @@ -/* - * LinearConstraint.cpp - * - * Created on: Aug 10, 2009 - * Author: alexgc +/** + * @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() { } @@ -55,7 +57,12 @@ ConstrainedConditionalGaussian::shared_ptr LinearConstraint::eliminate(const std } void LinearConstraint::print(const string& s) const { - cout << s << ": " << dump() << endl; + 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 { @@ -73,7 +80,7 @@ bool LinearConstraint::equals(const LinearConstraint& f, double tol) const { if (it == f.As.end()) return false; Matrix f_mat = it->second; // check matrix - if (!(f_mat == p.second)) return false; + if (!equal_with_abs_tol(f_mat,p.second,tol)) return false; } return true; } @@ -108,4 +115,31 @@ bool assert_equal(const LinearConstraint& actual, 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 index d386f149c..cf7174be3 100644 --- a/cpp/LinearConstraint.h +++ b/cpp/LinearConstraint.h @@ -1,14 +1,14 @@ -/* - * LinearConstraint.h - * - * Created on: Aug 10, 2009 - * Author: alexgc +/** + * @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" @@ -60,12 +60,14 @@ public: * @param matrices is the full map of A matrices * @param b is the RHS vector */ - LinearConstraint(const std::map& matrices, const Vector& b); + LinearConstraint(const std::map& matrices, + const Vector& b); /** * Default Destructor */ - ~LinearConstraint() {} + ~LinearConstraint() { + } /** * Eliminates the constraint @@ -75,26 +77,29 @@ public: * @return a constrained conditional gaussian for the variable that is a * function of its parents */ - ConstrainedConditionalGaussian::shared_ptr eliminate(const std::string& key); + ConstrainedConditionalGaussian::shared_ptr + eliminate(const std::string& key); - /** - * print - * @param s optional string naming the factor - */ - void print(const std::string& s="") const; + /** + * 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; + /** + * 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; + /** + * 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; } + const Vector& get_b() const { + return b; + } /** check if the constraint is connected to a particular node */ bool involves(const std::string& key) const; @@ -115,16 +120,27 @@ public: * @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; + std::list keys(const std::string& key = "") const; - /** - * @return the number of nodes the constraint connects - */ - std::size_t size() const {return As.size();} + /** + * @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); + /** assert equals for testing - prints when not equal */ -bool assert_equal(const LinearConstraint& actual, const LinearConstraint& expected, double tol=1e-9); +bool assert_equal(const LinearConstraint& actual, + const LinearConstraint& expected, double tol = 1e-9); } diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index ebac8f8bf..384d31234 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -17,8 +17,7 @@ using namespace std; #include "Ordering.h" #include "Matrix.h" #include "NonlinearFactor.h" -#include "LinearConstraint.h" -#include "ConstrainedConditionalGaussian.h" +#include "ConstrainedLinearFactorGraph.h" #include "smallExample.h" #include "Point2Prior.h" #include "Simulated2DOdometry.h" @@ -61,51 +60,12 @@ boost::shared_ptr sharedNonlinearFactorGraph( return nlfg; } +/* ************************************************************************* */ ExampleNonlinearFactorGraph createNonlinearFactorGraph() { return *sharedNonlinearFactorGraph(); } /* ************************************************************************* */ -//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 -// 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() { Vector v_x1(2); v_x1(0) = 0.; v_x1(1) = 0.; @@ -131,52 +91,11 @@ boost::shared_ptr sharedNoisyConfig() return c; } +/* ************************************************************************* */ FGConfig createNoisyConfig() { return *sharedNoisyConfig(); } -/* ************************************************************************* */ -//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 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() { Vector v_x1(2); v_x1(0) = -0.1; v_x1(1) = -0.1; @@ -289,26 +208,6 @@ ChordalBayesNet createSmallChordalBayesNet() 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 /* ************************************************************************* */ @@ -340,5 +239,175 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() { } /* ************************************************************************* */ +ConstrainedLinearFactorGraph createSingleConstraintGraph() { + // 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)); + + // 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)); + + // construct the graph + ConstrainedLinearFactorGraph fg; + fg.push_back(f1); + fg.push_back_constraint(f2); + + return fg; +} + +/* ************************************************************************* */ +ConstrainedLinearFactorGraph createMultiConstraintGraph() { + // unary factor 1 + double sigma = 0.1; + Matrix A = eye(2) / sigma; + Vector b = Vector_(2, -2.0, 2.0)/sigma; + LinearFactor::shared_ptr lf1(new LinearFactor("x", A, b)); + + // 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 fg; + fg.push_back(lf1); + fg.push_back_constraint(lc1); + fg.push_back_constraint(lc2); + + return fg; +} + +/* ************************************************************************* */ +//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 +// 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 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 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; +//} + +/* ************************************************************************* */ +//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; +//} } // namespace gtsam diff --git a/cpp/smallExample.h b/cpp/smallExample.h index 858dd98ad..3014471e1 100644 --- a/cpp/smallExample.h +++ b/cpp/smallExample.h @@ -12,7 +12,7 @@ #include //#include "ConstrainedNonlinearFactorGraph.h" // will be added back once design is solidified -//#include "ConstrainedLinearFactorGraph.h" +#include "ConstrainedLinearFactorGraph.h" #include "NonlinearFactorGraph.h" #include "ChordalBayesNet.h" #include "LinearFactorGraph.h" @@ -30,29 +30,12 @@ namespace gtsam { boost::shared_ptr sharedNonlinearFactorGraph(); ExampleNonlinearFactorGraph createNonlinearFactorGraph(); - /** - * Create small example constrained factor graph - */ - //ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph(); - - /** - * Create small example constrained nonlinear factor graph - */ -// ConstrainedNonlinearFactorGraph,FGConfig> -// createConstrainedNonlinearFactorGraph(); - /** * Create configuration to go with it * The ground truth configuration for the example above */ FGConfig createConfig(); - /** - * Create configuration for constrained example - * This is the ground truth version - */ - //FGConfig createConstrainedConfig(); - /** * create a noisy configuration for a nonlinear factor graph */ @@ -86,6 +69,32 @@ namespace gtsam { boost::shared_ptr sharedReallyNonlinearFactorGraph(); ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph(); + /* ******************************************************* */ + // Constrained Examples + /* ******************************************************* */ + + /** + * Creates a simple constrained graph with one linear factor and + * one binary constraint. + */ + ConstrainedLinearFactorGraph createSingleConstraintGraph(); + + /** + * Creates a constrained graph with a linear factor and two + * binary constraints that share a node + */ + ConstrainedLinearFactorGraph createMultiConstraintGraph(); + + /** + * These are the old examples from the EqualityFactor/DeltaFunction + * They should be updated for use at some point, but are disabled for now + */ + /** + * Create configuration for constrained example + * This is the ground truth version + */ + //FGConfig createConstrainedConfig(); + /** * Create a noisy configuration for linearization */ @@ -95,4 +104,15 @@ namespace gtsam { * Create the correct delta configuration */ //FGConfig createConstrainedCorrectDelta(); + + /** + * Create small example constrained factor graph + */ + //ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph(); + + /** + * Create small example constrained nonlinear factor graph + */ +// ConstrainedNonlinearFactorGraph,FGConfig> +// createConstrainedNonlinearFactorGraph(); } diff --git a/cpp/testConstrainedLinearFactorGraph.cpp b/cpp/testConstrainedLinearFactorGraph.cpp index 3fab95abd..e49414b0d 100644 --- a/cpp/testConstrainedLinearFactorGraph.cpp +++ b/cpp/testConstrainedLinearFactorGraph.cpp @@ -15,27 +15,8 @@ using namespace std; /* ************************************************************************* */ TEST( ConstrainedLinearFactorGraph, elimination1 ) { - // 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)); - - // 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)); - - // construct the graph - ConstrainedLinearFactorGraph fg; - fg.push_back(f1); - fg.push_back_constraint(f2); + // get the graph + ConstrainedLinearFactorGraph fg = createSingleConstraintGraph(); // verify construction of the graph CHECK(fg.size() == 2); @@ -48,6 +29,11 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) //verify changes and output CHECK(fg.size() == 1); CHECK(cbn->size() == 1); + 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(b2, Ax1, "y", Ay1); CHECK(expectedCCG1.equals(*(cbn->get("x")))); Matrix Ap(2,2); @@ -74,27 +60,8 @@ TEST( ConstrainedLinearFactorGraph, elimination1 ) /* ************************************************************************* */ TEST( ConstrainedLinearFactorGraph, optimize ) { - // 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)); - - // 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)); - - // construct the graph - ConstrainedLinearFactorGraph fg; - fg.push_back(f1); - fg.push_back_constraint(f2); + // create graph + ConstrainedLinearFactorGraph fg = createSingleConstraintGraph(); // perform optimization Ordering ord; @@ -111,6 +78,27 @@ TEST( ConstrainedLinearFactorGraph, optimize ) 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"); + FGConfig actual = fg.optimize(ord); + + FGConfig 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 ) { @@ -133,6 +121,163 @@ TEST( ConstrainedLinearFactorGraph, is_constrained ) 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->size() == 1); + CHECK(fg.size() == 2); + + // eliminate the induced constraint + ConstrainedConditionalGaussian::shared_ptr cg2 = fg.eliminate_constraint("y"); + CHECK(fg.size() == 1); + CHECK(cg2->size() == 1); + + // eliminate the linear factor + ConditionalGaussian::shared_ptr cg3 = fg.eliminate_one("z"); + CHECK(fg.size() == 0); + CHECK(cg3->size() == 0); + + // solve piecewise + FGConfig 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"); + + FGConfig actual = fg.optimize(ord); + + // verify + FGConfig 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 ) //{ diff --git a/cpp/testLinearConstraint.cpp b/cpp/testLinearConstraint.cpp index 51f18438d..fbd653a9a 100644 --- a/cpp/testLinearConstraint.cpp +++ b/cpp/testLinearConstraint.cpp @@ -127,13 +127,16 @@ TEST ( LinearConstraint, equals ) 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)); } /* ************************************************************************* */ @@ -240,6 +243,57 @@ TEST ( LinearConstraint, keys ) 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);} /* ************************************************************************* */