135 lines
3.3 KiB
C++
135 lines
3.3 KiB
C++
/*
|
|
* 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;
|
|
}
|
|
|
|
}
|