12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'svn/branches/unordered' 

Conflicts:
	gtsam/inference/JunctionTree-inst.h
release/4.3a0
Richard Roberts 2013-08-08 17:36:43 +00:00
parent c0ccec4656 41a1b3c30e
commit 957afb4835
38 changed files with 192 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.cproject
View File

@ -19,7 +19,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="-j6" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">

4
CMakeLists.txt
View File

@ -100,6 +100,10 @@ endif()
###############################################################################
# Find boost
# To change the path for boost, you will need to set:
# BOOST_ROOT: path to install prefix for boost
# Boost_NO_SYSTEM_PATHS: set to true to keep the find script from ignoring BOOST_ROOT
# If using Boost shared libs, set up auto linking for shared libs
if(MSVC AND NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_DYN_LINK)

2
gtsam/base/tests/testVector.cpp
View File

@ -199,7 +199,7 @@ TEST( TestVector, weightedPseudoinverse_constraint )
// verify
EXPECT(assert_equal(expected,actual));
EXPECT(isinf(precision));
EXPECT(std::isinf(precision));
}
/* ************************************************************************* */

22
gtsam/base/treeTraversal-inst.h
View File

@ -312,15 +312,18 @@ namespace gtsam {
/* ************************************************************************* */
/** Traversal function for PrintForest */
namespace {
template<typename NODE>
std::string
PrintForestVisitorPre(const boost::shared_ptr<NODE>& node, const std::string& parentString, const KeyFormatter& formatter)
struct PrintForestVisitorPre
{
// Print the current node
node->print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
const KeyFormatter& formatter;
PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {}
template<typename NODE> std::string operator()(const boost::shared_ptr<NODE>& node, const std::string& parentString)
{
// Print the current node
node->print(parentString + "-", formatter);
// Increment the indentation
return parentString + "| ";
}
};
}
/** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
@ -328,7 +331,8 @@ namespace gtsam {
template<class FOREST>
void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) {
typedef typename FOREST::Node Node;
DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, keyFormatter));
PrintForestVisitorPre visitor(keyFormatter);
DepthFirstForest(forest, str, visitor);
}
}

26
gtsam/base/types.h
View File

@ -25,6 +25,7 @@
#include <string>
#include <boost/function/function1.hpp>
#include <boost/range/concepts.hpp>
namespace gtsam {
@ -103,6 +104,31 @@ namespace gtsam {
operator T() const { return value; }
};
/** A helper class that behaves as a container with one element, and works with
* boost::range */
template<typename T>
class ListOfOneContainer {
T element_;
public:
typedef T value_type;
typedef const T* const_iterator;
typedef T* iterator;
ListOfOneContainer(const T& element) : element_(element) {}
const T* begin() const { return &element_; }
const T* end() const { return &element_ + 1; }
T* begin() { return &element_; }
T* end() { return &element_ + 1; }
size_t size() const { return 1; }
};
BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element);
}
/** An assertion that throws an exception if NDEBUG is not defined and
* evaluates to an empty statement otherwise. */
#ifdef NDEBUG

1
gtsam/inference/BayesTree-inst.h
View File

