Removed ConstrainedChordalBayesNet and associated test. It may return if ConstrainedConditionalGaussians need more involved processing in future versions, but for all versions of LinearConstraint, there is no need for a special Constrained CBN.
Alex Cunningham
parent
989f290c99
commit
3efe95abee
|
|
@ -1,134 +0,0 @@
|
|||
/*
|
||||
* ConstrainedChordalBayesNet.cpp
|
||||
*
|
||||
* Created on: Aug 11, 2009
|
||||
* Author: alexgc
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/foreach.hpp>
|
||||
#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<string, DeltaFunction::shared_ptr> p1;
|
||||
BOOST_FOREACH(p1, delta_nodes)
|
||||
{
|
||||
cout << " " << p1.first << endl;
|
||||
p1.second->print();
|
||||
}
|
||||
pair<string, ConditionalGaussian::shared_ptr> p2;
|
||||
BOOST_FOREACH(p2, nodes)
|
||||
{
|
||||
cout << " " << p2.first << endl;
|
||||
p2.second->print();
|
||||
}
|
||||
}
|
||||
|
||||
boost::shared_ptr<FGConfig> ConstrainedChordalBayesNet::optimize()
|
||||
{
|
||||
boost::shared_ptr<FGConfig> empty(new FGConfig);
|
||||
return optimize(empty);
|
||||
}
|
||||
|
||||
boost::shared_ptr<FGConfig> ConstrainedChordalBayesNet::optimize(const boost::shared_ptr<FGConfig> &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<string> 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<string, DeltaFunction::shared_ptr> 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<string, ConditionalGaussian::shared_ptr> p;
|
||||
BOOST_FOREACH(p, nodes)
|
||||
{
|
||||
ret.insert(p.first, p.second);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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<std::string, DeltaFunction::shared_ptr> delta_nodes;
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, DeltaFunction::shared_ptr>::const_iterator const_delta_iterator;
|
||||
typedef std::map<std::string, DeltaFunction::shared_ptr>::iterator delta_iterator;
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<ConstrainedChordalBayesNet> 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<FGConfig> optimize();
|
||||
boost::shared_ptr<FGConfig> optimize(const boost::shared_ptr<FGConfig> &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_ */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,115 +0,0 @@
|
|||
/*
|
||||
* testConstrainedChordalBayesNet.cpp
|
||||
*
|
||||
* Created on: Aug 11, 2009
|
||||
* Author: alexgc
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#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<FGConfig> 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<FGConfig> actual1 = ccbn.optimize();
|
||||
CHECK(expected.equals(*actual1));
|
||||
|
||||
// plug in a config
|
||||
boost::shared_ptr<FGConfig> c1(new FGConfig);
|
||||
c1->insert("x0", expected["x0"]);
|
||||
boost::shared_ptr<FGConfig> actual2 = ccbn.optimize(c1);
|
||||
CHECK(expected.equals(*actual2));
|
||||
|
||||
// plug in the other value
|
||||
boost::shared_ptr<FGConfig> c2(new FGConfig);
|
||||
c2->insert("x1", expected["x1"]);
|
||||
boost::shared_ptr<FGConfig> actual3 = ccbn.optimize(c2);
|
||||
CHECK(expected.equals(*actual3));
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue