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

Generalized constraint handling to create a LinearConstraint which implements linear equality constraints that can be eliminated as a part of a ConstrainedLinearFactorGraph. DeltaFunction has been changed to be a ConstrainedConditionalGaussian, which has a more robust solve() function. The new tests no longer use the "constrained" example from smallExample, so those functions have been commented. 

''Limitations: ''
 * Any given node can only have one constraint on it, but constraints can be of arbitrary size
 * Constraints can only be specified as a blockwise system, where each block must be square and invertible to support arbitrary elimination orderings.  
  * ConstrainedNonlinearFactorGraph is disabled until a better solution for handling constraints in the nonlinear case is determined.
release/4.3a0
Alex Cunningham 2009-10-08 13:57:22 +00:00
parent 3efe95abee
commit 66dac8a52f
23 changed files with 1501 additions and 969 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
.cproject
View File

@ -300,6 +300,7 @@
<buildTargets>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -307,7 +308,6 @@
</target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -323,6 +323,7 @@
</target>
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testVSLAMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -330,7 +331,6 @@
</target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -338,6 +338,7 @@
</target>
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConditionalGaussian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -345,7 +346,6 @@
</target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -361,7 +361,6 @@
</target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -369,6 +368,7 @@
</target>
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearOptimizer.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -376,7 +376,6 @@
</target>
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -384,7 +383,6 @@
</target>
<target name="testConstrainedNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -392,14 +390,22 @@
</target>
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testLinearConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
@ -407,12 +413,27 @@
</target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testConstrainedConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedConditionalGaussian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testConstrainedLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testConstrainedLinearFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>install</buildTarget>
@ -429,6 +450,7 @@
</target>
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>

3
cpp/ChordalBayesNet.h
View File

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

8
cpp/ConditionalGaussian.cpp
View File

@ -40,6 +40,14 @@ ConditionalGaussian::ConditionalGaussian(Vector d,
parents_.insert(make_pair(name2, T));
}
/* ************************************************************************* */
ConditionalGaussian::ConditionalGaussian(const Vector& d,
const Matrix& R,
const map<std::string, Matrix>& parents)
: R_(R), d_(d), parents_(parents)
{
}
/* ************************************************************************* */
void ConditionalGaussian::print(const string &s) const
{

8
cpp/ConditionalGaussian.h
View File

@ -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<std::string, Matrix>& parents);
/** deconstructor */
virtual ~ConditionalGaussian() {};

80
cpp/ConstrainedConditionalGaussian.cpp Normal file
View File

@ -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 <boost/numeric/ublas/lu.hpp>
#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<std::string, Matrix>& 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<string, Matrix>::const_iterator it = parents_.begin(); it
!= parents_.end(); it++) {
const string& j = it->first;
const Matrix& Aj = it->second;
rhs -= Aj * x[j];
}
// verify invertibility of A matrix
Matrix A = R_;
Matrix b = Matrix_(rhs.size(), 1, rhs);
if (A.size1() != A.size2())
throw invalid_argument("Matrix A is not invertible - non-square matrix");
using namespace boost::numeric::ublas;
if (lu_factorize(A))
throw invalid_argument("Matrix A is singular");
// get the actual solution
//FIXME: This is just the Matrix::solve() function, but due to name conflicts
// the compiler won't find the real version in Matrix.h
lu_substitute<const Matrix, Matrix> (A, b);
return Vector_(b);
//TODO: Handle overdetermined case
//TODO: Handle underdetermined case
}

107
cpp/ConstrainedConditionalGaussian.h Normal file
View File

@ -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<ConstrainedConditionalGaussian> 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<std::string, Matrix>& 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;
};
}

188
cpp/ConstrainedLinearFactorGraph.cpp
View File

