diff --git a/cpp/ConstrainedChordalBayesNet.cpp b/cpp/ConstrainedChordalBayesNet.cpp deleted file mode 100644 index 32c52154d..000000000 --- a/cpp/ConstrainedChordalBayesNet.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * ConstrainedChordalBayesNet.cpp - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - -#include -#include -#include "ConstrainedChordalBayesNet.h" - -// trick from some reading group -#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) - -namespace gtsam { - -using namespace std; - -ConstrainedChordalBayesNet::ConstrainedChordalBayesNet() -: ChordalBayesNet() -{ -} - -ConstrainedChordalBayesNet::ConstrainedChordalBayesNet(const ChordalBayesNet& cbn) -: ChordalBayesNet(cbn) -{ -} - -ConstrainedChordalBayesNet::~ConstrainedChordalBayesNet() { - // TODO Auto-generated destructor stub -} - -void ConstrainedChordalBayesNet::insert_df(const string& key, DeltaFunction::shared_ptr node) -{ - keys.push_front(key); - delta_nodes.insert(make_pair(key,node)); -} - -void ConstrainedChordalBayesNet::insert(const string& key, ConditionalGaussian::shared_ptr node) -{ - keys.push_front(key); - nodes.insert(make_pair(key,node)); -} - -bool ConstrainedChordalBayesNet::equals(const ConstrainedChordalBayesNet& cbn) const -{ - // check delta function nodes - if (delta_nodes.size() != cbn.delta_nodes.size()) return false; - const_delta_iterator it1 = delta_nodes.begin(), it2 = cbn.delta_nodes.begin(); - for(; it1 != delta_nodes.end(); it1++, it2++){ - const string& j1 = it1->first, j2 = it2->first; - DeltaFunction::shared_ptr node1 = it1->second, node2 = it2->second; - if (j1 != j2) return false; - if (!node1->equals(*node2)) return false; - } - - // use default equals - return convert().equals(cbn.convert()); -} - -bool assert_equal(const ConstrainedChordalBayesNet& expected, const ConstrainedChordalBayesNet& actual, double tol) -{ - bool ret = expected.equals(actual); - if (!ret) - { - cout << "Not Equal!" << endl; - expected.print("Expected"); - actual.print("Actual"); - } - return ret; -} - -void ConstrainedChordalBayesNet::print(const std::string& s) const -{ - cout << s << ":" << endl; - pair p1; - BOOST_FOREACH(p1, delta_nodes) - { - cout << " " << p1.first << endl; - p1.second->print(); - } - pair p2; - BOOST_FOREACH(p2, nodes) - { - cout << " " << p2.first << endl; - p2.second->print(); - } -} - -boost::shared_ptr ConstrainedChordalBayesNet::optimize() -{ - boost::shared_ptr empty(new FGConfig); - return optimize(empty); -} - -boost::shared_ptr ConstrainedChordalBayesNet::optimize(const boost::shared_ptr &c) -{ - // check if it is necessary to handle delta functions - if (delta_nodes.size() == 0) - { - return ChordalBayesNet::optimize(c); - } - - // verifying that there are no incorrect values for variables with delta functions - vector keys = c->get_names(); - BOOST_FOREACH(string k, keys) - { - if (delta_nodes.count(k) && !(delta_nodes[k]->get_value() == c->get(k))) - throw(std::invalid_argument("ConstrainedChordalBayesNet:: Passed incorrect value for " + k + " to optimize()")); - } - - // create a config with the delta functions - pair p; - BOOST_FOREACH(p, delta_nodes) - { - Vector v = p.second->get_value(); - string key = p.first; - c->insert(key, v); - } - return convert().optimize(c); -} - -ChordalBayesNet ConstrainedChordalBayesNet::convert() const -{ - ChordalBayesNet ret; - pair p; - BOOST_FOREACH(p, nodes) - { - ret.insert(p.first, p.second); - } - return ret; -} - -} diff --git a/cpp/ConstrainedChordalBayesNet.h b/cpp/ConstrainedChordalBayesNet.h deleted file mode 100644 index 39dcd6f35..000000000 --- a/cpp/ConstrainedChordalBayesNet.h +++ /dev/null @@ -1,74 +0,0 @@ -/* - * ConstrainedChordalBayesNet.h - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - -#ifndef CONSTRAINEDCHORDALBAYESNET_H_ -#define CONSTRAINEDCHORDALBAYESNET_H_ - -#include "ChordalBayesNet.h" -#include "DeltaFunction.h" - -namespace gtsam { - -class ConstrainedChordalBayesNet : public ChordalBayesNet{ - -protected: - std::map delta_nodes; - -public: - typedef std::map::const_iterator const_delta_iterator; - typedef std::map::iterator delta_iterator; - -public: - typedef boost::shared_ptr shared_ptr; - - /** - * Default Constructor - */ - ConstrainedChordalBayesNet(); - - /** - * Copies an existing ChordalBayesNet - */ - ConstrainedChordalBayesNet(const ChordalBayesNet& cbn); - - virtual ~ConstrainedChordalBayesNet(); - - /** insert: use reverse topological sort (i.e. parents last) */ - void insert_df(const std::string& key, DeltaFunction::shared_ptr node); - void insert(const std::string& key, ConditionalGaussian::shared_ptr node); - - /** optimize the solution - just a wrapper on the existing optimize implementation */ - boost::shared_ptr optimize(); - boost::shared_ptr optimize(const boost::shared_ptr &c); - - /** convert to a regular cbn - strips out the delta functions - * TODO: make this check whether this is a safe conversion - */ - ChordalBayesNet convert() const; - - /** get delta functions by key */ - DeltaFunction::shared_ptr get_delta(const std::string& key) const - { - const_delta_iterator cg = delta_nodes.find(key); - assert( cg != delta_nodes.end() ); - return cg->second; - } - - /** check equality */ - bool equals(const ConstrainedChordalBayesNet& cbn) const; - - /** prints the contents */ - void print(const std::string& s="") const; -}; - - -/** check equality for testing */ -bool assert_equal(const ConstrainedChordalBayesNet& expected, const ConstrainedChordalBayesNet& actual, double tol=1e-9); - -} - -#endif /* CONSTRAINEDCHORDALBAYESNET_H_ */ diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f3ead9a64..0ed00606f 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -58,22 +58,22 @@ testMatrix_LDADD = libgtsam.la # nodes sources += FGConfig.cpp Ordering.cpp LinearFactor.cpp NonlinearFactor.cpp -sources += ConditionalGaussian.cpp EqualityFactor.cpp DeltaFunction.cpp -check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testEqualityFactor testDeltaFunction +sources += ConditionalGaussian.cpp LinearConstraint.cpp ConstrainedConditionalGaussian.cpp +check_PROGRAMS += testFGConfig testLinearFactor testConditionalGaussian testNonlinearFactor testLinearConstraint testConstrainedConditionalGaussian example = smallExample.cpp testFGConfig_SOURCES = testFGConfig.cpp testLinearFactor_SOURCES = $(example) testLinearFactor.cpp testConditionalGaussian_SOURCES = $(example) testConditionalGaussian.cpp testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp -testEqualityFactor_SOURCES = $(example) testEqualityFactor.cpp -testDeltaFunction_SOURCES = testDeltaFunction.cpp +testLinearConstraint_SOURCES = $(example) testLinearConstraint.cpp +testConstrainedConditionalGaussian_SOURCES = testConstrainedConditionalGaussian.cpp testFGConfig_LDADD = libgtsam.la testLinearFactor_LDADD = libgtsam.la testConditionalGaussian_LDADD = libgtsam.la testNonlinearFactor_LDADD = libgtsam.la -testEqualityFactor_LDADD = libgtsam.la -testDeltaFunction_LDADD = libgtsam.la +testLinearConstraint_LDADD = libgtsam.la +testConstrainedConditionalGaussian_LDADD = libgtsam.la # not the correct way, I'm sure: Kai ? timeLinearFactor: timeLinearFactor.cpp @@ -81,14 +81,13 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include timeLinearFactor: LDFLAGS += -L.libs -lgtsam # graphs -sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp +sources += ChordalBayesNet.cpp sources += LinearFactorGraph.cpp sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp -check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph +check_PROGRAMS += testChordalBayesNet testFactorgraph check_PROGRAMS += testLinearFactorGraph testNonlinearFactorGraph testNonlinearOptimizer check_PROGRAMS += testConstrainedNonlinearFactorGraph testConstrainedLinearFactorGraph testChordalBayesNet_SOURCES = $(example) testChordalBayesNet.cpp -testConstrainedChordalBayesNet_SOURCES = $(example) testConstrainedChordalBayesNet.cpp testFactorgraph_SOURCES = testFactorgraph.cpp testLinearFactorGraph_SOURCES = $(example) testLinearFactorGraph.cpp testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp @@ -103,7 +102,6 @@ testNonlinearFactorGraph_LDADD = libgtsam.la testNonlinearOptimizer_LDADD = libgtsam.la testConstrainedNonlinearFactorGraph_LDADD = libgtsam.la testConstrainedLinearFactorGraph_LDADD = libgtsam.la -testConstrainedChordalBayesNet_LDADD = libgtsam.la # geometry sources += Point2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp diff --git a/cpp/testConstrainedChordalBayesNet.cpp b/cpp/testConstrainedChordalBayesNet.cpp deleted file mode 100644 index e10bb681d..000000000 --- a/cpp/testConstrainedChordalBayesNet.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * testConstrainedChordalBayesNet.cpp - * - * Created on: Aug 11, 2009 - * Author: alexgc - */ - -#include -#include -#include "ConstrainedChordalBayesNet.h" -#include "smallExample.h" - -using namespace gtsam; -using namespace std; - -TEST ( ConstrainedChordalBayesNet, basic ) -{ - ConstrainedChordalBayesNet ccbn = createConstrainedChordalBayesNet(); - FGConfig c = createConstrainedConfig(); - - // get data back out - DeltaFunction::shared_ptr x0 = ccbn.get_delta("x0"); - ConditionalGaussian::shared_ptr x1 = ccbn.get("x1"); - - Matrix R = eye(2); - Vector d = c["x1"]; - double sigma = 0.1; - ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); - - DeltaFunction::shared_ptr f2(new DeltaFunction(c["x0"], "x0")); - - CHECK(f1->equals(*x1)); - CHECK(f2->equals(*x0)); -} - -TEST ( ConstrainedChordalBayesNet, equals ) -{ - // basic check - ConstrainedChordalBayesNet ccbn1 = createConstrainedChordalBayesNet(); - ConstrainedChordalBayesNet ccbn2 = createConstrainedChordalBayesNet(); - CHECK(ccbn1.equals(ccbn2)); - - // ensure deltas are compared - ConstrainedChordalBayesNet ccbn3; - FGConfig c = createConstrainedConfig(); - Matrix R = eye(2); - Vector d = c["x1"]; - double sigma = 0.1; - ConditionalGaussian::shared_ptr f1(new ConditionalGaussian(d/sigma, R/sigma)); - ccbn3.insert("x1", f1); - - CHECK(!ccbn1.equals(ccbn3)); -} - -TEST ( ConstrainedChordalBayesNet, copy ) -{ - // use copy to allow testing with old example - ChordalBayesNet cbn = createSmallChordalBayesNet(); - ConstrainedChordalBayesNet actual(cbn); - - ConditionalGaussian::shared_ptr x = cbn.get("x"); - ConditionalGaussian::shared_ptr y = cbn.get("y"); - - ConstrainedChordalBayesNet expected; - expected.insert("x",x); - expected.insert("y",y); - - CHECK(assert_equal(actual, expected)); -} - -TEST ( ConstrainedChordalBayesNet, optimize_baseline ) -{ - // optimize simple example - ChordalBayesNet cbn = createSmallChordalBayesNet(); - ConstrainedChordalBayesNet ccbn(cbn); - boost::shared_ptr actual = ccbn.optimize(); - - // create expected - FGConfig expected; - Vector x(1), y(1); x(0)=4.; y(0)=5.; - expected.insert("x", x); - expected.insert("y", y); - - // verify - CHECK(expected.equals(*actual)); -} - -TEST ( ConstrainedChordalBayesNet, optimize ) -{ - ConstrainedChordalBayesNet ccbn = createConstrainedChordalBayesNet(); - FGConfig expected = createConstrainedConfig(); - - // full optimization - boost::shared_ptr actual1 = ccbn.optimize(); - CHECK(expected.equals(*actual1)); - - // plug in a config - boost::shared_ptr c1(new FGConfig); - c1->insert("x0", expected["x0"]); - boost::shared_ptr actual2 = ccbn.optimize(c1); - CHECK(expected.equals(*actual2)); - - // plug in the other value - boost::shared_ptr c2(new FGConfig); - c2->insert("x1", expected["x1"]); - boost::shared_ptr actual3 = ccbn.optimize(c2); - CHECK(expected.equals(*actual3)); -} - - - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} -/* ************************************************************************* */ -