@ -146,6 +146,7 @@ namespace gtsam {
}
/* ************************************************************************* */
// TODO: Clean up
namespace {
template<class FACTOR, class CLIQUE>
int _pushClique(FactorGraph<FACTOR>& fg, const boost::shared_ptr<CLIQUE>& clique) {

18
gtsam/inference/EliminateableFactorGraph.h
View File

@ -144,7 +144,7 @@ namespace gtsam {
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net
/** Do sequential elimination of some variables, in \c ordering provided, to produce a Bayes net
* and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B) \f$,
* where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and \f$
* B = X\backslash A \f$. */
@ -164,20 +164,20 @@ namespace gtsam {
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
/** Do multifrontal elimination of some variables, in \c ordering provided, to produce a Bayes
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
* \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
* \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(
const Ordering& ordering,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** Do multifrontal elimination of some variables in the given \c ordering to produce a Bayes
* tree and a remaining factor graph. This computes the factorization \f$ p(X) = p(A|B) p(B)
* \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the factor graph, and
* \f$ B = X\backslash A \f$. */
/** Do multifrontal elimination of the given \c variables in an ordering computed by COLAMD to
* produce a Bayes net and a remaining factor graph. This computes the factorization \f$ p(X)
* = p(A|B) p(B) \f$, where \f$ A = \f$ \c variables, \f$ X \f$ is all the variables in the
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(
const std::vector<Key>& variables,

2
gtsam/inference/EliminationTree-inst.h
View File

@ -251,7 +251,7 @@ namespace gtsam {
if(*it1 && *it2) {
if(!(*it1)->equals(**it2, tol))
return false;
} else if(*it1 && !*it2 || *it2 && !*it1) {
} else if((*it1 && !*it2) || (*it2 && !*it1)) {
return false;
}
}

4
gtsam/inference/FactorGraph.h
View File

@ -335,10 +335,10 @@ namespace gtsam {
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
/** Erase factor and rearrange other factors to take up the empty space */
void erase(const_iterator item) { factors_.erase(item); }
void erase(iterator item) { factors_.erase(item); }
/** Erase factors and rearrange other factors to take up the empty space */
void erase(const_iterator first, const_iterator last) { factors_.erase(first, last); }
void erase(iterator first, iterator last) { factors_.erase(first, last); }
/// @}
/// @name Advanced Interface

73
gtsam/inference/JunctionTree-inst.h
View File

@ -161,47 +161,49 @@ namespace gtsam {
/* ************************************************************************* */
// Elimination post-order visitor - combine the child factors with our own factors, add the
// resulting conditional to the BayesTree, and add the remaining factor to the parent. The
// extra argument 'eliminationFunction' is passed from JunctionTree::eliminate using
// boost::bind.
// resulting conditional to the BayesTree, and add the remaining factor to the parent.
template<class JUNCTIONTREE>
void eliminationPostOrderVisitor(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData,
const typename JUNCTIONTREE::Eliminate& eliminationFunction)
struct EliminationPostOrderVisitor
{
// Typedefs
typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
typedef typename JUNCTIONTREE::FactorType FactorType;
typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType;
typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
// Gather factors
FactorGraphType gatheredFactors;
gatheredFactors.reserve(node->factors.size() + node->children.size());
gatheredFactors += node->factors;
gatheredFactors += myData.childFactors;
// Check for Bayes tree orphan subtrees, and add them to our children
BOOST_FOREACH(const sharedFactor& f, node->factors)
const typename JUNCTIONTREE::Eliminate& eliminationFunction;
EliminationPostOrderVisitor(const typename JUNCTIONTREE::Eliminate& eliminationFunction) : eliminationFunction(eliminationFunction) {}
void operator()(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData)
{
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
{
myData.bayesTreeNode->children.push_back(asSubtree->clique);
asSubtree->clique->parent_ = myData.bayesTreeNode;
}
}
// Typedefs
typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
typedef typename JUNCTIONTREE::FactorType FactorType;
typedef typename JUNCTIONTREE::FactorGraphType FactorGraphType;
typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
// Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, Ordering(node->keys));
// Gather factors
FactorGraphType gatheredFactors;
gatheredFactors.reserve(node->factors.size() + node->children.size());
gatheredFactors += node->factors;
gatheredFactors += myData.childFactors;
// Check for Bayes tree orphan subtrees, and add them to our children
BOOST_FOREACH(const sharedFactor& f, node->factors)
{
if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
{
myData.bayesTreeNode->children.push_back(asSubtree->clique);
asSubtree->clique->parent_ = myData.bayesTreeNode;
}
}
// Do dense elimination step
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
eliminationFunction(gatheredFactors, Ordering(node->keys));
// Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
myData.bayesTreeNode->setEliminationResult(eliminationResult);
// Store remaining factor in parent's gathered factors
if(!eliminationResult.second->empty())
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
}
// Store remaining factor in parent's gathered factors
if(!eliminationResult.second->empty())
myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
}
};
}
/* ************************************************************************* */
@ -278,9 +280,10 @@ namespace gtsam {
// that contains all of the roots as its children. rootsContainer also stores the remaining
// uneliminated factors passed up from the roots.
EliminationData<This> rootsContainer(0, roots_.size());
EliminationPostOrderVisitor<This> visitorPost(function);
//tbb::task_scheduler_init init(1);
treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer, eliminationPreOrderVisitor<This>,
boost::bind(eliminationPostOrderVisitor<This>, _1, _2, function)/*, 10*/);
treeTraversal::DepthFirstForest/*Parallel*/(*this, rootsContainer,
eliminationPreOrderVisitor<This>, visitorPost/*, 10*/);
// Create BayesTree from roots stored in the dummy BayesTree node.
boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();

26
gtsam/inference/inference-inst.h
View File

@ -50,14 +50,20 @@ namespace gtsam {
/* ************************************************************************* */
template<class TREE, class RESULT>
void eliminationPostOrderVisitor(const typename TREE::sharedNode& node, EliminationData<TREE>& myData,
RESULT& result, const typename TREE::Eliminate& eliminationFunction)
struct EliminationPostOrderVisitor
{
// Call eliminate on the node and add the result to the parent's gathered factors
typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
if(!childFactor->empty())
myData.parentData->childFactors.push_back(childFactor);
}
RESULT& result;
const typename TREE::Eliminate& eliminationFunction;
EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) :
result(result), eliminationFunction(eliminationFunction) {}
void operator()(const typename TREE::sharedNode& node, EliminationData<TREE>& myData)
{
// Call eliminate on the node and add the result to the parent's gathered factors
typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
if(!childFactor->empty())
myData.parentData->childFactors.push_back(childFactor);
}
};
}
/* ************************************************************************* */
@ -79,12 +85,12 @@ namespace gtsam {
// elimination (using the gathered child factors) and store the result in the parent's
// gathered factors.
EliminationData<TREE> rootData(0, tree.roots().size());
treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>,
boost::bind(eliminationPostOrderVisitor<TREE,RESULT>, _1, _2, result, function));
EliminationPostOrderVisitor<TREE,RESULT> visitorPost(result, function);
treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>, visitorPost);
// Return remaining factors
return rootData.childFactors;
}
}
}
}