@ -1,14 +1,11 @@
/*
* ConstrainedLinearFactorGraph.cpp
*
* Created on: Aug 10, 2009
* Author: alexgc
/**
* @file ConstrainedLinearFactorGraph.cpp
* @author Alex Cunningham
*/
#include <iostream>
#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<LinearFactor::shared_ptr> 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<MutableLinearFactor> 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<string> constraint_keys = constraint->keys(key);
BOOST_FOREACH(string cur_key, constraint_keys) {
Matrix Ci = constraint->get_A(cur_key);
if (cur_key != key && !factor->involves(cur_key)) {
Matrix Ai = T*Ci;
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<MutableLinearFactor> 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<EqualityFactor::shared_ptr> new_vec;
BOOST_FOREACH(EqualityFactor::shared_ptr eq, eq_factors)
LinearConstraint::shared_ptr ret;
bool found_key = false;
vector<LinearConstraint::shared_ptr> 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<FGConfig> 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<MutableLinearFactor> 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;
}
}

90
cpp/ConstrainedLinearFactorGraph.h
View File

@ -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 <boost/shared_ptr.hpp>
#include <boost/foreach.hpp>
#include "LinearFactorGraph.h"
#include "EqualityFactor.h"
#include "ConstrainedChordalBayesNet.h"
#include "ChordalBayesNet.h"
#include "LinearConstraint.h"
namespace gtsam {
class ConstrainedLinearFactorGraph: public LinearFactorGraph {
protected:
std::vector<EqualityFactor::shared_ptr> eq_factors; /// collection of equality factors
std::vector<LinearConstraint::shared_ptr> constraints_; /// collection of equality factors
public:
// iterators for equality constraints - same interface as linear factors
typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::const_iterator constraint_const_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::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;
};
}

16
cpp/ConstrainedNonlinearFactorGraph.h
View File

@ -10,7 +10,7 @@
#include <boost/shared_ptr.hpp>
#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 Factor, class Config>
class ConstrainedNonlinearFactorGraph: public FactorGraph<Factor, Config> {
protected:
/** collection of equality factors */
std::vector<EqualityFactor::shared_ptr> eq_factors;
std::vector<LinearConstraint::shared_ptr> eq_factors;
public:
// iterators over equality factors
typedef std::vector<EqualityFactor::shared_ptr>::const_iterator eq_const_iterator;
typedef std::vector<EqualityFactor::shared_ptr>::iterator eq_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::const_iterator eq_const_iterator;
typedef std::vector<LinearConstraint::shared_ptr>::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);
}

60
cpp/DeltaFunction.cpp
View File

@ -1,60 +0,0 @@
/*
* DeltaFunction.cpp
*
* Created on: Aug 11, 2009
* Author: alexgc
*/
#include <iostream>
#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;
}
}

63
cpp/DeltaFunction.h
View File

@ -1,63 +0,0 @@
/*
* DeltaFunction.h
*
* Created on: Aug 11, 2009
* Author: alexgc
*/
#ifndef DELTAFUNCTION_H_
#define DELTAFUNCTION_H_
#include <string>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#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<DeltaFunction> 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_ */

75
cpp/EqualityFactor.cpp
View File

@ -1,75 +0,0 @@
/*
* EqualityFactor.cpp
*
* Created on: Aug 10, 2009
* Author: alexgc
*/
#include "EqualityFactor.h"
#include <iostream>
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<string> EqualityFactor::keys() const {
list<string> 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;
}
}

95
cpp/EqualityFactor.h
View File

@ -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<FGConfig> {
public:
typedef boost::shared_ptr<EqualityFactor> 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<std::string> 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_ */

111
cpp/LinearConstraint.cpp Normal file
View File

@ -0,0 +1,111 @@
/*
* LinearConstraint.cpp
*
* Created on: Aug 10, 2009
* Author: alexgc
*/
#include <iostream>
#include <boost/foreach.hpp>
#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<std::string, Matrix>& 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<string, Matrix> 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<string, Matrix> p;
BOOST_FOREACH(p, As) {
// check for key existence
const_iterator it = f.As.find(p.first);
if (it == f.As.end()) return false;
Matrix f_mat = it->second;
// check matrix
if (!(f_mat == p.second)) return false;
}
return true;
}
bool LinearConstraint::involves(const std::string& key) const {
return As.find(key) != As.end();
}
list<string> LinearConstraint::keys(const std::string& key) const {
list<string> ret;
pair<string, Matrix> p;
BOOST_FOREACH(p, As) {
if (p.first != key)
ret.push_back(p.first);
}
return ret;
}
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;
}
}

