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

Made some type changes to FactorIndices

release/4.3a0
dellaert 2016-02-26 00:01:59 -08:00
parent fad9462661
commit 2ca649a11f
2 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -43,7 +43,7 @@ bool ConcurrentIncrementalFilter::equals(const ConcurrentFilter& rhs, double tol
/* ************************************************************************* */ /* ************************************************************************* */
ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const NonlinearFactorGraph& newFactors, const Values& newTheta,
const boost::optional<FastList<Key> >& keysToMove, const boost::optional< std::vector<size_t> >& removeFactorIndices) { const boost::optional<FastList<Key> >& keysToMove, const boost::optional< FactorIndices >& removeFactorIndices) {
gttic(update); gttic(update);
@ -117,7 +117,7 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
gttic(cache_smoother_factors); gttic(cache_smoother_factors);
// Find the set of factors that will be removed // Find the set of factors that will be removed
std::vector<size_t> removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_); FactorIndices removedFactorSlots = FindAdjacentFactors(isam2_, *keysToMove, currentSmootherSummarizationSlots_);
// Cache these factors for later transmission to the smoother // Cache these factors for later transmission to the smoother
NonlinearFactorGraph removedFactors; NonlinearFactorGraph removedFactors;
BOOST_FOREACH(size_t slot, removedFactorSlots) { BOOST_FOREACH(size_t slot, removedFactorSlots) {
@ -134,8 +134,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No
gttoc(cache_smoother_factors); gttoc(cache_smoother_factors);
gttic(marginalize); gttic(marginalize);
std::vector<size_t> marginalFactorsIndices; FactorIndices marginalFactorsIndices;
std::vector<size_t> deletedFactorsIndices; FactorIndices deletedFactorsIndices;
isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices); isam2_.marginalizeLeaves(*keysToMove, marginalFactorsIndices, deletedFactorsIndices);
currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end()); currentSmootherSummarizationSlots_.insert(currentSmootherSummarizationSlots_.end(), marginalFactorsIndices.begin(), marginalFactorsIndices.end());
BOOST_FOREACH(size_t index, deletedFactorsIndices) { BOOST_FOREACH(size_t index, deletedFactorsIndices) {
@ -285,10 +285,10 @@ void ConcurrentIncrementalFilter::RecursiveMarkAffectedKeys(const Key& key, cons
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore) { FactorIndices ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore) {
// Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove) // Identify any new factors to be sent to the smoother (i.e. any factor involving keysToMove)
std::vector<size_t> removedFactorSlots; FactorIndices removedFactorSlots;
const VariableIndex& variableIndex = isam2.getVariableIndex(); const VariableIndex& variableIndex = isam2.getVariableIndex();
BOOST_FOREACH(Key key, keys) { BOOST_FOREACH(Key key, keys) {
const FastVector<size_t>& slots = variableIndex[key]; const FastVector<size_t>& slots = variableIndex[key];

8
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h
View File

@ -46,7 +46,7 @@ public:
* factors passed as \c newFactors update(). These indices may be * factors passed as \c newFactors update(). These indices may be
* used later to refer to the factors in order to remove them. * used later to refer to the factors in order to remove them.
*/ */
std::vector<size_t> newFactorsIndices; FactorIndices newFactorsIndices;
double error; ///< The final factor graph error double error; ///< The final factor graph error
@ -124,7 +124,7 @@ public:
*/ */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(),
const boost::optional<FastList<Key> >& keysToMove = boost::none, const boost::optional<FastList<Key> >& keysToMove = boost::none,
const boost::optional< std::vector<size_t> >& removeFactorIndices = boost::none); const boost::optional< FactorIndices >& removeFactorIndices = boost::none);
/** /**
* Perform any required operations before the synchronization process starts. * Perform any required operations before the synchronization process starts.
@ -170,7 +170,7 @@ protected:
// ??? // ???
NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization
std::vector<size_t> currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator FactorIndices currentSmootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization on the current separator
NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update) NonlinearFactorGraph smootherShortcut_; ///< A set of conditional factors from the old separator to the current separator (recursively calculated during each filter update)
// Storage for information to be sent to the smoother // Storage for information to be sent to the smoother
@ -183,7 +183,7 @@ private:
static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys); static void RecursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys);
/** Find the set of iSAM2 factors adjacent to 'keys' */ /** Find the set of iSAM2 factors adjacent to 'keys' */
static std::vector<size_t> FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const std::vector<size_t>& factorsToIgnore); static FactorIndices FindAdjacentFactors(const ISAM2& isam2, const FastList<Key>& keys, const FactorIndices& factorsToIgnore);
/** Update the shortcut marginal between the current separator keys and the previous separator keys */ /** Update the shortcut marginal between the current separator keys and the previous separator keys */
// TODO: Make this a static function // TODO: Make this a static function