3
gtsam/linear/GaussianBayesNet.cpp
View File

@ -30,6 +30,9 @@ using namespace gtsam;
namespace gtsam {
// Instantiate base class
template class FactorGraph<GaussianConditional>;
/* ************************************************************************* */
bool GaussianBayesNet::equals(const This& bn, double tol) const
{

4
gtsam/linear/GaussianBayesTree.cpp
View File

@ -26,6 +26,10 @@
namespace gtsam {
// Instantiate base class
template class BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>;
template class BayesTree<GaussianBayesTreeClique>;
/* ************************************************************************* */
namespace internal
{

2
gtsam/linear/GaussianConditional-inl.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
GaussianConditional::GaussianConditional(Index key, const Vector& d,
const Matrix& R, const PARENTS& parents, const SharedDiagonal& sigmas, const typename PARENTS::value_type*) :
BaseFactor(boost::join(
boost::assign::cref_list_of<1,typename PARENTS::value_type>(std::make_pair(key, R)),
ListOfOne<typename PARENTS::value_type>(std::make_pair(key, R)),
parents), d, sigmas),
BaseConditional(1) {}

3
gtsam/linear/GaussianEliminationTree.cpp
View File

@ -21,6 +21,9 @@
namespace gtsam {
// Instantiate base class
template class EliminationTree<GaussianBayesNet, GaussianFactorGraph>;
/* ************************************************************************* */
GaussianEliminationTree::GaussianEliminationTree(
const GaussianFactorGraph& factorGraph, const VariableIndex& structure,

4
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -35,6 +35,10 @@ using namespace gtsam;
namespace gtsam {
// Instantiate base classes
template class FactorGraph<GaussianFactor>;
template class EliminateableFactorGraph<GaussianFactorGraph>;
/* ************************************************************************* */
bool GaussianFactorGraph::equals(const This& fg, double tol) const
{

3
gtsam/linear/GaussianISAM.cpp
View File

@ -21,6 +21,9 @@
namespace gtsam {
// Instantiate base class
template class ISAM<GaussianBayesTree>;
/* ************************************************************************* */
GaussianISAM::GaussianISAM() {}

3
gtsam/linear/GaussianJunctionTree.cpp
View File

@ -22,6 +22,9 @@
namespace gtsam {
// Instantiate base class
template class JunctionTree<GaussianBayesTree, GaussianFactorGraph>;
/* ************************************************************************* */
GaussianJunctionTree::GaussianJunctionTree(
const GaussianEliminationTree& eliminationTree) :

4
gtsam/linear/HessianFactor-inl.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
GaussianFactor(keys), info_(augmentedInformation)
{
// Check number of variables
if(Base::keys_.size() != augmentedInformation.nBlocks() - 1)
if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1)
throw std::invalid_argument(
"Error in HessianFactor constructor input. Number of provided keys plus\n"
"one for the information vector must equal the number of provided matrix blocks.");
@ -38,4 +38,4 @@ namespace gtsam {
"must be the information vector, but the last provided block had more than one column.");
}
}
}

10
gtsam/linear/HessianFactor.cpp
View File

@ -168,12 +168,14 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
}
/* ************************************************************************* */
namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } }
namespace {
DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1)))
GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1)))
{
// Get the number of variables
size_t variable_count = js.size();
@ -385,9 +387,9 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
// Apply updates to the upper triangle
gttic(update);
for(DenseIndex j2=0; j2<update.info_.nBlocks(); ++j2) {
DenseIndex slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
DenseIndex slot2 = (j2 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j2];
for(DenseIndex j1=0; j1<=j2; ++j1) {
DenseIndex slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1];
DenseIndex slot1 = (j1 == (DenseIndex)update.size()) ? this->info_.nBlocks()-1 : slots[j1];
if(slot2 > slot1)
info_(slot1, slot2).noalias() += update.info_(j1, j2);
else if(slot1 > slot2)

1
gtsam/linear/IterativeSolver.h
View File

@ -14,6 +14,7 @@
#include <gtsam/global_includes.h>
#include <string>
#include <iostream>
namespace gtsam {

10
gtsam/linear/JacobianFactor-inl.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/linearExceptions.h>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
namespace gtsam {
@ -40,11 +39,11 @@ namespace gtsam {
Base(keys), Ab_(augmentedMatrix)
{
// Check noise model dimension
if(model && model->dim() != augmentedMatrix.rows())
if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
// Check number of variables
if(Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
throw std::invalid_argument(
"Error in JacobianFactor constructor input. Number of provided keys plus\n"
"one for the RHS vector must equal the number of provided matrix blocks.");
@ -71,7 +70,7 @@ namespace gtsam {
void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
{
// Check noise model dimension
if(noiseModel && noiseModel->dim() != b.size())
if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
throw InvalidNoiseModel(b.size(), noiseModel->dim());
// Resize base class key vector
@ -82,9 +81,8 @@ namespace gtsam {
// a single '1' to add a dimension for the b vector.
{
using boost::adaptors::transformed;
using boost::assign::cref_list_of;
namespace br = boost::range;
Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size());
Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), ListOfOne((DenseIndex)1)), b.size());
}
// Check and add terms

4
gtsam/linear/JacobianFactor.cpp
View File

@ -265,7 +265,7 @@ namespace gtsam {
// Allocate matrix and copy keys in order
gttic(allocate);
Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix
Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix
Base::keys_.resize(orderedSlots.size());
boost::range::copy( // Get variable keys
orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());
@ -354,7 +354,7 @@ namespace gtsam {
return false;
// Check noise model
if(model_ && !f.model_ || !model_ && f.model_)
if((model_ && !f.model_) || (!model_ && f.model_))
return false;
if(model_ && f.model_ && !model_->equals(*f.model_, tol))
return false;

2
gtsam/linear/tests/testGaussianBayesTreeUnordered.cpp
View File

@ -57,7 +57,7 @@ namespace {
GaussianBayesTreeClique::shared_ptr clique =
boost::make_shared<GaussianBayesTreeClique>(
boost::make_shared<GaussianConditional>(conditional));
clique->children = children;
clique->children.assign(children.begin(), children.end());
BOOST_FOREACH(const GaussianBayesTreeClique::shared_ptr& child, children)
child->parent_ = clique;
return clique;

4
gtsam/nonlinear/CMakeLists.txt
View File

@ -15,7 +15,9 @@ set(nonlinear_local_libs
# Files to exclude from compilation of tests and timing scripts
set(nonlinear_excluded_files
# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test
"" # Add to this list, with full path, to exclude
#"" # Add to this list, with full path, to exclude
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testLinearContainerFactor.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/tests/testWhiteNoiseFactor.cpp"
)
# Build tests

2
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -11,7 +11,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#if 0
namespace gtsam {
@ -160,4 +159,3 @@ private:
} // \namespace gtsam
#endif

3
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -32,6 +32,9 @@ using namespace std;
namespace gtsam {
// Instantiate base classes
template class FactorGraph<NonlinearFactor>;
/* ************************************************************************* */
double NonlinearFactorGraph::probPrime(const Values& c) const {
return exp(-0.5 * error(c));

12
gtsam/nonlinear/Values.cpp
View File

@ -82,10 +82,16 @@ namespace gtsam {
Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const Vector& singleDelta = delta[key_value->key]; // Delta for this value
VectorValues::const_iterator vector_item = delta.find(key_value->key);
Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
if(vector_item != delta.end()) {
// const Vector& singleDelta = delta[key_value->key]; // Delta for this value
const Vector& singleDelta = vector_item->second;
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
} else {
result.values_.insert(key, key_value->value.clone_()); // Add original version to result values
}
}
return result;

3
gtsam/symbolic/SymbolicBayesNet.cpp
View File

@ -24,6 +24,9 @@
namespace gtsam {
// Instantiate base class
template class FactorGraph<SymbolicConditional>;
/* ************************************************************************* */
bool SymbolicBayesNet::equals(const This& bn, double tol) const
{

4
gtsam/symbolic/SymbolicBayesTree.cpp
View File

@ -27,6 +27,10 @@
namespace gtsam {
// Instantiate base classes
template class BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph>;
template class BayesTree<SymbolicBayesTreeClique>;
/* ************************************************************************* */
SymbolicBayesTree::SymbolicBayesTree(const SymbolicBayesTree& other) :
Base(other) {}

3
gtsam/symbolic/SymbolicEliminationTree.cpp
View File

@ -21,6 +21,9 @@
namespace gtsam {
// Instantiate base class
template class EliminationTree<SymbolicBayesNet, SymbolicFactorGraph>;
/* ************************************************************************* */
SymbolicEliminationTree::SymbolicEliminationTree(
const SymbolicFactorGraph& factorGraph, const VariableIndex& structure,

4
gtsam/symbolic/SymbolicFactorGraph.cpp
View File

@ -27,6 +27,10 @@
namespace gtsam {
// Instantiate base classes
template class FactorGraph<SymbolicFactor>;
template class EliminateableFactorGraph<SymbolicFactorGraph>;
using namespace std;
/* ************************************************************************* */

3
gtsam/symbolic/SymbolicISAM.cpp
View File

@ -21,6 +21,9 @@
namespace gtsam {
// Instantiate base class
template class ISAM<SymbolicBayesTree>;
/* ************************************************************************* */
SymbolicISAM::SymbolicISAM() {}

3
gtsam/symbolic/SymbolicJunctionTree.cpp
View File

@ -22,6 +22,9 @@
namespace gtsam {
// Instantiate base class
template class JunctionTree<SymbolicBayesTree, SymbolicFactorGraph>;
/* ************************************************************************* */
SymbolicJunctionTree::SymbolicJunctionTree(
const SymbolicEliminationTree& eliminationTree) :

2
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -58,7 +58,7 @@ namespace {
boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(keys, nrFrontals)));
clique->children = children;
clique->children.assign(children.begin(), children.end());
BOOST_FOREACH(const SymbolicBayesTreeClique::shared_ptr& child, children)
child->parent_ = clique;
return clique;

3
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -19,9 +19,6 @@
#include <CppUnitLite/TestHarness.h>
#include <vector>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
#include <boost/make_shared.hpp>
#include <gtsam/base/TestableAssertions.h>

1
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace gtsam;

16
gtsam/symbolic/tests/testSymbolicJunctionTree.cpp
View File

@ -16,11 +16,6 @@
* @author Frank Dellaert
*/
#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp> // for operator +=
#include <boost/assign/std/set.hpp> // for operator +=
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
@ -28,6 +23,9 @@ using namespace boost::assign;
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <gtsam/symbolic/SymbolicJunctionTree.h>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
#include "symbolicExampleGraphs.h"
using namespace gtsam;
@ -44,10 +42,10 @@ TEST( JunctionTree, constructor )
SymbolicJunctionTree actual(SymbolicEliminationTree(simpleChain, order));
vector<Index> frontal1; frontal1 += 2, 3;
vector<Index> frontal2; frontal2 += 0, 1;
vector<Index> sep1;
vector<Index> sep2; sep2 += 2;
vector<Key> frontal1 = list_of(2)(3);
vector<Key> frontal2 = list_of(0)(1);
vector<Key> sep1;
vector<Key> sep2 = list_of(2);
EXPECT(assert_equal(frontal1, actual.roots().front()->keys));
//EXPECT(assert_equal(sep1, actual.roots().front()->separator));
LONGS_EQUAL(1, (long)actual.roots().front()->factors.size());