131
cpp/LinearConstraint.h Normal file
View File

@ -0,0 +1,131 @@
/*
* LinearConstraint.h
*
* Created on: Aug 10, 2009
* Author: alexgc
*/
#ifndef EQUALITYFACTOR_H_
#define EQUALITYFACTOR_H_
#include <list>
#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<LinearConstraint> shared_ptr;
typedef std::map<std::string, Matrix>::iterator iterator;
typedef std::map<std::string, Matrix>::const_iterator const_iterator;
protected:
std::map<std::string, Matrix> As; // linear matrices
Vector b; // right-hand-side
public:
/**
* Default constructor
*/
LinearConstraint();
/**
* Constructor with initialization of a unary equality factor
* Creates an identity matrix for the underlying implementation and the constraint
* value becomes the RHS value - use for setting a variable to a fixed value
* @param constraint the value that the variable node is defined as equal to
* @param key identifier for the variable node
*/
LinearConstraint(const Vector& constraint, const std::string& key);
/**
* Constructor for binary constraint
* @param key for first node
* @param A Matrix for first node
* @param key for second node
* @param A Matrix for second node
* @param RHS b vector
*/
LinearConstraint(const std::string& node1, const Matrix& A1,
const std::string& node2, const Matrix& A2, const Vector& b);
/**
* Constructor for arbitrary numbers of nodes
* @param matrices is the full map of A matrices
* @param b is the RHS vector
*/
LinearConstraint(const std::map<std::string, Matrix>& matrices, const Vector& b);
/**
* Default Destructor
*/
~LinearConstraint() {}
/**
* 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<std::string> 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_ */

187
cpp/smallExample.cpp
View File

@ -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<NonlinearFactor<FGConfig> , FGConfig> createConstrainedNonlinearFactorGraph() {
ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig> , 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<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()
@ -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

24
cpp/smallExample.h
View File

@ -11,10 +11,11 @@
#pragma once
#include <boost/shared_ptr.hpp>
#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<NonlinearFactor<FGConfig>,FGConfig>
createConstrainedNonlinearFactorGraph();
// ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,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();
}

130
cpp/testConstrainedConditionalGaussian.cpp Normal file
View File

@ -0,0 +1,130 @@
/**
* @file testConstrainedConditionalGaussian.cpp
* @brief tests for constrained conditional gaussians
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "ConstrainedConditionalGaussian.h"
using namespace gtsam;
/* ************************************************************************* */
TEST (ConstrainedConditionalGaussian, basic_unary1 )
{
Vector v(2); v(0)=1.0; v(1)=2.0;
// check unary constructor that doesn't require an R matrix
// assumed identity matrix
ConstrainedConditionalGaussian unary(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);}
/* ************************************************************************* */

601
cpp/testConstrainedLinearFactorGraph.cpp
View File

@ -1,11 +1,9 @@
/*
* testConstrainedLinearFactorGraph.cpp
*
* Created on: Aug 10, 2009
* Author: Alex Cunningham
/**
* @file testConstrainedLinearFactorGraph.cpp
* @author Alex Cunningham
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#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<LinearFactor::shared_ptr> f1_set;
f1_set.insert(f1);
boost::shared_ptr<MutableLinearFactor> 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<LinearFactor::shared_ptr> f1_set;
// f1_set.insert(f1);
// boost::shared_ptr<MutableLinearFactor> 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);
}
/* ************************************************************************* */

120
cpp/testConstrainedNonlinearFactorGraph.cpp
View File

