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
/** A map from keys to values */
typedef std::vector<Key> Indices;
typedef KeyVector Indices;
typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;

4
gtsam/discrete/DiscreteKey.cpp
View File

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

3
gtsam/discrete/DiscreteKey.h
View File

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

4
gtsam/discrete/Signature.cpp
View File

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

2
gtsam/discrete/Signature.h
View File

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

5
gtsam/geometry/CameraSet.h
View File

@ -19,10 +19,11 @@
#pragma once
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
#include <gtsam/base/Testable.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h>
#include <vector>
namespace gtsam {
@ -244,7 +245,7 @@ public:
template<int N> // N = 2 or 3
static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E,
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) {
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()));
// Check Schur complement update, same order, should just double
FastVector<Key> allKeys, keys;
KeyVector allKeys, keys;
allKeys.push_back(1);
allKeys.push_back(2);
keys.push_back(1);
@ -102,7 +102,7 @@ TEST(CameraSet, Pinhole) {
EXPECT(assert_equal((Matrix )(2.0 * schur), actualReduced.selfadjointView()));
// Check Schur complement update, keys reversed
FastVector<Key> keys2;
KeyVector keys2;
keys2.push_back(2);
keys2.push_back(1);
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.
gttic(Full_root_factoring);
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());
for(const Key j: *B->conditional()) {
C1_minus_B_set.erase(j); }
@ -356,7 +356,7 @@ namespace gtsam {
FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
}
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());
for(const Key j: *B->conditional()) {
C2_minus_B_set.erase(j); }
@ -460,7 +460,7 @@ namespace gtsam {
/* ************************************************************************* */
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
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.
* 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. */

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

@ -40,12 +40,12 @@ namespace gtsam {
/* ************************************************************************* */
template<class DERIVED, class FACTORGRAPH>
FastVector<Key>
KeyVector
BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
{
KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
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(),
indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
return S_setminus_B;
@ -53,14 +53,14 @@ namespace gtsam {
/* ************************************************************************* */
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
{
gttic(shortcut_indices);
KeySet allKeys = p_Cp_B.keys();
KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
FastVector<Key> S_setminus_B = separator_setminus_B(B);
FastVector<Key> keep;
KeyVector S_setminus_B = separator_setminus_B(B);
KeyVector keep;
// keep = S\B intersect allKeys (S_setminus_B is already sorted)
std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
allKeys.begin(), allKeys.end(), back_inserter(keep));
@ -113,7 +113,7 @@ namespace gtsam {
gttic(BayesTreeCliqueBase_shortcut);
// We only calculate the shortcut when this clique is not B
// 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())
{
// 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)
// 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
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)
// 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);
}
}

4
gtsam/inference/BayesTreeCliqueBase.h
View File

@ -149,12 +149,12 @@ namespace gtsam {
protected:
/// 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
* 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. */
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. */
void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }

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

@ -129,7 +129,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
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) {
gttic(eliminatePartialSequential);
@ -169,7 +169,7 @@ namespace gtsam {
template<class FACTORGRAPH>
std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
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) {
gttic(eliminatePartialMultifrontal);
@ -190,7 +190,7 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const std::vector<Key>&> variables,
boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering,
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
// COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering =
const KeyVector* variablesOrOrdering =
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::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@ -249,7 +249,7 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const std::vector<Key>&> variables,
boost::variant<const Ordering&, const KeyVector&> variables,
OptionalOrdering marginalizedVariableOrdering,
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
// COLAMD.
bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
const std::vector<Key>* variablesOrOrdering =
const KeyVector* variablesOrOrdering =
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::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
@ -308,7 +308,7 @@ namespace gtsam {
template<class FACTORGRAPH>
boost::shared_ptr<FACTORGRAPH>
EliminateableFactorGraph<FACTORGRAPH>::marginal(
const std::vector<Key>& variables,
const KeyVector& variables,
const Eliminate& function, OptionalVariableIndex variableIndex) const
{
if(variableIndex)

14
gtsam/inference/EliminateableFactorGraph.h
View File

@ -170,7 +170,7 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialSequential(
const std::vector<Key>& variables,
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
@ -190,14 +190,14 @@ namespace gtsam {
* factor graph, and \f$ B = X\backslash A \f$. */
std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
eliminatePartialMultifrontal(
const std::vector<Key>& variables,
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** 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
* 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
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
* will be computed with COLAMD.
@ -206,7 +206,7 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
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,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
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.
* @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
* 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
* out, i.e. all variables not in \c variables. If this is boost::none, the ordering
* will be computed with COLAMD.
@ -223,14 +223,14 @@ namespace gtsam {
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
* provided one will be computed. */
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,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const;
/** Compute the marginal factor graph of the requested variables. */
boost::shared_ptr<FactorGraphType> marginal(
const std::vector<Key>& variables,
const KeyVector& variables,
const Eliminate& function = EliminationTraitsType::DefaultEliminate,
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());
// 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 =
function(gatheredFactors, Ordering(keyAsVector));

10
gtsam/inference/Factor.h
View File

@ -58,15 +58,15 @@ namespace gtsam {
public:
/// Iterator over keys
typedef FastVector<Key>::iterator iterator;
typedef KeyVector::iterator iterator;
/// Const iterator over keys
typedef FastVector<Key>::const_iterator const_iterator;
typedef KeyVector::const_iterator const_iterator;
protected:
/// The keys involved in this factor
FastVector<Key> keys_;
KeyVector keys_;
/// @name Standard Constructors
/// @{
@ -112,7 +112,7 @@ namespace gtsam {
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/// 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 */
const_iterator begin() const { return keys_.begin(); }
@ -148,7 +148,7 @@ namespace gtsam {
/// @{
/** @return keys involved in this factor */
FastVector<Key>& keys() { return keys_; }
KeyVector& keys() { return keys_; }
/** Iterator at beginning of involved variable keys */
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;
const KeySet newFactorKeys = newFactors.keys();
if (!this->empty()) {
std::vector<Key> keyVector(newFactorKeys.begin(), newFactorKeys.end());
KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
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
const VariableIndex index(factors);
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
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 =
&_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;
// 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)
{
return Ordering(std::vector<Key>(1, variableIndex.begin()->first));
return Ordering(KeyVector(1, variableIndex.begin()->first));
}
const size_t nEntries = variableIndex.nEntries(), nFactors =
@ -75,7 +75,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
// Fill in input data for COLAMD
p[0] = 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;
for (auto key_factors: variableIndex) {
// Arrange factor indices into COLAMD format
@ -127,7 +127,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
const std::vector<Key>& constrainLast, bool forceOrder) {
const KeyVector& constrainLast, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedLast);
size_t n = variableIndex.size();
@ -154,7 +154,7 @@ Ordering Ordering::ColamdConstrainedLast(const VariableIndex& variableIndex,
/* ************************************************************************* */
Ordering Ordering::ColamdConstrainedFirst(const VariableIndex& variableIndex,
const std::vector<Key>& constrainFirst, bool forceOrder) {
const KeyVector& constrainFirst, bool forceOrder) {
gttic(Ordering_COLAMDConstrainedFirst);
const int none = -1;

22
gtsam/inference/Ordering.h
View File

@ -30,9 +30,9 @@
namespace gtsam {
class Ordering: public std::vector<Key> {
class Ordering: public KeyVector {
protected:
typedef std::vector<Key> Base;
typedef KeyVector Base;
public:
@ -93,12 +93,12 @@ public:
/// 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
/// 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
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering ColamdConstrainedLast(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainLast, bool forceOrder = false) {
const KeyVector& constrainLast, bool forceOrder = false) {
if (graph.empty())
return Ordering();
else
@ -108,11 +108,11 @@ public:
/// 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
/// 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
/// ordered after all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedLast(
const VariableIndex& variableIndex, const std::vector<Key>& constrainLast,
const VariableIndex& variableIndex, const KeyVector& constrainLast,
bool forceOrder = false);
/// 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
/// 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
/// 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
/// ordered before all the others, but will be rearranged by CCOLAMD to reduce fill-in as well.
template<class FACTOR>
static Ordering ColamdConstrainedFirst(const FactorGraph<FACTOR>& graph,
const std::vector<Key>& constrainFirst, bool forceOrder = false) {
const KeyVector& constrainFirst, bool forceOrder = false) {
if (graph.empty())
return Ordering();
else
@ -136,12 +136,12 @@ public:
/// 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
/// 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
/// reduce fill-in as well.
static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
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
/// for note on performance). This internally builds a VariableIndex so if you already have a
@ -175,7 +175,7 @@ public:
template<class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
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());
return Ordering(keys);
}

2
gtsam/linear/BinaryJacobianFactor.h
View File

@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor {
}
// Fixed-size matrix update
void updateHessian(const FastVector<Key>& infoKeys,
void updateHessian(const KeyVector& infoKeys,
SymmetricBlockMatrix* info) const {
gttic(updateHessian_BinaryJacobianFactor);
// 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 {
// 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
const Vector rhs = get_d() - get_S() * xS;
@ -145,10 +145,10 @@ namespace gtsam {
VectorValues GaussianConditional::solveOtherRHS(
const VectorValues& parents, const VectorValues& rhs) const {
// 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
const Vector rhsR = rhs.vector(FastVector<Key>(beginFrontals(), endFrontals()));
const Vector rhsR = rhs.vector(KeyVector(beginFrontals(), endFrontals()));
xS = rhsR - get_S() * xS;
// Solve Matrix
@ -171,7 +171,7 @@ namespace gtsam {
/* ************************************************************************* */
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()));
// 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 info The information matrix to be updated
*/
virtual void updateHessian(const FastVector<Key>& keys,
virtual void updateHessian(const KeyVector& keys,
SymmetricBlockMatrix* info) const = 0;
/// 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) :
GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) {
// 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 {
gttic(updateHessian_HessianFactor);
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
* 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);
/** 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 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
* @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 {
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 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 */
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
* 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) :
HessianFactor(js, Gs, gs, f) {
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 */
for (auto cg: boost::adaptors::reverse(*Rc1_)) {
/* 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 rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
const Vector xParent = getSubvector(x, keyInfo_, KeyVector(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals()));
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
/* 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 */
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().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());
/* 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 */
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 */
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 */
size_t idx = 0;
for ( const Key &key: keys ) {

4
gtsam/linear/SubgraphPreconditioner.h
View File

@ -298,10 +298,10 @@ namespace gtsam {
};
/* 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 */
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) */

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

@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
VectorValues actual = gbn.optimizeGradientSearch();
// 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);
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();
// 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(expectedFromBN, actual, 1e-5));

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

@ -41,7 +41,7 @@ const double tol = 1e-5;
/* ************************************************************************* */
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(1, GaussianFactor::Slot(keys,4));
EXPECT_LONGS_EQUAL(2, GaussianFactor::Slot(keys,1));
@ -252,7 +252,7 @@ TEST(HessianFactor, ConstructorNWay)
(1, dx1)
(2, dx2);
std::vector<Key> js;
KeyVector js;
js.push_back(0); js.push_back(1); js.push_back(2);
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);
@ -517,7 +517,7 @@ TEST(HessianFactor, gradientAtZero)
// test gradient at zero
VectorValues expectedG = pair_list_of<Key, Vector>(0, -g1) (1, -g2);
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)));
VectorValues actualG = factor.gradientAtZero();
EXPECT(assert_equal(expectedG, actualG));

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

@ -350,7 +350,7 @@ TEST(JacobianFactor, operators )
VectorValues expectedG;
expectedG.insert(1, 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)));
VectorValues actualG = lf.gradientAtZero();
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));
// 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<Vector> gs; gs += g1, g2, g3;
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(4, 5), actual[2]));
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)));
// Check exceptions
@ -101,7 +101,7 @@ TEST(VectorValues, subvector)
init.insert(12, Vector2(4, 5));
init.insert(13, Vector2(6, 7));
std::vector<Key> keys;
KeyVector keys;
keys += 10, 12, 13;
Vector expSubVector = (Vector(5) << 1, 4, 5, 6, 7).finished();
EXPECT(assert_equal(expSubVector, init.vector(keys)));
@ -197,7 +197,7 @@ TEST(VectorValues, convert)
EXPECT(assert_equal(expected, actual2));
// 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)));
// Test version with dims argument
@ -222,7 +222,7 @@ TEST(VectorValues, vector_sub)
expected << 1, 6, 7;
// Test FastVector version
FastVector<Key> keys = list_of(0)(5);
KeyVector keys = list_of(0)(5);
EXPECT(assert_equal(expected, vv.vector(keys)));
// 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
// Get VectorValues slice corresponding to current variables
Vector gR =
grad.vector(FastVector<Key>(clique->conditional()->beginFrontals(),
grad.vector(KeyVector(clique->conditional()->beginFrontals(),
clique->conditional()->endFrontals()));
Vector gS =
grad.vector(FastVector<Key>(clique->conditional()->beginParents(),
grad.vector(KeyVector(clique->conditional()->beginParents(),
clique->conditional()->endParents()));
// 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(
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,
ISAM2Result* result) {
// TODO(dellaert): new factors are linearized twice,
@ -243,7 +243,7 @@ boost::shared_ptr<KeySet> ISAM2::recalculate(
gttic(removetop);
Cliques orphans;
GaussianBayesNet affectedBayesNet;
this->removeTop(FastVector<Key>(markedKeys.begin(), markedKeys.end()),
this->removeTop(KeyVector(markedKeys.begin(), markedKeys.end()),
affectedBayesNet, orphans);
gttoc(removetop);
@ -667,7 +667,7 @@ ISAM2Result ISAM2::update(
// NOTE: we use assign instead of the iterator constructor here because this
// is a vector of size_t, so the constructor unintentionally resolves to
// vector(size_t count, Key value) instead of the iterator constructor.
FastVector<Key> observedKeys;
KeyVector observedKeys;
observedKeys.reserve(markedKeys.size());
for (Key index : markedKeys) {
if (unusedIndices.find(index) ==
@ -945,7 +945,7 @@ void ISAM2::marginalizeLeaves(
// conditional
auto cg = clique->conditional();
const KeySet cliqueFrontals(cg->beginFrontals(), cg->endFrontals());
FastVector<Key> cliqueFrontalsToEliminate;
KeyVector cliqueFrontalsToEliminate;
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(),
leafKeys.begin(), leafKeys.end(),
std::back_inserter(cliqueFrontalsToEliminate));
@ -967,7 +967,7 @@ void ISAM2::marginalizeLeaves(
cg->matrixObject().rowStart() = dimToRemove;
// Change the keys in the clique
FastVector<Key> originalKeys;
KeyVector originalKeys;
originalKeys.swap(cg->keys());
cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
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(
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,
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);
info.blockMatrix_.invertInPlace();
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
// 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);
// Information matrix will be returned with sorted keys
std::vector<Key> variablesSorted = variables;
KeyVector variablesSorted = variables;
std::sort(variablesSorted.begin(), variablesSorted.end());
// Get dimensions from factor graph

6
gtsam/nonlinear/Marginals.h
View File

@ -74,10 +74,10 @@ public:
Matrix marginalCovariance(Key variable) const;
/** 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 */
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
JointMarginal jointMarginalInformation(const KeyVector& variables) const;
/** Optimize the bayes tree */
VectorValues optimize() const;
@ -130,7 +130,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
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()) {}
friend class Marginals;

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -52,7 +52,7 @@ 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());
shared_ptr new_factor = clone();
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
* @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

8
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

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

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

@ -47,7 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) {
EXPECT(!actFactor.isHessian());
// check keys
FastVector<Key> expKeys; expKeys += l1, l2;
KeyVector expKeys; expKeys += l1, l2;
EXPECT(assert_container_equality(expKeys, actFactor.keys()));
Values values;
@ -246,7 +246,7 @@ TEST( testLinearContainerFactor, creation ) {
LinearContainerFactor actual(linear_factor, full_values);
// Verify the keys
FastVector<Key> expKeys;
KeyVector expKeys;
expKeys += l3, l5;
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
FastVector<Key> createKeyVector(const Vector& I) {
FastVector<Key> set;
KeyVector createKeyVector(const Vector& I) {
KeyVector set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyVector from indices using symbol
FastVector<Key> createKeyVector(std::string s, const Vector& I) {
FastVector<Key> set;
KeyVector createKeyVector(std::string s, const Vector& I) {
KeyVector set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
@ -222,12 +222,12 @@ Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
/// Convert from local to world coordinates
Values localToWorld(const Values& local, const Pose2& base,
const FastVector<Key> user_keys = FastVector<Key>()) {
const KeyVector user_keys = KeyVector()) {
Values world;
// if no keys given, get all keys from local values
FastVector<Key> keys(user_keys);
KeyVector keys(user_keys);
if (keys.size()==0)
keys = local.keys();

2
gtsam/slam/InitializePose3.cpp
View File

@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
else
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];
Matrix M9 = Z_9x9;
M9.block(0,0,3,3) = Rij;

4
gtsam/slam/JacobianFactorQ.h
View File

@ -37,7 +37,7 @@ public:
}
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys, //
JacobianFactorQ(const KeyVector& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
@ -50,7 +50,7 @@ public:
}
/// 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 Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
Base() {

4
gtsam/slam/JacobianFactorQR.h
View File

@ -28,7 +28,7 @@ public:
/**
* 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 Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
@ -46,7 +46,7 @@ public:
// eliminate the point
boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg;
std::vector<Key> variables;
KeyVector variables;
variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg");

4
gtsam/slam/JacobianFactorSVD.h
View File

@ -38,7 +38,7 @@ public:
}
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys, //
JacobianFactorSVD(const KeyVector& keys, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
Matrix zeroMatrix = Matrix::Zero(0, D);
@ -58,7 +58,7 @@ public:
*
* @Fblocks:
*/
JacobianFactorSVD(const FastVector<Key>& keys,
JacobianFactorSVD(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
const Vector& b, //
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
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 Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
@ -108,7 +108,7 @@ public:
return D;
}
virtual void updateHessian(const FastVector<Key>& keys,
virtual void updateHessian(const KeyVector& keys,
SymmetricBlockMatrix* info) const {
throw std::runtime_error(
"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
*/
void add(ZVector& measurements, std::vector<Key>& cameraKeys) {
void add(ZVector& measurements, KeyVector& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
@ -310,7 +310,7 @@ public:
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
const KeyVector allKeys) const {
Matrix E;
Vector b;
computeJacobians(Fs, E, b, cameras, point);

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -179,7 +179,7 @@ public:
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
std::vector<Key> js;
KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
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
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];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
}
// put regularized measurements in the 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];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
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 F3 = 3 * Matrix26::Ones();
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
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;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys2;
KeyVector keys2;
keys2 += 0,1,2,3;
Vector x = xvalues.vector(keys2);
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_right);
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
@ -195,7 +195,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
@ -293,7 +293,7 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
@ -370,7 +370,7 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
@ -450,7 +450,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
@ -526,7 +526,7 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
projectToMultipleCameras(cam1, cam2, cam3, landmark5, measurements_cam5);
vector<Key> views;
KeyVector views;
views.push_back(c1);
views.push_back(c2);
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_right);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
@ -236,7 +236,7 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
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, landmark3, measurements_cam3);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -370,7 +370,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
measurements_cam1.push_back(cam2.project(landmark1));
// Create smart factors
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
b.setZero();
// Create smart factors
FastVector<Key> keys;
KeyVector keys;
keys.push_back(x1);
keys.push_back(x2);
@ -520,7 +520,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
using namespace vanillaPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -577,7 +577,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
using namespace vanillaPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -638,7 +638,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -701,7 +701,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -767,7 +767,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
using namespace vanillaPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -821,7 +821,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
using namespace vanillaPose2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -869,7 +869,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, CheckHessian) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -955,7 +955,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) {
using namespace vanillaPose2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
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
using namespace vanillaPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1099,7 +1099,7 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
using namespace vanillaPose2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
using namespace vanillaPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1186,7 +1186,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
using namespace vanillaPose2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1260,7 +1260,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1309,7 +1309,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
using namespace bundlerPose;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1421,7 +1421,7 @@ TEST(SmartProjectionPoseFactor, serialize2) {
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
vector<Key> key_view;
KeyVector key_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
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();
// 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::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) {
SymbolicBayesNet bn;
bn += SymbolicConditional(_A_, _B_);
std::vector<Key> keys;
KeyVector keys;
keys.push_back(_B_);
keys.push_back(_C_);
keys.push_back(_D_);

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

@ -33,7 +33,7 @@ using namespace boost::assign;
/* ************************************************************************* */
#ifdef TRACK_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());
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;
SymbolicFactorGraph::shared_ptr 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(expectedBayesNet, *actualBayesNet2));
@ -137,7 +137,7 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
SymbolicBayesTree::shared_ptr actualBayesTree2;
SymbolicFactorGraph::shared_ptr 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(expectedBayesTree2, *actualBayesTree2));

2
gtsam_unstable/discrete/CSP.h
View File

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

2
gtsam_unstable/discrete/Constraint.h
View File

@ -38,7 +38,7 @@ namespace gtsam {
protected:
/// Construct n-way factor
Constraint(const std::vector<Key>& js) :
Constraint(const KeyVector& 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];
// for all values in this domain
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
* @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
virtual Constraint::shared_ptr partiallyApply(

6
gtsam_unstable/linear/RawQP.cpp
View File

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

2
gtsam_unstable/linear/RawQP.h
View File

@ -49,7 +49,7 @@ private:
std::string name_;
std::unordered_map<Key, double> up;
std::unordered_map<Key, double> lo;
std::vector<Key> Free;
KeyVector Free;
const bool debug = false;
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
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{
ordering_ = Ordering::Colamd(factors_);
}

2
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() {
variableIndex_ = VariableIndex(factors_);
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);
// .first is the eliminated Bayes tree, while .second is the remaining factor graph
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
NonlinearFactorGraph marginalFactors;

2
gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp
View File

@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization()
}
// Create the set of clique keys LC:
std::vector<Key> cliqueKeys;
KeyVector cliqueKeys;
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
for(Key key: clique->conditional()->frontals()) {
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:
std::vector<Key> cliqueKeys;
KeyVector cliqueKeys;
for(const ISAM2Clique::shared_ptr& clique: separatorCliques) {
for(Key key: clique->conditional()->frontals()) {
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
// Factors are erased in the graph
// 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) {
for (const Key key : keys) {
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 marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -419,7 +419,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
ordering.push_back(3);
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
KeyVector linearIndices;
linearIndices.push_back(1);
linearIndices.push_back(2);
@ -1008,7 +1008,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_1 )
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
KeyVector linearIndices;
linearIndices.push_back(1);
GaussianFactorGraph result = *linearFactorGraph.eliminatePartialMultifrontal(linearIndices, EliminateCholesky).second;
@ -1062,7 +1062,7 @@ TEST( ConcurrentBatchFilter, CalculateMarginals_2 )
GaussianFactorGraph linearFactorGraph = *factorGraph.linearize(newValues);
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
KeyVector linearIndices;
linearIndices.push_back(1);
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) {
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;
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 marginal = *linearGraph.eliminatePartialMultifrontal(vector<Key>(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(KeyVector(keysToMarginalize.begin(), keysToMarginalize.end()), EliminateCholesky).second;
NonlinearFactorGraph LinearContainerForGaussianMarginals;
for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -429,7 +429,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
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;
@ -513,7 +513,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
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;
@ -1108,12 +1108,12 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_1 )
newValues.insert(3, value3);
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
KeyVector linearIndices;
linearIndices.push_back(1);
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;
for(const GaussianFactor::shared_ptr& factor: marginal) {
@ -1157,14 +1157,14 @@ TEST( ConcurrentIncrementalFilter, CalculateMarginals_2 )
// Create the set of marginalizable variables
std::vector<Key> linearIndices;
KeyVector linearIndices;
linearIndices.push_back(1);
linearIndices.push_back(2);
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;
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) {
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);
expectedSmootherSummarization.resize(0);

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

@ -584,7 +584,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
KeySet allkeys = LinFactorGraph->keys();
for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues)
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);
expectedSmootherSummarization.resize(0);

54
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -40,10 +40,10 @@ public:
private:
typedef BetweenFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base;
typedef NonlinearFactor Base;
gtsam::Key key1_;
gtsam::Key key2_;
Key key1_;
Key key2_;
VALUE measured_; /** The measurement */
@ -114,38 +114,38 @@ public:
/** 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();
}
/* ************************************************************************* */
/**
* 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$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* 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
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
return boost::shared_ptr<JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1, A2;
std::vector<gtsam::Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A);
Matrix A1, A2;
std::vector<Matrix> A(this->size());
Vector b = -whitenedError(x, A);
A1 = A[0];
A2 = A[1];
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key1_, A1, key2_, A2, b,
gtsam::noiseModel::Unit::Create(b.size())));
return GaussianFactor::shared_ptr(
new JacobianFactor(key1_, A1, key2_, A2, b,
noiseModel::Unit::Create(b.size())));
}
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
Vector whitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
bool debug = true;
@ -181,11 +181,11 @@ public:
Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->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_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)[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;
@ -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& p2 = x.at<T>(key2_);
@ -328,8 +328,8 @@ public:
}
/* ************************************************************************* */
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
* (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
std::vector<gtsam::Key> Keys;
KeyVector Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
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) {
/* 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).
@ -385,12 +385,12 @@ public:
// update inlier and outlier noise models
Matrix covRinlier =
(model_inlier_->R().transpose() * model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(
model_inlier_ = noiseModel::Gaussian::Covariance(
covRinlier + cov_state);
Matrix covRoutlier =
(model_outlier_->R().transpose() * model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(
model_outlier_ = noiseModel::Gaussian::Covariance(
covRoutlier + cov_state);
// model_inlier_->print("after:");
@ -426,4 +426,4 @@ private:
};
// \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();
// Create structures for Hessian Factors
std::vector<Key> js;
KeyVector js;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
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 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) {
Base::add(measurements, poseKeys);
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 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) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i));

60
gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h
View File

@ -43,16 +43,16 @@ namespace gtsam {
private:
typedef TransformBtwRobotsUnaryFactorEM<VALUE> This;
typedef gtsam::NonlinearFactor Base;
typedef NonlinearFactor Base;
gtsam::Key key_;
Key key_;
VALUE measured_; /** The measurement */
gtsam::Values valA_; // given values for robot A map\trajectory
gtsam::Values valB_; // given values for robot B map\trajectory
gtsam::Key keyA_; // key of robot A to which the measurement refers
gtsam::Key keyB_; // key of robot B to which the measurement refers
Values valA_; // given values for robot A map\trajectory
Values valB_; // given values for robot B map\trajectory
Key keyA_; // key of robot A to which the measurement refers
Key keyB_; // key of robot B to which the measurement refers
// TODO: create function to update valA_ and valB_
@ -79,7 +79,7 @@ namespace gtsam {
/** Constructor */
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 double prior_inlier, const double prior_outlier,
const bool flag_bump_up_near_zero_probs = false,
@ -97,7 +97,7 @@ namespace gtsam {
/** 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 */
@ -134,7 +134,7 @@ namespace gtsam {
/** 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_)) )
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();
}
/* ************************************************************************* */
/**
* 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$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
/* 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
if (!this->active(x))
return boost::shared_ptr<gtsam::JacobianFactor>();
return boost::shared_ptr<JacobianFactor>();
//std::cout<<"About to linearize"<<std::endl;
gtsam::Matrix A1;
std::vector<gtsam::Matrix> A(this->size());
gtsam::Vector b = -whitenedError(x, A);
Matrix A1;
std::vector<Matrix> A(this->size());
Vector b = -whitenedError(x, A);
A1 = A[0];
return gtsam::GaussianFactor::shared_ptr(
new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
return GaussianFactor::shared_ptr(
new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
}
/* ************************************************************************* */
gtsam::Vector whitenedError(const gtsam::Values& x,
boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
Vector whitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
bool debug = true;
@ -227,7 +227,7 @@ namespace gtsam {
Matrix H_inlier = sqrt(p_inlier)*model_inlier_->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] = 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);
@ -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)
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 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 */
std::vector<gtsam::Key> Keys;
KeyVector Keys;
Keys.push_back(keyA_);
Keys.push_back(keyB_);
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
* (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
* (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
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();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
// model_inlier_->print("after:");
// 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);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -455,7 +455,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -548,7 +548,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create smart factors
std::vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -614,7 +614,7 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -680,7 +680,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -754,7 +754,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
// double excludeLandmarksFutherThanDist = 2;
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -822,7 +822,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -919,7 +919,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
//
// vector<Key> views;
// KeyVector views;
// views.push_back(x1);
// views.push_back(x2);
// views.push_back(x3);
@ -980,7 +980,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
//
// vector<Key> views;
// KeyVector views;
// views.push_back(x1);
// views.push_back(x2);
// views.push_back(x3);
@ -1040,7 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1128,7 +1128,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
//
// vector<Key> views;
// KeyVector views;
// views.push_back(x1);
// views.push_back(x2);
// views.push_back(x3);
@ -1194,7 +1194,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
//
// vector<Key> views;
// KeyVector views;
// views.push_back(x1);
// views.push_back(x2);
// views.push_back(x3);
@ -1268,7 +1268,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, Hessian ){
//
// vector<Key> views;
// KeyVector views;
// views.push_back(x1);
// views.push_back(x2);
//
@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
@ -1379,7 +1379,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationNonDegenerate ) {
vector<Key> views;
KeyVector views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);

2
tests/testExpressionFactor.cpp
View File

@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) {
Point2_ expression = project(transform_to(x_, p_));
// Get and check keys and dims
FastVector<Key> keys;
KeyVector keys;
FastVector<int> dims;
boost::tie(keys, dims) = expression.keysAndDims();
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.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;
vector<Key> variables(3);
KeyVector variables(3);
variables[0] = x1;
variables[1] = l2;
variables[2] = x3;
@ -227,7 +227,7 @@ TEST(Marginals, order) {
Marginals marginals(fg, vals);
KeySet set = fg.keys();
FastVector<Key> keys(set.begin(), set.end());
KeyVector keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys);
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
static const int D = CAMERA::dimension;
typedef Eigen::Matrix<double, 2, D> Matrix2D;
FastVector<Key> keys;
KeyVector keys;
vector <Matrix2D> Fblocks;
for (size_t i = 0; i < m; i++) {
keys.push_back(i);