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

Globally replaced FastSet<Key> with KeySet.

release/4.3a0
dellaert 2015-06-20 18:38:25 -07:00
parent d1be7caed5
commit 128bac616c
43 changed files with 133 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/base/tests/testFastContainers.cpp
View File

@ -7,13 +7,14 @@
* @author Alex Cunningham * @author Alex Cunningham
*/ */
#include <CppUnitLite/TestHarness.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/assign/std/set.hpp> #include <boost/assign/std/set.hpp>
#include <gtsam/base/FastSet.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/FastVector.h>
using namespace boost::assign; using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
@ -21,7 +22,7 @@ using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( testFastContainers, KeySet ) { TEST( testFastContainers, KeySet ) {
FastVector<size_t> init_vector; FastVector<Key> init_vector;
init_vector += 2, 3, 4, 5; init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector); FastSet<size_t> actSet(init_vector);

10
gtsam/base/tests/testSerializationBase.cpp
View File

@ -16,6 +16,8 @@
* @date Feb 7, 2012 * @date Feb 7, 2012
*/ */
#include <gtsam/inference/Key.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
@ -60,10 +62,10 @@ TEST (Serialization, FastMap) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, FastSet) { TEST (Serialization, FastSet) {
FastSet<double> set; KeySet set;
set.insert(1.0); set.insert(1);
set.insert(2.0); set.insert(2);
set.insert(3.0); set.insert(3);
EXPECT(equality(set)); EXPECT(equality(set));
EXPECT(equalityXML(set)); EXPECT(equalityXML(set));

4
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -39,8 +39,8 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const { KeySet DiscreteFactorGraph::keys() const {
FastSet<Key> keys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end()); if (factor) keys.insert(factor->begin(), factor->end());
return keys; return keys;

2
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -120,7 +120,7 @@ public:
} }
/** Return the set of variables involved in the factors (set union) */ /** Return the set of variables involved in the factors (set union) */
FastSet<Key> keys() const; KeySet keys() const;
/** return product of all factors as a single factor */ /** return product of all factors as a single factor */
DecisionTreeFactor product() const; DecisionTreeFactor product() const;

1
gtsam/discrete/Potentials.h
View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h> #include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteKey.h> #include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/inference/Key.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <set> #include <set>

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

@ -344,7 +344,7 @@ namespace gtsam {
gttic(Full_root_factoring); gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; { boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
FastVector<Key> C1_minus_B; { FastVector<Key> C1_minus_B; {
FastSet<Key> C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) { BOOST_FOREACH(const Key j, *B->conditional()) {
C1_minus_B_set.erase(j); } C1_minus_B_set.erase(j); }
C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
@ -356,7 +356,7 @@ namespace gtsam {
} }
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; { boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
FastVector<Key> C2_minus_B; { FastVector<Key> C2_minus_B; {
FastSet<Key> C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
BOOST_FOREACH(const Key j, *B->conditional()) { BOOST_FOREACH(const Key j, *B->conditional()) {
C2_minus_B_set.erase(j); } C2_minus_B_set.erase(j); }
C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());

6
gtsam/inference/BayesTree.h
View File

@ -19,13 +19,13 @@
#pragma once #pragma once
#include <string> #include <gtsam/inference/Key.h>
#include <gtsam/base/types.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h> #include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <string>
namespace gtsam { namespace gtsam {
// Forward declarations // Forward declarations

8
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -44,8 +44,8 @@ namespace gtsam {
FastVector<Key> FastVector<Key>
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{ {
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B; FastVector<Key> S_setminus_B;
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
@ -58,8 +58,8 @@ namespace gtsam {
const derived_ptr& B, const FactorGraphType& p_Cp_B) const const derived_ptr& B, const FactorGraphType& p_Cp_B) const
{ {
gttic(shortcut_indices); gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys(); KeySet allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end()); KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B = separator_setminus_B(B); FastVector<Key> S_setminus_B = separator_setminus_B(B);
FastVector<Key> keep; FastVector<Key> keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted) // keep = S\B intersect allKeys (S_setminus_B is already sorted)

5
gtsam/inference/FactorGraph-inst.h
View File

@ -28,6 +28,7 @@
#include <stdio.h> #include <stdio.h>
#include <sstream> #include <sstream>
#include <iostream> // for cout :-(
namespace gtsam { namespace gtsam {
@ -72,8 +73,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
FastSet<Key> FactorGraph<FACTOR>::keys() const { KeySet FactorGraph<FACTOR>::keys() const {
FastSet<Key> allKeys; KeySet allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor) if (factor)
allKeys.insert(factor->begin(), factor->end()); allKeys.insert(factor->begin(), factor->end());

2
gtsam/inference/FactorGraph.h
View File

@ -332,7 +332,7 @@ namespace gtsam {
size_t nrFactors() const; size_t nrFactors() const;
/** Potentially very slow function to return all keys involved */ /** Potentially very slow function to return all keys involved */
FastSet<Key> keys() const; KeySet keys() const;
/** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ /** 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); } inline bool exists(size_t idx) const { return idx < size() && at(idx); }

4
gtsam/inference/ISAM-inst.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
BayesNetType bn; BayesNetType bn;
if (!this->empty()) { if (!this->empty()) {
const FastSet<Key> newFactorKeys = newFactors.keys(); const KeySet newFactorKeys = newFactors.keys();
this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); this->removeTop(std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans);
} }
@ -44,7 +44,7 @@ namespace gtsam {
// eliminate into a Bayes net // eliminate into a Bayes net
const VariableIndex varIndex(factors); const VariableIndex varIndex(factors);
const FastSet<Key> newFactorKeys = newFactors.keys(); const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering = const Ordering constrainedOrdering =
Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end())); Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);

4
gtsam/inference/MetisIndex-inl.h
View File

@ -26,8 +26,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTOR> template<class FACTOR>
void MetisIndex::augment(const FactorGraph<FACTOR>& factors) { void MetisIndex::augment(const FactorGraph<FACTOR>& factors) {
std::map<int32_t, FastSet<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
std::map<int32_t, FastSet<int32_t> >::iterator iAdjMapIt; std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
std::set<Key> keySet; std::set<Key> keySet;
/* ********** Convert to CSR format ********** */ /* ********** Convert to CSR format ********** */

2
gtsam/inference/Ordering.h
View File

@ -149,7 +149,7 @@ namespace gtsam {
/// Return a natural Ordering. Typically used by iterative solvers /// Return a natural Ordering. Typically used by iterative solvers
template <class FACTOR> template <class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) { static Ordering Natural(const FactorGraph<FACTOR> &fg) {
FastSet<Key> src = fg.keys(); KeySet src = fg.keys();
std::vector<Key> keys(src.begin(), src.end()); std::vector<Key> keys(src.begin(), src.end());
std::stable_sort(keys.begin(), keys.end()); std::stable_sort(keys.begin(), keys.end());
return Ordering(keys); return Ordering(keys);

5
gtsam/inference/VariableIndex.h
View File

@ -17,14 +17,11 @@
#pragma once #pragma once
#include <gtsam/base/FastList.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <boost/none.hpp>
#include <boost/optional/optional.hpp> #include <boost/optional/optional.hpp>
#include <boost/smart_ptr/shared_ptr.hpp> #include <boost/smart_ptr/shared_ptr.hpp>

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -47,7 +47,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Key> keys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) if (factor)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());

2
gtsam/linear/GaussianFactorGraph.h
View File

@ -135,7 +135,7 @@ namespace gtsam {
* Return the set of variables involved in the factors (computes a set * Return the set of variables involved in the factors (computes a set
* union). * union).
*/ */
typedef FastSet<Key> Keys; typedef KeySet Keys;
Keys keys() const; Keys keys() const;
/* return a map of (Key, dimension) */ /* return a map of (Key, dimension) */

28
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -84,11 +84,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots, void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
FastSet<Key>& replacedKeys, Base::Nodes& nodes, KeySet& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables) KeySet& fixedVariables)
{ {
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
BOOST_FOREACH(Key key, unusedKeys) { BOOST_FOREACH(Key key, unusedKeys) {
@ -103,10 +103,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const FastVect
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{ {
FastSet<Key> relinKeys; KeySet relinKeys;
if(const double* threshold = boost::get<double>(&relinearizeThreshold)) if(const double* threshold = boost::get<double>(&relinearizeThreshold))
{ {
@ -131,7 +131,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double threshold, void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold,
const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) const VectorValues& delta, const ISAM2Clique::shared_ptr& clique)
{ {
// Check the current clique for relinearization // Check the current clique for relinearization
@ -153,7 +153,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Key>& relinKeys, double thresho
} }
/* ************************************************************************* */ /* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<char,Vector>& thresholds, void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap<char,Vector>& thresholds,
const VectorValues& delta, const VectorValues& delta,
const ISAM2Clique::shared_ptr& clique) const ISAM2Clique::shared_ptr& clique)
{ {
@ -185,11 +185,11 @@ void CheckRelinearizationRecursiveMap(FastSet<Key>& relinKeys, const FastMap<cha
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots, KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) const ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{ {
FastSet<Key> relinKeys; KeySet relinKeys;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
if(relinearizeThreshold.type() == typeid(double)) if(relinearizeThreshold.type() == typeid(double))
CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root); CheckRelinearizationRecursiveDouble(relinKeys, boost::get<double>(relinearizeThreshold), delta, root);
@ -200,7 +200,7 @@ FastSet<Key> ISAM2::Impl::CheckRelinearizationPartial(const FastVector<ISAM2::sh
} }
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask) void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask)
{ {
static const bool debug = false; static const bool debug = false;
// does the separator contain any of the variables? // does the separator contain any of the variables?
@ -224,7 +224,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, co
/* ************************************************************************* */ /* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta,
const FastSet<Key>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) const KeySet& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter)
{ {
// If debugging, invalidate if requested, otherwise do not invalidate. // If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions // Invalidating means setting expmapped entries to Inf, to trigger assertions
@ -272,7 +272,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots, size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) { const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount; size_t lastBacksubVariableCount;
@ -300,7 +300,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
/* ************************************************************************* */ /* ************************************************************************* */
namespace internal { namespace internal {
void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Key>& replacedKeys, void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const KeySet& replacedKeys,
const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need // Check if any frontal or separator keys were recalculated, if so, we need
@ -344,7 +344,7 @@ void updateRgProd(const boost::shared_ptr<ISAM2Clique>& clique, const FastSet<Ke
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys, size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd) { const VectorValues& gradAtZero, VectorValues& RgProd) {
// Update variables // Update variables

18
gtsam/nonlinear/ISAM2-impl.h
View File

@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl {
/** /**
* Remove variables from the ISAM2 system. * Remove variables from the ISAM2 system.
*/ */
static void RemoveVariables(const FastSet<Key>& unusedKeys, const FastVector<ISAM2::sharedClique>& roots, static void RemoveVariables(const KeySet& unusedKeys, const FastVector<ISAM2::sharedClique>& roots,
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes, VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables); KeySet& fixedVariables);
/** /**
* Find the set of variables to be relinearized according to relinearizeThreshold. * Find the set of variables to be relinearized according to relinearizeThreshold.
@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or * @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold * equal to relinearizeThreshold
*/ */
static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta, static KeySet CheckRelinearizationFull(const VectorValues& delta,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/** /**
@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or * @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold * equal to relinearizeThreshold
*/ */
static FastSet<Key> CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots, static KeySet CheckRelinearizationPartial(const FastVector<ISAM2::sharedClique>& roots,
const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
/** /**
@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* *
* Alternatively could we trace up towards the root for each variable here? * Alternatively could we trace up towards the root for each variable here?
*/ */
static void FindAll(ISAM2Clique::shared_ptr clique, FastSet<Key>& keys, const FastSet<Key>& markedMask); static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask);
/** /**
* Apply expmap to the given values, but only for indices appearing in * Apply expmap to the given values, but only for indices appearing in
@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @param keyFormatter Formatter for printing nonlinear keys during debugging * @param keyFormatter Formatter for printing nonlinear keys during debugging
*/ */
static void ExpmapMasked(Values& values, const VectorValues& delta, static void ExpmapMasked(Values& values, const VectorValues& delta,
const FastSet<Key>& mask, const KeySet& mask,
boost::optional<VectorValues&> invalidateIfDebug = boost::none, boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter); const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
* Update the Newton's method step point, using wildfire * Update the Newton's method step point, using wildfire
*/ */
static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots, static size_t UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>& roots,
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold); const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold);
/** /**
* Update the RgProd (R*g) incrementally taking into account which variables * Update the RgProd (R*g) incrementally taking into account which variables
* have been recalculated in \c replacedKeys. Only used in Dogleg. * have been recalculated in \c replacedKeys. Only used in Dogleg.
*/ */
static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet<Key>& replacedKeys, static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys,
const VectorValues& gradAtZero, VectorValues& RgProd); const VectorValues& gradAtZero, VectorValues& RgProd);
/** /**

12
gtsam/nonlinear/ISAM2-inl.h
View File

@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
namespace internal { namespace internal {
template<class CLIQUE> template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold, void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count) KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
{ {
// if none of the variables in this clique (frontal and separator!) changed // if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the // significantly, then by the running intersection property, none of the
@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
template<class CLIQUE> template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold, bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
FastSet<Key>& changed, const FastSet<Key>& replaced, VectorValues& delta, size_t& count) KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count)
{ {
// if none of the variables in this clique (frontal and separator!) changed // if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the // significantly, then by the running intersection property, none of the
@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta) { size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta) {
FastSet<Key> changed; KeySet changed;
int count = 0; int count = 0;
// starting from the root, call optimize on each conditional // starting from the root, call optimize on each conditional
if(root) if(root)
@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta) size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta)
{ {
FastSet<Key> changed; KeySet changed;
size_t count = 0; size_t count = 0;
if (root) { if (root) {

38
gtsam/nonlinear/ISAM2.cpp
View File

@ -194,7 +194,7 @@ FastSet<size_t> ISAM2::getAffectedFactors(const FastList<Key>& keys) const {
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
GaussianFactorGraph::shared_ptr GaussianFactorGraph::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const
{ {
gttic(getAffectedFactors); gttic(getAffectedFactors);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys); FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastS
gttic(affectedKeysSet); gttic(affectedKeysSet);
// for fast lookup below // for fast lookup below
FastSet<Key> affectedKeysSet; KeySet affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
gttoc(affectedKeysSet); gttoc(affectedKeysSet);
@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys, boost::shared_ptr<KeySet > ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const vector<Key>& observedKeys, const vector<Key>& observedKeys,
const FastSet<Key>& unusedIndices, const KeySet& unusedIndices,
const boost::optional<FastMap<Key,int> >& constrainKeys, const boost::optional<FastMap<Key,int> >& constrainKeys,
ISAM2Result& result) ISAM2Result& result)
{ {
@ -326,7 +326,7 @@ boost::shared_ptr<FastSet<Key> > ISAM2::recalculate(const FastSet<Key>& markedKe
affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals());
gttoc(affectedKeys); gttoc(affectedKeys);
boost::shared_ptr<FastSet<Key> > affectedKeysSet(new FastSet<Key>()); // Will return this result boost::shared_ptr<KeySet > affectedKeysSet(new KeySet()); // Will return this result
if(affectedKeys.size() >= theta_.size() * batchThreshold) if(affectedKeys.size() >= theta_.size() * batchThreshold)
{ {
@ -566,17 +566,17 @@ ISAM2Result ISAM2::update(
variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors);
// Compute unused keys and indices // Compute unused keys and indices
FastSet<Key> unusedKeys; KeySet unusedKeys;
FastSet<Key> unusedIndices; KeySet unusedIndices;
{ {
// Get keys from removed factors and new factors, and compute unused keys, // Get keys from removed factors and new factors, and compute unused keys,
// i.e., keys that are empty now and do not appear in the new factors. // i.e., keys that are empty now and do not appear in the new factors.
FastSet<Key> removedAndEmpty; KeySet removedAndEmpty;
BOOST_FOREACH(Key key, removeFactors.keys()) { BOOST_FOREACH(Key key, removeFactors.keys()) {
if(variableIndex_[key].empty()) if(variableIndex_[key].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key); removedAndEmpty.insert(removedAndEmpty.end(), key);
} }
FastSet<Key> newFactorSymbKeys = newFactors.keys(); KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
@ -602,10 +602,10 @@ ISAM2Result ISAM2::update(
gttic(gather_involved_keys); gttic(gather_involved_keys);
// 3. Mark linear update // 3. Mark linear update
FastSet<Key> markedKeys = newFactors.keys(); // Get keys from new factors KeySet markedKeys = newFactors.keys(); // Get keys from new factors
// Also mark keys involved in removed factors // Also mark keys involved in removed factors
{ {
FastSet<Key> markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors
markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
} }
// Also mark any provided extra re-eliminate keys // Also mark any provided extra re-eliminate keys
@ -632,7 +632,7 @@ ISAM2Result ISAM2::update(
gttoc(gather_involved_keys); gttoc(gather_involved_keys);
// Check relinearization if we're at the nth step, or we are using a looser loop relin threshold // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
FastSet<Key> relinKeys; KeySet relinKeys;
if (relinearizeThisStep) { if (relinearizeThisStep) {
gttic(gather_relinearize_keys); gttic(gather_relinearize_keys);
// 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
@ -659,7 +659,7 @@ ISAM2Result ISAM2::update(
result.detail->variableStatus[key].isRelinearized = true; } } result.detail->variableStatus[key].isRelinearized = true; } }
// Add the variables being relinearized to the marked keys // Add the variables being relinearized to the marked keys
FastSet<Key> markedRelinMask; KeySet markedRelinMask;
BOOST_FOREACH(const Key key, relinKeys) BOOST_FOREACH(const Key key, relinKeys)
markedRelinMask.insert(key); markedRelinMask.insert(key);
markedKeys.insert(relinKeys.begin(), relinKeys.end()); markedKeys.insert(relinKeys.begin(), relinKeys.end());
@ -674,7 +674,7 @@ ISAM2Result ISAM2::update(
// Relin involved keys for detailed results // Relin involved keys for detailed results
if(params_.enableDetailedResults) { if(params_.enableDetailedResults) {
FastSet<Key> involvedRelinKeys; KeySet involvedRelinKeys;
BOOST_FOREACH(const sharedClique& root, roots_) BOOST_FOREACH(const sharedClique& root, roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask); Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Key key, involvedRelinKeys) { BOOST_FOREACH(Key key, involvedRelinKeys) {
@ -726,7 +726,7 @@ ISAM2Result ISAM2::update(
gttic(recalculate); gttic(recalculate);
// 8. Redo top of Bayes tree // 8. Redo top of Bayes tree
boost::shared_ptr<FastSet<Key> > replacedKeys; boost::shared_ptr<KeySet > replacedKeys;
if(!markedKeys.empty() || !observedKeys.empty()) if(!markedKeys.empty() || !observedKeys.empty())
replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result);
@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
boost::optional<std::vector<size_t>&> deletedFactorsIndices) boost::optional<std::vector<size_t>&> deletedFactorsIndices)
{ {
// Convert to ordered set // Convert to ordered set
FastSet<Key> leafKeys(leafKeysList.begin(), leafKeysList.end()); KeySet leafKeys(leafKeysList.begin(), leafKeysList.end());
// Keep track of marginal factors - map from clique to the marginal factors // Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children. // that should be incorporated into it, passed up from it's children.
@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
FastSet<size_t> factorIndicesToRemove; FastSet<size_t> factorIndicesToRemove;
// Keep track of variables removed in subtrees // Keep track of variables removed in subtrees
FastSet<Key> leafKeysRemoved; KeySet leafKeysRemoved;
// Remove each variable and its subtrees // Remove each variable and its subtrees
BOOST_FOREACH(Key j, leafKeys) { BOOST_FOREACH(Key j, leafKeys) {
@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
// Reeliminate the linear graph to get the marginal and discard the conditional // Reeliminate the linear graph to get the marginal and discard the conditional
const FastSet<Key> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate; FastVector<Key> cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate)); std::back_inserter(cliqueFrontalsToEliminate));
@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end());
// Remove the marginalized variables // Remove the marginalized variables
Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
deltaReplacedMask_, nodes_, fixedVariables_); deltaReplacedMask_, nodes_, fixedVariables_);
} }

16
gtsam/nonlinear/ISAM2.h
View File

@ -460,7 +460,7 @@ protected:
* This is \c mutable because it is used internally to not update delta_ * This is \c mutable because it is used internally to not update delta_
* until it is needed. * until it is needed.
*/ */
mutable FastSet<Key> deltaReplacedMask_; // TODO: Make sure accessed in the right way mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way
/** All original nonlinear factors are stored here to use during relinearization */ /** All original nonlinear factors are stored here to use during relinearization */
NonlinearFactorGraph nonlinearFactors_; NonlinearFactorGraph nonlinearFactors_;
@ -476,7 +476,7 @@ protected:
/** Set of variables that are involved with linear factors from marginalized /** Set of variables that are involved with linear factors from marginalized
* variables and thus cannot have their linearization points changed. */ * variables and thus cannot have their linearization points changed. */
FastSet<Key> fixedVariables_; KeySet fixedVariables_;
int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization
@ -614,7 +614,7 @@ public:
const VariableIndex& getVariableIndex() const { return variableIndex_; } const VariableIndex& getVariableIndex() const { return variableIndex_; }
/** Access the nonlinear variable index */ /** Access the nonlinear variable index */
const FastSet<Key>& getFixedVariables() const { return fixedVariables_; } const KeySet& getFixedVariables() const { return fixedVariables_; }
size_t lastAffectedVariableCount; size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount; size_t lastAffectedFactorCount;
@ -641,11 +641,11 @@ public:
protected: protected:
FastSet<size_t> getAffectedFactors(const FastList<Key>& keys) const; FastSet<size_t> getAffectedFactors(const FastList<Key>& keys) const;
GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastSet<Key>& relinKeys) const; GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const KeySet& relinKeys) const;
GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
virtual boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys, virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result); const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
void updateDelta(bool forceFullSolve = false) const; void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2 }; // ISAM2
@ -666,11 +666,11 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
/// @return The number of variables that were solved for /// @return The number of variables that were solved for
template<class CLIQUE> template<class CLIQUE>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const FastSet<Key>& replaced, VectorValues& delta); double threshold, const KeySet& replaced, VectorValues& delta);
template<class CLIQUE> template<class CLIQUE>
size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, size_t optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
double threshold, const FastSet<Key>& replaced, VectorValues& delta); double threshold, const KeySet& replaced, VectorValues& delta);
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
template<class CLIQUE> template<class CLIQUE>

8
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -70,7 +70,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << " size=\"" << formatting.figureWidthInches << "," << stm << " size=\"" << formatting.figureWidthInches << "," <<
formatting.figureHeightInches << "\";\n\n"; formatting.figureHeightInches << "\";\n\n";
FastSet<Key> keys = this->keys(); KeySet keys = this->keys();
// Local utility function to extract x and y coordinates // Local utility function to extract x and y coordinates
struct { boost::optional<Point2> operator()( struct { boost::optional<Point2> operator()(
@ -144,7 +144,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if (formatting.mergeSimilarFactors) { if (formatting.mergeSimilarFactors) {
// Remove duplicate factors // Remove duplicate factors
FastSet<vector<Key> > structure; std::set<vector<Key> > structure;
BOOST_FOREACH(const sharedFactor& factor, *this){ BOOST_FOREACH(const sharedFactor& factor, *this){
if(factor) { if(factor) {
vector<Key> factorKeys = factor->keys(); vector<Key> factorKeys = factor->keys();
@ -234,8 +234,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FastSet<Key> NonlinearFactorGraph::keys() const { KeySet NonlinearFactorGraph::keys() const {
FastSet<Key> keys; KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor) if(factor)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys as an ordered set - ordering is by key value */ /** return keys as an ordered set - ordering is by key value */
FastSet<Key> keys() const; KeySet keys() const;
/** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
double error(const Values& c) const; double error(const Values& c) const;

4
gtsam/symbolic/SymbolicFactor-inst.h
View File

@ -38,7 +38,7 @@ namespace gtsam
EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys) EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
{ {
// Gather all keys // Gather all keys
FastSet<Key> allKeys; KeySet allKeys;
BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) { BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
allKeys.insert(factor->begin(), factor->end()); allKeys.insert(factor->begin(), factor->end());
} }
@ -50,7 +50,7 @@ namespace gtsam
} }
// Sort frontal keys // Sort frontal keys
FastSet<Key> frontals(keys); KeySet frontals(keys);
const size_t nFrontals = keys.size(); const size_t nFrontals = keys.size();
// Build a key vector with the frontals followed by the separator // Build a key vector with the frontals followed by the separator

2
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -193,7 +193,7 @@ private:
// add the belief factor for each neighbor variable to this star graph // add the belief factor for each neighbor variable to this star graph
// also record the factor index for later modification // also record the factor index for later modification
FastSet<Key> neighbors = star->keys(); KeySet neighbors = star->keys();
neighbors.erase(key); neighbors.erase(key);
CorrectedBeliefIndices correctedBeliefIndices; CorrectedBeliefIndices correctedBeliefIndices;
BOOST_FOREACH(Key neighbor, neighbors) { BOOST_FOREACH(Key neighbor, neighbors) {

2
gtsam_unstable/linear/QPSolver.h
View File

@ -48,7 +48,7 @@ class QPSolver {
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process.
VariableIndex costVariableIndex_, equalityVariableIndex_, VariableIndex costVariableIndex_, equalityVariableIndex_,
inequalityVariableIndex_; inequalityVariableIndex_;
FastSet<Key> constrainedKeys_; //!< all constrained keys, will become factors in the dual graph KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph
public: public:
/// Constructor /// Constructor

6
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -463,7 +463,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys, void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys,
const std::string& label) { const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { BOOST_FOREACH(gtsam::Key key, keys) {
@ -531,13 +531,13 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
"BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: ");
// Get the set of all keys involved in the factor graph // Get the set of all keys involved in the factor graph
FastSet<Key> allKeys(graph.keys()); KeySet allKeys(graph.keys());
if (debug) if (debug)
PrintKeySet(allKeys, PrintKeySet(allKeys,
"BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); "BatchFixedLagSmoother::calculateMarginalFactors All Keys: ");
// Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
FastSet<Key> remainingKeys; KeySet remainingKeys;
std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
if (debug) if (debug)

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -161,7 +161,7 @@ protected:
private: private:
/** Private methods for printing debug information */ /** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label); static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label); static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);

6
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm
if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); } if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); }
// Find the set of new separator keys // Find the set of new separator keys
FastSet<Key> newSeparatorKeys; KeySet newSeparatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key); newSeparatorKeys.insert(key_value.key);
} }
@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
} }
// Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove
FastSet<Key> newSeparatorKeys = removedFactors.keys(); KeySet newSeparatorKeys = removedFactors.keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
newSeparatorKeys.insert(key_value.key); newSeparatorKeys.insert(key_value.key);
} }
@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList<Key>& keysToMove) {
if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); } if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); }
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys = newSeparatorKeys; KeySet shortcutKeys = newSeparatorKeys;
BOOST_FOREACH(Key key, smootherSummarization_.keys()) { BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
shortcutKeys.insert(key); shortcutKeys.insert(key);
} }

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
} }
// Get the set of separator keys // Get the set of separator keys
gtsam::FastSet<Key> separatorKeys; gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key); separatorKeys.insert(key_value.key);
} }

8
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -52,16 +52,16 @@ namespace internal {
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
// Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
FastSet<Key> rootKeys; KeySet rootKeys;
FastSet<Key> allKeys(graph.keys()); KeySet allKeys(graph.keys());
std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end())); std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));
// Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys
FastSet<Key> marginalizeKeys; KeySet marginalizeKeys;
std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end()));
if(marginalizeKeys.size() == 0) { if(marginalizeKeys.size() == 0) {

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -152,7 +152,7 @@ namespace internal {
/** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors. /** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors.
* Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */ * Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */
NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction);
} }

6
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth
previousSmootherSummarization_ = smootherSummarization; previousSmootherSummarization_ = smootherSummarization;
// Find the set of new separator keys // Find the set of new separator keys
const FastSet<Key>& newSeparatorKeys = isam2_.getFixedVariables(); const KeySet& newSeparatorKeys = isam2_.getFixedVariables();
// Use the shortcut to calculate an updated marginal on the current separator // Use the shortcut to calculate an updated marginal on the current separator
// Combine just the shortcut and the previousSmootherSummarization // Combine just the shortcut and the previousSmootherSummarization
@ -312,7 +312,7 @@ std::vector<size_t> ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2
void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) { void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) {
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys; KeySet shortcutKeys;
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) { if(factor) {
@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// variables that result from marginalizing out all of the other variables // variables that result from marginalizing out all of the other variables
// Find the set of current separator keys // Find the set of current separator keys
const FastSet<Key>& separatorKeys = isam2_.getFixedVariables(); const KeySet& separatorKeys = isam2_.getFixedVariables();
// Find all cliques that contain any separator variables // Find all cliques that contain any separator variables
std::set<ISAM2Clique::shared_ptr> separatorCliques; std::set<ISAM2Clique::shared_ptr> separatorCliques;

2
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
} }
// Get the set of separator keys // Get the set of separator keys
gtsam::FastSet<Key> separatorKeys; gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
separatorKeys.insert(key_value.key); separatorKeys.insert(key_value.key);
} }

2
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering...
GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
FastSet<Key> eliminateKeys = linearFactors->keys(); KeySet eliminateKeys = linearFactors->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
eliminateKeys.erase(key_value.key); eliminateKeys.erase(key_value.key);
} }

2
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys(); KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
} }

2
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
// GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys(); KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
std::vector<Key> variables(allkeys.begin(), allkeys.end()); std::vector<Key> variables(allkeys.begin(), allkeys.end());

4
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -70,7 +70,7 @@ namespace gtsam {
* @param body_P_sensor is the transform from body to sensor frame (default identity) * @param body_P_sensor is the transform from body to sensor frame (default identity)
*/ */
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K, KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) : boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) { throwCheirality_(false), verboseCheirality_(false) {
@ -91,7 +91,7 @@ namespace gtsam {
* @param body_P_sensor is the transform from body to sensor frame (default identity) * @param body_P_sensor is the transform from body to sensor frame (default identity)
*/ */
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K, KeySet poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality, bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) : boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),

2
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
View File

@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){
n_measPixel << 10, 10, 10, 10, 10, 10; n_measPixel << 10, 10, 10, 10, 10, 10;
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
FastSet<Key> views; KeySet views;
views.insert(x1); views.insert(x1);
views.insert(x2); views.insert(x2);
views.insert(x3); views.insert(x3);

8
matlab.h
View File

@ -71,16 +71,16 @@ FastVector<Key> createKeyVector(string s, const Vector& I) {
} }
// Create a KeySet from indices // Create a KeySet from indices
FastSet<Key> createKeySet(const Vector& I) { KeySet createKeySet(const Vector& I) {
FastSet<Key> set; KeySet set;
for (int i = 0; i < I.size(); i++) for (int i = 0; i < I.size(); i++)
set.insert(I[i]); set.insert(I[i]);
return set; return set;
} }
// Create a KeySet from indices using symbol // Create a KeySet from indices using symbol
FastSet<Key> createKeySet(string s, const Vector& I) { KeySet createKeySet(string s, const Vector& I) {
FastSet<Key> set; KeySet set;
char c = s[0]; char c = s[0];
for (int i = 0; i < I.size(); i++) for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i])); set.insert(symbol(c, I[i]));

4
tests/testGaussianISAM2.cpp
View File

@ -186,12 +186,12 @@ done:
// Permuted<VectorValues> permuted(permutation, values); // Permuted<VectorValues> permuted(permutation, values);
// //
// // After permutation, the indices above the threshold are 2 and 2 // // After permutation, the indices above the threshold are 2 and 2
// FastSet<Key> expected; // KeySet expected;
// expected.insert(2); // expected.insert(2);
// expected.insert(3); // expected.insert(3);
// //
// // Indices checked by CheckRelinearization // // Indices checked by CheckRelinearization
// FastSet<Key> actual = Impl::CheckRelinearization(permuted, 0.1); // KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
// //
// EXPECT(assert_equal(expected, actual)); // EXPECT(assert_equal(expected, actual));
//} //}

2
tests/testMarginals.cpp
View File

@ -226,7 +226,7 @@ TEST(Marginals, order) {
vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2)); vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
Marginals marginals(fg, vals); Marginals marginals(fg, vals);
FastSet<Key> set = fg.keys(); KeySet set = fg.keys();
FastVector<Key> keys(set.begin(), set.end()); FastVector<Key> keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys); JointMarginal joint = marginals.jointMarginalCovariance(keys);

4
tests/testNonlinearFactorGraph.cpp
View File

@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error )
TEST( NonlinearFactorGraph, keys ) TEST( NonlinearFactorGraph, keys )
{ {
NonlinearFactorGraph fg = createNonlinearFactorGraph(); NonlinearFactorGraph fg = createNonlinearFactorGraph();
FastSet<Key> actual = fg.keys(); KeySet actual = fg.keys();
LONGS_EQUAL(3, (long)actual.size()); LONGS_EQUAL(3, (long)actual.size());
FastSet<Key>::const_iterator it = actual.begin(); KeySet::const_iterator it = actual.begin();
LONGS_EQUAL((long)L(1), (long)*(it++)); LONGS_EQUAL((long)L(1), (long)*(it++));
LONGS_EQUAL((long)X(1), (long)*(it++)); LONGS_EQUAL((long)X(1), (long)*(it++));
LONGS_EQUAL((long)X(2), (long)*(it++)); LONGS_EQUAL((long)X(2), (long)*(it++));