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

Use KeyVector everywhere to avoid conversions

release/4.3a0
Frank Dellaert 2018-11-08 00:58:50 -05:00
parent 43bae4dc2f
commit 2aa43e11bd
85 changed files with 283 additions and 281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -72,7 +72,7 @@ public:
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
/** A map from keys to values */ /** A map from keys to values */
typedef std::vector<Key> Indices; typedef KeyVector Indices;
typedef Assignment<Key> Values; typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues; typedef boost::shared_ptr<Values> sharedValues;

4
gtsam/discrete/DiscreteKey.cpp
View File

@ -31,8 +31,8 @@ namespace gtsam {
} }
} }
vector<Key> DiscreteKeys::indices() const { KeyVector DiscreteKeys::indices() const {
vector < Key > js; KeyVector js;
for(const DiscreteKey& key: *this) for(const DiscreteKey& key: *this)
js.push_back(key.first); js.push_back(key.first);
return js; return js;

3
gtsam/discrete/DiscreteKey.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/inference/Key.h>
#include <map> #include <map>
#include <string> #include <string>
@ -53,7 +54,7 @@ namespace gtsam {
GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs); GTSAM_EXPORT DiscreteKeys(const std::vector<int>& cs);
/// Return a vector of indices /// Return a vector of indices
GTSAM_EXPORT std::vector<Key> indices() const; GTSAM_EXPORT KeyVector indices() const;
/// Return a map from index to cardinality /// Return a map from index to cardinality
GTSAM_EXPORT std::map<Key,size_t> cardinalities() const; GTSAM_EXPORT std::map<Key,size_t> cardinalities() const;

4
gtsam/discrete/Signature.cpp
View File

@ -130,8 +130,8 @@ namespace gtsam {
return keys; return keys;
} }
vector<Key> Signature::indices() const { KeyVector Signature::indices() const {
vector<Key> js; KeyVector js;
js.push_back(key_.first); js.push_back(key_.first);
for(const DiscreteKey& key: parents_) for(const DiscreteKey& key: parents_)
js.push_back(key.first); js.push_back(key.first);

2
gtsam/discrete/Signature.h
View File

@ -90,7 +90,7 @@ namespace gtsam {
DiscreteKeys discreteKeysParentsFirst() const; DiscreteKeys discreteKeysParentsFirst() const;
/** All key indices, with variable key first */ /** All key indices, with variable key first */
std::vector<Key> indices() const; KeyVector indices() const;
// the CPT as parsed, if successful // the CPT as parsed, if successful
const boost::optional<Table>& table() const { const boost::optional<Table>& table() const {

3
gtsam/geometry/CameraSet.h
View File

@ -23,6 +23,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/SymmetricBlockMatrix.h> #include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -244,7 +245,7 @@ public:
template<int N> // N = 2 or 3 template<int N> // N = 2 or 3
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
const Eigen::Matrix<double, N, N>& P, const Vector& b, const Eigen::Matrix<double, N, N>& P, const Vector& b,
const FastVector<Key>& allKeys, const FastVector<Key>& keys, const KeyVector& allKeys, const KeyVector& keys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) { /*output ->*/SymmetricBlockMatrix& augmentedHessian) {
assert(keys.size()==Fs.size()); assert(keys.size()==Fs.size());

4
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -93,7 +93,7 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal(schur, actualReduced.selfadjointView())); EXPECT(assert_equal(schur, actualReduced.selfadjointView()));
// Check Schur complement update, same order, should just double // Check Schur complement update, same order, should just double
FastVector<Key> allKeys, keys; KeyVector allKeys, keys;
allKeys.push_back(1); allKeys.push_back(1);
allKeys.push_back(2); allKeys.push_back(2);
keys.push_back(1); keys.push_back(1);
@ -102,7 +102,7 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView())); EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
// Check Schur complement update, keys reversed // Check Schur complement update, keys reversed
FastVector<Key> keys2; KeyVector keys2;
keys2.push_back(2); keys2.push_back(2);
keys2.push_back(1); keys2.push_back(1);
Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced);

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

@ -344,7 +344,7 @@ namespace gtsam {
// Get the set of variables to eliminate, which is C1\B. // Get the set of variables to eliminate, which is C1\B.
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; { KeyVector C1_minus_B; {
KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
for(const Key j: *B->conditional()) { for(const Key j: *B->conditional()) {
C1_minus_B_set.erase(j); } C1_minus_B_set.erase(j); }
@ -356,7 +356,7 @@ namespace gtsam {
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
} }
boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; { boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
FastVector<Key> C2_minus_B; { KeyVector C2_minus_B; {
KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
for(const Key j: *B->conditional()) { for(const Key j: *B->conditional()) {
C2_minus_B_set.erase(j); } C2_minus_B_set.erase(j); }
@ -460,7 +460,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CLIQUE> template<class CLIQUE>
void BayesTree<CLIQUE>::removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans) void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans)
{ {
// process each key of the new factor // process each key of the new factor
for(const Key& j: keys) for(const Key& j: keys)

2
gtsam/inference/BayesTree.h
View File

@ -214,7 +214,7 @@ namespace gtsam {
* Given a list of indices, turn "contaminated" part of the tree back into a factor graph. * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
* Factors and orphans are added to the in/out arguments. * Factors and orphans are added to the in/out arguments.
*/ */
void removeTop(const FastVector<Key>& keys, BayesNetType& bn, Cliques& orphans); void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans);
/** /**
* Remove the requested subtree. */ * Remove the requested subtree. */

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

@ -40,12 +40,12 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
FastVector<Key> KeyVector
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{ {
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B; KeyVector 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));
return S_setminus_B; return S_setminus_B;
@ -53,14 +53,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH> template<class DERIVED, class FACTORGRAPH>
FastVector<Key> BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices( KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
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);
KeySet allKeys = p_Cp_B.keys(); KeySet allKeys = p_Cp_B.keys();
KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B = separator_setminus_B(B); KeyVector S_setminus_B = separator_setminus_B(B);
FastVector<Key> keep; KeyVector keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted) // keep = S\B intersect allKeys (S_setminus_B is already sorted)
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep)); allKeys.begin(), allKeys.end(), back_inserter(keep));
@ -113,7 +113,7 @@ namespace gtsam {
gttic(BayesTreeCliqueBase_shortcut); gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B // We only calculate the shortcut when this clique is not B
// and when the S\B is not empty // and when the S\B is not empty
FastVector<Key> S_setminus_B = separator_setminus_B(B); KeyVector S_setminus_B = separator_setminus_B(B);
if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty()) if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
{ {
// Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
@ -124,7 +124,7 @@ namespace gtsam {
p_Cp_B += parent->conditional_; // P(Fp|Sp) p_Cp_B += parent->conditional_; // P(Fp|Sp)
// Determine the variables we want to keepSet, S union B // Determine the variables we want to keepSet, S union B
FastVector<Key> keep = shortcut_indices(B, p_Cp_B); KeyVector keep = shortcut_indices(B, p_Cp_B);
// Marginalize out everything except S union B // Marginalize out everything except S union B
boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function); boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
@ -170,7 +170,7 @@ namespace gtsam {
p_Cp += parent->conditional_; // P(Fp|Sp) p_Cp += parent->conditional_; // P(Fp|Sp)
// The variables we want to keepSet are exactly the ones in S // The variables we want to keepSet are exactly the ones in S
FastVector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents());
cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function);
} }
} }

