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
*/
#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/set.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <CppUnitLite/TestHarness.h>
using namespace boost::assign;
using namespace gtsam;
@ -21,7 +22,7 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( testFastContainers, KeySet ) {
FastVector<size_t> init_vector;
FastVector<Key> init_vector;
init_vector += 2, 3, 4, 5;
FastSet<size_t> actSet(init_vector);

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

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

4
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -39,8 +39,8 @@ namespace gtsam {
}
/* ************************************************************************* */
FastSet<Key> DiscreteFactorGraph::keys() const {
FastSet<Key> keys;
KeySet DiscreteFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end());
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) */
FastSet<Key> keys() const;
KeySet keys() const;
/** return product of all factors as a single factor */
DecisionTreeFactor product() const;

1
gtsam/discrete/Potentials.h
View File

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

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

@ -344,7 +344,7 @@ namespace gtsam {
gttic(Full_root_factoring);
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_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()) {
C1_minus_B_set.erase(j); }
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; {
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()) {
C2_minus_B_set.erase(j); }
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
#include <string>
#include <gtsam/base/types.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h>
#include <gtsam/base/FastVector.h>
#include <string>
namespace gtsam {
// Forward declarations

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

@ -44,8 +44,8 @@ namespace gtsam {
FastVector<Key>
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{
FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B;
std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
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
{
gttic(shortcut_indices);
FastSet<Key> allKeys = p_Cp_B.keys();
FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
KeySet allKeys = p_Cp_B.keys();
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B = separator_setminus_B(B);
FastVector<Key> keep;
// 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 <sstream>
#include <iostream> // for cout :-(
namespace gtsam {
@ -72,8 +73,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
FastSet<Key> FactorGraph<FACTOR>::keys() const {
FastSet<Key> allKeys;
KeySet FactorGraph<FACTOR>::keys() const {
KeySet allKeys;
BOOST_FOREACH(const sharedFactor& factor, factors_)
if (factor)
allKeys.insert(factor->begin(), factor->end());

2
gtsam/inference/FactorGraph.h
View File

@ -332,7 +332,7 @@ namespace gtsam {
size_t nrFactors() const;
/** 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 */
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
BayesNetType bn;
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);
}
@ -44,7 +44,7 @@ namespace gtsam {
// eliminate into a Bayes net
const VariableIndex varIndex(factors);
const FastSet<Key> newFactorKeys = newFactors.keys();
const KeySet newFactorKeys = newFactors.keys();
const Ordering constrainedOrdering =
Ordering::colamdConstrainedLast(varIndex, std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end()));
Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex);

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

@ -26,8 +26,8 @@ namespace gtsam {
/* ************************************************************************* */
template<class FACTOR>
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, FastSet<int32_t> >::iterator iAdjMapIt;
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, std::set<int32_t> >::iterator iAdjMapIt;
std::set<Key> keySet;
/* ********** 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
template <class FACTOR>
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::stable_sort(keys.begin(), keys.end());
return Ordering(keys);

5
gtsam/inference/VariableIndex.h
View File

@ -17,14 +17,11 @@
#pragma once
#include <gtsam/base/FastList.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/dllexport.h>
#include <boost/none.hpp>
#include <boost/optional/optional.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 {
FastSet<Key> keys;
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor)
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
* union).
*/
typedef FastSet<Key> Keys;
typedef KeySet Keys;
Keys keys() const;
/* 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,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
FastSet<Key>& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables)
KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables)
{
variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end());
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)
{
FastSet<Key> relinKeys;
KeySet relinKeys;
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)
{
// 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 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 ISAM2Params::RelinearizationThreshold& relinearizeThreshold)
{
FastSet<Key> relinKeys;
KeySet relinKeys;
BOOST_FOREACH(const ISAM2::sharedClique& root, roots) {
if(relinearizeThreshold.type() == typeid(double))
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;
// 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,
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.
// 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,
const FastSet<Key>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;
@ -300,7 +300,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
/* ************************************************************************* */
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) {
// 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) {
// 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.
*/
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,
VectorValues& RgProd, FastSet<Key>& replacedKeys, Base::Nodes& nodes,
FastSet<Key>& fixedVariables);
VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes,
KeySet& fixedVariables);
/**
* 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
* equal to relinearizeThreshold
*/
static FastSet<Key> CheckRelinearizationFull(const VectorValues& delta,
static KeySet CheckRelinearizationFull(const VectorValues& delta,
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
* 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);
/**
@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
*
* 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
@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const VectorValues& delta,
const FastSet<Key>& mask,
const KeySet& mask,
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
* Update the Newton's method step point, using wildfire
*/
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
* 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);
/**

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

@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
namespace internal {
template<class CLIQUE>
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
// 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>
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
// 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>
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const FastSet<Key>& keys, VectorValues& delta) {
FastSet<Key> changed;
size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const KeySet& keys, VectorValues& delta) {
KeySet changed;
int count = 0;
// starting from the root, call optimize on each conditional
if(root)
@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold,
/* ************************************************************************* */
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;
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)
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);
FastSet<size_t> candidates = getAffectedFactors(affectedKeys);
@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Key>& affectedKeys, const FastS
gttic(affectedKeysSet);
// for fast lookup below
FastSet<Key> affectedKeysSet;
KeySet affectedKeysSet;
affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end());
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 FastSet<Key>& unusedIndices,
const KeySet& unusedIndices,
const boost::optional<FastMap<Key,int> >& constrainKeys,
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());
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)
{
@ -566,17 +566,17 @@ ISAM2Result ISAM2::update(
variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors);
// Compute unused keys and indices
FastSet<Key> unusedKeys;
FastSet<Key> unusedIndices;
KeySet unusedKeys;
KeySet unusedIndices;
{
// 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.
FastSet<Key> removedAndEmpty;
KeySet removedAndEmpty;
BOOST_FOREACH(Key key, removeFactors.keys()) {
if(variableIndex_[key].empty())
removedAndEmpty.insert(removedAndEmpty.end(), key);
}
FastSet<Key> newFactorSymbKeys = newFactors.keys();
KeySet newFactorSymbKeys = newFactors.keys();
std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
@ -602,10 +602,10 @@ ISAM2Result ISAM2::update(
gttic(gather_involved_keys);
// 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
{
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
}
// Also mark any provided extra re-eliminate keys
@ -632,7 +632,7 @@ ISAM2Result ISAM2::update(
gttoc(gather_involved_keys);
// 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) {
gttic(gather_relinearize_keys);
// 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; } }
// Add the variables being relinearized to the marked keys
FastSet<Key> markedRelinMask;
KeySet markedRelinMask;
BOOST_FOREACH(const Key key, relinKeys)
markedRelinMask.insert(key);
markedKeys.insert(relinKeys.begin(), relinKeys.end());
@ -674,7 +674,7 @@ ISAM2Result ISAM2::update(
// Relin involved keys for detailed results
if(params_.enableDetailedResults) {
FastSet<Key> involvedRelinKeys;
KeySet involvedRelinKeys;
BOOST_FOREACH(const sharedClique& root, roots_)
Impl::FindAll(root, involvedRelinKeys, markedRelinMask);
BOOST_FOREACH(Key key, involvedRelinKeys) {
@ -726,7 +726,7 @@ ISAM2Result ISAM2::update(
gttic(recalculate);
// 8. Redo top of Bayes tree
boost::shared_ptr<FastSet<Key> > replacedKeys;
boost::shared_ptr<KeySet > replacedKeys;
if(!markedKeys.empty() || !observedKeys.empty())
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)
{
// 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
// 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;
// Keep track of variables removed in subtrees
FastSet<Key> leafKeysRemoved;
KeySet leafKeysRemoved;
// Remove each variable and its subtrees
BOOST_FOREACH(Key j, leafKeys) {
@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
// 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;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate));
@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end());
// 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_);
}

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_
* 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 */
NonlinearFactorGraph nonlinearFactors_;
@ -476,7 +476,7 @@ protected:
/** Set of variables that are involved with linear factors from marginalized
* 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
@ -614,7 +614,7 @@ public:
const VariableIndex& getVariableIndex() const { return variableIndex_; }
/** Access the nonlinear variable index */
const FastSet<Key>& getFixedVariables() const { return fixedVariables_; }
const KeySet& getFixedVariables() const { return fixedVariables_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
@ -641,11 +641,11 @@ public:
protected:
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);
virtual boost::shared_ptr<FastSet<Key> > recalculate(const FastSet<Key>& markedKeys, const FastSet<Key>& relinKeys,
const std::vector<Key>& observedKeys, const FastSet<Key>& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
virtual boost::shared_ptr<KeySet > recalculate(const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const boost::optional<FastMap<Key,int> >& constrainKeys, ISAM2Result& result);
void updateDelta(bool forceFullSolve = false) const;
}; // ISAM2
@ -666,11 +666,11 @@ template<> struct traits<ISAM2> : public Testable<ISAM2> {};
/// @return The number of variables that were solved for
template<class CLIQUE>
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>
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)
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 << "," <<
formatting.figureHeightInches << "\";\n\n";
FastSet<Key> keys = this->keys();
KeySet keys = this->keys();
// Local utility function to extract x and y coordinates
struct { boost::optional<Point2> operator()(
@ -144,7 +144,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if (formatting.mergeSimilarFactors) {
// Remove duplicate factors
FastSet<vector<Key> > structure;
std::set<vector<Key> > structure;
BOOST_FOREACH(const sharedFactor& factor, *this){
if(factor) {
vector<Key> factorKeys = factor->keys();
@ -234,8 +234,8 @@ double NonlinearFactorGraph::error(const Values& c) const {
}
/* ************************************************************************* */
FastSet<Key> NonlinearFactorGraph::keys() const {
FastSet<Key> keys;
KeySet NonlinearFactorGraph::keys() const {
KeySet keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -106,7 +106,7 @@ namespace gtsam {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** 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 */
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)
{
// Gather all keys
FastSet<Key> allKeys;
KeySet allKeys;
BOOST_FOREACH(const boost::shared_ptr<FACTOR>& factor, factors) {
allKeys.insert(factor->begin(), factor->end());
}
@ -50,7 +50,7 @@ namespace gtsam
}
// Sort frontal keys
FastSet<Key> frontals(keys);
KeySet frontals(keys);
const size_t nFrontals = keys.size();
// 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
// also record the factor index for later modification
FastSet<Key> neighbors = star->keys();
KeySet neighbors = star->keys();
neighbors.erase(key);
CorrectedBeliefIndices correctedBeliefIndices;
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.
VariableIndex costVariableIndex_, equalityVariableIndex_,
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:
/// 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) {
std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) {
@ -531,13 +531,13 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
"BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: ");
// Get the set of all keys involved in the factor graph
FastSet<Key> allKeys(graph.keys());
KeySet allKeys(graph.keys());
if (debug)
PrintKeySet(allKeys,
"BatchFixedLagSmoother::calculateMarginalFactors All Keys: ");
// Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
FastSet<Key> remainingKeys;
KeySet remainingKeys;
std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
if (debug)

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -161,7 +161,7 @@ protected:
private:
/** Private methods for printing debug information */
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 GaussianFactor::shared_ptr& factor);
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); }
// Find the set of new separator keys
FastSet<Key> newSeparatorKeys;
KeySet newSeparatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
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
FastSet<Key> newSeparatorKeys = removedFactors.keys();
KeySet newSeparatorKeys = removedFactors.keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
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); }
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys = newSeparatorKeys;
KeySet shortcutKeys = newSeparatorKeys;
BOOST_FOREACH(Key key, smootherSummarization_.keys()) {
shortcutKeys.insert(key);
}

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
}
// Get the set of separator keys
gtsam::FastSet<Key> separatorKeys;
gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
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,
const FastSet<Key>& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) {
// Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys
FastSet<Key> rootKeys;
FastSet<Key> allKeys(graph.keys());
KeySet rootKeys;
KeySet allKeys(graph.keys());
std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end()));
// 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()));
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.
* Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */
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;
// 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
// 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) {
// Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys
FastSet<Key> shortcutKeys;
KeySet shortcutKeys;
BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) {
const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot);
if(factor) {
@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
// variables that result from marginalizing out all of the other variables
// 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
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
gtsam::FastSet<Key> separatorKeys;
gtsam::KeySet separatorKeys;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) {
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...
GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues);
FastSet<Key> eliminateKeys = linearFactors->keys();
KeySet eliminateKeys = linearFactors->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
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);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys();
KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) {
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);
// GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate();
FastSet<Key> allkeys = LinFactorGraph->keys();
KeySet allkeys = LinFactorGraph->keys();
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues)
allkeys.erase(key_value.key);
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)
*/
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) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {
@ -91,7 +91,7 @@ namespace gtsam {
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
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,
boost::optional<POSE> body_P_sensor = boost::none) :
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;
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
FastSet<Key> views;
KeySet views;
views.insert(x1);
views.insert(x2);
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
FastSet<Key> createKeySet(const Vector& I) {
FastSet<Key> set;
KeySet createKeySet(const Vector& I) {
KeySet set;
for (int i = 0; i < I.size(); i++)
set.insert(I[i]);
return set;
}
// Create a KeySet from indices using symbol
FastSet<Key> createKeySet(string s, const Vector& I) {
FastSet<Key> set;
KeySet createKeySet(string s, const Vector& I) {
KeySet set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));

4
tests/testGaussianISAM2.cpp
View File

@ -186,12 +186,12 @@ done:
// Permuted<VectorValues> permuted(permutation, values);
//
// // After permutation, the indices above the threshold are 2 and 2
// FastSet<Key> expected;
// KeySet expected;
// expected.insert(2);
// expected.insert(3);
//
// // Indices checked by CheckRelinearization
// FastSet<Key> actual = Impl::CheckRelinearization(permuted, 0.1);
// KeySet actual = Impl::CheckRelinearization(permuted, 0.1);
//
// 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));
Marginals marginals(fg, vals);
FastSet<Key> set = fg.keys();
KeySet set = fg.keys();
FastVector<Key> keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys);

4
tests/testNonlinearFactorGraph.cpp
View File

@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error )
TEST( NonlinearFactorGraph, keys )
{
NonlinearFactorGraph fg = createNonlinearFactorGraph();
FastSet<Key> actual = fg.keys();
KeySet actual = fg.keys();
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)X(1), (long)*(it++));
LONGS_EQUAL((long)X(2), (long)*(it++));