New add_factors method
Frank Dellaert
parent
da1b7f92f2
commit
495a840921
|
|
@ -26,74 +26,105 @@
|
|||
#include <boost/bind.hpp>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <algorithm>
|
||||
#include <iostream> // for cout :-(
|
||||
#include <sstream>
|
||||
#include <iostream> // for cout :-(
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
std::stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i])
|
||||
factors_[i]->print(ss.str(), formatter);
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
void FactorGraph<FACTOR>::print(const std::string& s,
|
||||
const KeyFormatter& formatter) const {
|
||||
std::cout << s << std::endl;
|
||||
std::cout << "size: " << size() << std::endl;
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
std::stringstream ss;
|
||||
ss << "factor " << i << ": ";
|
||||
if (factors_[i]) factors_[i]->print(ss.str(), formatter);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
|
||||
// check whether the two factor graphs have the same number of factors.
|
||||
if (factors_.size() != fg.size()) return false;
|
||||
|
||||
// check whether the factors are the same, in same order.
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
if (f1 == NULL && f2 == NULL) continue;
|
||||
if (f1 == NULL || f2 == NULL) return false;
|
||||
if (!f1->equals(*f2, tol)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
size_t FactorGraph<FACTOR>::nrFactors() const {
|
||||
size_t size_ = 0;
|
||||
for (const sharedFactor& factor : factors_)
|
||||
if (factor) size_++;
|
||||
return size_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
KeySet FactorGraph<FACTOR>::keys() const {
|
||||
KeySet keys;
|
||||
for (const sharedFactor& factor : this->factors_) {
|
||||
if (factor) keys.insert(factor->begin(), factor->end());
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
KeyVector FactorGraph<FACTOR>::keyVector() const {
|
||||
KeyVector keys;
|
||||
keys.reserve(2 * size()); // guess at size
|
||||
for (const sharedFactor& factor : factors_)
|
||||
if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
|
||||
std::sort(keys.begin(), keys.end());
|
||||
auto last = std::unique(keys.begin(), keys.end());
|
||||
keys.erase(last, keys.end());
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
template <typename CONTAINER, typename>
|
||||
FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
|
||||
bool useEmptySlots) {
|
||||
const size_t num_factors = factors.size();
|
||||
FactorIndices newFactorIndices(num_factors);
|
||||
if (useEmptySlots) {
|
||||
size_t i = 0;
|
||||
for (size_t j = 0; j < num_factors; ++j) {
|
||||
// Loop to find the next available factor slot
|
||||
do {
|
||||
if (i >= size())
|
||||
// Make room for remaining factors, happens only once.
|
||||
resize(size() + num_factors - j);
|
||||
else if (at(i))
|
||||
++i; // Move on to the next slot or past end.
|
||||
else
|
||||
break; // We found an empty slot, break to fill it.
|
||||
} while (true);
|
||||
|
||||
// Use the current slot, updating graph and newFactorSlots.
|
||||
at(i) = factors[j];
|
||||
newFactorIndices[j] = i;
|
||||
}
|
||||
} else {
|
||||
// We're not looking for unused slots, so just add the factors at the end.
|
||||
for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
|
||||
push_back(factors);
|
||||
}
|
||||
return newFactorIndices;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
|
||||
/** check whether the two factor graphs have the same number of factors_ */
|
||||
if (factors_.size() != fg.size()) return false;
|
||||
|
||||
/** check whether the factors_ are the same */
|
||||
for (size_t i = 0; i < factors_.size(); i++) {
|
||||
// TODO: Doesn't this force order of factor insertion?
|
||||
sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
|
||||
if (f1 == NULL && f2 == NULL) continue;
|
||||
if (f1 == NULL || f2 == NULL) return false;
|
||||
if (!f1->equals(*f2, tol)) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
size_t FactorGraph<FACTOR>::nrFactors() const {
|
||||
size_t size_ = 0;
|
||||
for(const sharedFactor& factor: factors_)
|
||||
if (factor) size_++;
|
||||
return size_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
KeySet FactorGraph<FACTOR>::keys() const {
|
||||
KeySet keys;
|
||||
for(const sharedFactor& factor: this->factors_) {
|
||||
if(factor)
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
}
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class FACTOR>
|
||||
KeyVector FactorGraph<FACTOR>::keyVector() const {
|
||||
KeyVector keys;
|
||||
keys.reserve(2 * size()); // guess at size
|
||||
for (const sharedFactor& factor: factors_)
|
||||
if (factor)
|
||||
keys.insert(keys.end(), factor->begin(), factor->end());
|
||||
std::sort(keys.begin(), keys.end());
|
||||
auto last = std::unique(keys.begin(), keys.end());
|
||||
keys.erase(last, keys.end());
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -273,6 +273,14 @@ class FactorGraph {
|
|||
bayesTree.addFactorsToGraph(*this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add new factors to a factor graph and returns a list of new factor indices,
|
||||
* optionally finding and reusing empty factor slots.
|
||||
*/
|
||||
template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
|
||||
FactorIndices add_factors(const CONTAINER& factors,
|
||||
bool useEmptySlots = false);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -15,14 +15,16 @@
|
|||
* @author Christian Potthast
|
||||
**/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
||||
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/symbolic/SymbolicBayesNet.h>
|
||||
#include <gtsam/symbolic/SymbolicBayesTree.h>
|
||||
#include <gtsam/symbolic/SymbolicConditional.h>
|
||||
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/assign/std/set.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -46,11 +48,10 @@ TEST(SymbolicFactorGraph, keys2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SymbolicFactorGraph, eliminateFullSequential)
|
||||
{
|
||||
TEST(SymbolicFactorGraph, eliminateFullSequential) {
|
||||
// Test with simpleTestGraph1
|
||||
Ordering order;
|
||||
order += 0,1,2,3,4;
|
||||
order += 0, 1, 2, 3, 4;
|
||||
SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order);
|
||||
EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1));
|
||||
|
||||
|
|
@ -60,24 +61,20 @@ TEST(SymbolicFactorGraph, eliminateFullSequential)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SymbolicFactorGraph, eliminatePartialSequential)
|
||||
{
|
||||
TEST(SymbolicFactorGraph, eliminatePartialSequential) {
|
||||
// Eliminate 0 and 1
|
||||
const Ordering order = list_of(0)(1);
|
||||
|
||||
const SymbolicBayesNet expectedBayesNet = list_of
|
||||
(SymbolicConditional(0,1,2))
|
||||
(SymbolicConditional(1,2,3,4));
|
||||
const SymbolicBayesNet expectedBayesNet =
|
||||
list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4));
|
||||
|
||||
const SymbolicFactorGraph expectedSfg = list_of
|
||||
(SymbolicFactor(2,3))
|
||||
(SymbolicFactor(4,5))
|
||||
(SymbolicFactor(2,3,4));
|
||||
const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))(
|
||||
SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
|
||||
|
||||
SymbolicBayesNet::shared_ptr actualBayesNet;
|
||||
SymbolicFactorGraph::shared_ptr actualSfg;
|
||||
boost::tie(actualBayesNet, actualSfg) =
|
||||
simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1)));
|
||||
simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1)));
|
||||
|
||||
EXPECT(assert_equal(expectedSfg, *actualSfg));
|
||||
EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
|
||||
|
|
@ -85,75 +82,71 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
|
|||
SymbolicBayesNet::shared_ptr actualBayesNet2;
|
||||
SymbolicFactorGraph::shared_ptr actualSfg2;
|
||||
boost::tie(actualBayesNet2, actualSfg2) =
|
||||
simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
|
||||
simpleTestGraph2.eliminatePartialSequential(
|
||||
list_of(0)(1).convert_to_container<KeyVector>());
|
||||
|
||||
EXPECT(assert_equal(expectedSfg, *actualSfg2));
|
||||
EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SymbolicFactorGraph, eliminateFullMultifrontal)
|
||||
{
|
||||
Ordering ordering; ordering += 0,1,2,3;
|
||||
SymbolicBayesTree actual1 =
|
||||
*simpleChain.eliminateMultifrontal(ordering);
|
||||
TEST(SymbolicFactorGraph, eliminateFullMultifrontal) {
|
||||
Ordering ordering;
|
||||
ordering += 0, 1, 2, 3;
|
||||
SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering);
|
||||
EXPECT(assert_equal(simpleChainBayesTree, actual1));
|
||||
|
||||
SymbolicBayesTree actual2 =
|
||||
*asiaGraph.eliminateMultifrontal(asiaOrdering);
|
||||
SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering);
|
||||
EXPECT(assert_equal(asiaBayesTree, actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
|
||||
{
|
||||
TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
|
||||
SymbolicBayesTree expectedBayesTree;
|
||||
SymbolicConditional::shared_ptr root = boost::make_shared<SymbolicConditional>(
|
||||
SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
|
||||
expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeClique>(root));
|
||||
SymbolicConditional::shared_ptr root =
|
||||
boost::make_shared<SymbolicConditional>(
|
||||
SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
|
||||
expectedBayesTree.insertRoot(
|
||||
boost::make_shared<SymbolicBayesTreeClique>(root));
|
||||
|
||||
SymbolicFactorGraph expectedFactorGraph = list_of
|
||||
(SymbolicFactor(0,1))
|
||||
(SymbolicFactor(0,2))
|
||||
(SymbolicFactor(1,3))
|
||||
(SymbolicFactor(2,3))
|
||||
(SymbolicFactor(1));
|
||||
SymbolicFactorGraph expectedFactorGraph =
|
||||
list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))(
|
||||
SymbolicFactor(2, 3))(SymbolicFactor(1));
|
||||
|
||||
SymbolicBayesTree::shared_ptr actualBayesTree;
|
||||
SymbolicFactorGraph::shared_ptr actualFactorGraph;
|
||||
boost::tie(actualBayesTree, actualFactorGraph) =
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5)));
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5)));
|
||||
|
||||
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
|
||||
EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
|
||||
|
||||
SymbolicBayesTree expectedBayesTree2;
|
||||
SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared<SymbolicBayesTreeClique>(
|
||||
boost::make_shared<SymbolicConditional>(4,1));
|
||||
SymbolicBayesTreeClique::shared_ptr root2 =
|
||||
boost::make_shared<SymbolicBayesTreeClique>(
|
||||
boost::make_shared<SymbolicConditional>(4, 1));
|
||||
root2->children.push_back(boost::make_shared<SymbolicBayesTreeClique>(
|
||||
boost::make_shared<SymbolicConditional>(5,4)));
|
||||
boost::make_shared<SymbolicConditional>(5, 4)));
|
||||
expectedBayesTree2.insertRoot(root2);
|
||||
|
||||
SymbolicBayesTree::shared_ptr actualBayesTree2;
|
||||
SymbolicFactorGraph::shared_ptr actualFactorGraph2;
|
||||
boost::tie(actualBayesTree2, actualFactorGraph2) =
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
|
||||
simpleTestGraph2.eliminatePartialMultifrontal(
|
||||
list_of<Key>(4)(5).convert_to_container<KeyVector>());
|
||||
|
||||
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
|
||||
EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet)
|
||||
{
|
||||
SymbolicBayesNet expectedBayesNet = list_of
|
||||
(SymbolicConditional(0, 1, 2))
|
||||
(SymbolicConditional(1, 2, 3))
|
||||
(SymbolicConditional(2, 3))
|
||||
(SymbolicConditional(3));
|
||||
TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
|
||||
SymbolicBayesNet expectedBayesNet =
|
||||
list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))(
|
||||
SymbolicConditional(2, 3))(SymbolicConditional(3));
|
||||
|
||||
SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet(
|
||||
Ordering(list_of(0)(1)(2)(3)));
|
||||
Ordering(list_of(0)(1)(2)(3)));
|
||||
EXPECT(assert_equal(expectedBayesNet, actual1));
|
||||
}
|
||||
|
||||
|
|
@ -167,104 +160,75 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
|
|||
|
||||
// create expected Chordal bayes Net
|
||||
SymbolicBayesNet expected;
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(0,1,2));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(1,2));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(0, 1, 2));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(1, 2));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(2));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(3,4));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(3, 4));
|
||||
expected.push_back(boost::make_shared<SymbolicConditional>(4));
|
||||
|
||||
Ordering order;
|
||||
order += 0,1,2,3,4;
|
||||
order += 0, 1, 2, 3, 4;
|
||||
SymbolicBayesNet actual = *fg.eliminateSequential(order);
|
||||
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST(SymbolicFactorGraph, marginals)
|
||||
//{
|
||||
// // Create factor graph
|
||||
// SymbolicFactorGraph fg;
|
||||
// fg.push_factor(0, 1);
|
||||
// fg.push_factor(0, 2);
|
||||
// fg.push_factor(1, 4);
|
||||
// fg.push_factor(2, 4);
|
||||
// fg.push_factor(3, 4);
|
||||
//
|
||||
// // eliminate
|
||||
// SymbolicSequentialSolver solver(fg);
|
||||
// SymbolicBayesNet::shared_ptr actual = solver.eliminate();
|
||||
// SymbolicBayesNet expected;
|
||||
// expected.push_front(boost::make_shared<IndexConditional>(4));
|
||||
// expected.push_front(boost::make_shared<IndexConditional>(3, 4));
|
||||
// expected.push_front(boost::make_shared<IndexConditional>(2, 4));
|
||||
// expected.push_front(boost::make_shared<IndexConditional>(1, 2, 4));
|
||||
// expected.push_front(boost::make_shared<IndexConditional>(0, 1, 2));
|
||||
// EXPECT(assert_equal(expected,*actual));
|
||||
//
|
||||
// {
|
||||
// // jointBayesNet
|
||||
// vector<Index> js;
|
||||
// js.push_back(0);
|
||||
// js.push_back(4);
|
||||
// js.push_back(3);
|
||||
// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
|
||||
// SymbolicBayesNet expectedBN;
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(3));
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(4, 3));
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(0, 4));
|
||||
// EXPECT( assert_equal(expectedBN,*actualBN));
|
||||
//
|
||||
// // jointFactorGraph
|
||||
// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
|
||||
// SymbolicFactorGraph expectedFG;
|
||||
// expectedFG.push_factor(0, 4);
|
||||
// expectedFG.push_factor(4, 3);
|
||||
// expectedFG.push_factor(3);
|
||||
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
|
||||
// }
|
||||
//
|
||||
// {
|
||||
// // jointBayesNet
|
||||
// vector<Index> js;
|
||||
// js.push_back(0);
|
||||
// js.push_back(2);
|
||||
// js.push_back(3);
|
||||
// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
|
||||
// SymbolicBayesNet expectedBN;
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(2));
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(3, 2));
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(0, 3, 2));
|
||||
// EXPECT( assert_equal(expectedBN,*actualBN));
|
||||
//
|
||||
// // jointFactorGraph
|
||||
// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
|
||||
// SymbolicFactorGraph expectedFG;
|
||||
// expectedFG.push_factor(0, 3, 2);
|
||||
// expectedFG.push_factor(3, 2);
|
||||
// expectedFG.push_factor(2);
|
||||
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
|
||||
// }
|
||||
//
|
||||
// {
|
||||
// // conditionalBayesNet
|
||||
// vector<Index> js;
|
||||
// js.push_back(0);
|
||||
// js.push_back(2);
|
||||
// js.push_back(3);
|
||||
// size_t nrFrontals = 2;
|
||||
// SymbolicBayesNet::shared_ptr actualBN = //
|
||||
// solver.conditionalBayesNet(js, nrFrontals);
|
||||
// SymbolicBayesNet expectedBN;
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(2, 3));
|
||||
// expectedBN.push_front(boost::make_shared<IndexConditional>(0, 2, 3));
|
||||
// EXPECT( assert_equal(expectedBN,*actualBN));
|
||||
// }
|
||||
//}
|
||||
TEST(SymbolicFactorGraph, marginals) {
|
||||
// Create factor graph
|
||||
SymbolicFactorGraph fg;
|
||||
fg.push_factor(0, 1);
|
||||
fg.push_factor(0, 2);
|
||||
fg.push_factor(1, 4);
|
||||
fg.push_factor(2, 4);
|
||||
fg.push_factor(3, 4);
|
||||
|
||||
// eliminate
|
||||
Ordering ord(list_of(3)(4)(2)(1)(0));
|
||||
auto actual = fg.eliminateSequential(ord);
|
||||
SymbolicBayesNet expected;
|
||||
expected.emplace_shared<SymbolicConditional>(3, 4);
|
||||
expected.emplace_shared<SymbolicConditional>(4, 1, 2);
|
||||
expected.emplace_shared<SymbolicConditional>(2, 0, 1);
|
||||
expected.emplace_shared<SymbolicConditional>(1, 0);
|
||||
expected.emplace_shared<SymbolicConditional>(0);
|
||||
EXPECT(assert_equal(expected, *actual));
|
||||
|
||||
{
|
||||
// jointBayesNet
|
||||
Ordering ord(list_of(0)(4)(3));
|
||||
auto actual = fg.eliminatePartialSequential(ord);
|
||||
SymbolicBayesNet expectedBN;
|
||||
expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
|
||||
expectedBN.emplace_shared<SymbolicConditional>(4, 1, 2, 3);
|
||||
expectedBN.emplace_shared<SymbolicConditional>(3, 1, 2);
|
||||
EXPECT(assert_equal(expectedBN, *(actual.first)));
|
||||
}
|
||||
|
||||
{
|
||||
// jointBayesNet
|
||||
Ordering ord(list_of(0)(2)(3));
|
||||
auto actual = fg.eliminatePartialSequential(ord);
|
||||
SymbolicBayesNet expectedBN;
|
||||
expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
|
||||
expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
|
||||
expectedBN.emplace_shared<SymbolicConditional>(3, 4);
|
||||
EXPECT(assert_equal(expectedBN, *(actual.first)));
|
||||
}
|
||||
|
||||
{
|
||||
// conditionalBayesNet
|
||||
Ordering ord(list_of(0)(2));
|
||||
auto actual = fg.eliminatePartialSequential(ord);
|
||||
SymbolicBayesNet expectedBN;
|
||||
expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
|
||||
expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
|
||||
EXPECT(assert_equal(expectedBN, *(actual.first)));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SymbolicFactorGraph, constructFromBayesNet )
|
||||
{
|
||||
TEST(SymbolicFactorGraph, constructFromBayesNet) {
|
||||
// create expected factor graph
|
||||
SymbolicFactorGraph expected;
|
||||
expected.push_factor(0, 1, 2);
|
||||
|
|
@ -284,8 +248,7 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SymbolicFactorGraph, constructFromBayesTree )
|
||||
{
|
||||
TEST(SymbolicFactorGraph, constructFromBayesTree) {
|
||||
// create expected factor graph
|
||||
SymbolicFactorGraph expected;
|
||||
expected.push_factor(_E_, _L_, _B_);
|
||||
|
|
@ -300,8 +263,7 @@ TEST( SymbolicFactorGraph, constructFromBayesTree )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SymbolicFactorGraph, push_back )
|
||||
{
|
||||
TEST(SymbolicFactorGraph, push_back) {
|
||||
// Create two factor graphs and expected combined graph
|
||||
SymbolicFactorGraph fg1, fg2, expected;
|
||||
|
||||
|
|
@ -321,8 +283,47 @@ TEST( SymbolicFactorGraph, push_back )
|
|||
actual.push_back(fg1);
|
||||
actual.push_back(fg2);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
// combine in second way
|
||||
SymbolicFactorGraph actual2 = fg1;
|
||||
actual2.push_back(fg2);
|
||||
CHECK(assert_equal(expected, actual2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
TEST(SymbolicFactorGraph, add_factors) {
|
||||
SymbolicFactorGraph fg1;
|
||||
fg1.push_factor(10);
|
||||
fg1 += SymbolicFactor::shared_ptr(); // empty slot!
|
||||
fg1.push_factor(11);
|
||||
|
||||
SymbolicFactorGraph fg2;
|
||||
fg2.push_factor(1);
|
||||
fg2.push_factor(2);
|
||||
|
||||
SymbolicFactorGraph expected;
|
||||
expected.push_factor(10);
|
||||
expected.push_factor(1);
|
||||
expected.push_factor(11);
|
||||
expected.push_factor(2);
|
||||
const FactorIndices expectedIndices = list_of(1)(3);
|
||||
const FactorIndices actualIndices = fg1.add_factors(fg2, true);
|
||||
|
||||
EXPECT(assert_equal(expected, fg1));
|
||||
EXPECT(assert_container_equality(expectedIndices, actualIndices));
|
||||
|
||||
expected.push_factor(1);
|
||||
expected.push_factor(2);
|
||||
const FactorIndices expectedIndices2 = list_of(4)(5);
|
||||
const FactorIndices actualIndices2 = fg1.add_factors(fg2, false);
|
||||
|
||||
EXPECT(assert_equal(expected, fg1));
|
||||
EXPECT(assert_container_equality(expectedIndices2, actualIndices2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue