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

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
release/4.3a0
Alex Cunningham 2009-10-14 15:32:05 +00:00
parent 92b920cb48
commit 8f20523e7f
9 changed files with 686 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/ConditionalGaussian.h
View File

@ -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

143
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -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<LinearConstraint::shared_ptr> constraint_separator = find_constraints_and_remove(key);
if (constraint_separator.size() == 0)
throw invalid_argument("No constraints on node " + key);
// split out a constraint to apply
LinearConstraint::shared_ptr constraint = pick_constraint(constraint_separator);
// perform change of variables on the remaining constraints and reinsert
update_constraints(key, constraint_separator, constraint);
// perform elimination on the constraint itself to get the constrained conditional gaussian
ConstrainedConditionalGaussian::shared_ptr ccg = constraint->eliminate(key);
@ -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<MutableLinearFactor> new_factor(new MutableLinearFactor);
// store the block matrices
map<string, Matrix> blocks;
// get the A1 term and reconstruct new_factor without the eliminated block
Matrix A1 = factor->get_A(key);
list<string> factor_keys = factor->keys();
BOOST_FOREACH(string cur_key, factor_keys) {
if (cur_key != key)
blocks.insert(make_pair(cur_key, factor->get_A(cur_key)));
}
// get T = A1*inv(C1) term
Matrix 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<MutableLinearFactor> 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<LinearConstraint::shared_ptr> 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<LinearConstraint::shared_ptr>& constraints) const {
if (constraints.size() == 0)
throw invalid_argument("Constraint set is empty!");
return constraints[0];
}
/* ************************************************************************* */
void ConstrainedLinearFactorGraph::update_constraints(const std::string& key,
const std::vector<LinearConstraint::shared_ptr>& separator,
const LinearConstraint::shared_ptr& primary) {
// Implements update for each constraint, where primary is
// C1*x1 = d - C2*x2 - ...
// and each constraint is
// A1*x1 + A2*x2 + ... = b;
// extract components from primary
Matrix invC1 = inverse(primary->get_A(key));
Vector d = primary->get_b();
// perform transform on each constraint
// constraint c is the one being updated
BOOST_FOREACH(LinearConstraint::shared_ptr updatee, separator) {
if (!updatee->equals(*primary)) {
// build transform matrix
Matrix A1 = updatee->get_A(key);
Matrix T = A1 * invC1;
// copy existing nodes into new factor without the eliminated variable
list<string> updatee_keys = updatee->keys(key);
map<string, Matrix> blocks;
BOOST_FOREACH(string cur_key, updatee_keys) {
blocks[cur_key] = updatee->get_A(cur_key);
}
// loop over all nodes in separator of constraint
list<string> primary_keys = primary->keys(key); // keys that are not key
BOOST_FOREACH(string cur_key, primary_keys) {
Matrix Ci = primary->get_A(cur_key);
if (cur_key != key && !updatee->involves(cur_key)) {
Matrix Ai = T*Ci;
blocks[cur_key] = -1 * Ai;
} else if (cur_key != key) {
Matrix Ai = updatee->get_A(cur_key) - T*Ci;
blocks[cur_key] = Ai;
}
}
Vector rhs = updatee->get_b() - T * d;
LinearConstraint::shared_ptr new_constraint(new LinearConstraint(blocks, rhs));
// reinsert into graph
push_back_constraint(new_constraint);
}
}
}
/* ************************************************************************* */
FGConfig ConstrainedLinearFactorGraph::optimize(const Ordering& ordering) {
ChordalBayesNet::shared_ptr cbn = eliminate(ordering);
boost::shared_ptr<FGConfig> newConfig = cbn->optimize();
return *newConfig;
}
/* ************************************************************************* */
std::vector<LinearConstraint::shared_ptr>
ConstrainedLinearFactorGraph::find_constraints_and_remove(const string& key)
{
vector<LinearConstraint::shared_ptr> found, uninvolved;
BOOST_FOREACH(LinearConstraint::shared_ptr constraint, constraints_) {
if (constraint->involves(key)) {
found.push_back(constraint);
} else {
uninvolved.push_back(constraint);
}
}
constraints_ = uninvolved;
return found;
}
/* ************************************************************************* */
void ConstrainedLinearFactorGraph::print(const std::string& s) const
{
cout << "ConstrainedFactorGraph: " << s << endl;
@ -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;

36
cpp/ConstrainedLinearFactorGraph.h
View File

@ -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<LinearConstraint::shared_ptr>& constraints) const;
/**
* Eliminates the specified constraint for the selected variable,
* and then performs a change of variables on all the constraints in the separator
* and reinserts them into the graph.
* @param key is the identifier for the node to eliminate
* @param separator is the set of constraints to transform and reinsert
* @param constraint is the primary constraint to eliminate
*/
void update_constraints(const std::string& key,
const std::vector<LinearConstraint::shared_ptr>& separator,
const LinearConstraint::shared_ptr& constraint);
/**
* Eliminates a node with a constraint on it
* Other factors have a change of variables performed via Schur complement to remove the
* eliminated node.
* 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<LinearConstraint::shared_ptr> find_constraints_and_remove(const std::string& key);
/**
* This function returns the best ordering for this linear factor

48
cpp/LinearConstraint.cpp
View File

@ -1,18 +1,20 @@
/*
* LinearConstraint.cpp
*
* Created on: Aug 10, 2009
* Author: alexgc
/**
* @file LinearConstraint.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "LinearConstraint.h"
#include "Matrix.h"
namespace gtsam {
using namespace std;
// trick from some reading group
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
LinearConstraint::LinearConstraint() {
}
@ -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<LinearConstraint::shared_ptr>& constraints) {
map<string, Matrix> blocks;
Vector rhs;
BOOST_FOREACH(LinearConstraint::shared_ptr c, constraints) {
string key; Matrix A;
FOREACH_PAIR( key, A, c->As) {
if (blocks.find(key) == blocks.end())
blocks[key] = A;
else {
Matrix Aold = blocks[key];
if (A.size1() != Aold.size1() || A.size2() != Aold.size2())
throw invalid_argument("Dimension mismatch");
blocks[key] = A + Aold;
}
}
// assemble rhs
if (rhs.size() == 0)
rhs = c->get_b();
else
rhs = rhs + c->get_b();
}
LinearConstraint::shared_ptr result(new LinearConstraint(blocks, rhs));
return result;
}
}

38
cpp/LinearConstraint.h
View File

@ -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 <list>
#include <set>
#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<std::string, Matrix>& matrices, const Vector& b);
LinearConstraint(const std::map<std::string, Matrix>& matrices,
const Vector& b);
/**
* Default Destructor
*/
~LinearConstraint() {}
~LinearConstraint() {
}
/**
* Eliminates the constraint
@ -75,7 +77,8 @@ 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
@ -94,7 +97,9 @@ public:
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;
@ -120,11 +125,22 @@ public:
/**
* @return the number of nodes the constraint connects
*/
std::size_t size() const {return As.size();}
std::size_t size() const {
return As.size();
}
// friends
friend LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
};
/**
* Combines constraints into one constraint
*/
LinearConstraint::shared_ptr combineConstraints(const std::set<LinearConstraint::shared_ptr>& constraints);
/** 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);
}

277
cpp/smallExample.cpp
View File

@ -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,50 +60,11 @@ boost::shared_ptr<const ExampleNonlinearFactorGraph> 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<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , 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()
{
@ -131,52 +91,11 @@ boost::shared_ptr<const FGConfig> 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<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , 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

56
cpp/smallExample.h
View File

@ -12,7 +12,7 @@
#include <boost/shared_ptr.hpp>
//#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<const ExampleNonlinearFactorGraph > sharedNonlinearFactorGraph();
ExampleNonlinearFactorGraph createNonlinearFactorGraph();
/**
* Create small example constrained factor graph
*/
//ConstrainedLinearFactorGraph createConstrainedLinearFactorGraph();
/**
* Create small example constrained nonlinear factor graph
*/
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,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<const ExampleNonlinearFactorGraph> 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<NonlinearFactor<FGConfig>,FGConfig>
// createConstrainedNonlinearFactorGraph();
}