4
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -149,12 +149,12 @@ namespace gtsam {
protected: protected:
/// Calculate set \f$ S \setminus B \f$ for shortcut calculations /// Calculate set \f$ S \setminus B \f$ for shortcut calculations
FastVector<Key> separator_setminus_B(const derived_ptr& B) const; KeyVector separator_setminus_B(const derived_ptr& B) const;
/** Determine variable indices to keep in recursive separator shortcut calculation The factor /** Determine variable indices to keep in recursive separator shortcut calculation The factor
* graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables * graph p_Cp_B has keys from the parent clique Cp and from B. But we only keep the variables
* not in S union B. */ * not in S union B. */
FastVector<Key> shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const; KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
/** Non-recursive delete cached shortcuts and marginals - internal only. */ /** Non-recursive delete cached shortcuts and marginals - internal only. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; } void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }

18
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -129,7 +129,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> > std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential( EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialSequential); gttic(eliminatePartialSequential);
@ -169,7 +169,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> > std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal( EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
const std::vector<Key>& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(variableIndex) { if(variableIndex) {
gttic(eliminatePartialMultifrontal); gttic(eliminatePartialMultifrontal);
@ -190,7 +190,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType> boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering, OptionalOrdering marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
@ -223,9 +223,9 @@ namespace gtsam {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0); bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering = const KeyVector* variablesOrOrdering =
unmarginalizedAreOrdered ? unmarginalizedAreOrdered ?
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables); boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@ -249,7 +249,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree( EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering, OptionalOrdering marginalizedVariableOrdering,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
@ -282,9 +282,9 @@ namespace gtsam {
// No ordering was provided for the marginalized variables, so order them using constrained // No ordering was provided for the marginalized variables, so order them using constrained
// COLAMD. // COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0); bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering = const KeyVector* variablesOrOrdering =
unmarginalizedAreOrdered ? unmarginalizedAreOrdered ?
boost::get<const Ordering&>(&variables) : boost::get<const std::vector<Key>&>(&variables); boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
Ordering totalOrdering = Ordering totalOrdering =
Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@ -308,7 +308,7 @@ namespace gtsam {
template<class FACTORGRAPH> template<class FACTORGRAPH>
boost::shared_ptr<FACTORGRAPH> boost::shared_ptr<FACTORGRAPH>
EliminateableFactorGraph<FACTORGRAPH>::marginal( EliminateableFactorGraph<FACTORGRAPH>::marginal(
const std::vector<Key>& variables, const KeyVector& variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const const Eliminate& function, OptionalVariableIndex variableIndex) const
{ {
if(variableIndex) if(variableIndex)

14
gtsam/inference/EliminateableFactorGraph.h
View File

@ -170,7 +170,7 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */ * factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential( eliminatePartialSequential(
const std::vector<Key>& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
@ -190,14 +190,14 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */ * factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal( eliminatePartialMultifrontal(
const std::vector<Key>& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
/** Compute the marginal of the requested variables and return the result as a Bayes net. /** Compute the marginal of the requested variables and return the result as a Bayes net.
* @param variables Determines the variables whose marginal to compute, if provided as an * @param variables Determines the variables whose marginal to compute, if provided as an
* Ordering they will be ordered in the returned BayesNet as specified, and if provided * Ordering they will be ordered in the returned BayesNet as specified, and if provided
* as a vector<Key> they will be ordered using constrained COLAMD. * as a KeyVector they will be ordered using constrained COLAMD.
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering * out, i.e. all variables not in \c variables. If this is boost::none, the ordering
* will be computed with COLAMD. * will be computed with COLAMD.
@ -206,7 +206,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering = boost::none, OptionalOrdering marginalizedVariableOrdering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
@ -214,7 +214,7 @@ namespace gtsam {
/** Compute the marginal of the requested variables and return the result as a Bayes tree. /** Compute the marginal of the requested variables and return the result as a Bayes tree.
* @param variables Determines the variables whose marginal to compute, if provided as an * @param variables Determines the variables whose marginal to compute, if provided as an
* Ordering they will be ordered in the returned BayesNet as specified, and if provided * Ordering they will be ordered in the returned BayesNet as specified, and if provided
* as a vector<Key> they will be ordered using constrained COLAMD. * as a KeyVector they will be ordered using constrained COLAMD.
* @param marginalizedVariableOrdering Optional ordering for the variables being marginalized * @param marginalizedVariableOrdering Optional ordering for the variables being marginalized
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering * out, i.e. all variables not in \c variables. If this is boost::none, the ordering
* will be computed with COLAMD. * will be computed with COLAMD.
@ -223,14 +223,14 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not * @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */ * provided one will be computed. */
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree( boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const std::vector<Key>&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering = boost::none, OptionalOrdering marginalizedVariableOrdering = boost::none,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;
/** Compute the marginal factor graph of the requested variables. */ /** Compute the marginal factor graph of the requested variables. */
boost::shared_ptr<FactorGraphType> marginal( boost::shared_ptr<FactorGraphType> marginal(
const std::vector<Key>& variables, const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const; OptionalVariableIndex variableIndex = boost::none) const;

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

@ -48,7 +48,7 @@ namespace gtsam {
gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
// Do dense elimination step // Do dense elimination step
FastVector<Key> keyAsVector(1); keyAsVector[0] = key; KeyVector keyAsVector(1); keyAsVector[0] = key;
std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult = std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
function(gatheredFactors, Ordering(keyAsVector)); function(gatheredFactors, Ordering(keyAsVector));

10
gtsam/inference/Factor.h
View File

@ -58,15 +58,15 @@ namespace gtsam {
public: public:
/// Iterator over keys /// Iterator over keys
typedef FastVector<Key>::iterator iterator; typedef KeyVector::iterator iterator;
/// Const iterator over keys /// Const iterator over keys
typedef FastVector<Key>::const_iterator const_iterator; typedef KeyVector::const_iterator const_iterator;
protected: protected:
/// The keys involved in this factor /// The keys involved in this factor
FastVector<Key> keys_; KeyVector keys_;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -112,7 +112,7 @@ namespace gtsam {
const_iterator find(Key key) const { return std::find(begin(), end(), key); } const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// Access the factor's involved variable keys /// Access the factor's involved variable keys
const FastVector<Key>& keys() const { return keys_; } const KeyVector& keys() const { return keys_; }
/** Iterator at beginning of involved variable keys */ /** Iterator at beginning of involved variable keys */
const_iterator begin() const { return keys_.begin(); } const_iterator begin() const { return keys_.begin(); }
@ -148,7 +148,7 @@ namespace gtsam {
/// @{ /// @{
/** @return keys involved in this factor */ /** @return keys involved in this factor */
FastVector<Key>& keys() { return keys_; } KeyVector& keys() { return keys_; }
/** Iterator at beginning of involved variable keys */ /** Iterator at beginning of involved variable keys */
iterator begin() { return keys_.begin(); } iterator begin() { return keys_.begin(); }

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

@ -30,7 +30,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
BayesNetType bn; BayesNetType bn;
const KeySet newFactorKeys = newFactors.keys(); const KeySet newFactorKeys = newFactors.keys();
if (!this->empty()) { if (!this->empty()) {
std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end()); KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
this->removeTop(keyVector, bn, orphans); this->removeTop(keyVector, bn, orphans);
} }
@ -46,7 +46,7 @@ void ISAM<BAYESTREE>::update_internal(const FactorGraphType& newFactors,
// Get an ordering where the new keys are eliminated last // Get an ordering where the new keys are eliminated last
const VariableIndex index(factors); const VariableIndex index(factors);
const Ordering ordering = Ordering::ColamdConstrainedLast(index, const Ordering ordering = Ordering::ColamdConstrainedLast(index,
std::vector<Key>(newFactorKeys.begin(), newFactorKeys.end())); KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
// eliminate all factors (top, added, orphans) into a new Bayes tree // eliminate all factors (top, added, orphans) into a new Bayes tree
auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);

2
gtsam/inference/Key.h
View File

@ -52,7 +52,7 @@ GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
static const gtsam::KeyFormatter MultiRobotKeyFormatter = static const gtsam::KeyFormatter MultiRobotKeyFormatter =
&_multirobotKeyFormatter; &_multirobotKeyFormatter;
/// Useful typedef for operations with Values - allows for matlab interface /// Define collection type once and for all - also used in wrappers
typedef FastVector<Key> KeyVector; typedef FastVector<Key> KeyVector;
// TODO(frank): Nothing fast about these :-( // TODO(frank): Nothing fast about these :-(

8
gtsam/inference/Ordering.cpp
View File

@ -61,7 +61,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
if (nVars == 1) if (nVars == 1)
{ {
return Ordering(std::vector<Key>(1, variableIndex.begin()->first)); return Ordering(KeyVector(1, variableIndex.begin()->first));
} }
const size_t nEntries = variableIndex.nEntries(), nFactors = const size_t nEntries = variableIndex.nEntries(), nFactors =
@ -75,7 +75,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
// Fill in input data for COLAMD // Fill in input data for COLAMD
p[0] = 0; p[0] = 0;
int count = 0; int count = 0;
vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order KeyVector keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
size_t index = 0; size_t index = 0;
for (auto key_factors: variableIndex) { for (auto key_factors: variableIndex) {
// Arrange factor indices into COLAMD format // Arrange factor indices into COLAMD format
@ -127,7 +127,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex, Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder) { const KeyVector& constrainLast, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedLast); gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size(); size_t n = variableIndex.size();
@ -154,7 +154,7 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
/* ************************************************************************* */ /* ************************************************************************* */
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex, Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder) { const KeyVector& constrainFirst, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedFirst); gttic(Ordering_COLAMDConstrainedFirst);
const int none = -1; const int none = -1;

22
gtsam/inference/Ordering.h
View File

@ -30,9 +30,9 @@
namespace gtsam { namespace gtsam {
class Ordering: public std::vector<Key> { class Ordering: public KeyVector {
protected: protected:
typedef std::vector<Key> Base; typedef KeyVector Base;
public: public:
@ -93,12 +93,12 @@ public:
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
/// constrainLast will be ordered in the same order specified in the vector<Key> \c /// constrainLast will be ordered in the same order specified in the KeyVector \c
/// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR> template<class FACTOR>
static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph, static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainLast, bool forceOrder = false) { const KeyVector& constrainLast, bool forceOrder = false) {
if (graph.empty()) if (graph.empty())
return Ordering(); return Ordering();
else else
@ -108,11 +108,11 @@ public:
/// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This /// Compute a fill-reducing ordering using constrained COLAMD from a VariableIndex. This
/// function constrains the variables in \c constrainLast to the end of the ordering, and orders /// function constrains the variables in \c constrainLast to the end of the ordering, and orders
/// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the /// all other variables before in a fill-reducing ordering. If \c forceOrder is true, the
/// variables in \c constrainLast will be ordered in the same order specified in the vector<Key> /// variables in \c constrainLast will be ordered in the same order specified in the KeyVector
/// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be /// \c constrainLast. If \c forceOrder is false, the variables in \c constrainLast will be
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. /// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedLast( static GTSAM_EXPORT Ordering ColamdConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast, const VariableIndex& variableIndex, const KeyVector& constrainLast,
bool forceOrder = false); bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
@ -120,12 +120,12 @@ public:
/// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains /// VariableIndex, it is faster to use COLAMD(const VariableIndex&). This function constrains
/// the variables in \c constrainLast to the end of the ordering, and orders all other variables /// the variables in \c constrainLast to the end of the ordering, and orders all other variables
/// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c /// before in a fill-reducing ordering. If \c forceOrder is true, the variables in \c
/// constrainFirst will be ordered in the same order specified in the vector<Key> \c /// constrainFirst will be ordered in the same order specified in the KeyVector \c
/// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be /// constrainFirst. If \c forceOrder is false, the variables in \c constrainFirst will be
/// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well. /// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR> template<class FACTOR>
static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph, static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) { const KeyVector& constrainFirst, bool forceOrder = false) {
if (graph.empty()) if (graph.empty())
return Ordering(); return Ordering();
else else
@ -136,12 +136,12 @@ public:
/// function constrains the variables in \c constrainFirst to the front of the ordering, and /// function constrains the variables in \c constrainFirst to the front of the ordering, and
/// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the /// orders all other variables after in a fill-reducing ordering. If \c forceOrder is true, the
/// variables in \c constrainFirst will be ordered in the same order specified in the /// variables in \c constrainFirst will be ordered in the same order specified in the
/// vector<Key> \c constrainFirst. If \c forceOrder is false, the variables in \c /// KeyVector \c constrainFirst. If \c forceOrder is false, the variables in \c
/// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to /// constrainFirst will be ordered before all the others, but will be rearranged by CCOLAMD to
/// reduce fill-in as well. /// reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedFirst( static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
const VariableIndex& variableIndex, const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder = false); const KeyVector& constrainFirst, bool forceOrder = false);
/// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details /// Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details
/// for note on performance). This internally builds a VariableIndex so if you already have a /// for note on performance). This internally builds a VariableIndex so if you already have a
@ -175,7 +175,7 @@ public:
template<class FACTOR> template<class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) { static Ordering Natural(const FactorGraph<FACTOR> &fg) {
KeySet src = fg.keys(); KeySet src = fg.keys();
std::vector<Key> keys(src.begin(), src.end()); KeyVector 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);
} }

2
gtsam/linear/BinaryJacobianFactor.h
View File

@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor {
} }
// Fixed-size matrix update // Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys, void updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const {
gttic(updateHessian_BinaryJacobianFactor); gttic(updateHessian_BinaryJacobianFactor);
// Whiten the factor if it has a noise model // Whiten the factor if it has a noise model

8
gtsam/linear/GaussianConditional.cpp
View File

@ -117,7 +117,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues GaussianConditional::solve(const VectorValues& x) const { VectorValues GaussianConditional::solve(const VectorValues& x) const {
// Concatenate all vector values that correspond to parent variables // Concatenate all vector values that correspond to parent variables
const Vector xS = x.vector(FastVector<Key>(beginParents(), endParents())); const Vector xS = x.vector(KeyVector(beginParents(), endParents()));
// Update right-hand-side // Update right-hand-side
const Vector rhs = get_d() - get_S() * xS; const Vector rhs = get_d() - get_S() * xS;
@ -145,10 +145,10 @@ namespace gtsam {
VectorValues GaussianConditional::solveOtherRHS( VectorValues GaussianConditional::solveOtherRHS(
const VectorValues& parents, const VectorValues& rhs) const { const VectorValues& parents, const VectorValues& rhs) const {
// Concatenate all vector values that correspond to parent variables // Concatenate all vector values that correspond to parent variables
Vector xS = parents.vector(FastVector<Key>(beginParents(), endParents())); Vector xS = parents.vector(KeyVector(beginParents(), endParents()));
// Instead of updating getb(), update the right-hand-side from the given rhs // Instead of updating getb(), update the right-hand-side from the given rhs
const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals())); const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS; xS = rhsR - get_S() * xS;
// Solve Matrix // Solve Matrix
@ -171,7 +171,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = gy.vector(FastVector<Key>(beginFrontals(), endFrontals())); Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals()));
frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R()));
// Check for indeterminant solution // Check for indeterminant solution

2
gtsam/linear/GaussianFactor.h
View File

@ -126,7 +126,7 @@ namespace gtsam {
* @param scatter A mapping from variable index to slot index in this HessianFactor * @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated * @param info The information matrix to be updated
*/ */
virtual void updateHessian(const FastVector<Key>& keys, virtual void updateHessian(const KeyVector& keys,
SymmetricBlockMatrix* info) const = 0; SymmetricBlockMatrix* info) const = 0;
/// y += alpha * A'*A*x /// y += alpha * A'*A*x

4
gtsam/linear/HessianFactor.cpp
View File

@ -155,7 +155,7 @@ DenseIndex _getSizeHF(const Vector& m) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Key>& js, HessianFactor::HessianFactor(const KeyVector& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) : const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
// Get the number of variables // Get the number of variables
@ -356,7 +356,7 @@ double HessianFactor::error(const VectorValues& c) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void HessianFactor::updateHessian(const FastVector<Key>& infoKeys, void HessianFactor::updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const {
gttic(updateHessian_HessianFactor); gttic(updateHessian_HessianFactor);
assert(info); assert(info);

4
gtsam/linear/HessianFactor.h
View File

@ -159,7 +159,7 @@ namespace gtsam {
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term. * of the linear vector term, and f the constant term.
*/ */
HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs, HessianFactor(const KeyVector& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f); const std::vector<Vector>& gs, double f);
/** Constructor with an arbitrary number of keys and with the augmented information matrix /** Constructor with an arbitrary number of keys and with the augmented information matrix
@ -310,7 +310,7 @@ namespace gtsam {
* @param keys THe ordered vector of keys for the information matrix to be updated * @param keys THe ordered vector of keys for the information matrix to be updated
* @param info The information matrix to be updated * @param info The information matrix to be updated
*/ */
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const; void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
/** Update another Hessian factor /** Update another Hessian factor
* @param other the HessianFactor to be updated * @param other the HessianFactor to be updated

2
gtsam/linear/JacobianFactor.cpp
View File

@ -501,7 +501,7 @@ map<Key, Matrix> JacobianFactor::hessianBlockDiagonal() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void JacobianFactor::updateHessian(const FastVector<Key>& infoKeys, void JacobianFactor::updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const {
gttic(updateHessian_JacobianFactor); gttic(updateHessian_JacobianFactor);

2
gtsam/linear/JacobianFactor.h
View File

@ -283,7 +283,7 @@ namespace gtsam {
* @param scatter A mapping from variable index to slot index in this HessianFactor * @param scatter A mapping from variable index to slot index in this HessianFactor
* @param info The information matrix to be updated * @param info The information matrix to be updated
*/ */
void updateHessian(const FastVector<Key>& keys, SymmetricBlockMatrix* info) const; void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const;
/** Return A*x */ /** Return A*x */
Vector operator*(const VectorValues& x) const; Vector operator*(const VectorValues& x) const;

2
gtsam/linear/RegularHessianFactor.h
View File

@ -36,7 +36,7 @@ public:
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces * quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term. * of the linear vector term, and f the constant term.
*/ */
RegularHessianFactor(const std::vector<Key>& js, RegularHessianFactor(const KeyVector& js,
const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) : const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
HessianFactor(js, Gs, gs, f) { HessianFactor(js, Gs, gs, f) {
checkInvariants(); checkInvariants();

14
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -571,14 +571,14 @@ void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
/* in place back substitute */ /* in place back substitute */
for (auto cg: boost::adaptors::reverse(*Rc1_)) { for (auto cg: boost::adaptors::reverse(*Rc1_)) {
/* collect a subvector of x that consists of the parents of cg (S) */ /* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents())); const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals())); const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
/* compute the solution for the current pivot */ /* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent); const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
/* assign subvector of sol to the frontal variables */ /* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x); setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
} }
} }
@ -590,7 +590,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
/* in place back substitute */ /* in place back substitute */
for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) { for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals())); const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal); // const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal); const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
@ -598,7 +598,7 @@ void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */ /* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x); setSubvector(solFrontal, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()), x);
/* substract from parent variables */ /* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
@ -626,7 +626,7 @@ void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo
} }
/*****************************************************************************/ /*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) { Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) {
/* a cache of starting index and dim */ /* a cache of starting index and dim */
typedef vector<pair<size_t, size_t> > Cache; typedef vector<pair<size_t, size_t> > Cache;
@ -652,7 +652,7 @@ Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<
} }
/*****************************************************************************/ /*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) { void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst) {
/* use the cache */ /* use the cache */
size_t idx = 0; size_t idx = 0;
for ( const Key &key: keys ) { for ( const Key &key: keys ) {

4
gtsam/linear/SubgraphPreconditioner.h
View File

@ -298,10 +298,10 @@ namespace gtsam {
}; };
/* get subvectors */ /* get subvectors */
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys); Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys);
/* set subvectors */ /* set subvectors */
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst); void setSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys, Vector &dst);
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */ /* build a factor subgraph, which is defined as a set of weighted edges (factors) */

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

@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
VectorValues actual = gbn.optimizeGradientSearch(); VectorValues actual = gbn.optimizeGradientSearch();
// Check that points agree // Check that points agree
FastVector<Key> keys = list_of(0)(1)(2)(3)(4); KeyVector keys = list_of(0)(1)(2)(3)(4);
Vector actualAsVector = actual.vector(keys); Vector actualAsVector = actual.vector(keys);
EXPECT(assert_equal(expected, actualAsVector, 1e-5)); EXPECT(assert_equal(expected, actualAsVector, 1e-5));

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

@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
VectorValues actual = bt.optimizeGradientSearch(); VectorValues actual = bt.optimizeGradientSearch();
// Check that points agree // Check that points agree
FastVector<Key> keys = list_of(0)(1)(2)(3)(4); KeyVector keys = list_of(0)(1)(2)(3)(4);
EXPECT(assert_equal(expected, actual.vector(keys), 1e-5)); EXPECT(assert_equal(expected, actual.vector(keys), 1e-5));
EXPECT(assert_equal(expectedFromBN, actual, 1e-5)); EXPECT(assert_equal(expectedFromBN, actual, 1e-5));

6
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -41,7 +41,7 @@ const double tol = 1e-5;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(HessianFactor, Slot) TEST(HessianFactor, Slot)
{ {
FastVector<Key> keys = list_of(2)(4)(1); KeyVector keys = list_of(2)(4)(1);
EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2)); EXPECT_LONGS_EQUAL(0, GaussianFactor::Slot(keys,2));
EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4)); EXPECT_LONGS_EQUAL(1, GaussianFactor::Slot(keys,4));
EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1)); EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
@ -252,7 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
(1, dx1) (1, dx1)
(2, dx2); (2, dx2);
std::vector<Key> js; KeyVector js;
js.push_back(0); js.push_back(1); js.push_back(2); js.push_back(0); js.push_back(1); js.push_back(2);
std::vector<Matrix> Gs; std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
@ -517,7 +517,7 @@ TEST(HessianFactor, gradientAtZero)
// test gradient at zero // test gradient at zero
VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2); VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); Matrix A; Vector b; boost::tie(A,b) = factor.jacobian();
FastVector<Key> keys; keys += 0,1; KeyVector keys; keys += 0,1;
EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys))); EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys)));
VectorValues actualG = factor.gradientAtZero(); VectorValues actualG = factor.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG)); EXPECT(assert_equal(expectedG, actualG));

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

@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
VectorValues expectedG; VectorValues expectedG;
expectedG.insert(1, Vector2(20,-10)); expectedG.insert(1, Vector2(20,-10));
expectedG.insert(2, Vector2(-20, 10)); expectedG.insert(2, Vector2(-20, 10));
FastVector<Key> keys; keys += 1,2; KeyVector keys; keys += 1,2;
EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys)));
VectorValues actualG = lf.gradientAtZero(); VectorValues actualG = lf.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG)); EXPECT(assert_equal(expectedG, actualG));

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

@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors)
EXPECT(assert_equal(factor,factor2)); EXPECT(assert_equal(factor,factor2));
// Test n-way constructor // Test n-way constructor
vector<Key> keys; keys += 0, 1, 3; KeyVector keys; keys += 0, 1, 3;
vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33; vector<Matrix> Gs; Gs += G11, G12, G13, G22, G23, G33;
vector<Vector> gs; gs += g1, g2, g3; vector<Vector> gs; gs += g1, g2, g3;
RegularHessianFactor<2> factor3(keys, Gs, gs, f); RegularHessianFactor<2> factor3(keys, Gs, gs, f);

8
gtsam/linear/tests/testVectorValues.cpp
View File

@ -62,7 +62,7 @@ TEST(VectorValues, basics)
EXPECT(assert_equal(Vector2(2, 3), actual[1])); EXPECT(assert_equal(Vector2(2, 3), actual[1]));
EXPECT(assert_equal(Vector2(4, 5), actual[2])); EXPECT(assert_equal(Vector2(4, 5), actual[2]));
EXPECT(assert_equal(Vector2(6, 7), actual[5])); EXPECT(assert_equal(Vector2(6, 7), actual[5]));
FastVector<Key> keys = list_of(0)(1)(2)(5); KeyVector keys = list_of(0)(1)(2)(5);
EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys))); EXPECT(assert_equal((Vector(7) << 1, 2, 3, 4, 5, 6, 7).finished(), actual.vector(keys)));
// Check exceptions // Check exceptions
@ -101,7 +101,7 @@ TEST(VectorValues, subvector)
init.insert(12, Vector2(4, 5)); init.insert(12, Vector2(4, 5));
init.insert(13, Vector2(6, 7)); init.insert(13, Vector2(6, 7));
std::vector<Key> keys; KeyVector keys;
keys += 10, 12, 13; keys += 10, 12, 13;
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished(); Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
EXPECT(assert_equal(expSubVector, init.vector(keys))); EXPECT(assert_equal(expSubVector, init.vector(keys)));
@ -197,7 +197,7 @@ TEST(VectorValues, convert)
EXPECT(assert_equal(expected, actual2)); EXPECT(assert_equal(expected, actual2));
// Test other direction, note vector() is not guaranteed to give right result // Test other direction, note vector() is not guaranteed to give right result
FastVector<Key> keys = list_of(0)(1)(2)(5); KeyVector keys = list_of(0)(1)(2)(5);
EXPECT(assert_equal(x, actual.vector(keys))); EXPECT(assert_equal(x, actual.vector(keys)));
// Test version with dims argument // Test version with dims argument
@ -222,7 +222,7 @@ TEST(VectorValues, vector_sub)
expected << 1, 6, 7; expected << 1, 6, 7;
// Test FastVector version // Test FastVector version
FastVector<Key> keys = list_of(0)(5); KeyVector keys = list_of(0)(5);
EXPECT(assert_equal(expected, vv.vector(keys))); EXPECT(assert_equal(expected, vv.vector(keys)));
// Test version with dims argument // Test version with dims argument

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