@ -15,66 +15,66 @@ using namespace gtsam;
typedef boost::shared_ptr<NonlinearFactor<FGConfig> > shared;
typedef ConstrainedNonlinearFactorGraph<NonlinearFactor<FGConfig>,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);}

27
cpp/testDeltaFunction.cpp
View File

@ -1,27 +0,0 @@
/*
* testDeltaFunction.cpp
*
* Created on: Aug 11, 2009
* Author: alexgc
*/
#include <CppUnitLite/TestHarness.h>
#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);}
/* ************************************************************************* */

62
cpp/testEqualityFactor.cpp
View File

@ -1,62 +0,0 @@
/*
* testEqualityFactor.cpp
*
* Created on: Aug 10, 2009
* Author: Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#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);}
/* ************************************************************************* */

246
cpp/testLinearConstraint.cpp Normal file
View File

@ -0,0 +1,246 @@
/**
* @file testLinearConstraint.cpp
* @brief Tests for linear constraints
* @author Alex Cunningham
*/
#include <CppUnitLite/TestHarness.h>
#include "LinearConstraint.h"
#include "smallExample.h"
using namespace gtsam;
using namespace std;
/* ************************************************************************* */
TEST ( LinearConstraint, basic_unary )
{
// create an initialized factor with a unary factor
Vector v(2); v(0)=1.2; v(1)=3.4;
string key = "x0";
LinearConstraint factor(v, key);
// get the data back out of it
CHECK(assert_equal(v, factor.get_b()));
Matrix expected = eye(2);
CHECK(assert_equal(expected, factor.get_A("x0")));
}
/* ************************************************************************* */
TEST( LinearConstraint, basic_binary )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
LinearConstraint lc("x1", A1, "x2", A2, b);
// verify contents
CHECK( assert_equal(A1, lc.get_A("x1")));
CHECK( assert_equal(A2, lc.get_A("x2")));
CHECK( assert_equal(b, lc.get_b()));
}
/* ************************************************************************* */
TEST( LinearConstraint, basic_arbitrary )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Matrix A3(2,2);
A3(0,0) = 10.0 ; A3(0,1) = 7.0;
A3(1,0) = 7.0 ; A3(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
// build a map
map<string, Matrix> matrices;
matrices.insert(make_pair("x1", A1));
matrices.insert(make_pair("x2", A2));
matrices.insert(make_pair("x3", A3));
LinearConstraint lc(matrices, b);
// verify contents
CHECK( assert_equal(A1, lc.get_A("x1")));
CHECK( assert_equal(A2, lc.get_A("x2")));
CHECK( assert_equal(A3, lc.get_A("x3")));
CHECK( assert_equal(b, lc.get_b()));
}
/* ************************************************************************* */
TEST ( LinearConstraint, size )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Matrix A3(2,2);
A3(0,0) = 10.0 ; A3(0,1) = 7.0;
A3(1,0) = 7.0 ; A3(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
// build some constraints
LinearConstraint lc1(b, "x1");
LinearConstraint lc2("x1", A1, "x2", A2, b);
map<string, Matrix> matrices;
matrices.insert(make_pair("x1", A1));
matrices.insert(make_pair("x2", A2));
matrices.insert(make_pair("x3", A3));
LinearConstraint lc3(matrices, b);
CHECK(lc1.size() == 1);
CHECK(lc2.size() == 2);
CHECK(lc3.size() == 3);
}
/* ************************************************************************* */
TEST ( LinearConstraint, equals )
{
Matrix A1(2,2);
A1(0,0) = -10.0 ; A1(0,1) = 0.0;
A1(1,0) = 0.0 ; A1(1,1) = -10.0;
Matrix A2(2,2);
A2(0,0) = 10.0 ; A2(0,1) = 0.0;
A2(1,0) = 0.0 ; A2(1,1) = 10.0;
Vector b(2);
b(0) = 2 ; b(1) = -1;
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<string> actual = lc.keys();
list<string> expected;
expected.push_back("x1");
expected.push_back("x2");
CHECK(actual == expected);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */