From 2aa43e11bd3b6468c5fa821b022f05f06829a500 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 8 Nov 2018 00:58:50 -0500 Subject: [PATCH] Use KeyVector everywhere to avoid conversions --- gtsam/discrete/DiscreteFactorGraph.h | 2 +- gtsam/discrete/DiscreteKey.cpp | 4 +- gtsam/discrete/DiscreteKey.h | 3 +- gtsam/discrete/Signature.cpp | 4 +- gtsam/discrete/Signature.h | 2 +- gtsam/geometry/CameraSet.h | 5 +- gtsam/geometry/tests/testCameraSet.cpp | 4 +- gtsam/inference/BayesTree-inst.h | 6 +- gtsam/inference/BayesTree.h | 2 +- gtsam/inference/BayesTreeCliqueBase-inst.h | 16 ++--- gtsam/inference/BayesTreeCliqueBase.h | 4 +- .../inference/EliminateableFactorGraph-inst.h | 18 +++--- gtsam/inference/EliminateableFactorGraph.h | 14 ++--- gtsam/inference/EliminationTree-inst.h | 2 +- gtsam/inference/Factor.h | 10 ++-- gtsam/inference/ISAM-inst.h | 4 +- gtsam/inference/Key.h | 2 +- gtsam/inference/Ordering.cpp | 8 +-- gtsam/inference/Ordering.h | 22 +++---- gtsam/linear/BinaryJacobianFactor.h | 2 +- gtsam/linear/GaussianConditional.cpp | 8 +-- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/HessianFactor.cpp | 4 +- gtsam/linear/HessianFactor.h | 4 +- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/RegularHessianFactor.h | 2 +- gtsam/linear/SubgraphPreconditioner.cpp | 14 ++--- gtsam/linear/SubgraphPreconditioner.h | 4 +- gtsam/linear/tests/testGaussianBayesNet.cpp | 2 +- gtsam/linear/tests/testGaussianBayesTree.cpp | 2 +- gtsam/linear/tests/testHessianFactor.cpp | 6 +- gtsam/linear/tests/testJacobianFactor.cpp | 2 +- .../linear/tests/testRegularHessianFactor.cpp | 2 +- gtsam/linear/tests/testVectorValues.cpp | 8 +-- gtsam/nonlinear/ISAM2-impl.cpp | 4 +- gtsam/nonlinear/ISAM2.cpp | 10 ++-- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/Marginals.cpp | 6 +- gtsam/nonlinear/Marginals.h | 6 +- gtsam/nonlinear/NonlinearFactor.cpp | 2 +- gtsam/nonlinear/NonlinearFactor.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 8 +-- .../tests/testLinearContainerFactor.cpp | 4 +- gtsam/nonlinear/utilities.h | 12 ++-- gtsam/slam/InitializePose3.cpp | 2 +- gtsam/slam/JacobianFactorQ.h | 4 +- gtsam/slam/JacobianFactorQR.h | 4 +- gtsam/slam/JacobianFactorSVD.h | 4 +- gtsam/slam/RegularImplicitSchurFactor.h | 4 +- gtsam/slam/SmartFactorBase.h | 4 +- gtsam/slam/SmartProjectionFactor.h | 2 +- gtsam/slam/lago.cpp | 4 +- .../tests/testRegularImplicitSchurFactor.cpp | 4 +- .../tests/testSmartProjectionCameraFactor.cpp | 12 ++-- .../tests/testSmartProjectionPoseFactor.cpp | 40 ++++++------- gtsam/symbolic/SymbolicFactor-inst.h | 2 +- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 2 +- gtsam/symbolic/tests/testSymbolicFactor.cpp | 2 +- .../tests/testSymbolicFactorGraph.cpp | 4 +- gtsam_unstable/discrete/CSP.h | 2 +- gtsam_unstable/discrete/Constraint.h | 2 +- gtsam_unstable/discrete/Domain.cpp | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/linear/RawQP.cpp | 6 +- gtsam_unstable/linear/RawQP.h | 2 +- .../nonlinear/ConcurrentBatchFilter.cpp | 2 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 2 +- .../ConcurrentFilteringAndSmoothing.cpp | 2 +- .../nonlinear/ConcurrentIncrementalFilter.cpp | 2 +- .../ConcurrentIncrementalSmoother.cpp | 2 +- .../nonlinear/NonlinearClusterTree.h | 2 +- .../tests/testConcurrentBatchFilter.cpp | 8 +-- .../tests/testConcurrentBatchSmoother.cpp | 2 +- .../tests/testConcurrentIncrementalFilter.cpp | 14 ++--- .../testConcurrentIncrementalSmootherDL.cpp | 2 +- .../testConcurrentIncrementalSmootherGN.cpp | 2 +- gtsam_unstable/slam/BetweenFactorEM.h | 54 ++++++++--------- .../slam/SmartStereoProjectionFactor.h | 2 +- .../slam/SmartStereoProjectionPoseFactor.h | 4 +- .../slam/TransformBtwRobotsUnaryFactorEM.h | 60 +++++++++---------- .../testSmartStereoProjectionPoseFactor.cpp | 32 +++++----- tests/testExpressionFactor.cpp | 2 +- tests/testMarginals.cpp | 4 +- timing/timeSchurFactors.cpp | 2 +- 85 files changed, 283 insertions(+), 281 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index cdfd7d522..4c2607f1f 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -72,7 +72,7 @@ public: typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class /** A map from keys to values */ - typedef std::vector Indices; + typedef KeyVector Indices; typedef Assignment Values; typedef boost::shared_ptr sharedValues; diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index b4fd2e5a1..5ddad22b0 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -31,8 +31,8 @@ namespace gtsam { } } - vector DiscreteKeys::indices() const { - vector < Key > js; + KeyVector DiscreteKeys::indices() const { + KeyVector js; for(const DiscreteKey& key: *this) js.push_back(key.first); return js; diff --git a/gtsam/discrete/DiscreteKey.h b/gtsam/discrete/DiscreteKey.h index bc2bd862d..c041c7e8e 100644 --- a/gtsam/discrete/DiscreteKey.h +++ b/gtsam/discrete/DiscreteKey.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -53,7 +54,7 @@ namespace gtsam { GTSAM_EXPORT DiscreteKeys(const std::vector& cs); /// Return a vector of indices - GTSAM_EXPORT std::vector indices() const; + GTSAM_EXPORT KeyVector indices() const; /// Return a map from index to cardinality GTSAM_EXPORT std::map cardinalities() const; diff --git a/gtsam/discrete/Signature.cpp b/gtsam/discrete/Signature.cpp index 0ee5c63b8..89e763703 100644 --- a/gtsam/discrete/Signature.cpp +++ b/gtsam/discrete/Signature.cpp @@ -130,8 +130,8 @@ namespace gtsam { return keys; } - vector Signature::indices() const { - vector js; + KeyVector Signature::indices() const { + KeyVector js; js.push_back(key_.first); for(const DiscreteKey& key: parents_) js.push_back(key.first); diff --git a/gtsam/discrete/Signature.h b/gtsam/discrete/Signature.h index 6d49c7e4c..587cd6b30 100644 --- a/gtsam/discrete/Signature.h +++ b/gtsam/discrete/Signature.h @@ -90,7 +90,7 @@ namespace gtsam { DiscreteKeys discreteKeysParentsFirst() const; /** All key indices, with variable key first */ - std::vector indices() const; + KeyVector indices() const; // the CPT as parsed, if successful const boost::optional& table() const { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 709172c5b..1f5d6f774 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -19,10 +19,11 @@ #pragma once #include -#include // for Cheirality exception +#include // for Cheirality exception #include #include #include +#include #include namespace gtsam { @@ -244,7 +245,7 @@ public: template // N = 2 or 3 static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const FastVector& allKeys, const FastVector& keys, + const KeyVector& allKeys, const KeyVector& keys, /*output ->*/SymmetricBlockMatrix& augmentedHessian) { assert(keys.size()==Fs.size()); diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 560206b9f..ab9227a08 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -93,7 +93,7 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(schur, actualReduced.selfadjointView())); // Check Schur complement update, same order, should just double - FastVector 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 keys2; + KeyVector keys2; keys2.push_back(2); keys2.push_back(1); Set::UpdateSchurComplement(Fs, E, P, b, allKeys, keys2, actualReduced); diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 96d191419..33a23056b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -344,7 +344,7 @@ namespace gtsam { // Get the set of variables to eliminate, which is C1\B. gttic(Full_root_factoring); boost::shared_ptr p_C1_B; { - FastVector 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 p_C2_B; { - FastVector 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 - void BayesTree::removeTop(const FastVector& keys, BayesNetType& bn, Cliques& orphans) + void BayesTree::removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans) { // process each key of the new factor for(const Key& j: keys) diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index c22a5e257..ee0f3c7b5 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -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& keys, BayesNetType& bn, Cliques& orphans); + void removeTop(const KeyVector& keys, BayesNetType& bn, Cliques& orphans); /** * Remove the requested subtree. */ diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 31a8c55b6..6bcfb434d 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -40,12 +40,12 @@ namespace gtsam { /* ************************************************************************* */ template - FastVector + KeyVector BayesTreeCliqueBase::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 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 - FastVector BayesTreeCliqueBase::shortcut_indices( + KeyVector BayesTreeCliqueBase::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 S_setminus_B = separator_setminus_B(B); - FastVector 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 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 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 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 indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); + KeyVector indicesS(this->conditional()->beginParents(), this->conditional()->endParents()); cachedSeparatorMarginal_ = *p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), boost::none, function); } } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 055a03939..317ba1c44 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -149,12 +149,12 @@ namespace gtsam { protected: /// Calculate set \f$ S \setminus B \f$ for shortcut calculations - FastVector 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 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; } diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 43bcffb09..af2a91257 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -129,7 +129,7 @@ namespace gtsam { template std::pair::BayesNetType>, boost::shared_ptr > EliminateableFactorGraph::eliminatePartialSequential( - const std::vector& 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 std::pair::BayesTreeType>, boost::shared_ptr > EliminateableFactorGraph::eliminatePartialMultifrontal( - const std::vector& 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 boost::shared_ptr::BayesNetType> EliminateableFactorGraph::marginalMultifrontalBayesNet( - boost::variant&> variables, + boost::variant 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(&variables) != 0); - const std::vector* variablesOrOrdering = + const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get&>(&variables); + boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); @@ -249,7 +249,7 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::marginalMultifrontalBayesTree( - boost::variant&> variables, + boost::variant 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(&variables) != 0); - const std::vector* variablesOrOrdering = + const KeyVector* variablesOrOrdering = unmarginalizedAreOrdered ? - boost::get(&variables) : boost::get&>(&variables); + boost::get(&variables) : boost::get(&variables); Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered); @@ -308,7 +308,7 @@ namespace gtsam { template boost::shared_ptr EliminateableFactorGraph::marginal( - const std::vector& variables, + const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const { if(variableIndex) diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 891f22e61..a8f68ca2e 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -170,7 +170,7 @@ namespace gtsam { * factor graph, and \f$ B = X\backslash A \f$. */ std::pair, boost::shared_ptr > eliminatePartialSequential( - const std::vector& 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 > eliminatePartialMultifrontal( - const std::vector& 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 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 marginalMultifrontalBayesNet( - boost::variant&> variables, + boost::variant 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 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 marginalMultifrontalBayesTree( - boost::variant&> variables, + boost::variant 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 marginal( - const std::vector& variables, + const KeyVector& variables, const Eliminate& function = EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index 90540aaa6..333f898b8 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -48,7 +48,7 @@ namespace gtsam { gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); // Do dense elimination step - FastVector keyAsVector(1); keyAsVector[0] = key; + KeyVector keyAsVector(1); keyAsVector[0] = key; std::pair, boost::shared_ptr > eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 3e41d7692..d44d82954 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -58,15 +58,15 @@ namespace gtsam { public: /// Iterator over keys - typedef FastVector::iterator iterator; + typedef KeyVector::iterator iterator; /// Const iterator over keys - typedef FastVector::const_iterator const_iterator; + typedef KeyVector::const_iterator const_iterator; protected: /// The keys involved in this factor - FastVector 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& 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& keys() { return keys_; } + KeyVector& keys() { return keys_; } /** Iterator at beginning of involved variable keys */ iterator begin() { return keys_.begin(); } diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 4471567fc..785b2507d 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -30,7 +30,7 @@ void ISAM::update_internal(const FactorGraphType& newFactors, BayesNetType bn; const KeySet newFactorKeys = newFactors.keys(); if (!this->empty()) { - std::vector keyVector(newFactorKeys.begin(), newFactorKeys.end()); + KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end()); this->removeTop(keyVector, bn, orphans); } @@ -46,7 +46,7 @@ void ISAM::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(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); diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 2c3eb84c6..d400a33c0 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -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 KeyVector; // TODO(frank): Nothing fast about these :-( diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index b94f01689..1165b4a0f 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -61,7 +61,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, if (nVars == 1) { - return Ordering(std::vector(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 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& 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& constrainFirst, bool forceOrder) { + const KeyVector& constrainFirst, bool forceOrder) { gttic(Ordering_COLAMDConstrainedFirst); const int none = -1; diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index ae7a10f44..f37de12fe 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -30,9 +30,9 @@ namespace gtsam { -class Ordering: public std::vector { +class Ordering: public KeyVector { protected: - typedef std::vector 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 \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 static Ordering ColamdConstrainedLast(const FactorGraph& graph, - const std::vector& 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 + /// 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& 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 \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 static Ordering ColamdConstrainedFirst(const FactorGraph& graph, - const std::vector& 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 \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& 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 static Ordering Natural(const FactorGraph &fg) { KeySet src = fg.keys(); - std::vector keys(src.begin(), src.end()); + KeyVector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); return Ordering(keys); } diff --git a/gtsam/linear/BinaryJacobianFactor.h b/gtsam/linear/BinaryJacobianFactor.h index 462258762..343548613 100644 --- a/gtsam/linear/BinaryJacobianFactor.h +++ b/gtsam/linear/BinaryJacobianFactor.h @@ -48,7 +48,7 @@ struct BinaryJacobianFactor: JacobianFactor { } // Fixed-size matrix update - void updateHessian(const FastVector& infoKeys, + void updateHessian(const KeyVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_BinaryJacobianFactor); // Whiten the factor if it has a noise model diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 60187d129..93217c027 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -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(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(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(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(beginFrontals(), endFrontals())); + Vector frontalVec = gy.vector(KeyVector(beginFrontals(), endFrontals())); frontalVec = gtsam::backSubstituteUpper(frontalVec, Matrix(get_R())); // Check for indeterminant solution diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 7f9c5ea3c..8c72ee734 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -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& keys, + virtual void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const = 0; /// y += alpha * A'*A*x diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 29b2b8591..61ce84fe9 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -155,7 +155,7 @@ DenseIndex _getSizeHF(const Vector& m) { } /* ************************************************************************* */ -HessianFactor::HessianFactor(const std::vector& js, +HessianFactor::HessianFactor(const KeyVector& js, const std::vector& Gs, const std::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& infoKeys, +void HessianFactor::updateHessian(const KeyVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_HessianFactor); assert(info); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index e28bcdd81..cb9da0f1a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -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& js, const std::vector& Gs, + HessianFactor(const KeyVector& js, const std::vector& Gs, const std::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& keys, SymmetricBlockMatrix* info) const; + void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const; /** Update another Hessian factor * @param other the HessianFactor to be updated diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 56a5dc085..1f5e5557c 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -501,7 +501,7 @@ map JacobianFactor::hessianBlockDiagonal() const { } /* ************************************************************************* */ -void JacobianFactor::updateHessian(const FastVector& infoKeys, +void JacobianFactor::updateHessian(const KeyVector& infoKeys, SymmetricBlockMatrix* info) const { gttic(updateHessian_JacobianFactor); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d7814f541..36d1e12da 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -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& keys, SymmetricBlockMatrix* info) const; + void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const; /** Return A*x */ Vector operator*(const VectorValues& x) const; diff --git a/gtsam/linear/RegularHessianFactor.h b/gtsam/linear/RegularHessianFactor.h index f4df9d96b..1fe7009bc 100644 --- a/gtsam/linear/RegularHessianFactor.h +++ b/gtsam/linear/RegularHessianFactor.h @@ -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& js, + RegularHessianFactor(const KeyVector& js, const std::vector& Gs, const std::vector& gs, double f) : HessianFactor(js, Gs, gs, f) { checkInvariants(); diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index d51f365be..d796e28b7 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -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(cg->beginParents(), cg->endParents())); - const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(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().solve(rhsFrontal - cg->get_S() * xParent); /* assign subvector of sol to the frontal variables */ - setSubvector(solFrontal, keyInfo_, FastVector(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 & cg: *Rc1_) { - const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); + const Vector rhsFrontal = getSubvector(x, keyInfo_, KeyVector(cg->beginFrontals(), cg->endFrontals())); // const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); const Vector solFrontal = cg->get_R().transpose().triangularView().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(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 &keys) { +Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys) { /* a cache of starting index and dim */ typedef vector > 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 &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 ) { diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index e74b92df1..e440f32e4 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -298,10 +298,10 @@ namespace gtsam { }; /* get subvectors */ - Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys); + Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const KeyVector &keys); /* set subvectors */ - void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &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) */ diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 89b44f1f8..45545b8ea 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -229,7 +229,7 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { VectorValues actual = gbn.optimizeGradientSearch(); // Check that points agree - FastVector 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)); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index e5634c357..97809bfbc 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -287,7 +287,7 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { VectorValues actual = bt.optimizeGradientSearch(); // Check that points agree - FastVector 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)); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 61fa8bd2c..31030451b 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -41,7 +41,7 @@ const double tol = 1e-5; /* ************************************************************************* */ TEST(HessianFactor, Slot) { - FastVector 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 js; + KeyVector js; js.push_back(0); js.push_back(1); js.push_back(2); std::vector 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(0, -g1) (1, -g2); Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); - FastVector 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)); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 26f02923b..2403895c2 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -350,7 +350,7 @@ TEST(JacobianFactor, operators ) VectorValues expectedG; expectedG.insert(1, Vector2(20,-10)); expectedG.insert(2, Vector2(-20, 10)); - FastVector 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)); diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp index 1618451f3..c610255c0 100644 --- a/gtsam/linear/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -64,7 +64,7 @@ TEST(RegularHessianFactor, Constructors) EXPECT(assert_equal(factor,factor2)); // Test n-way constructor - vector keys; keys += 0, 1, 3; + KeyVector keys; keys += 0, 1, 3; vector Gs; Gs += G11, G12, G13, G22, G23, G33; vector gs; gs += g1, g2, g3; RegularHessianFactor<2> factor3(keys, Gs, gs, f); diff --git a/gtsam/linear/tests/testVectorValues.cpp b/gtsam/linear/tests/testVectorValues.cpp index 7e972903a..d5e5aee68 100644 --- a/gtsam/linear/tests/testVectorValues.cpp +++ b/gtsam/linear/tests/testVectorValues.cpp @@ -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 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 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 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 keys = list_of(0)(5); + KeyVector keys = list_of(0)(5); EXPECT(assert_equal(expected, vv.vector(keys))); // Test version with dims argument diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8a8813af5..2cedeea9f 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -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(clique->conditional()->beginFrontals(), + grad.vector(KeyVector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); Vector gS = - grad.vector(FastVector(clique->conditional()->beginParents(), + grad.vector(KeyVector(clique->conditional()->beginParents(), clique->conditional()->endParents())); // Compute R*g and S*g for this clique diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index df07050de..32a24b51d 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -195,7 +195,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(const Cliques& orphans) { /* ************************************************************************* */ boost::shared_ptr ISAM2::recalculate( const KeySet& markedKeys, const KeySet& relinKeys, - const vector& observedKeys, const KeySet& unusedIndices, + const KeyVector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result* result) { // TODO(dellaert): new factors are linearized twice, @@ -243,7 +243,7 @@ boost::shared_ptr ISAM2::recalculate( gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(FastVector(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 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 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 originalKeys; + KeyVector originalKeys; originalKeys.swap(cg->keys()); cg->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end()); cg->nrFrontals() -= nToRemove; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5d448d786..04bd3d3eb 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -299,7 +299,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { virtual boost::shared_ptr recalculate( const KeySet& markedKeys, const KeySet& relinKeys, - const std::vector& observedKeys, const KeySet& unusedIndices, + const KeyVector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result* result); diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 68da1250e..fcbbf2f44 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -80,14 +80,14 @@ Matrix Marginals::marginalCovariance(Key variable) const { } /* ************************************************************************* */ -JointMarginal Marginals::jointMarginalCovariance(const std::vector& variables) const { +JointMarginal Marginals::jointMarginalCovariance(const KeyVector& variables) const { JointMarginal info = jointMarginalInformation(variables); info.blockMatrix_.invertInPlace(); return info; } /* ************************************************************************* */ -JointMarginal Marginals::jointMarginalInformation(const std::vector& 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& variab Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1); // Information matrix will be returned with sorted keys - std::vector variablesSorted = variables; + KeyVector variablesSorted = variables; std::sort(variablesSorted.begin(), variablesSorted.end()); // Get dimensions from factor graph diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 35b0770c2..e0a78042d 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -74,10 +74,10 @@ public: Matrix marginalCovariance(Key variable) const; /** Compute the joint marginal covariance of several variables */ - JointMarginal jointMarginalCovariance(const std::vector& variables) const; + JointMarginal jointMarginalCovariance(const KeyVector& variables) const; /** Compute the joint marginal information of several variables */ - JointMarginal jointMarginalInformation(const std::vector& 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& dims, const std::vector& keys) : + JointMarginal(const Matrix& fullMatrix, const std::vector& dims, const KeyVector& keys) : blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {} friend class Marginals; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 5ff022023..c705e3c8f 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -52,7 +52,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey( /* ************************************************************************* */ NonlinearFactor::shared_ptr NonlinearFactor::rekey( - const std::vector& new_keys) const { + const KeyVector& new_keys) const { assert(new_keys.size() == keys().size()); shared_ptr new_factor = clone(); new_factor->keys() = new_keys; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index dd4c9123a..4cddc7dda 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -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& new_keys) const; + shared_ptr rekey(const KeyVector& new_keys) const; }; // \class NonlinearFactor diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 56234c13c..a4bdd83e3 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -165,10 +165,10 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if (formatting.mergeSimilarFactors) { // Remove duplicate factors - std::set > structure; + std::set structure; for (const sharedFactor& factor : factors_) { if (factor) { - vector 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& 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& keys = factor->keys(); + const KeyVector& keys = factor->keys(); if (formatting.binaryEdges && keys.size()==2) { stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; } else { diff --git a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp index df1df0d20..5321eb2c4 100644 --- a/gtsam/nonlinear/tests/testLinearContainerFactor.cpp +++ b/gtsam/nonlinear/tests/testLinearContainerFactor.cpp @@ -47,7 +47,7 @@ TEST( testLinearContainerFactor, generic_jacobian_factor ) { EXPECT(!actFactor.isHessian()); // check keys - FastVector 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 expKeys; + KeyVector expKeys; expKeys += l3, l5; EXPECT(assert_container_equality(expKeys, actual.keys())); diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 3816f26f8..58e7e78af 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -54,16 +54,16 @@ FastList createKeyList(std::string s, const Vector& I) { } // Create a KeyVector from indices -FastVector createKeyVector(const Vector& I) { - FastVector 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 createKeyVector(std::string s, const Vector& I) { - FastVector 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 user_keys = FastVector()) { + const KeyVector user_keys = KeyVector()) { Values world; // if no keys given, get all keys from local values - FastVector keys(user_keys); + KeyVector keys(user_keys); if (keys.size()==0) keys = local.keys(); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 58408e7e3..3979996da 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -53,7 +53,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { else std::cout << "Error in buildLinearOrientationGraph" << std::endl; - const FastVector& 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; diff --git a/gtsam/slam/JacobianFactorQ.h b/gtsam/slam/JacobianFactorQ.h index 3f5290c58..23cadbee9 100644 --- a/gtsam/slam/JacobianFactorQ.h +++ b/gtsam/slam/JacobianFactorQ.h @@ -37,7 +37,7 @@ public: } /// Empty constructor with keys - JacobianFactorQ(const FastVector& 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& keys, + JacobianFactorQ(const KeyVector& keys, const std::vector >& FBlocks, const Matrix& E, const Matrix3& P, const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : Base() { diff --git a/gtsam/slam/JacobianFactorQR.h b/gtsam/slam/JacobianFactorQR.h index 9e61f5324..aef10eb86 100644 --- a/gtsam/slam/JacobianFactorQR.h +++ b/gtsam/slam/JacobianFactorQR.h @@ -28,7 +28,7 @@ public: /** * Constructor */ - JacobianFactorQR(const FastVector& keys, + JacobianFactorQR(const KeyVector& keys, const std::vector >& 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 bn; GaussianFactorGraph::shared_ptr fg; - std::vector variables; + KeyVector variables; variables.push_back(pointKey); boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR); //fg->print("fg"); diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index e7bc5864d..b94714dd6 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -38,7 +38,7 @@ public: } /// Empty constructor with keys - JacobianFactorSVD(const FastVector& 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& keys, + JacobianFactorSVD(const KeyVector& keys, const std::vector >& Fblocks, const Matrix& Enull, const Vector& b, // const SharedDiagonal& model = SharedDiagonal()) : diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 47c9a4c7b..07b749811 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -48,7 +48,7 @@ public: } /// Construct from blocks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const FastVector& keys, + RegularImplicitSchurFactor(const KeyVector& keys, const std::vector >& 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& keys, + virtual void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const { throw std::runtime_error( "RegularImplicitSchurFactor::updateHessian non implemented"); diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 497ebbc5b..489a4adf4 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -131,7 +131,7 @@ public: /** * Add a bunch of measurements, together with the camera keys */ - void add(ZVector& measurements, std::vector& 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 allKeys) const { + const KeyVector allKeys) const { Matrix E; Vector b; computeJacobians(Fs, E, b, cameras, point); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 0d9f90d22..e554e0c85 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -179,7 +179,7 @@ public: size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector js; + KeyVector js; std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index e1cf9cea2..34c8385e8 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -168,14 +168,14 @@ GaussianFactorGraph buildLinearOrientationGraph( // put original measurements in the spanning tree for(const size_t& factorId: spanningTreeIds) { - const FastVector& 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& 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); diff --git a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp index 6f9371e68..02893e628 100644 --- a/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp +++ b/gtsam/slam/tests/testRegularImplicitSchurFactor.cpp @@ -42,7 +42,7 @@ const Matrix26 F0 = Matrix26::Ones(); const Matrix26 F1 = 2 * Matrix26::Ones(); const Matrix26 F3 = 3 * Matrix26::Ones(); const vector > FBlocks = list_of(F0)(F1)(F3); -const FastVector keys = list_of(0)(1)(3); +const KeyVector keys = list_of(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 keys2; + KeyVector keys2; keys2 += 0,1,2,3; Vector x = xvalues.vector(keys2); Vector expected = Vector::Zero(24); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 17fec7b9f..be0a9df88 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -170,7 +170,7 @@ TEST( SmartProjectionCameraFactor, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector 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 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 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 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 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 views; + KeyVector views; views.push_back(c1); views.push_back(c2); views.push_back(c3); diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 7d3d9c63c..080046dd4 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -189,7 +189,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - vector 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 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 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 views; + KeyVector views; views.push_back(x1); views.push_back(x2); @@ -459,7 +459,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { b.setZero(); // Create smart factors - FastVector 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 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 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 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 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 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 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 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 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 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 views; + KeyVector views; views.push_back(x1); views.push_back(x2); @@ -1133,7 +1133,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { using namespace vanillaPose; - vector 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 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 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 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_view; + KeyVector key_view; Point2Vector meas_view; key_view.push_back(Symbol('x', 1)); meas_view.push_back(Point2(10, 10)); diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index 47bb06515..e3b4c3297 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -57,7 +57,7 @@ namespace gtsam const size_t nFrontals = keys.size(); // Build a key vector with the frontals followed by the separator - FastVector 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); diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 99a09adc9..f7df47f5f 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -81,7 +81,7 @@ TEST( SymbolicBayesNet, combine ) TEST(SymbolicBayesNet, saveGraph) { SymbolicBayesNet bn; bn += SymbolicConditional(_A_, _B_); - std::vector keys; + KeyVector keys; keys.push_back(_B_); keys.push_back(_C_); keys.push_back(_D_); diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 57c3a2d5f..18f3b84cc 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -33,7 +33,7 @@ using namespace boost::assign; /* ************************************************************************* */ #ifdef TRACK_ELIMINATE TEST(SymbolicFactor, eliminate) { - vector 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 fragment = *actual.eliminate(3); diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index a26d2f581..3fd318456 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -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 >()); + simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container()); 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(4)(5).convert_to_container >()); + simpleTestGraph2.eliminatePartialMultifrontal(list_of(4)(5).convert_to_container()); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2)); EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2)); diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 93f5c5d7d..bbdadd3dc 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -22,7 +22,7 @@ namespace gtsam { public: /** A map from keys to values */ - typedef std::vector Indices; + typedef KeyVector Indices; typedef Assignment Values; typedef boost::shared_ptr sharedValues; diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index d8f4689a8..32fb6f1ce 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -38,7 +38,7 @@ namespace gtsam { protected: /// Construct n-way factor - Constraint(const std::vector& js) : + Constraint(const KeyVector& js) : DiscreteFactor(js) { } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 72d424baf..740ef067c 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -57,7 +57,7 @@ namespace gtsam { } /* ************************************************************************* */ - bool Domain::checkAllDiff(const vector keys, vector& domains) { + bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { Key j = keys_[0]; // for all values in this domain for(size_t value: values_) { diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 812da9932..5dd597e20 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -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 keys, std::vector& domains); + bool checkAllDiff(const KeyVector keys, std::vector& domains); /// Partially apply known values virtual Constraint::shared_ptr partiallyApply( diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp index ec71cae5b..47672a947 100644 --- a/gtsam_unstable/linear/RawQP.cpp +++ b/gtsam_unstable/linear/RawQP.cpp @@ -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 Gs; + std::vector gs; for (auto kv : varname_to_key) { keys.push_back(kv.second); } diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h index aadf11e50..70b2a9d27 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/gtsam_unstable/linear/RawQP.h @@ -49,7 +49,7 @@ private: std::string name_; std::unordered_map up; std::unordered_map lo; - std::vector Free; + KeyVector Free; const bool debug = false; public: diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 884bba9a3..51a959075 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -362,7 +362,7 @@ void ConcurrentBatchFilter::reorder(const boost::optional >& 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(keysToMove->begin(), keysToMove->end())); + ordering_ = Ordering::ColamdConstrainedFirst(factors_, KeyVector(keysToMove->begin(), keysToMove->end())); }else{ ordering_ = Ordering::Colamd(factors_); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index d64b697b8..0fc975958 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -231,7 +231,7 @@ void ConcurrentBatchSmoother::reorder() { variableIndex_ = VariableIndex(factors_); KeyVector separatorKeys = separatorValues_.keys(); - ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, std::vector(separatorKeys.begin(), separatorKeys.end())); + ordering_ = Ordering::ColamdConstrainedLast(variableIndex_, KeyVector(separatorKeys.begin(), separatorKeys.end())); } diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 61f1c889f..70cb3e268 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -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(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second; + KeyVector(marginalizeKeys.begin(), marginalizeKeys.end()), eliminateFunction).second; // Wrap in nonlinear container factors NonlinearFactorGraph marginalFactors; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 3913ba95c..e7c8229ef 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -353,7 +353,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() } // Create the set of clique keys LC: - std::vector cliqueKeys; + KeyVector cliqueKeys; for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { for(Key key: clique->conditional()->frontals()) { cliqueKeys.push_back(key); diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index e95c1c81d..41a94b876 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -194,7 +194,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Create the set of clique keys LC: - std::vector cliqueKeys; + KeyVector cliqueKeys; for(const ISAM2Clique::shared_ptr& clique: separatorCliques) { for(Key key: clique->conditional()->frontals()) { cliqueKeys.push_back(key); diff --git a/gtsam_unstable/nonlinear/NonlinearClusterTree.h b/gtsam_unstable/nonlinear/NonlinearClusterTree.h index b1f463c26..a483c9521 100644 --- a/gtsam_unstable/nonlinear/NonlinearClusterTree.h +++ b/gtsam_unstable/nonlinear/NonlinearClusterTree.h @@ -19,7 +19,7 @@ class NonlinearClusterTree : public ClusterTree { // 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& keys, + NonlinearCluster(const VariableIndex& variableIndex, const KeyVector& keys, NonlinearFactorGraph* graph) { for (const Key key : keys) { std::vector factors; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp index 4aff1b465..7b683208c 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp @@ -81,7 +81,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(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 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 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 linearIndices; + KeyVector linearIndices; linearIndices.push_back(1); linearIndices.push_back(2); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index a0b6e2c1b..8ecd087c5 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -563,7 +563,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { eliminateKeys.erase(key_value.key); } - std::vector variables(eliminateKeys.begin(), eliminateKeys.end()); + KeyVector variables(eliminateKeys.begin(), eliminateKeys.end()); GaussianFactorGraph result = *linearFactors->eliminatePartialMultifrontal(variables, EliminateCholesky).second; expectedSmootherSummarization.resize(0); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index d6b7ab851..3cd59efec 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -98,7 +98,7 @@ NonlinearFactorGraph CalculateMarginals(const NonlinearFactorGraph& factorGraph, GaussianFactorGraph linearGraph = *factorGraph.linearize(linPoint); - GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(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(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(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 linearIndices; + KeyVector linearIndices; linearIndices.push_back(1); GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); - GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(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 linearIndices; + KeyVector linearIndices; linearIndices.push_back(1); linearIndices.push_back(2); GaussianFactorGraph linearGraph = *factorGraph.linearize(newValues); - GaussianFactorGraph marginal = *linearGraph.eliminatePartialMultifrontal(vector(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) { diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index c265bf5d1..d7bddece2 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -583,7 +583,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) for(const Values::ConstKeyValuePair& key_value: filterSeparatorValues) { allkeys.erase(key_value.key); } - std::vector variables(allkeys.begin(), allkeys.end()); + KeyVector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index fdf9f08b5..82d8f2153 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -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 variables(allkeys.begin(), allkeys.end()); + KeyVector variables(allkeys.begin(), allkeys.end()); std::pair result = LinFactorGraph->eliminatePartialSequential(variables, EliminateCholesky); expectedSmootherSummarization.resize(0); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 56b1269f0..b009927d6 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -40,10 +40,10 @@ public: private: typedef BetweenFactorEM 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 linearize( - const gtsam::Values& x) const { + virtual boost::shared_ptr linearize( + const Values& x) const { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return boost::shared_ptr(); //std::cout<<"About to linearize"< A(this->size()); - gtsam::Vector b = -whitenedError(x, A); + Matrix A1, A2; + std::vector 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&> H = boost::none) const { + Vector whitenedError(const Values& x, + boost::optional&> 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(key1_); const T& p2 = x.at(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 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 diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index b7d06b268..f32dd3b3e 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -202,7 +202,7 @@ public: size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors - std::vector js; + KeyVector js; std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 3b4c5e0db..7ea5a4c2f 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -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 measurements, std::vector poseKeys, + void add(std::vector measurements, KeyVector poseKeys, std::vector > 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 measurements, std::vector poseKeys, + void add(std::vector measurements, KeyVector poseKeys, const boost::shared_ptr K) { for (size_t i = 0; i < measurements.size(); i++) { Base::add(measurements.at(i), poseKeys.at(i)); diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index bf10ae531..b6d906fd4 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -43,16 +43,16 @@ namespace gtsam { private: typedef TransformBtwRobotsUnaryFactorEM 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); } + virtual NonlinearFactor::shared_ptr clone() const { return boost::make_shared(*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 linearize(const gtsam::Values& x) const { + virtual boost::shared_ptr linearize(const Values& x) const { // Only linearize if the factor is active if (!this->active(x)) - return boost::shared_ptr(); + return boost::shared_ptr(); //std::cout<<"About to linearize"< A(this->size()); - gtsam::Vector b = -whitenedError(x, A); + Matrix A1; + std::vector 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&> H = boost::none) const { + Vector whitenedError(const Values& x, + boost::optional&> 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(keyA_); T orgB_T_currB = valB_.at(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 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: "< views; + KeyVector views; views.push_back(x1); views.push_back(x2); @@ -313,7 +313,7 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) { vector measurements_l3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector 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 measurements_l3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); - vector 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 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 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 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 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 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 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 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 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 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 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 views; +// KeyVector views; // views.push_back(x1); // views.push_back(x2); // @@ -1309,7 +1309,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) { /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) { - vector 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 views; + KeyVector views; views.push_back(x1); views.push_back(x2); views.push_back(x3); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index b6b196acc..769b458e4 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -231,7 +231,7 @@ TEST(ExpressionFactor, Shallow) { Point2_ expression = project(transform_to(x_, p_)); // Get and check keys and dims - FastVector keys; + KeyVector keys; FastVector dims; boost::tie(keys, dims) = expression.keysAndDims(); LONGS_EQUAL(2,keys.size()); diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 392b84deb..844510018 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -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 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 keys(set.begin(), set.end()); + KeyVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); LONGS_EQUAL(3, (long)joint(0,0).rows()); diff --git a/timing/timeSchurFactors.cpp b/timing/timeSchurFactors.cpp index e64c34340..f00e5d6f7 100644 --- a/timing/timeSchurFactors.cpp +++ b/timing/timeSchurFactors.cpp @@ -39,7 +39,7 @@ void timeAll(size_t m, size_t N) { // create F static const int D = CAMERA::dimension; typedef Eigen::Matrix Matrix2D; - FastVector keys; + KeyVector keys; vector Fblocks; for (size_t i = 0; i < m; i++) { keys.push_back(i);