diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 35b3ee5a4..5ac1a55a9 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -104,7 +104,7 @@ namespace gtsam { } // If we have child tasks, start subtasks and wait for them to complete - set_ref_count(1 + node->children.size()); + set_ref_count(1 + (int)node->children.size()); spawn(childTasks); wait_for_all(); } diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index ea642ce17..351aaf870 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -96,7 +96,7 @@ namespace gtsam { size_t nrMergedChildren = 0; assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size()); // Loop over children - int combinedProblemSize = symbolicElimResult.first->size(); + int combinedProblemSize = (int)symbolicElimResult.first->size(); for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) { // Check if we should merge the child if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) { @@ -105,7 +105,7 @@ namespace gtsam { const typename JunctionTreeUnordered::Node& childToMerge = *myData.myJTNode->children[child - nrMergedChildren]; // Merge keys, factors, and children. - myData.myJTNode->keys.insert(myData.myJTNode->keys.end(), childToMerge.keys.begin(), childToMerge.keys.end()); + myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end()); myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end()); myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); // Remove child from list. diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index b13e5ebb1..11d0a6fcf 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -20,6 +20,7 @@ #include #include +#include #include diff --git a/gtsam/linear/JacobianFactorUnordered-inl.h b/gtsam/linear/JacobianFactorUnordered-inl.h index 6c3cee8d3..9168207c1 100644 --- a/gtsam/linear/JacobianFactorUnordered-inl.h +++ b/gtsam/linear/JacobianFactorUnordered-inl.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include #include @@ -66,8 +65,8 @@ namespace gtsam { /* ************************************************************************* */ namespace { - const Matrix& _getPairSecond(const std::pair& p) { - return p.second; + DenseIndex _getCols(const std::pair& p) { + return p.second.cols(); } } @@ -85,14 +84,12 @@ namespace gtsam { // Gather dimensions - uses boost range adaptors to take terms, extract .second which are the // matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with // a single '1' to add a dimension for the b vector. - using boost::adaptors::map_values; - using boost::adaptors::transformed; - using boost::join; - Ab_ = VerticalBlockMatrix(join( - terms - | transformed(&_getPairSecond) - | transformed(boost::mem_fn(&Matrix::cols)), - boost::assign::cref_list_of<1,DenseIndex>(1)), b.size()); + { + using boost::adaptors::transformed; + using boost::join; + using boost::assign::cref_list_of; + Ab_ = VerticalBlockMatrix(join(terms | transformed(_getCols), cref_list_of<1,DenseIndex>(1)), b.size()); + } // Check and add terms typedef std::pair Term; diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp index 6c14700ab..f395cc1a7 100644 --- a/gtsam/linear/JacobianFactorUnordered.cpp +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -46,6 +46,7 @@ #endif #include #include +#include #include #include diff --git a/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp new file mode 100644 index 000000000..864f35456 --- /dev/null +++ b/gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp @@ -0,0 +1,180 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGaussianBayesTree.cpp + * @date Jul 8, 2010 + * @author Kai Ni + */ + +#include +#include + +#include +#include // for operator += +#include // for operator += +using namespace boost::assign; + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define TEST TEST_UNSAFE + +namespace { + const Key x1=1, x2=2, x3=3, x4=4; + const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5); + const GaussianFactorGraphUnordered chain = list_of + (JacobianFactorUnordered(x2, Matrix_(1,1,1.), x1, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) + (JacobianFactorUnordered(x2, Matrix_(1,1,1.), x3, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) + (JacobianFactorUnordered(x3, Matrix_(1,1,1.), x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)) + (JacobianFactorUnordered(x4, Matrix_(1,1,1.), Vector_(1,1.), chainNoise)); + const OrderingUnordered chainOrdering = OrderingUnordered(list_of(x2)(x1)(x3)(x4)); + + /* ************************************************************************* */ + // Helper functions for below + GaussianBayesTreeCliqueUnordered::shared_ptr MakeClique(const GaussianConditionalUnordered& conditional) + { + return boost::make_shared( + boost::make_shared(conditional)); + } + + template + GaussianBayesTreeCliqueUnordered::shared_ptr MakeClique( + const GaussianConditionalUnordered& conditional, const CHILDREN& children) + { + GaussianBayesTreeCliqueUnordered::shared_ptr clique = + boost::make_shared( + boost::make_shared(conditional)); + clique->children = children; + BOOST_FOREACH(const GaussianBayesTreeCliqueUnordered::shared_ptr& child, children) + child->parent_ = clique; + return clique; + } +} + +/* ************************************************************************* */ +/** + * x1 - x2 - x3 - x4 + * x3 x4 + * x2 x1 : x3 + * + * x2 x1 x3 x4 b + * 1 1 1 + * 1 1 1 + * 1 1 1 + * 1 1 + * + * 1 0 0 1 + */ +TEST( GaussianBayesTree, eliminate ) +{ + GaussianBayesTreeUnordered bt = *chain.eliminateMultifrontal(chainOrdering); + + Matrix two = Matrix_(1,1,2.); + Matrix one = Matrix_(1,1,1.); + + GaussianBayesTreeUnordered bayesTree_expected; + bayesTree_expected.insertRoot( + MakeClique(GaussianConditionalUnordered(pair_list_of (x3, Matrix_(2,1, 2., 0.)) (x4, Matrix_(2,1, 2., 2.)), 2, Vector_(2, 2., 2.)), list_of + (MakeClique(GaussianConditionalUnordered(pair_list_of (x2, Matrix_(2,1, -2.*sqrt(2.), 0.)) (x1, Matrix_(2,1, -sqrt(2.), -sqrt(2.))) (x3, Matrix_(2,1, -sqrt(2.), sqrt(2.))), 2, Vector_(2, -2.*sqrt(2.), 0.)))))); + + EXPECT(assert_equal(bayesTree_expected, bt)); +} + +/* ************************************************************************* */ +TEST( GaussianBayesTree, optimizeMultiFrontal ) +{ + VectorValuesUnordered expected = pair_list_of + (x1, Vector_(1, 0.)) + (x2, Vector_(1, 1.)) + (x3, Vector_(1, 0.)) + (x4, Vector_(1, 1.)); + + VectorValuesUnordered actual = chain.eliminateMultifrontal(chainOrdering)->optimize(); + EXPECT(assert_equal(expected,actual)); +} + +/* ************************************************************************* */ +TEST(GaussianBayesTree, complicatedMarginal) { + + // Create the conditionals to go in the BayesTree + GaussianBayesTreeUnordered bt; + bt.insertRoot( + MakeClique(GaussianConditionalUnordered(pair_list_of (11, (Matrix(3,1) << 0.0971, 0, 0).finished()) + (12, (Matrix(3,2) << 0.3171, 0.4387, 0.9502, 0.3816, 0, 0.7655).finished()), + 2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished()), list_of + (MakeClique(GaussianConditionalUnordered(pair_list_of (9, (Matrix(3,1) << 0.7952, 0, 0).finished()) + (10, (Matrix(3,2) << 0.4456, 0.7547, 0.6463, 0.2760, 0, 0.6797).finished()) + (11, (Matrix(3,1) << 0.6551, 0.1626, 0.1190).finished()) + (12, (Matrix(3,2) << 0.4984, 0.5853, 0.9597, 0.2238, 0.3404, 0.7513).finished()), + 2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished()))) + (MakeClique(GaussianConditionalUnordered(pair_list_of (7, (Matrix(3,1) << 0.2551, 0, 0).finished()) + (8, (Matrix(3,2) << 0.8909, 0.1386, 0.9593, 0.1493, 0, 0.2575).finished()) + (11, (Matrix(3,1) << 0.8407, 0.2543, 0.8143).finished()), + 2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished()), list_of + (MakeClique(GaussianConditionalUnordered(pair_list_of (5, (Matrix(3,1) << 0.2435, 0, 0).finished()) + (6, (Matrix(3,2) << 0.4733, 0.1966, 0.3517, 0.2511, 0.8308, 0.0).finished()) + // NOTE the non-upper-triangular form + // here since this test was written when we had column permutations + // from LDL. The code still works currently (does not enfore + // upper-triangularity in this case) but this test will need to be + // redone if this stops working in the future + (7, (Matrix(3,1) << 0.5853, 0.5497, 0.9172).finished()) + (8, (Matrix(3,2) << 0.2858, 0.3804, 0.7572, 0.5678, 0.7537, 0.0759).finished()), + 2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished()), list_of + (MakeClique(GaussianConditionalUnordered(pair_list_of (3, (Matrix(3,1) << 0.0540, 0, 0).finished()) + (4, (Matrix(3,2) << 0.9340, 0.4694, 0.1299, 0.0119, 0, 0.3371).finished()) + (6, (Matrix(3,2) << 0.1622, 0.5285, 0.7943, 0.1656, 0.3112, 0.6020).finished()), + 2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished()))) + (MakeClique(GaussianConditionalUnordered(pair_list_of (1, (Matrix(3,1) << 0.2630, 0, 0).finished()) + (2, (Matrix(3,2) << 0.7482, 0.2290, 0.4505, 0.9133, 0, 0.1524).finished()) + (5, (Matrix(3,1) << 0.8258, 0.5383, 0.9961).finished()), + 2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished()))))))))); + + // Marginal on 5 + Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); + //GaussianConditionalUnordered actualJacobianChol = *bt.marginalFactor(5, EliminateCholesky); + GaussianConditionalUnordered actualJacobianQR = *bt.marginalFactor(5, EliminateQRUnordered); + //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same + LONGS_EQUAL(1, (long)actualJacobianQR.rows()); + LONGS_EQUAL(1, (long)actualJacobianQR.size()); + LONGS_EQUAL(5, (long)actualJacobianQR.keys()[0]); + Matrix actualA = actualJacobianQR.getA(actualJacobianQR.begin()); + Matrix actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); + + // Marginal on 6 +// expectedCov = (Matrix(2,2) << +// 8471.2, 2886.2, +// 2886.2, 1015.8).finished(); + expectedCov = (Matrix(2,2) << + 1015.8, 2886.2, + 2886.2, 8471.2).finished(); + //actualJacobianChol = bt.marginalFactor(6, EliminateCholesky); + actualJacobianQR = *bt.marginalFactor(6, EliminateQRUnordered); + //EXPECT(assert_equal(actualJacobianChol, actualJacobianQR)); // Check that Chol and QR obtained marginals are the same + LONGS_EQUAL(2, (long)actualJacobianQR.rows()); + LONGS_EQUAL(1, (long)actualJacobianQR.size()); + LONGS_EQUAL(6, (long)actualJacobianQR.keys()[0]); + actualA = actualJacobianQR.getA(actualJacobianQR.begin()); + actualCov = inverse(actualA.transpose() * actualA); + EXPECT(assert_equal(expectedCov, actualCov, 1e1)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp similarity index 100% rename from gtsam/linear/tests/testGaussianJunctionTree.cpp rename to gtsam/linear/tests/testGaussianJunctionTreeObsolete.cpp