229
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -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<LinearConstraint::shared_ptr> actual1 = fg1.find_constraints_and_remove("y");
CHECK(fg1.size() == 2);
CHECK(actual1.size() == 1);
CHECK((*actual1.begin())->equals(*lc1));
vector<LinearConstraint::shared_ptr> actual2 = fg2.find_constraints_and_remove("x");
CHECK(fg2.size() == 1);
CHECK(actual2.size() == 2);
CHECK((*actual1.begin())->equals(*lc1));
LinearConstraint::shared_ptr act = *(++actual2.begin());
CHECK(act->equals(*lc2));
}
/* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, update_constraints )
{
// create a graph
ConstrainedLinearFactorGraph fg1 = createMultiConstraintGraph();
// process constraints - picking first constraint on x
vector<LinearConstraint::shared_ptr> constraints = fg1.find_constraints_and_remove("x");
CHECK(constraints.size() == 2);
CHECK(fg1.size() == 1); // both constraints removed
LinearConstraint::shared_ptr primary = constraints[0];
LinearConstraint::shared_ptr updatee = constraints[1];
fg1.update_constraints("x", constraints, primary);
CHECK(fg1.size() == 2); // induced constraint added back
// expected induced constraint
Matrix Ar(2,2);
Ar(0, 0) = -16.6666; Ar(0, 1) = -6.6666;
Ar(1, 0) = 10.0; Ar(1, 1) = 0.0;
Matrix A22(2,2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0;
Vector br = Vector_(2, 0.0, 5.0);
LinearConstraint::shared_ptr exp(new LinearConstraint("y", Ar, "z", A22, br));
// evaluate
CHECK(assert_equal(*(fg1.constraint_at(0)), *exp, 1e-4));
}
/* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, find_constraints_and_remove )
{
// constraint 1
Matrix A11(2,2);
A11(0,0) = 1.0 ; A11(0,1) = 2.0;
A11(1,0) = 2.0 ; A11(1,1) = 1.0;
Matrix A12(2,2);
A12(0,0) = 10.0 ; A12(0,1) = 0.0;
A12(1,0) = 0.0 ; A12(1,1) = 10.0;
Vector b1(2);
b1(0) = 1.0; b1(1) = 2.0;
LinearConstraint::shared_ptr lc1(new LinearConstraint("x", A11, "y", A12, b1));
// constraint 2
Matrix A21(2,2);
A21(0,0) = 3.0 ; A21(0,1) = 4.0;
A21(1,0) = -1.0 ; A21(1,1) = -2.0;
Matrix A22(2,2);
A22(0,0) = 1.0 ; A22(0,1) = 1.0;
A22(1,0) = 1.0 ; A22(1,1) = 2.0;
Vector b2(2);
b2(0) = 3.0; b2(1) = 4.0;
LinearConstraint::shared_ptr lc2(new LinearConstraint("x", A21, "z", A22, b2));
// construct the graph
ConstrainedLinearFactorGraph fg1;
fg1.push_back_constraint(lc1);
fg1.push_back_constraint(lc2);
// constraints on x
vector<LinearConstraint::shared_ptr> expected1, actual1;
expected1.push_back(lc1);
expected1.push_back(lc2);
actual1 = fg1.find_constraints_and_remove("x");
CHECK(fg1.size() == 0);
CHECK(expected1.size() == actual1.size());
vector<LinearConstraint::shared_ptr>::const_iterator exp1, act1;
for(exp1=expected1.begin(), act1=actual1.begin(); act1 != actual1.end(); ++act1, ++exp1) {
CHECK((*exp1)->equals(**act1));
}
}
/* ************************************************************************* */
TEST( ConstrainedLinearFactorGraph, eliminate_multi_constraint )
{
ConstrainedLinearFactorGraph fg = createMultiConstraintGraph();
// eliminate the constraint
ConstrainedConditionalGaussian::shared_ptr cg1 = fg.eliminate_constraint("x");
CHECK(cg1->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 )
//{

54
cpp/testLinearConstraint.cpp
View File

@ -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<LinearConstraint::shared_ptr> constraints;
constraints.insert(lc1);
constraints.insert(lc2);
LinearConstraint::shared_ptr actual = combineConstraints(constraints);
// expected
Matrix A1 = A11 + A21;
Matrix A2 = A12;
Matrix A3 = A22;
Vector b = b1 + b2;
map<string, Matrix> blocks;
blocks.insert(make_pair("x", A1));
blocks.insert(make_pair("y", A2));
blocks.insert(make_pair("z", A3));
LinearConstraint expected(blocks, b);
// verify
CHECK(actual->equals(expected));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */