Modernized enable_if, re-grouped methods to add factors
Frank Dellaert
parent
d365b427bd
commit
da1b7f92f2
|
|
@ -22,350 +22,374 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/FastVector.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
#include <Eigen/Core> // for Eigen::aligned_allocator
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
|
||||
namespace gtsam {
|
||||
/// Define collection type:
|
||||
typedef FastVector<FactorIndex> FactorIndices;
|
||||
|
||||
// Forward declarations
|
||||
template<class CLIQUE> class BayesTree;
|
||||
// Forward declarations
|
||||
template <class CLIQUE>
|
||||
class BayesTree;
|
||||
|
||||
/** Helper */
|
||||
template<class C>
|
||||
class CRefCallPushBack
|
||||
{
|
||||
C& obj;
|
||||
public:
|
||||
CRefCallPushBack(C& obj) : obj(obj) {}
|
||||
template<typename A>
|
||||
void operator()(const A& a) { obj.push_back(a); }
|
||||
};
|
||||
/** Helper */
|
||||
template <class C>
|
||||
class CRefCallPushBack {
|
||||
C& obj;
|
||||
|
||||
/** Helper */
|
||||
template<class C>
|
||||
class RefCallPushBack
|
||||
{
|
||||
C& obj;
|
||||
public:
|
||||
RefCallPushBack(C& obj) : obj(obj) {}
|
||||
template<typename A>
|
||||
void operator()(A& a) { obj.push_back(a); }
|
||||
};
|
||||
public:
|
||||
explicit CRefCallPushBack(C& obj) : obj(obj) {}
|
||||
template <typename A>
|
||||
void operator()(const A& a) {
|
||||
obj.push_back(a);
|
||||
}
|
||||
};
|
||||
|
||||
/** Helper */
|
||||
template<class C>
|
||||
class CRefCallAddCopy
|
||||
{
|
||||
C& obj;
|
||||
public:
|
||||
CRefCallAddCopy(C& obj) : obj(obj) {}
|
||||
template<typename A>
|
||||
void operator()(const A& a) { obj.addCopy(a); }
|
||||
};
|
||||
/** Helper */
|
||||
template <class C>
|
||||
class RefCallPushBack {
|
||||
C& obj;
|
||||
|
||||
public:
|
||||
explicit RefCallPushBack(C& obj) : obj(obj) {}
|
||||
template <typename A>
|
||||
void operator()(A& a) {
|
||||
obj.push_back(a);
|
||||
}
|
||||
};
|
||||
|
||||
/** Helper */
|
||||
template <class C>
|
||||
class CRefCallAddCopy {
|
||||
C& obj;
|
||||
|
||||
public:
|
||||
explicit CRefCallAddCopy(C& obj) : obj(obj) {}
|
||||
template <typename A>
|
||||
void operator()(const A& a) {
|
||||
obj.addCopy(a);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* A factor graph is a bipartite graph with factor nodes connected to variable
|
||||
* nodes. In this class, however, only factor nodes are kept around.
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template <class FACTOR>
|
||||
class FactorGraph {
|
||||
public:
|
||||
typedef FACTOR FactorType; ///< factor type
|
||||
typedef boost::shared_ptr<FACTOR>
|
||||
sharedFactor; ///< Shared pointer to a factor
|
||||
typedef sharedFactor value_type;
|
||||
typedef typename FastVector<sharedFactor>::iterator iterator;
|
||||
typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
|
||||
|
||||
private:
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This>
|
||||
shared_ptr; ///< Shared pointer for this class
|
||||
|
||||
/// Check if a DERIVEDFACTOR is in fact derived from FactorType.
|
||||
template <typename DERIVEDFACTOR>
|
||||
using IsDerived = typename std::enable_if<
|
||||
std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
|
||||
|
||||
/// Check if T has a value_type derived from FactorType.
|
||||
template <typename T>
|
||||
using HasDerivedValueType = typename std::enable_if<
|
||||
std::is_base_of<FactorType, typename T::value_type>::value>::type;
|
||||
|
||||
/// Check if T has a value_type derived from FactorType.
|
||||
template <typename T>
|
||||
using HasDerivedElementType = typename std::enable_if<std::is_base_of<
|
||||
FactorType, typename T::value_type::element_type>::value>::type;
|
||||
|
||||
protected:
|
||||
/** concept check, makes sure FACTOR defines print and equals */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
|
||||
/** Collection of factors */
|
||||
FastVector<sharedFactor> factors_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** Default constructor */
|
||||
FactorGraph() {}
|
||||
|
||||
/** Constructor from iterator over factors (shared_ptr or plain objects) */
|
||||
template <typename ITERATOR>
|
||||
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
push_back(firstFactor, lastFactor);
|
||||
}
|
||||
|
||||
/** Construct from container of factors (shared_ptr or plain objects) */
|
||||
template <class CONTAINER>
|
||||
explicit FactorGraph(const CONTAINER& factors) {
|
||||
push_back(factors);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
public:
|
||||
/// @name Adding Single Factors
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* A factor graph is a bipartite graph with factor nodes connected to variable nodes.
|
||||
* In this class, however, only factor nodes are kept around.
|
||||
* \nosubgrouping
|
||||
* Reserve space for the specified number of factors if you know in
|
||||
* advance how many there will be (works like FastVector::reserve).
|
||||
*/
|
||||
template<class FACTOR>
|
||||
class FactorGraph {
|
||||
void reserve(size_t size) { factors_.reserve(size); }
|
||||
|
||||
public:
|
||||
typedef FACTOR FactorType; ///< factor type
|
||||
typedef boost::shared_ptr<FACTOR> sharedFactor; ///< Shared pointer to a factor
|
||||
typedef sharedFactor value_type;
|
||||
typedef typename FastVector<sharedFactor>::iterator iterator;
|
||||
typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
|
||||
/// Add a factor directly using a shared_ptr.
|
||||
template <class DERIVEDFACTOR>
|
||||
IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
factors_.push_back(boost::shared_ptr<FACTOR>(factor));
|
||||
}
|
||||
|
||||
private:
|
||||
typedef FactorGraph<FACTOR> This; ///< Typedef for this class
|
||||
typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer for this class
|
||||
/// Emplace a shared pointer to factor of given type.
|
||||
template <class DERIVEDFACTOR, class... Args>
|
||||
IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
|
||||
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
|
||||
Eigen::aligned_allocator<DERIVEDFACTOR>(),
|
||||
std::forward<Args>(args)...));
|
||||
}
|
||||
|
||||
protected:
|
||||
/** concept check, makes sure FACTOR defines print and equals */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
|
||||
/**
|
||||
* Add a factor by value, will be copy-constructed (use push_back with a
|
||||
* shared_ptr to avoid the copy).
|
||||
*/
|
||||
template <class DERIVEDFACTOR>
|
||||
IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
|
||||
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
|
||||
Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
|
||||
}
|
||||
|
||||
/** Collection of factors */
|
||||
FastVector<sharedFactor> factors_;
|
||||
/// `add` is a synonym for push_back.
|
||||
template <class DERIVEDFACTOR>
|
||||
IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
/// `+=` works well with boost::assign list inserter.
|
||||
template <class DERIVEDFACTOR>
|
||||
typename std::enable_if<
|
||||
std::is_base_of<FactorType, DERIVEDFACTOR>::value,
|
||||
boost::assign::list_inserter<RefCallPushBack<This>>>::type
|
||||
operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
|
||||
factor);
|
||||
}
|
||||
|
||||
/** Default constructor */
|
||||
FactorGraph() {}
|
||||
/// @}
|
||||
/// @name Adding via iterators
|
||||
/// @{
|
||||
|
||||
/** Constructor from iterator over factors (shared_ptr or plain objects) */
|
||||
template<typename ITERATOR>
|
||||
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
|
||||
/**
|
||||
* Push back many factors with an iterator over shared_ptr (factors are not
|
||||
* copied)
|
||||
*/
|
||||
template <typename ITERATOR>
|
||||
HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
|
||||
ITERATOR lastFactor) {
|
||||
factors_.insert(end(), firstFactor, lastFactor);
|
||||
}
|
||||
|
||||
/** Construct from container of factors (shared_ptr or plain objects) */
|
||||
template<class CONTAINER>
|
||||
explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }
|
||||
/// Push back many factors with an iterator (factors are copied)
|
||||
template <typename ITERATOR>
|
||||
HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
|
||||
ITERATOR lastFactor) {
|
||||
for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
/// @}
|
||||
/// @name Adding via container
|
||||
/// @{
|
||||
|
||||
// TODO: are these needed?
|
||||
/**
|
||||
* Push back many factors as shared_ptr's in a container (factors are not
|
||||
* copied)
|
||||
*/
|
||||
template <typename CONTAINER>
|
||||
HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
|
||||
///**
|
||||
// * @brief Constructor from a Bayes net
|
||||
// * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
|
||||
// * @return a factor graph with all the conditionals, as factors
|
||||
// */
|
||||
//template<class CONDITIONAL>
|
||||
//FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
|
||||
/// Push back non-pointer objects in a container (factors are copied).
|
||||
template <typename CONTAINER>
|
||||
HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
|
||||
///** convert from Bayes tree */
|
||||
//template<class CONDITIONAL, class CLIQUE>
|
||||
//FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
|
||||
/**
|
||||
* Add a factor or container of factors, including STL collections,
|
||||
* BayesTrees, etc.
|
||||
*/
|
||||
template <class FACTOR_OR_CONTAINER>
|
||||
void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
|
||||
push_back(factorOrContainer);
|
||||
}
|
||||
|
||||
///** convert from a derived type */
|
||||
//template<class DERIVEDFACTOR>
|
||||
//FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
|
||||
// factors_.assign(factors.begin(), factors.end());
|
||||
//}
|
||||
/**
|
||||
* Add a factor or container of factors, including STL collections,
|
||||
* BayesTrees, etc.
|
||||
*/
|
||||
template <class FACTOR_OR_CONTAINER>
|
||||
boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
|
||||
const FACTOR_OR_CONTAINER& factorOrContainer) {
|
||||
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
|
||||
factorOrContainer);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @}
|
||||
/// @name Specialized versions
|
||||
/// @{
|
||||
|
||||
public:
|
||||
/// @name Adding Factors
|
||||
/// @{
|
||||
/**
|
||||
* Push back a BayesTree as a collection of factors.
|
||||
* NOTE: This should be hidden in derived classes in favor of a
|
||||
* type-specialized version that calls this templated function.
|
||||
*/
|
||||
template <class CLIQUE>
|
||||
typename std::enable_if<
|
||||
std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
|
||||
push_back(const BayesTree<CLIQUE>& bayesTree) {
|
||||
bayesTree.addFactorsToGraph(*this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reserve space for the specified number of factors if you know in
|
||||
* advance how many there will be (works like FastVector::reserve).
|
||||
*/
|
||||
void reserve(size_t size) { factors_.reserve(size); }
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
// TODO: are these needed?
|
||||
/** print out graph */
|
||||
void print(const std::string& s = "FactorGraph",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
|
||||
/** Check equality */
|
||||
bool equals(const This& fg, double tol = 1e-9) const;
|
||||
/// @}
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
void push_back(const sharedFactor& factor) {
|
||||
factors_.push_back(factor); }
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Emplace a factor */
|
||||
template<class DERIVEDFACTOR, class... Args>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
emplace_shared(Args&&... args) {
|
||||
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
|
||||
}
|
||||
/** return the number of factors (including any null factors set by remove()
|
||||
* ). */
|
||||
size_t size() const { return factors_.size(); }
|
||||
|
||||
/** push back many factors with an iterator over shared_ptr (factors are not copied) */
|
||||
template<typename ITERATOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
|
||||
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
factors_.insert(end(), firstFactor, lastFactor); }
|
||||
/** Check if the graph is empty (null factors set by remove() will cause
|
||||
* this to return false). */
|
||||
bool empty() const { return factors_.empty(); }
|
||||
|
||||
/** push back many factors as shared_ptr's in a container (factors are not copied) */
|
||||
template<typename CONTAINER>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
|
||||
push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
/** Get a specific factor by index (this checks array bounds and may throw
|
||||
* an exception, as opposed to operator[] which does not).
|
||||
*/
|
||||
const sharedFactor at(size_t i) const { return factors_.at(i); }
|
||||
|
||||
/** push back a BayesTree as a collection of factors. NOTE: This should be hidden in derived
|
||||
* classes in favor of a type-specialized version that calls this templated function. */
|
||||
template<class CLIQUE>
|
||||
typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
|
||||
push_back(const BayesTree<CLIQUE>& bayesTree) {
|
||||
bayesTree.addFactorsToGraph(*this);
|
||||
}
|
||||
/** Get a specific factor by index (this checks array bounds and may throw
|
||||
* an exception, as opposed to operator[] which does not).
|
||||
*/
|
||||
sharedFactor& at(size_t i) { return factors_.at(i); }
|
||||
|
||||
//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
|
||||
/** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
|
||||
* the copy). */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
push_back(const DERIVEDFACTOR& factor) {
|
||||
factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
|
||||
}
|
||||
//#endif
|
||||
/** Get a specific factor by index (this does not check array bounds, as
|
||||
* opposed to at() which does).
|
||||
*/
|
||||
const sharedFactor operator[](size_t i) const { return at(i); }
|
||||
|
||||
/** push back many factors with an iterator over plain factors (factors are copied) */
|
||||
template<typename ITERATOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
|
||||
push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
|
||||
for (ITERATOR f = firstFactor; f != lastFactor; ++f)
|
||||
push_back(*f);
|
||||
}
|
||||
/** Get a specific factor by index (this does not check array bounds, as
|
||||
* opposed to at() which does).
|
||||
*/
|
||||
sharedFactor& operator[](size_t i) { return at(i); }
|
||||
|
||||
/** push back many factors as non-pointer objects in a container (factors are copied) */
|
||||
template<typename CONTAINER>
|
||||
typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
|
||||
push_back(const CONTAINER& container) {
|
||||
push_back(container.begin(), container.end());
|
||||
}
|
||||
/** Iterator to beginning of factors. */
|
||||
const_iterator begin() const { return factors_.begin(); }
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
|
||||
boost::assign::list_inserter<RefCallPushBack<This> > >::type
|
||||
operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
|
||||
}
|
||||
/** Iterator to end of factors. */
|
||||
const_iterator end() const { return factors_.end(); }
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
operator+=(const sharedFactor& factor) {
|
||||
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factor);
|
||||
}
|
||||
/** Get the first factor */
|
||||
sharedFactor front() const { return factors_.front(); }
|
||||
|
||||
/** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
|
||||
template<class FACTOR_OR_CONTAINER>
|
||||
boost::assign::list_inserter<CRefCallPushBack<This> >
|
||||
operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
|
||||
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
|
||||
}
|
||||
/** Get the last factor */
|
||||
sharedFactor back() const { return factors_.back(); }
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
template<class DERIVEDFACTOR>
|
||||
typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
|
||||
add(boost::shared_ptr<DERIVEDFACTOR> factor) {
|
||||
push_back(factor);
|
||||
}
|
||||
/// @}
|
||||
/// @name Modifying Factor Graphs (imperative, discouraged)
|
||||
/// @{
|
||||
|
||||
/** Add a factor directly using a shared_ptr */
|
||||
void add(const sharedFactor& factor) {
|
||||
push_back(factor);
|
||||
}
|
||||
/** non-const STL-style begin() */
|
||||
iterator begin() { return factors_.begin(); }
|
||||
|
||||
/** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
|
||||
template<class FACTOR_OR_CONTAINER>
|
||||
void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
|
||||
push_back(factorOrContainer);
|
||||
}
|
||||
/** non-const STL-style end() */
|
||||
iterator end() { return factors_.end(); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/** Directly resize the number of factors in the graph. If the new size is
|
||||
* less than the original, factors at the end will be removed. If the new
|
||||
* size is larger than the original, null factors will be appended.
|
||||
*/
|
||||
void resize(size_t size) { factors_.resize(size); }
|
||||
|
||||
/** print out graph */
|
||||
void print(const std::string& s = "FactorGraph",
|
||||
const KeyFormatter& formatter = DefaultKeyFormatter) const;
|
||||
/** delete factor without re-arranging indexes by inserting a NULL pointer
|
||||
*/
|
||||
void remove(size_t i) { factors_[i].reset(); }
|
||||
|
||||
/** Check equality */
|
||||
bool equals(const This& fg, double tol = 1e-9) const;
|
||||
/// @}
|
||||
/** replace a factor by index */
|
||||
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
|
||||
|
||||
public:
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/** Erase factor and rearrange other factors to take up the empty space */
|
||||
iterator erase(iterator item) { return factors_.erase(item); }
|
||||
|
||||
/** return the number of factors (including any null factors set by remove() ). */
|
||||
size_t size() const { return factors_.size(); }
|
||||
/** Erase factors and rearrange other factors to take up the empty space */
|
||||
iterator erase(iterator first, iterator last) {
|
||||
return factors_.erase(first, last);
|
||||
}
|
||||
|
||||
/** Check if the graph is empty (null factors set by remove() will cause this to return false). */
|
||||
bool empty() const { return factors_.empty(); }
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
|
||||
* opposed to operator[] which does not).
|
||||
*/
|
||||
const sharedFactor at(size_t i) const { return factors_.at(i); }
|
||||
/** return the number of non-null factors */
|
||||
size_t nrFactors() const;
|
||||
|
||||
/** Get a specific factor by index (this checks array bounds and may throw an exception, as
|
||||
* opposed to operator[] which does not).
|
||||
*/
|
||||
sharedFactor& at(size_t i) { return factors_.at(i); }
|
||||
/** Potentially slow function to return all keys involved, sorted, as a set
|
||||
*/
|
||||
KeySet keys() const;
|
||||
|
||||
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
|
||||
* does).
|
||||
*/
|
||||
const sharedFactor operator[](size_t i) const { return at(i); }
|
||||
/** Potentially slow function to return all keys involved, sorted, as a
|
||||
* vector
|
||||
*/
|
||||
KeyVector keyVector() const;
|
||||
|
||||
/** Get a specific factor by index (this does not check array bounds, as opposed to at() which
|
||||
* does).
|
||||
*/
|
||||
sharedFactor& operator[](size_t i) { return at(i); }
|
||||
/** MATLAB interface utility: Checks whether a factor index idx exists in
|
||||
* the graph and is a live pointer */
|
||||
inline bool exists(size_t idx) const { return idx < size() && at(idx); }
|
||||
|
||||
/** Iterator to beginning of factors. */
|
||||
const_iterator begin() const { return factors_.begin();}
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
ar& BOOST_SERIALIZATION_NVP(factors_);
|
||||
}
|
||||
|
||||
/** Iterator to end of factors. */
|
||||
const_iterator end() const { return factors_.end(); }
|
||||
|
||||
/** Get the first factor */
|
||||
sharedFactor front() const { return factors_.front(); }
|
||||
|
||||
/** Get the last factor */
|
||||
sharedFactor back() const { return factors_.back(); }
|
||||
|
||||
/// @}
|
||||
/// @name Modifying Factor Graphs (imperative, discouraged)
|
||||
/// @{
|
||||
|
||||
/** non-const STL-style begin() */
|
||||
iterator begin() { return factors_.begin();}
|
||||
|
||||
/** non-const STL-style end() */
|
||||
iterator end() { return factors_.end(); }
|
||||
|
||||
/** Directly resize the number of factors in the graph. If the new size is less than the
|
||||
* original, factors at the end will be removed. If the new size is larger than the original,
|
||||
* null factors will be appended.
|
||||
*/
|
||||
void resize(size_t size) { factors_.resize(size); }
|
||||
|
||||
/** delete factor without re-arranging indexes by inserting a NULL pointer */
|
||||
void remove(size_t i) { factors_[i].reset();}
|
||||
|
||||
/** replace a factor by index */
|
||||
void replace(size_t index, sharedFactor factor) { at(index) = factor; }
|
||||
|
||||
/** Erase factor and rearrange other factors to take up the empty space */
|
||||
iterator erase(iterator item) { return factors_.erase(item); }
|
||||
|
||||
/** Erase factors and rearrange other factors to take up the empty space */
|
||||
iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** return the number of non-null factors */
|
||||
size_t nrFactors() const;
|
||||
|
||||
/** Potentially slow function to return all keys involved, sorted, as a set */
|
||||
KeySet keys() const;
|
||||
|
||||
/** Potentially slow function to return all keys involved, sorted, as a vector */
|
||||
KeyVector keyVector() const;
|
||||
|
||||
/** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
|
||||
inline bool exists(size_t idx) const { return idx < size() && at(idx); }
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(factors_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // FactorGraph
|
||||
|
||||
} // namespace gtsam
|
||||
/// @}
|
||||
}; // FactorGraph
|
||||
} // namespace gtsam
|
||||
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
|
|
|
|||
Loading…
Reference in New Issue