@ -239,10 +239,10 @@ void updateRgProd(const ISAM2::sharedClique& clique, const KeySet& replacedKeys,
// Update the current variable // Update the current variable
// Get VectorValues slice corresponding to current variables // Get VectorValues slice corresponding to current variables
Vector gR = Vector gR =
grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(), grad.vector(KeyVector(clique->conditional()->beginFrontals(),
clique->conditional()->endFrontals())); clique->conditional()->endFrontals()));
Vector gS = Vector gS =
grad.vector(FastVector<Key>(clique->conditional()->beginParents(), grad.vector(KeyVector(clique->conditional()->beginParents(),
clique->conditional()->endParents())); clique->conditional()->endParents()));
// Compute R*g and S*g for this clique // Compute R*g and S*g for this clique

10
gtsam/nonlinear/ISAM2.cpp
View File

@ -195,7 +195,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) {
/* ************************************************************************* */ /* ************************************************************************* */
boost::shared_ptr<KeySet> ISAM2::recalculate( boost::shared_ptr<KeySet> ISAM2::recalculate(
const KeySet& markedKeys, const KeySet& relinKeys, const KeySet& markedKeys, const KeySet& relinKeys,
const vector<Key>& observedKeys, const KeySet& unusedIndices, const KeyVector& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys, const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result* result) { ISAM2Result* result) {
// TODO(dellaert): new factors are linearized twice, // TODO(dellaert): new factors are linearized twice,
@ -243,7 +243,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
gttic(removetop); gttic(removetop);
Cliques orphans; Cliques orphans;
GaussianBayesNet affectedBayesNet; GaussianBayesNet affectedBayesNet;
this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()), this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
affectedBayesNet, orphans); affectedBayesNet, orphans);
gttoc(removetop); gttoc(removetop);
@ -667,7 +667,7 @@ ISAM2Result ISAM2::update(
// NOTE: we use assign instead of the iterator constructor here because this // NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to // is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Key value) instead of the iterator constructor. // vector(size_t count, Key value) instead of the iterator constructor.
FastVector<Key> observedKeys; KeyVector observedKeys;
observedKeys.reserve(markedKeys.size()); observedKeys.reserve(markedKeys.size());
for (Key index : markedKeys) { for (Key index : markedKeys) {
if (unusedIndices.find(index) == if (unusedIndices.find(index) ==
@ -945,7 +945,7 @@ void ISAM2::marginalizeLeaves(
// conditional // conditional
auto cg = clique->conditional(); auto cg = clique->conditional();
const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals()); const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate; KeyVector cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
leafKeys.begin(), leafKeys.end(), leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate)); std::back_inserter(cliqueFrontalsToEliminate));
@ -967,7 +967,7 @@ void ISAM2::marginalizeLeaves(
cg->matrixObject().rowStart() = dimToRemove; cg->matrixObject().rowStart() = dimToRemove;
// Change the keys in the clique // Change the keys in the clique
FastVector<Key> originalKeys; KeyVector originalKeys;
originalKeys.swap(cg->keys()); originalKeys.swap(cg->keys());
cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
cg->nrFrontals() -= nToRemove; cg->nrFrontals() -= nToRemove;

2
gtsam/nonlinear/ISAM2.h
View File

@ -299,7 +299,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
virtual boost::shared_ptr<KeySet> recalculate( virtual boost::shared_ptr<KeySet> recalculate(
const KeySet& markedKeys, const KeySet& relinKeys, const KeySet& markedKeys, const KeySet& relinKeys,
const std::vector<Key>& observedKeys, const KeySet& unusedIndices, const KeyVector& observedKeys, const KeySet& unusedIndices,
const boost::optional<FastMap<Key, int> >& constrainKeys, const boost::optional<FastMap<Key, int> >& constrainKeys,
ISAM2Result* result); ISAM2Result* result);

6
gtsam/nonlinear/Marginals.cpp
View File

@ -80,14 +80,14 @@ Matrix Marginals::marginalCovariance(Key variable) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
JointMarginal Marginals::jointMarginalCovariance(const std::vector<Key>& variables) const { JointMarginal Marginals::jointMarginalCovariance(const KeyVector& variables) const {
JointMarginal info = jointMarginalInformation(variables); JointMarginal info = jointMarginalInformation(variables);
info.blockMatrix_.invertInPlace(); info.blockMatrix_.invertInPlace();
return info; return info;
} }
/* ************************************************************************* */ /* ************************************************************************* */
JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variables) const { JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) const {
// If 2 variables, we can use the BayesTree::joint function, otherwise we // If 2 variables, we can use the BayesTree::joint function, otherwise we
// have to use sequential elimination. // have to use sequential elimination.
@ -119,7 +119,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
// Information matrix will be returned with sorted keys // Information matrix will be returned with sorted keys
std::vector<Key> variablesSorted = variables; KeyVector variablesSorted = variables;
std::sort(variablesSorted.begin(), variablesSorted.end()); std::sort(variablesSorted.begin(), variablesSorted.end());
// Get dimensions from factor graph // Get dimensions from factor graph

6
gtsam/nonlinear/Marginals.h
View File

@ -74,10 +74,10 @@ public:
Matrix marginalCovariance(Key variable) const; Matrix marginalCovariance(Key variable) const;
/** Compute the joint marginal covariance of several variables */ /** Compute the joint marginal covariance of several variables */
JointMarginal jointMarginalCovariance(const std::vector<Key>& variables) const; JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
/** Compute the joint marginal information of several variables */ /** Compute the joint marginal information of several variables */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const; JointMarginal jointMarginalInformation(const KeyVector& variables) const;
/** Optimize the bayes tree */ /** Optimize the bayes tree */
VectorValues optimize() const; VectorValues optimize() const;
@ -130,7 +130,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected: protected:
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const std::vector<Key>& keys) : JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {} blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
friend class Marginals; friend class Marginals;

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey( NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const { const KeyVector& new_keys) const {
assert(new_keys.size() == keys().size()); assert(new_keys.size() == keys().size());
shared_ptr new_factor = clone(); shared_ptr new_factor = clone();
new_factor->keys() = new_keys; new_factor->keys() = new_keys;

2
gtsam/nonlinear/NonlinearFactor.h
View File

@ -141,7 +141,7 @@ public:
* Clones a factor and fully replaces its keys * Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys * @param new_keys is the full replacement set of keys
*/ */
shared_ptr rekey(const std::vector<Key>& new_keys) const; shared_ptr rekey(const KeyVector& new_keys) const;
}; // \class NonlinearFactor }; // \class NonlinearFactor

8
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -165,10 +165,10 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
if (formatting.mergeSimilarFactors) { if (formatting.mergeSimilarFactors) {
// Remove duplicate factors // Remove duplicate factors
std::set<vector<Key> > structure; std::set<KeyVector > structure;
for (const sharedFactor& factor : factors_) { for (const sharedFactor& factor : factors_) {
if (factor) { if (factor) {
vector<Key> factorKeys = factor->keys(); KeyVector factorKeys = factor->keys();
std::sort(factorKeys.begin(), factorKeys.end()); std::sort(factorKeys.begin(), factorKeys.end());
structure.insert(factorKeys); structure.insert(factorKeys);
} }
@ -176,7 +176,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Create factors and variable connections // Create factors and variable connections
size_t i = 0; size_t i = 0;
for(const vector<Key>& factorKeys: structure){ for(const KeyVector& factorKeys: structure){
// Make each factor a dot // Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point"; stm << " factor" << i << "[label=\"\", shape=point";
{ {
@ -199,7 +199,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
for(size_t i = 0; i < size(); ++i) { for(size_t i = 0; i < size(); ++i) {
const NonlinearFactor::shared_ptr& factor = at(i); const NonlinearFactor::shared_ptr& factor = at(i);
if(formatting.plotFactorPoints) { if(formatting.plotFactorPoints) {
const FastVector<Key>& keys = factor->keys(); const KeyVector& keys = factor->keys();
if (formatting.binaryEdges && keys.size()==2) { if (formatting.binaryEdges && keys.size()==2) {
stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n";
} else { } else {

4
gtsam/nonlinear/tests/testLinearContainerFactor.cpp
View File

@ -47,7 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
EXPECT(!actFactor.isHessian()); EXPECT(!actFactor.isHessian());
// check keys // check keys
FastVector<Key> expKeys; expKeys += l1, l2; KeyVector expKeys; expKeys += l1, l2;
EXPECT(assert_container_equality(expKeys, actFactor.keys())); EXPECT(assert_container_equality(expKeys, actFactor.keys()));
Values values; Values values;
@ -246,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) {
LinearContainerFactor actual(linear_factor, full_values); LinearContainerFactor actual(linear_factor, full_values);
// Verify the keys // Verify the keys
FastVector<Key> expKeys; KeyVector expKeys;
expKeys += l3, l5; expKeys += l3, l5;
EXPECT(assert_container_equality(expKeys, actual.keys())); EXPECT(assert_container_equality(expKeys, actual.keys()));

12
gtsam/nonlinear/utilities.h
View File

@ -54,16 +54,16 @@ FastList<Key> createKeyList(std::string s, const Vector& I) {
} }
// Create a KeyVector from indices // Create a KeyVector from indices
FastVector<Key> createKeyVector(const Vector& I) { KeyVector createKeyVector(const Vector& I) {
FastVector<Key> set; KeyVector set;
for (int i = 0; i < I.size(); i++) for (int i = 0; i < I.size(); i++)
set.push_back(I[i]); set.push_back(I[i]);
return set; return set;
} }
// Create a KeyVector from indices using symbol // Create a KeyVector from indices using symbol
FastVector<Key> createKeyVector(std::string s, const Vector& I) { KeyVector createKeyVector(std::string s, const Vector& I) {
FastVector<Key> set; KeyVector 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.push_back(Symbol(c, I[i])); set.push_back(Symbol(c, I[i]));
@ -222,12 +222,12 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
/// Convert from local to world coordinates /// Convert from local to world coordinates
Values localToWorld(const Values& local, const Pose2& base, Values localToWorld(const Values& local, const Pose2& base,
const FastVector<Key> user_keys = FastVector<Key>()) { const KeyVector user_keys = KeyVector()) {
Values world; Values world;
// if no keys given, get all keys from local values // if no keys given, get all keys from local values
FastVector<Key> keys(user_keys); KeyVector keys(user_keys);
if (keys.size()==0) if (keys.size()==0)
keys = local.keys(); keys = local.keys();

2
gtsam/slam/InitializePose3.cpp
View File

@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
else else
std::cout << "Error in buildLinearOrientationGraph" << std::endl; std::cout << "Error in buildLinearOrientationGraph" << std::endl;
const FastVector<Key>& keys = factor->keys(); const KeyVector& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
Matrix M9 = Z_9x9; Matrix M9 = Z_9x9;
M9.block(0,0,3,3) = Rij; M9.block(0,0,3,3) = Rij;

4
gtsam/slam/JacobianFactorQ.h
View File

@ -37,7 +37,7 @@ public:
} }
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys, // JacobianFactorQ(const KeyVector& keys, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
@ -50,7 +50,7 @@ public:
} }
/// Constructor /// Constructor
JacobianFactorQ(const FastVector<Key>& keys, JacobianFactorQ(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {

4
gtsam/slam/JacobianFactorQR.h
View File

@ -28,7 +28,7 @@ public:
/** /**
* Constructor * Constructor
*/ */
JacobianFactorQR(const FastVector<Key>& keys, JacobianFactorQR(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, // const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
@ -46,7 +46,7 @@ public:
// eliminate the point // eliminate the point
boost::shared_ptr<GaussianBayesNet> bn; boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg; GaussianFactorGraph::shared_ptr fg;
std::vector<Key> variables; KeyVector variables;
variables.push_back(pointKey); variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg"); //fg->print("fg");

4
gtsam/slam/JacobianFactorSVD.h
View File

@ -38,7 +38,7 @@ public:
} }
/// Empty constructor with keys /// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys, // JacobianFactorSVD(const KeyVector& keys, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :
Base() { Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
@ -58,7 +58,7 @@ public:
* *
* @Fblocks: * @Fblocks:
*/ */
JacobianFactorSVD(const FastVector<Key>& keys, JacobianFactorSVD(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
const Vector& b, // const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal()) :

4
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -48,7 +48,7 @@ public:
} }
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b /// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const FastVector<Key>& keys, RegularImplicitSchurFactor(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P, const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
const Vector& b) : const Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
@ -108,7 +108,7 @@ public:
return D; return D;
} }
virtual void updateHessian(const FastVector<Key>& keys, virtual void updateHessian(const KeyVector& keys,
SymmetricBlockMatrix* info) const { SymmetricBlockMatrix* info) const {
throw std::runtime_error( throw std::runtime_error(
"RegularImplicitSchurFactor::updateHessian non implemented"); "RegularImplicitSchurFactor::updateHessian non implemented");

4
gtsam/slam/SmartFactorBase.h
View File

@ -131,7 +131,7 @@ public:
/** /**
* Add a bunch of measurements, together with the camera keys * Add a bunch of measurements, together with the camera keys
*/ */
void add(ZVector& measurements, std::vector<Key>& cameraKeys) { void add(ZVector& measurements, KeyVector& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i)); this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i)); this->keys_.push_back(cameraKeys.at(i));
@ -310,7 +310,7 @@ public:
void updateAugmentedHessian(const Cameras& cameras, const Point3& point, void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping, const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian, SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const { const KeyVector allKeys) const {
Matrix E; Matrix E;
Vector b; Vector b;
computeJacobians(Fs, E, b, cameras, point); computeJacobians(Fs, E, b, cameras, point);

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -179,7 +179,7 @@ public:
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector<Key> js; KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);

4
gtsam/slam/lago.cpp
View File

@ -168,14 +168,14 @@ GaussianFactorGraph buildLinearOrientationGraph(
// put original measurements in the spanning tree // put original measurements in the spanning tree
for(const size_t& factorId: spanningTreeIds) { for(const size_t& factorId: spanningTreeIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const KeyVector& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
} }
// put regularized measurements in the chordsIds // put regularized measurements in the chordsIds
for(const size_t& factorId: chordsIds) { for(const size_t& factorId: chordsIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const KeyVector& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0); double key1_DeltaTheta_key2 = deltaTheta(0);

4
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3); const vector<Matrix26, Eigen::aligned_allocator<Matrix26> > FBlocks = list_of<Matrix26>(F0)(F1)(F3);
const FastVector<Key> keys = list_of<Key>(0)(1)(3); const KeyVector keys = list_of<Key>(0)(1)(3);
// RHS and sigmas // RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished(); const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.).finished();
@ -86,7 +86,7 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3; F << F0, Matrix::Zero(2, d * 3), Matrix::Zero(2, d), F1, Matrix::Zero(2, d*2), Matrix::Zero(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x // Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys2; KeyVector keys2;
keys2 += 0,1,2,3; keys2 += 0,1,2,3;
Vector x = xvalues.vector(keys2); Vector x = xvalues.vector(keys2);
Vector expected = Vector::Zero(24); Vector expected = Vector::Zero(24);

12
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -170,7 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) {
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
@ -195,7 +195,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2)); SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2)); SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2)); SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
views.push_back(c3); views.push_back(c3);
@ -293,7 +293,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
views.push_back(c3); views.push_back(c3);
@ -370,7 +370,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
views.push_back(c3); views.push_back(c3);
@ -450,7 +450,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
views.push_back(c3); views.push_back(c3);
@ -526,7 +526,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5); projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views; KeyVector views;
views.push_back(c1); views.push_back(c1);
views.push_back(c2); views.push_back(c2);
views.push_back(c3); views.push_back(c3);

40
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -189,7 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
measurements.push_back(level_uv); measurements.push_back(level_uv);
measurements.push_back(level_uv_right); measurements.push_back(level_uv_right);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
@ -236,7 +236,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors // Create smart factors
std::vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -299,7 +299,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -370,7 +370,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
measurements_cam1.push_back(cam2.project(landmark1)); measurements_cam1.push_back(cam2.project(landmark1));
// Create smart factors // Create smart factors
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
b.setZero(); b.setZero();
// Create smart factors // Create smart factors
FastVector<Key> keys; KeyVector keys;
keys.push_back(x1); keys.push_back(x1);
keys.push_back(x2); keys.push_back(x2);
@ -520,7 +520,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -577,7 +577,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -638,7 +638,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2; double excludeLandmarksFutherThanDist = 2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -701,7 +701,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
double excludeLandmarksFutherThanDist = 1e10; double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -767,7 +767,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -821,7 +821,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -869,7 +869,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionPoseFactor, CheckHessian) { TEST( SmartProjectionPoseFactor, CheckHessian) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -955,7 +955,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
using namespace vanillaPose2; using namespace vanillaPose2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1014,7 +1014,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
// this test considers a condition in which the cheirality constraint is triggered // this test considers a condition in which the cheirality constraint is triggered
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
using namespace vanillaPose2; using namespace vanillaPose2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
using namespace vanillaPose; using namespace vanillaPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1186,7 +1186,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
using namespace vanillaPose2; using namespace vanillaPose2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1260,7 +1260,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1309,7 +1309,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
using namespace bundlerPose; using namespace bundlerPose;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1421,7 +1421,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
SmartFactor factor(model, sharedK, bts, params); SmartFactor factor(model, sharedK, bts, params);
// insert some measurments // insert some measurments
vector<Key> key_view; KeyVector key_view;
Point2Vector meas_view; Point2Vector meas_view;
key_view.push_back(Symbol('x', 1)); key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10)); meas_view.push_back(Point2(10, 10));

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

@ -57,7 +57,7 @@ namespace gtsam
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
FastVector<Key> orderedKeys(allKeys.size()); KeyVector orderedKeys(allKeys.size());
std::copy(keys.begin(), keys.end(), orderedKeys.begin()); std::copy(keys.begin(), keys.end(), orderedKeys.begin());
std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals); std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);

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

@ -81,7 +81,7 @@ TEST( SymbolicBayesNet, combine )
TEST(SymbolicBayesNet, saveGraph) { TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNet bn; SymbolicBayesNet bn;
bn += SymbolicConditional(_A_, _B_); bn += SymbolicConditional(_A_, _B_);
std::vector<Key> keys; KeyVector keys;
keys.push_back(_B_); keys.push_back(_B_);
keys.push_back(_C_); keys.push_back(_C_);
keys.push_back(_D_); keys.push_back(_D_);

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

@ -33,7 +33,7 @@ using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef TRACK_ELIMINATE #ifdef TRACK_ELIMINATE
TEST(SymbolicFactor, eliminate) { TEST(SymbolicFactor, eliminate) {
vector<Key> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11; KeyVector keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
IndexFactor actual(keys.begin(), keys.end()); IndexFactor actual(keys.begin(), keys.end());
BayesNet<IndexConditional> fragment = *actual.eliminate(3); BayesNet<IndexConditional> fragment = *actual.eliminate(3);

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

@ -85,7 +85,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
SymbolicBayesNet::shared_ptr actualBayesNet2; SymbolicBayesNet::shared_ptr actualBayesNet2;
SymbolicFactorGraph::shared_ptr actualSfg2; SymbolicFactorGraph::shared_ptr actualSfg2;
boost::tie(actualBayesNet2, actualSfg2) = boost::tie(actualBayesNet2, actualSfg2) =
simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<vector<Key> >()); simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
EXPECT(assert_equal(expectedSfg, *actualSfg2)); EXPECT(assert_equal(expectedSfg, *actualSfg2));
EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
@ -137,7 +137,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
SymbolicBayesTree::shared_ptr actualBayesTree2; SymbolicBayesTree::shared_ptr actualBayesTree2;
SymbolicFactorGraph::shared_ptr actualFactorGraph2; SymbolicFactorGraph::shared_ptr actualFactorGraph2;
boost::tie(actualBayesTree2, actualFactorGraph2) = boost::tie(actualBayesTree2, actualFactorGraph2) =
simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<vector<Key> >()); simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));

2
gtsam_unstable/discrete/CSP.h
View File

@ -22,7 +22,7 @@ namespace gtsam {
public: public:
/** A map from keys to values */ /** A map from keys to values */
typedef std::vector<Key> Indices; typedef KeyVector Indices;
typedef Assignment<Key> Values; typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues; typedef boost::shared_ptr<Values> sharedValues;

2
gtsam_unstable/discrete/Constraint.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
protected: protected:
/// Construct n-way factor /// Construct n-way factor
Constraint(const std::vector<Key>& js) : Constraint(const KeyVector& js) :
DiscreteFactor(js) { DiscreteFactor(js) {
} }

2
gtsam_unstable/discrete/Domain.cpp
View File

@ -57,7 +57,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Domain::checkAllDiff(const vector<Key> keys, vector<Domain>& domains) { bool Domain::checkAllDiff(const KeyVector keys, vector<Domain>& domains) {
Key j = keys_[0]; Key j = keys_[0];
// for all values in this domain // for all values in this domain
for(size_t value: values_) { for(size_t value: values_) {

2
gtsam_unstable/discrete/Domain.h
View File

@ -104,7 +104,7 @@ namespace gtsam {
* If found, we make this a singleton... Called in AllDiff::ensureArcConsistency * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency
* @param keys connected domains through alldiff * @param keys connected domains through alldiff
*/ */
bool checkAllDiff(const std::vector<Key> keys, std::vector<Domain>& domains); bool checkAllDiff(const KeyVector keys, std::vector<Domain>& domains);
/// Partially apply known values /// Partially apply known values
virtual Constraint::shared_ptr partiallyApply( virtual Constraint::shared_ptr partiallyApply(

6
gtsam_unstable/linear/RawQP.cpp
View File

@ -203,9 +203,9 @@ void RawQP::addQuadTerm(
} }
QP RawQP::makeQP() { QP RawQP::makeQP() {
std::vector < Key > keys; KeyVector keys;
std::vector < Matrix > Gs; std::vector<Matrix> Gs;
std::vector < Vector > gs; std::vector<Vector> gs;
for (auto kv : varname_to_key) { for (auto kv : varname_to_key) {
keys.push_back(kv.second); keys.push_back(kv.second);
} }

2
gtsam_unstable/linear/RawQP.h
View File

@ -49,7 +49,7 @@ private:
std::string name_; std::string name_;
std::unordered_map<Key, double> up; std::unordered_map<Key, double> up;
std::unordered_map<Key, double> lo; std::unordered_map<Key, double> lo;
std::vector<Key> Free; KeyVector Free;
const bool debug = false; const bool debug = false;
public: public:

2
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -362,7 +362,7 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
if(keysToMove && keysToMove->size() > 0) { if(keysToMove && keysToMove->size() > 0) {
ordering_ = Ordering::ColamdConstrainedFirst(factors_, std::vector<Key>(keysToMove->begin(), keysToMove->end())); ordering_ = Ordering::ColamdConstrainedFirst(factors_, KeyVector(keysToMove->begin(), keysToMove->end()));
}else{ }else{
ordering_ = Ordering::Colamd(factors_); ordering_ = Ordering::Colamd(factors_);
} }

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
variableIndex_ = VariableIndex(factors_); variableIndex_ = VariableIndex(factors_);
KeyVector separatorKeys = separatorValues_.keys(); KeyVector separatorKeys = separatorValues_.keys();
ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector<Key>(separatorKeys.begin(), separatorKeys.end())); ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end()));
} }

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -72,7 +72,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
// .first is the eliminated Bayes tree, while .second is the remaining factor graph // .first is the eliminated Bayes tree, while .second is the remaining factor graph
GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal( GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal(
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second; KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second;
// Wrap in nonlinear container factors // Wrap in nonlinear container factors
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;

2
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
} }
// Create the set of clique keys LC: // Create the set of clique keys LC:
std::vector<Key> cliqueKeys; KeyVector cliqueKeys;
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
for(Key key: clique->conditional()->frontals()) { for(Key key: clique->conditional()->frontals()) {
cliqueKeys.push_back(key); cliqueKeys.push_back(key);

2
gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp
View File

@ -194,7 +194,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() {
} }
// Create the set of clique keys LC: // Create the set of clique keys LC:
std::vector<Key> cliqueKeys; KeyVector cliqueKeys;
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
for(Key key: clique->conditional()->frontals()) { for(Key key: clique->conditional()->frontals()) {
cliqueKeys.push_back(key); cliqueKeys.push_back(key);

2
gtsam_unstable/nonlinear/NonlinearClusterTree.h
View File

@ -19,7 +19,7 @@ class NonlinearClusterTree : public ClusterTree<NonlinearFactorGraph> {
// Given graph, index, add factors with specified keys into // Given graph, index, add factors with specified keys into
// Factors are erased in the graph // Factors are erased in the graph
// TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead // TODO(frank): fairly hacky and inefficient. Think about iterating the graph once instead
NonlinearCluster(const VariableIndex& variableIndex, const std::vector<Key>& keys, NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys,
NonlinearFactorGraph* graph) { NonlinearFactorGraph* graph) {
for (const Key key : keys) { for (const Key key : keys) {
std::vector<NonlinearFactor::shared_ptr> factors; std::vector<NonlinearFactor::shared_ptr> factors;

8
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -81,7 +81,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals; NonlinearFactorGraph LinearContainerForGaussianMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -419,7 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
ordering.push_back(3); ordering.push_back(3);
// Create the set of marginalizable variables // Create the set of marginalizable variables
std::vector<Key> linearIndices; KeyVector linearIndices;
linearIndices.push_back(1); linearIndices.push_back(1);
linearIndices.push_back(2); linearIndices.push_back(2);
@ -1008,7 +1008,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues); GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
// Create the set of marginalizable variables // Create the set of marginalizable variables
std::vector<Key> linearIndices; KeyVector linearIndices;
linearIndices.push_back(1); linearIndices.push_back(1);
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second; GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
@ -1062,7 +1062,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues); GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
// Create the set of marginalizable variables // Create the set of marginalizable variables
std::vector<Key> linearIndices; KeyVector linearIndices;
linearIndices.push_back(1); linearIndices.push_back(1);
linearIndices.push_back(2); linearIndices.push_back(2);

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

@ -563,7 +563,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
eliminateKeys.erase(key_value.key); eliminateKeys.erase(key_value.key);
} }
std::vector<Key> variables(eliminateKeys.begin(), eliminateKeys.end()); KeyVector variables(eliminateKeys.begin(), eliminateKeys.end());
GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second;
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);

14
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -98,7 +98,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph,
GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals; NonlinearFactorGraph LinearContainerForGaussianMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -429,7 +429,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues); GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues); GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMove.begin(), keysToMove.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedGraph; NonlinearFactorGraph expectedGraph;
@ -1108,12 +1108,12 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
newValues.insert(3, value3); newValues.insert(3, value3);
// Create the set of marginalizable variables // Create the set of marginalizable variables
std::vector<Key> linearIndices; KeyVector linearIndices;
linearIndices.push_back(1); linearIndices.push_back(1);
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -1157,14 +1157,14 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
// Create the set of marginalizable variables // Create the set of marginalizable variables
std::vector<Key> linearIndices; KeyVector linearIndices;
linearIndices.push_back(1); linearIndices.push_back(1);
linearIndices.push_back(2); linearIndices.push_back(2);
GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues);
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second; GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(linearIndices.begin(), linearIndices.end()), EliminateCholesky).second;
NonlinearFactorGraph expectedMarginals; NonlinearFactorGraph expectedMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) { for(const GaussianFactor::shared_ptr& factor: marginal) {

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

@ -583,7 +583,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) {
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
} }
std::vector<Key> variables(allkeys.begin(), allkeys.end()); KeyVector variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);

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

@ -584,7 +584,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
KeySet allkeys = LinFactorGraph->keys(); KeySet allkeys = LinFactorGraph->keys();
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
allkeys.erase(key_value.key); allkeys.erase(key_value.key);
std::vector<Key> variables(allkeys.begin(), allkeys.end()); KeyVector variables(allkeys.begin(), allkeys.end());
std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); std::pair<GaussianBayesNet::shared_ptr, GaussianFactorGraph::shared_ptr> result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky);
expectedSmootherSummarization.resize(0); expectedSmootherSummarization.resize(0);

54
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -40,10 +40,10 @@ public:
private: private:
typedef BetweenFactorEM<VALUE> This; typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base; typedef NonlinearFactor Base;
gtsam::Key key1_; Key key1_;
gtsam::Key key2_; Key key2_;
VALUE measured_; /** The measurement */ VALUE measured_; /** The measurement */
@ -114,38 +114,38 @@ public:
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/* ************************************************************************* */ /* ************************************************************************* */
virtual double error(const gtsam::Values& x) const { virtual double error(const Values& x) const {
return whitenedError(x).squaredNorm(); return whitenedError(x).squaredNorm();
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor, * Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
/* This version of linearize recalculates the noise model each time */ /* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize( virtual boost::shared_ptr<GaussianFactor> linearize(
const gtsam::Values& x) const { const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl; //std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1, A2; Matrix A1, A2;
std::vector<gtsam::Matrix> A(this->size()); std::vector<Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A); Vector b = -whitenedError(x, A);
A1 = A[0]; A1 = A[0];
A2 = A[1]; A2 = A[1];
return gtsam::GaussianFactor::shared_ptr( return GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b, new JacobianFactor(key1_, A1, key2_, A2, b,
gtsam::noiseModel::Unit::Create(b.size()))); noiseModel::Unit::Create(b.size())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x, Vector whitenedError(const Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
bool debug = true; bool debug = true;
@ -181,11 +181,11 @@ public:
Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1); Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1); Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
Matrix H1_aug = gtsam::stack(2, &H1_inlier, &H1_outlier); Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2); Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2); Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
Matrix H2_aug = gtsam::stack(2, &H2_inlier, &H2_outlier); Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
(*H)[0].resize(H1_aug.rows(), H1_aug.cols()); (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
(*H)[1].resize(H2_aug.rows(), H2_aug.cols()); (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
@ -229,7 +229,7 @@ public:
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { Vector calcIndicatorProb(const Values& x) const {
bool debug = false; bool debug = false;
@ -285,7 +285,7 @@ public:
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const { Vector unwhitenedError(const Values& x) const {
const T& p1 = x.at<T>(key1_); const T& p1 = x.at<T>(key1_);
const T& p2 = x.at<T>(key2_); const T& p2 = x.at<T>(key2_);
@ -328,8 +328,8 @@ public:
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, void updateNoiseModels(const Values& values,
const gtsam::NonlinearFactorGraph& graph) { const NonlinearFactorGraph& graph) {
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
* *
@ -340,7 +340,7 @@ public:
*/ */
// get joint covariance of the involved states // get joint covariance of the involved states
std::vector<gtsam::Key> Keys; KeyVector Keys;
Keys.push_back(key1_); Keys.push_back(key1_);
Keys.push_back(key2_); Keys.push_back(key2_);
Marginals marginals(graph, values, Marginals::QR); Marginals marginals(graph, values, Marginals::QR);
@ -353,7 +353,7 @@ public:
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, void updateNoiseModels_givenCovs(const Values& values,
const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) { const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
@ -385,12 +385,12 @@ public:
// update inlier and outlier noise models // update inlier and outlier noise models
Matrix covRinlier = Matrix covRinlier =
(model_inlier_->R().transpose() * model_inlier_->R()).inverse(); (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance( model_inlier_ = noiseModel::Gaussian::Covariance(
covRinlier + cov_state); covRinlier + cov_state);
Matrix covRoutlier = Matrix covRoutlier =
(model_outlier_->R().transpose() * model_outlier_->R()).inverse(); (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance( model_outlier_ = noiseModel::Gaussian::Covariance(
covRoutlier + cov_state); covRoutlier + cov_state);
// model_inlier_->print("after:"); // model_inlier_->print("after:");
@ -426,4 +426,4 @@ private:
}; };
// \class BetweenFactorEM // \class BetweenFactorEM
}/// namespace gtsam } // namespace gtsam

2
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -202,7 +202,7 @@ public:
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector<Key> js; KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);

4
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -92,7 +92,7 @@ public:
* @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param Ks vector of calibration objects * @param Ks vector of calibration objects
*/ */
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys, void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) { std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys); Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
@ -106,7 +106,7 @@ public:
* @param poseKeys vector of keys corresponding to the camera observing the same landmark * @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param K the (known) camera calibration (same for all measurements) * @param K the (known) camera calibration (same for all measurements)
*/ */
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys, void add(std::vector<StereoPoint2> measurements, KeyVector poseKeys,
const boost::shared_ptr<Cal3_S2Stereo> K) { const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i)); Base::add(measurements.at(i), poseKeys.at(i));

60
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -43,16 +43,16 @@ namespace gtsam {
private: private:
typedef TransformBtwRobotsUnaryFactorEM<VALUE> This; typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base; typedef NonlinearFactor Base;
gtsam::Key key_; Key key_;
VALUE measured_; /** The measurement */ VALUE measured_; /** The measurement */
gtsam::Values valA_; // given values for robot A map\trajectory Values valA_; // given values for robot A map\trajectory
gtsam::Values valB_; // given values for robot B map\trajectory Values valB_; // given values for robot B map\trajectory
gtsam::Key keyA_; // key of robot A to which the measurement refers Key keyA_; // key of robot A to which the measurement refers
gtsam::Key keyB_; // key of robot B to which the measurement refers Key keyB_; // key of robot B to which the measurement refers
// TODO: create function to update valA_ and valB_ // TODO: create function to update valA_ and valB_
@ -79,7 +79,7 @@ namespace gtsam {
/** Constructor */ /** Constructor */
TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB, TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
const gtsam::Values& valA, const gtsam::Values& valB, const Values& valA, const Values& valB,
const SharedGaussian& model_inlier, const SharedGaussian& model_outlier, const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
const double prior_inlier, const double prior_outlier, const double prior_inlier, const double prior_outlier,
const bool flag_bump_up_near_zero_probs = false, const bool flag_bump_up_near_zero_probs = false,
@ -97,7 +97,7 @@ namespace gtsam {
/** Clone */ /** Clone */
virtual gtsam::NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); } virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
/** implement functions needed for Testable */ /** implement functions needed for Testable */
@ -134,7 +134,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/* ************************************************************************* */ /* ************************************************************************* */
void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){ void setValAValB(const Values& valA, const Values& valB){
if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) ) if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
throw("something is wrong!"); throw("something is wrong!");
@ -151,36 +151,36 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
virtual double error(const gtsam::Values& x) const { virtual double error(const Values& x) const {
return whitenedError(x).squaredNorm(); return whitenedError(x).squaredNorm();
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Linearize a non-linearFactorN to get a gtsam::GaussianFactor, * Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
/* This version of linearize recalculates the noise model each time */ /* This version of linearize recalculates the noise model each time */
virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Only linearize if the factor is active // Only linearize if the factor is active
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl; //std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1; Matrix A1;
std::vector<gtsam::Matrix> A(this->size()); std::vector<Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A); Vector b = -whitenedError(x, A);
A1 = A[0]; A1 = A[0];
return gtsam::GaussianFactor::shared_ptr( return GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size()))); new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x, Vector whitenedError(const Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
bool debug = true; bool debug = true;
@ -227,7 +227,7 @@ namespace gtsam {
Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh); Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh); Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
Matrix H_aug = gtsam::stack(2, &H_inlier, &H_outlier); Matrix H_aug = stack(2, &H_inlier, &H_outlier);
(*H)[0].resize(H_aug.rows(),H_aug.cols()); (*H)[0].resize(H_aug.rows(),H_aug.cols());
(*H)[0] = H_aug; (*H)[0] = H_aug;
@ -246,7 +246,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x) const { Vector calcIndicatorProb(const Values& x) const {
Vector err = unwhitenedError(x); Vector err = unwhitenedError(x);
@ -254,7 +254,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector calcIndicatorProb(const gtsam::Values& x, const gtsam::Vector& err) const { Vector calcIndicatorProb(const Values& x, const Vector& err) const {
// Calculate indicator probabilities (inlier and outlier) // Calculate indicator probabilities (inlier and outlier)
Vector err_wh_inlier = model_inlier_->whiten(err); Vector err_wh_inlier = model_inlier_->whiten(err);
@ -288,7 +288,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
gtsam::Vector unwhitenedError(const gtsam::Values& x) const { Vector unwhitenedError(const Values& x) const {
T orgA_T_currA = valA_.at<T>(keyA_); T orgA_T_currA = valA_.at<T>(keyA_);
T orgB_T_currB = valB_.at<T>(keyB_); T orgB_T_currB = valB_.at<T>(keyB_);
@ -325,10 +325,10 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) { void updateNoiseModels(const Values& values, const Marginals& marginals) {
/* given marginals version, don't need to marginal multiple times if update a lot */ /* given marginals version, don't need to marginal multiple times if update a lot */
std::vector<gtsam::Key> Keys; KeyVector Keys;
Keys.push_back(keyA_); Keys.push_back(keyA_);
Keys.push_back(keyB_); Keys.push_back(keyB_);
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
@ -340,7 +340,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ void updateNoiseModels(const Values& values, const NonlinearFactorGraph& graph){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
* *
@ -358,7 +358,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
* *
@ -389,10 +389,10 @@ namespace gtsam {
// update inlier and outlier noise models // update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
// model_inlier_->print("after:"); // model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl; // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;

32
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -274,7 +274,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
Ks.push_back(K); Ks.push_back(K);
Ks.push_back(K); Ks.push_back(K);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3); cam2, cam3, landmark3);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -455,7 +455,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1, vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3); cam2, cam3, landmark3);
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors // Create smart factors
std::vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -614,7 +614,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -680,7 +680,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
// double excludeLandmarksFutherThanDist = 2; // double excludeLandmarksFutherThanDist = 2;
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -822,7 +822,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -919,7 +919,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
// //
// vector<Key> views; // KeyVector views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
@ -980,7 +980,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
// //
// vector<Key> views; // KeyVector views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
@ -1040,7 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, CheckHessian) { TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1128,7 +1128,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
// //
// vector<Key> views; // KeyVector views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
@ -1194,7 +1194,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
// //
// vector<Key> views; // KeyVector views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
@ -1268,7 +1268,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, Hessian ){ //TEST( SmartStereoProjectionPoseFactor, Hessian ){
// //
// vector<Key> views; // KeyVector views;
// views.push_back(x1); // views.push_back(x1);
// views.push_back(x2); // views.push_back(x2);
// //
@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);
@ -1379,7 +1379,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) { TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
vector<Key> views; KeyVector views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
views.push_back(x3); views.push_back(x3);

2
tests/testExpressionFactor.cpp
View File

@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) {
Point2_ expression = project(transform_to(x_, p_)); Point2_ expression = project(transform_to(x_, p_));
// Get and check keys and dims // Get and check keys and dims
FastVector<Key> keys; KeyVector keys;
FastVector<int> dims; FastVector<int> dims;
boost::tie(keys, dims) = expression.keysAndDims(); boost::tie(keys, dims) = expression.keysAndDims();
LONGS_EQUAL(2,keys.size()); LONGS_EQUAL(2,keys.size());

4
tests/testMarginals.cpp
View File

@ -139,7 +139,7 @@ TEST(Marginals, planarSLAMmarginals) {
0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770, 0.151935669757191, 0.007741936219615, 0.090000180000270, -0.000000000000000, 0.000000000000000, 0.160967924878730, 0.007741936219615, 0.004516127560770,
-0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193, -0.104516127560770, 0.351935664055174, 0.000000000000000, 0.090000180000270, 0.040000000000000, 0.007741936219615, 0.351935664055174, 0.056129031890193,
-0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615; -0.050967744878460, 0.056129031890193, 0.000000000000000, 0.000000000000000, 0.010000000000000, 0.004516127560770, 0.056129031890193, 0.027741936219615;
vector<Key> variables(3); KeyVector variables(3);
variables[0] = x1; variables[0] = x1;
variables[1] = l2; variables[1] = l2;
variables[2] = x3; variables[2] = x3;
@ -227,7 +227,7 @@ TEST(Marginals, order) {
Marginals marginals(fg, vals); Marginals marginals(fg, vals);
KeySet set = fg.keys(); KeySet set = fg.keys();
FastVector<Key> keys(set.begin(), set.end()); KeyVector keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys); JointMarginal joint = marginals.jointMarginalCovariance(keys);
LONGS_EQUAL(3, (long)joint(0,0).rows()); LONGS_EQUAL(3, (long)joint(0,0).rows());

2
timing/timeSchurFactors.cpp
View File

@ -39,7 +39,7 @@ void timeAll(size_t m, size_t N) {
// create F // create F
static const int D = CAMERA::dimension; static const int D = CAMERA::dimension;
typedef Eigen::Matrix<double, 2, D> Matrix2D; typedef Eigen::Matrix<double, 2, D> Matrix2D;
FastVector<Key> keys; KeyVector keys;
vector <Matrix2D> Fblocks; vector <Matrix2D> Fblocks;
for (size_t i = 0; i < m; i++) { for (size_t i = 0; i < m; i++) {
keys.push_back(i); keys.push_back(i);