From ca2aa0cfd4bcbf80c2f18ebf040e72dc7aefab96 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Jun 2015 20:33:33 -0700 Subject: [PATCH 01/14] Refactoring to get grouped factors to compile again --- gtsam/slam/RegularHessianFactor.h | 97 +++++++++++++------ gtsam/slam/tests/testRegularHessianFactor.cpp | 81 +++++++++++----- 2 files changed, 122 insertions(+), 56 deletions(-) diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 5765f67fd..be14067db 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include #include @@ -27,15 +28,11 @@ namespace gtsam { template class RegularHessianFactor: public HessianFactor { -private: - - // Use Eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - public: + typedef Eigen::Matrix VectorD; + typedef Eigen::Matrix MatrixD; + /** Construct an n-way factor. Gs contains the upper-triangle blocks of the * quadratic term (the Hessian matrix) provided in row-order, gs the pieces * of the linear vector term, and f the constant term. @@ -43,27 +40,68 @@ public: RegularHessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : HessianFactor(js, Gs, gs, f) { + checkInvariants(); } /** Construct a binary factor. Gxx are the upper-triangle blocks of the * quadratic term (the Hessian matrix), gx the pieces of the linear vector * term, and f the constant term. */ - RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, - const Vector& g1, const Matrix& G22, const Vector& g2, double f) : + RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12, + const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) { } + /** Construct a ternary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. + */ + RegularHessianFactor(Key j1, Key j2, Key j3, + const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1, + const MatrixD& G22, const MatrixD& G23, const VectorD& g2, + const MatrixD& G33, const VectorD& g3, double f) : + HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { + } + /** Constructor with an arbitrary number of keys and with the augmented information matrix * specified as a block matrix. */ template RegularHessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) : HessianFactor(keys, augmentedInformation) { + checkInvariants(); } + /// Construct from RegularJacobianFactor + RegularHessianFactor(const RegularJacobianFactor& jf) + : HessianFactor(jf) {} + + /// Construct from a GaussianFactorGraph + RegularHessianFactor(const GaussianFactorGraph& factors, + boost::optional scatter = boost::none) + : HessianFactor(factors, scatter) { + checkInvariants(); + } + +private: + + /// Check invariants after construction + void checkInvariants() { + if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D) + throw std::invalid_argument( + "RegularHessianFactor constructor was given non-regular factors"); + } + + // Use Eigen magic to access raw memory + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + // Scratch space for multiplyHessianAdd - mutable std::vector y; + // According to link below this is thread-safe. + // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe + mutable std::vector y_; + +public: /** y += alpha * A'*A*x */ virtual void multiplyHessianAdd(double alpha, const VectorValues& x, @@ -74,32 +112,32 @@ public: /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { - // Create a vector of temporary y values, corresponding to rows i - y.resize(size()); - BOOST_FOREACH(DVector & yi, y) + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) yi.setZero(); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - DVector xj(D); + // And fill the above temporary y_ values, to be added into yvalues after + VectorD xj(D); for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { Key key = keys_[j]; const double* xj = x + key * D; DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() * ConstDMap(xj); + y_[i] += info_(j, j).selfadjointView() * ConstDMap(xj); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(xj); } // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) { Key key = keys_[i]; - DMap(yvalues + key * D) += alpha * y[i]; + DMap(yvalues + key * D) += alpha * y_[i]; } } @@ -107,28 +145,27 @@ public: void multiplyHessianAdd(double alpha, const double* x, double* yvalues, std::vector offsets) const { - // Create a vector of temporary y values, corresponding to rows i - std::vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); + // Create a vector of temporary y_ values, corresponding to rows i + y_.resize(size()); + BOOST_FOREACH(VectorD & yi, y_) + yi.setZero(); // Accessing the VectorValues one by one is expensive // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after + // And fill the above temporary y_ values, to be added into yvalues after for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { DenseIndex i = 0; for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() + y_[i] += info_(j, j).selfadjointView() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); // for below diagonal, we take transpose block from upper triangular part for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() + y_[i] += info_(i, j).knownOffDiagonal() * ConstDMap(x + offsets[keys_[j]], offsets[keys_[j] + 1] - offsets[keys_[j]]); } @@ -136,7 +173,7 @@ public: // copy to yvalues for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) DMap(yvalues + offsets[keys_[i]], - offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i]; + offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i]; } /** Return the diagonal of the Hessian for this factor (raw memory version) */ @@ -158,7 +195,7 @@ public: for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) { Key j = keys_[pos]; // Get the diagonal block, and insert its diagonal - DVector dj = -info_(pos, size()).knownOffDiagonal(); + VectorD dj = -info_(pos, size()).knownOffDiagonal(); DMap(d + D * j) += dj; } } diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index e2b8ac3cf..efdef9d44 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -29,30 +30,58 @@ using namespace gtsam; using namespace boost::assign; /* ************************************************************************* */ -TEST(RegularHessianFactor, ConstructorNWay) +TEST(RegularHessianFactor, Constructors) { - Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); - Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); - Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); + // First construct a regular JacobianFactor + Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; + Vector2 b(1,2); + vector > terms; + terms += make_pair(0, A1), make_pair(1, A2), make_pair(3, A3); + RegularJacobianFactor<2> jf(terms, b); - Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); - Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); + // Test conversion from JacobianFactor + RegularHessianFactor<2> factor(jf); - Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); + Matrix G11 = I_2x2; + Matrix G12 = I_2x2; + Matrix G13 = I_2x2; - Vector g1 = (Vector(2) << -7, -9).finished(); - Vector g2 = (Vector(2) << -9, 1).finished(); - Vector g3 = (Vector(2) << 2, 3).finished(); + Matrix G22 = I_2x2; + Matrix G23 = I_2x2; + + Matrix G33 = I_2x2; + + Vector2 g1 = b, g2 = b, g3 = b; double f = 10; - std::vector js; - js.push_back(0); js.push_back(1); js.push_back(3); - 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); - std::vector gs; - gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); - RegularHessianFactor<2> factor(js, Gs, gs, f); + // Test ternary constructor + RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); + EXPECT(assert_equal(factor,factor2)); + + // Test n-way constructor + vector 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); + EXPECT(assert_equal(factor, factor3)); + + // Test constructor from Gaussian Factor Graph + GaussianFactorGraph gfg; + gfg += jf; + RegularHessianFactor<2> factor4(gfg); + EXPECT(assert_equal(factor, factor4)); + GaussianFactorGraph gfg2; + gfg2 += factor; + RegularHessianFactor<2> factor5(gfg); + EXPECT(assert_equal(factor, factor5)); + + // Test constructor from Information matrix + Matrix info = factor.augmentedInformation(); + vector dims; dims += 2, 2, 2; + SymmetricBlockMatrix sym(dims, info, true); + RegularHessianFactor<2> factor6(keys, sym); + EXPECT(assert_equal(factor, factor6)); // multiplyHessianAdd: { @@ -61,13 +90,13 @@ TEST(RegularHessianFactor, ConstructorNWay) HessianFactor::const_iterator i1 = factor.begin(); HessianFactor::const_iterator i2 = i1 + 1; Vector X(6); X << 1,2,3,4,5,6; - Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + Vector Y(6); Y << 9, 12, 9, 12, 9, 12; EXPECT(assert_equal(Y,AtA*X)); VectorValues x = map_list_of - (0, (Vector(2) << 1,2).finished()) - (1, (Vector(2) << 3,4).finished()) - (3, (Vector(2) << 5,6).finished()); + (0, Vector2(1,2)) + (1, Vector2(3,4)) + (3, Vector2(5,6)); VectorValues expected; expected.insert(0, Y.segment<2>(0)); @@ -77,15 +106,15 @@ TEST(RegularHessianFactor, ConstructorNWay) // VectorValues version double alpha = 1.0; VectorValues actualVV; - actualVV.insert(0, zero(2)); - actualVV.insert(1, zero(2)); - actualVV.insert(3, zero(2)); + actualVV.insert(0, Vector2::Zero()); + actualVV.insert(1, Vector2::Zero()); + actualVV.insert(3, Vector2::Zero()); factor.multiplyHessianAdd(alpha, x, actualVV); EXPECT(assert_equal(expected, actualVV)); // RAW ACCESS - Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; - Vector fast_y = gtsam::zero(8); + Vector expected_y(8); expected_y << 9, 12, 9, 12, 0, 0, 9, 12; + Vector fast_y = Vector8::Zero(); double xvalues[8] = {1,2,3,4,0,0,5,6}; factor.multiplyHessianAdd(alpha, xvalues, fast_y.data()); EXPECT(assert_equal(expected_y, fast_y)); From b91ed6dc6ee404f1832deab575706dc1c0b82a3f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 16 Jun 2015 21:17:22 -0700 Subject: [PATCH 02/14] Got rid of warning --- gtsam/slam/SmartFactorBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index cdbe71718..c448dbed4 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -643,7 +643,6 @@ public: // gs = F' * (b - E * P * E' * b) MatrixDD matrixBlock; - typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix FastMap KeySlotMap; for (size_t slot = 0; slot < allKeys.size(); slot++) From 8e1b4cc3be0f8f364091596bc275103f9725758d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 12:16:57 -0700 Subject: [PATCH 03/14] Fixed bug in test --- gtsam/slam/tests/testRegularHessianFactor.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp index efdef9d44..6457e45fe 100644 --- a/gtsam/slam/tests/testRegularHessianFactor.cpp +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -33,6 +33,7 @@ using namespace boost::assign; TEST(RegularHessianFactor, Constructors) { // First construct a regular JacobianFactor + // 0.5*|x0 + x1 + x3 - [1;2]|^2 = 0.5*|A*x-b|^2, with A=[I I I] Matrix A1 = I_2x2, A2 = I_2x2, A3 = I_2x2; Vector2 b(1,2); vector > terms; @@ -42,6 +43,9 @@ TEST(RegularHessianFactor, Constructors) // Test conversion from JacobianFactor RegularHessianFactor<2> factor(jf); + // 0.5*|A*x-b|^2 = 0.5*(Ax-b)'*(Ax-b) = 0.5*x'*A'A*x - x'*A'b + 0.5*b'*b + // Compare with comment in HessianFactor: E(x) = 0.5 x^T G x - x^T g + 0.5 f + // Hence G = I6, g A'*b = [b;b;b], and f = b'*b = 1+4 = 5 Matrix G11 = I_2x2; Matrix G12 = I_2x2; Matrix G13 = I_2x2; @@ -53,7 +57,7 @@ TEST(RegularHessianFactor, Constructors) Vector2 g1 = b, g2 = b, g3 = b; - double f = 10; + double f = 5; // Test ternary constructor RegularHessianFactor<2> factor2(0, 1, 3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f); From 5719ba27fad1013d45ac921bf2e26f4a0e4332c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:28:25 -0700 Subject: [PATCH 04/14] Reducing header bloat by introducing new ThreadSafeException header. --- gtsam/base/FastSet.h | 9 +- gtsam/base/FastVector.h | 47 +++--- gtsam/base/SymmetricBlockMatrix.cpp | 1 + gtsam/base/SymmetricBlockMatrix.h | 23 ++- gtsam/base/TestableAssertions.h | 11 +- gtsam/base/ThreadsafeException.h | 152 ++++++++++++++++++ .../treeTraversal/parallelTraversalTasks.h | 1 + gtsam/base/types.h | 108 +------------ gtsam/geometry/CalibratedCamera.h | 5 + gtsam/inference/VariableIndex-inl.h | 2 + gtsam/inference/VariableIndex.h | 13 +- gtsam/inference/tests/testKey.cpp | 1 + gtsam/linear/HessianFactor.cpp | 18 ++- gtsam/linear/SubgraphPreconditioner.cpp | 35 +++- gtsam/linear/SubgraphPreconditioner.h | 21 ++- gtsam/linear/linearExceptions.cpp | 1 + gtsam/linear/linearExceptions.h | 5 +- tests/testMarginals.cpp | 3 +- 18 files changed, 282 insertions(+), 174 deletions(-) create mode 100644 gtsam/base/ThreadsafeException.h diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 73df17b0d..c3352dfd5 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -19,15 +19,18 @@ #pragma once #include + #include #include #include #include #include -#include -#include -#include + #include +#include +#include +#include +#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index d154ea52a..0a4932e01 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -18,12 +18,9 @@ #pragma once -#include +#include #include -#include - -#include -#include +#include namespace gtsam { @@ -35,20 +32,27 @@ namespace gtsam { * @addtogroup base */ template -class FastVector: public std::vector::type> { +class FastVector: public std::vector::type> { public: - typedef std::vector::type> Base; + typedef std::vector::type> Base; /** Default constructor */ - FastVector() {} + FastVector() { + } /** Constructor from size */ - explicit FastVector(size_t size) : Base(size) {} + explicit FastVector(size_t size) : + Base(size) { + } /** Constructor from size and initial values */ - explicit FastVector(size_t size, const VALUE& initial) : Base(size, initial) {} + explicit FastVector(size_t size, const VALUE& initial) : + Base(size, initial) { + } /** Constructor from a range, passes through to base class */ template @@ -56,33 +60,22 @@ public: // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(first != last) + if (first != last) Base::assign(first, last); } - /** Copy constructor from a FastList */ - FastVector(const FastList& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - - /** Copy constructor from a FastSet */ - FastVector(const FastSet& x) { - if(x.size() > 0) - Base::assign(x.begin(), x.end()); - } - /** Copy constructor from the base class */ - FastVector(const Base& x) : Base(x) {} + FastVector(const Base& x) : + Base(x) { + } /** Copy constructor from a standard STL container */ template - FastVector(const std::vector& x) - { + FastVector(const std::vector& x) { // This if statement works around a bug in boost pool allocator and/or // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. - if(x.size() > 0) + if (x.size() > 0) Base::assign(x.begin(), x.end()); } diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 0fbdfeefc..7cca63092 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 41584c7f9..1f81ca1f9 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -17,9 +17,21 @@ */ #pragma once -#include #include +#include #include +#include +#include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { @@ -247,13 +259,8 @@ namespace gtsam { } }; - /* ************************************************************************* */ - class CholeskyFailed : public gtsam::ThreadsafeException - { - public: - CholeskyFailed() throw() {} - virtual ~CholeskyFailed() throw() {} - }; + /// Foward declare exception class + class CholeskyFailed; } diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index 04d3fc676..f976be0e7 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -17,13 +17,14 @@ #pragma once -#include -#include -#include +#include +#include + #include #include -#include -#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h new file mode 100644 index 000000000..3bdd42608 --- /dev/null +++ b/gtsam/base/ThreadsafeException.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ThreadSafeException.h + * @brief Base exception type that uses tbb_exception if GTSAM is compiled with TBB + * @author Richard Roberts + * @date Aug 21, 2010 + * @addtogroup base + */ + +#pragma once + +#include +#include +#include +#include + +#ifdef GTSAM_USE_TBB +#include +#include +#endif + +namespace gtsam { + +/// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. +template +class ThreadsafeException: +#ifdef GTSAM_USE_TBB + public tbb::tbb_exception +#else +public std::exception +#endif +{ +#ifdef GTSAM_USE_TBB +private: + typedef tbb::tbb_exception Base; +protected: + typedef std::basic_string, + tbb::tbb_allocator > String; +#else +private: + typedef std::exception Base; +protected: + typedef std::string String; +#endif + +protected: + bool dynamic_; ///< Whether this object was moved + mutable boost::optional description_; ///< Optional description + + /// Default constructor is protected - may only be created from derived classes + ThreadsafeException() : + dynamic_(false) { + } + + /// Copy constructor is protected - may only be created from derived classes + ThreadsafeException(const ThreadsafeException& other) : + Base(other), dynamic_(false) { + } + + /// Construct with description string + ThreadsafeException(const std::string& description) : + dynamic_(false), description_( + String(description.begin(), description.end())) { + } + + /// Default destructor doesn't have the throw() + virtual ~ThreadsafeException() throw () { + } + +public: + // Implement functions for tbb_exception +#ifdef GTSAM_USE_TBB + virtual tbb::tbb_exception* move() throw () { + void* cloneMemory = scalable_malloc(sizeof(DERIVED)); + if (!cloneMemory) { + std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; + exit(-1); + } + DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); + clone->dynamic_ = true; + return clone; + } + + virtual void destroy() throw () { + if (dynamic_) { + DERIVED* derivedPtr = static_cast(this); + derivedPtr->~DERIVED(); + scalable_free(derivedPtr); + } + } + + virtual void throw_self() { + throw *static_cast(this); + } + + virtual const char* name() const throw () { + return typeid(DERIVED).name(); + } +#endif + + virtual const char* what() const throw () { + return description_ ? description_->c_str() : ""; + } +}; + +/// Thread-safe runtime error exception +class RuntimeErrorThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + RuntimeErrorThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe out of range exception +class OutOfRangeThreadsafe: public ThreadsafeException { +public: + /// Construct with a string describing the exception + OutOfRangeThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Thread-safe invalid argument exception +class InvalidArgumentThreadsafe: public ThreadsafeException< + InvalidArgumentThreadsafe> { +public: + /// Construct with a string describing the exception + InvalidArgumentThreadsafe(const std::string& description) : + ThreadsafeException(description) { + } +}; + +/// Indicate Cholesky factorization failure +class CholeskyFailed : public gtsam::ThreadsafeException +{ +public: + CholeskyFailed() throw() {} + virtual ~CholeskyFailed() throw() {} +}; + +} // namespace gtsam diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 7986f9534..c1df47a01 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -24,6 +24,7 @@ #ifdef GTSAM_USE_TBB # include +# include # undef max // TBB seems to include windows.h and we don't want these macros # undef min # undef ERROR diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a66db13c8..a325fb1ad 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,19 +20,16 @@ #pragma once #include -#include -#include +#include +#include #include -#include +#include #include -#include -#include #ifdef GTSAM_USE_TBB #include #include #include -#include #endif #ifdef GTSAM_USE_EIGEN_MKL_OPENMP @@ -73,7 +70,6 @@ namespace gtsam { /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; - /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on @@ -148,104 +144,6 @@ namespace gtsam { return ListOfOneContainer(element); } - /* ************************************************************************* */ - /// Base exception type that uses tbb_exception if GTSAM is compiled with TBB. - template - class ThreadsafeException : -#ifdef GTSAM_USE_TBB - public tbb::tbb_exception -#else - public std::exception -#endif - { -#ifdef GTSAM_USE_TBB - private: - typedef tbb::tbb_exception Base; - protected: - typedef std::basic_string, tbb::tbb_allocator > String; -#else - private: - typedef std::exception Base; - protected: - typedef std::string String; -#endif - - protected: - bool dynamic_; ///< Whether this object was moved - mutable boost::optional description_; ///< Optional description - - /// Default constructor is protected - may only be created from derived classes - ThreadsafeException() : dynamic_(false) {} - - /// Copy constructor is protected - may only be created from derived classes - ThreadsafeException(const ThreadsafeException& other) : Base(other), dynamic_(false) {} - - /// Construct with description string - ThreadsafeException(const std::string& description) : dynamic_(false), description_(String(description.begin(), description.end())) {} - - /// Default destructor doesn't have the throw() - virtual ~ThreadsafeException() throw () {} - - public: - // Implement functions for tbb_exception -#ifdef GTSAM_USE_TBB - virtual tbb::tbb_exception* move() throw () { - void* cloneMemory = scalable_malloc(sizeof(DERIVED)); - if (!cloneMemory) { - std::cerr << "Failed allocating memory to copy thrown exception, exiting now." << std::endl; - exit(-1); - } - DERIVED* clone = ::new(cloneMemory) DERIVED(static_cast(*this)); - clone->dynamic_ = true; - return clone; - } - - virtual void destroy() throw() { - if (dynamic_) { - DERIVED* derivedPtr = static_cast(this); - derivedPtr->~DERIVED(); - scalable_free(derivedPtr); - } - } - - virtual void throw_self() { - throw *static_cast(this); } - - virtual const char* name() const throw() { - return typeid(DERIVED).name(); } -#endif - - virtual const char* what() const throw() { - return description_ ? description_->c_str() : ""; } - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class RuntimeErrorThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - RuntimeErrorThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe runtime error exception - class OutOfRangeThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - OutOfRangeThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - - /* ************************************************************************* */ - /// Threadsafe invalid argument exception - class InvalidArgumentThreadsafe : public ThreadsafeException - { - public: - /// Construct with a string describing the exception - InvalidArgumentThreadsafe(const std::string& description) : ThreadsafeException(description) {} - }; - /* ************************************************************************* */ #ifdef __clang__ # pragma clang diagnostic push diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f17cc6441..0b278bade 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -19,6 +19,11 @@ #pragma once #include +#include +#include +#include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index 82eb85c76..c00a3633e 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -18,6 +18,8 @@ #pragma once #include +#include +#include namespace gtsam { diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index bcaec6ee4..d4b8eeacf 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,17 +17,18 @@ #pragma once -#include #include #include -#include +#include #include -#include +#include +#include -#include +#include +#include +#include -#include -#include +#include #include namespace gtsam { diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 1033c0cc9..b0708e660 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,6 +14,7 @@ * @author Alex Cunningham */ +#include #include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 21f4b117f..f3e54cffb 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -15,17 +15,19 @@ * @date Dec 8, 2010 */ -#include +#include + #include #include -#include -#include #include -#include -#include -#include -#include +#include +#include #include +#include +#include +#include +#include +#include #include #include @@ -469,7 +471,7 @@ std::pair, // Erase the eliminated keys in the remaining factor jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate); - } catch (CholeskyFailed&) { + } catch (const CholeskyFailed& e) { throw IndeterminantLinearSystemException(keys.front()); } diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index f5ee4ddfc..e0bd8d8cd 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,20 +15,41 @@ * @author: Frank Dellaert */ -#include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include #include +#include +#include +#include +#include +#include + +#include +#include +#include #include +#include #include -#include +#include +#include +#include +#include // accumulate #include #include +#include +#include #include #include diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 350963bcf..c1600dd45 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -17,13 +17,32 @@ #pragma once -#include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include + +#include #include +#include +#include +#include +#include + +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ + namespace gtsam { // Forward declarations diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 88758ae7a..a4168af2d 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/linear/linearExceptions.h b/gtsam/linear/linearExceptions.h index 63bc61e80..32db27fc9 100644 --- a/gtsam/linear/linearExceptions.h +++ b/gtsam/linear/linearExceptions.h @@ -17,9 +17,8 @@ */ #pragma once -#include -#include -#include +#include +#include namespace gtsam { diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index dd22ae738..0af4c4a57 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -226,7 +226,8 @@ TEST(Marginals, order) { vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); - FastVector keys(fg.keys()); + FastSet set = fg.keys(); + FastVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); LONGS_EQUAL(3, (long)joint(0,0).rows()); From 63c28f89bfa0f959bdc3f1616bef8a9ecedd6b51 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:37:11 -0700 Subject: [PATCH 05/14] Removed system-specific header --- gtsam/base/types.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a325fb1ad..4010eb70e 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #ifdef GTSAM_USE_TBB From 8ba8a0ff947e0e0ac85ffa49b54a23cdef74402f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 15:47:30 -0700 Subject: [PATCH 06/14] Removed system specific header (damn eclipse organize includes!) --- gtsam/linear/SubgraphPreconditioner.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index c1600dd45..e74b92df1 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -32,7 +32,6 @@ #include #include -#include #include #include #include From d2bf0120fc5c2740c2bce439348ba75c470bf93b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 20 Jun 2015 15:50:57 -0700 Subject: [PATCH 07/14] And another one ! --- gtsam/linear/SubgraphPreconditioner.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index e0bd8d8cd..ee2447d2f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include From e0afc0e05c63e87bd9e855bd97a0d36990e849c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 18:36:03 -0700 Subject: [PATCH 08/14] Removed mpl-based Testable scheme with GTSAM 4 traits. This means you now have to be Testable to be a "FastSet". Future plan is to completely get rid of ths data structure, which is probably all but fast. --- gtsam/base/FastSet.h | 122 ++++++++++++++----------------------------- 1 file changed, 39 insertions(+), 83 deletions(-) diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index c3352dfd5..8c23ae9e5 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,29 +18,22 @@ #pragma once -#include - -#include -#include -#include #include #include +#include +#include -#include -#include -#include +#include #include -#include -BOOST_MPL_HAS_XXX_TRAIT_DEF(print) +namespace boost { +namespace serialization { +class access; +} /* namespace serialization */ +} /* namespace boost */ namespace gtsam { -// This is used internally to allow this container to be Testable even when it -// contains non-testable elements. -template -struct FastSetTestableHelper; - /** * FastSet is a thin wrapper around std::set that uses the boost * fast_pool_allocator instead of the default STL allocator. This is just a @@ -48,12 +41,16 @@ struct FastSetTestableHelper; * we've seen that the fast_pool_allocator can lead to speedups of several %. * @addtogroup base */ -template -class FastSet: public std::set, typename internal::FastDefaultAllocator::type> { +template +class FastSet: public std::set, + typename internal::FastDefaultAllocator::type> { + + BOOST_CONCEPT_ASSERT ((IsTestable )); public: - typedef std::set, typename internal::FastDefaultAllocator::type> Base; + typedef std::set, + typename internal::FastDefaultAllocator::type> Base; /** Default constructor */ FastSet() { @@ -62,23 +59,23 @@ public: /** Constructor from a range, passes through to base class */ template explicit FastSet(INPUTITERATOR first, INPUTITERATOR last) : - Base(first, last) { + Base(first, last) { } /** Constructor from a iterable container, passes through to base class */ template explicit FastSet(const INPUTCONTAINER& container) : - Base(container.begin(), container.end()) { + Base(container.begin(), container.end()) { } /** Copy constructor from another FastSet */ FastSet(const FastSet& x) : - Base(x) { + Base(x) { } /** Copy constructor from the base set class */ FastSet(const Base& x) : - Base(x) { + Base(x) { } #ifdef GTSAM_ALLOCATOR_BOOSTPOOL @@ -88,7 +85,7 @@ public: // STL vector where if the size is zero, the pool allocator will allocate // huge amounts of memory. if(x.size() > 0) - Base::insert(x.begin(), x.end()); + Base::insert(x.begin(), x.end()); } #endif @@ -98,17 +95,31 @@ public: } /** Handy 'exists' function */ - bool exists(const VALUE& e) const { return this->find(e) != this->end(); } + bool exists(const VALUE& e) const { + return this->find(e) != this->end(); + } - /** Print to implement Testable */ - void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } + /** Print to implement Testable: pretty basic */ + void print(const std::string& str = "") const { + for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it) + traits::Print(*it, str); + } /** Check for equality within tolerance to implement Testable */ - bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + bool equals(const FastSet& other, double tol = 1e-9) const { + typename Base::const_iterator it1 = this->begin(), it2 = other.begin(); + while (it1 != this->end()) { + if (it2 == other.end() || !traits::Equals(*it2, *it2, tol)) + return false; + ++it1; + ++it2; + } + return true; + } /** insert another set: handy for MATLAB access */ void merge(const FastSet& other) { - Base::insert(other.begin(),other.end()); + Base::insert(other.begin(), other.end()); } private: @@ -120,59 +131,4 @@ private: } }; -// This is the default Testable interface for *non*Testable elements, which -// uses stream operators. -template -struct FastSetTestableHelper { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - std::cout << " " << *it << "\n"; - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - fabs((double)(*it1) - (double)(*it2)) > tol) - return false; - ++it1; - ++it2; - } - return true; - } -}; - -// This is the Testable interface for Testable elements -template -struct FastSetTestableHelper >::type> { - - typedef FastSet Set; - - static void print(const Set& set, const std::string& str) { - std::cout << str << "\n"; - for (typename Set::const_iterator it = set.begin(); it != set.end(); ++it) - it->print(" "); - std::cout.flush(); - } - - static bool equals(const Set& set1, const Set& set2, double tol) { - typename Set::const_iterator it1 = set1.begin(); - typename Set::const_iterator it2 = set2.begin(); - while (it1 != set1.end()) { - if (it2 == set2.end() || - !it1->equals(*it2, tol)) - return false; - ++it1; - ++it2; - } - return true; - } -}; - } From d1be7caed5f15c1b78b024fe3c8f48d466f05d24 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 18:37:25 -0700 Subject: [PATCH 09/14] Made Key Testable, and moved all but base type away from types.h --- gtsam/base/types.cpp | 35 ---------------- gtsam/base/types.h | 13 ------ gtsam/inference/Key.cpp | 55 ++++++++++++++++--------- gtsam/inference/Key.h | 90 +++++++++++++++++++++++++++-------------- 4 files changed, 95 insertions(+), 98 deletions(-) delete mode 100644 gtsam/base/types.cpp diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp deleted file mode 100644 index 03e7fd120..000000000 --- a/gtsam/base/types.cpp +++ /dev/null @@ -1,35 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file types.h - * @brief Typedefs for easier changing of types - * @author Richard Roberts - * @date Aug 21, 2010 - * @addtogroup base - */ - -#include -#include -#include - -namespace gtsam { - - /* ************************************************************************* */ - std::string _defaultKeyFormatter(Key key) { - const Symbol asSymbol(key); - if(asSymbol.chr() > 0) - return (std::string)asSymbol; - else - return boost::lexical_cast(key); - } - -} \ No newline at end of file diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 4010eb70e..69e4e4192 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -54,18 +53,6 @@ namespace gtsam { /// Integer nonlinear key type typedef size_t Key; - /// Typedef for a function to format a key, i.e. to convert it to a string - typedef boost::function KeyFormatter; - - // Helper function for DefaultKeyFormatter - GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - - /// The default KeyFormatter, which is used if no KeyFormatter is passed to - /// a nonlinear 'print' function. Automatically detects plain integer keys - /// and Symbol keys. - static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - - /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 0b9be2f1c..a2a6906d1 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -17,57 +17,72 @@ * @date Feb 20, 2012 */ -#include - -#include -#include - #include #include +#include +#include +#include + +using namespace std; + namespace gtsam { /* ************************************************************************* */ -std::string _multirobotKeyFormatter(Key key) { +string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if (asSymbol.chr() > 0) + return (string) asSymbol; + else + return boost::lexical_cast(key); +} + +/* ************************************************************************* */ +void PrintKey(Key key, const string& s, const KeyFormatter& keyFormatter) { + cout << s << keyFormatter(key); +} + +/* ************************************************************************* */ +string _multirobotKeyFormatter(Key key) { const LabeledSymbol asLabeledSymbol(key); if (asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0) - return (std::string) asLabeledSymbol; + return (string) asLabeledSymbol; const Symbol asSymbol(key); if (asLabeledSymbol.chr() > 0) - return (std::string) asSymbol; + return (string) asSymbol; else - return boost::lexical_cast(key); + return boost::lexical_cast(key); } /* ************************************************************************* */ template -static void print(const CONTAINER& keys, const std::string& s, +static void Print(const CONTAINER& keys, const string& s, const KeyFormatter& keyFormatter) { - std::cout << s << " "; + cout << s << " "; if (keys.empty()) - std::cout << "(none)" << std::endl; + cout << "(none)" << endl; else { BOOST_FOREACH(const Key& key, keys) - std::cout << keyFormatter(key) << " "; - std::cout << std::endl; + cout << keyFormatter(key) << " "; + cout << endl; } } /* ************************************************************************* */ -void printKeyList(const KeyList& keys, const std::string& s, +void PrintKeyList(const KeyList& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeyVector(const KeyVector& keys, const std::string& s, +void PrintKeyVector(const KeyVector& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ -void printKeySet(const KeySet& keys, const std::string& s, +void PrintKeySet(const KeySet& keys, const string& s, const KeyFormatter& keyFormatter) { - print(keys, s, keyFormatter); + Print(keys, s, keyFormatter); } /* ************************************************************************* */ diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index e2be79dc7..d2b342575 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -17,45 +17,75 @@ */ #pragma once -#include -#include - -#include -#include #include -#include #include +#include +#include +#include +#include +#include + +#include namespace gtsam { +/// Typedef for a function to format a key, i.e. to convert it to a string +typedef boost::function KeyFormatter; - // Helper function for Multi-robot Key Formatter - GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); +// Helper function for DefaultKeyFormatter +GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); - /// - /// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain - /// integer keys. This keyformatter will need to be passed in to override the default - /// formatter in print functions. - /// - /// Checks for LabeledSymbol, Symbol and then plain keys, in order. - static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter; +/// The default KeyFormatter, which is used if no KeyFormatter is passed to +/// a nonlinear 'print' function. Automatically detects plain integer keys +/// and Symbol keys. +static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - /// Useful typedefs for operations with Values - allow for matlab interfaces - typedef FastList KeyList; - typedef FastVector KeyVector; - typedef FastSet KeySet; - typedef FastMap KeyGroupMap; +// Helper function for Multi-robot Key Formatter +GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyList(const KeyList& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// +/// A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain +/// integer keys. This keyformatter will need to be passed in to override the default +/// formatter in print functions. +/// +/// Checks for LabeledSymbol, Symbol and then plain keys, in order. +static const gtsam::KeyFormatter MultiRobotKeyFormatter = + &_multirobotKeyFormatter; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeyVector(const KeyVector& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); +/// Useful typedef for operations with Values - allows for matlab interface +typedef FastVector KeyVector; - /// Utility function to print sets of keys with optional prefix - GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter); -} +// TODO(frank): Nothing fast about these :-( +typedef FastList KeyList; +typedef FastSet KeySet; +typedef FastMap KeyGroupMap; + +/// Utility function to print one key with optional prefix +GTSAM_EXPORT void PrintKey(Key key, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyList(const KeyList& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeyVector(const KeyVector& keys, const std::string& s = + "", const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +/// Utility function to print sets of keys with optional prefix +GTSAM_EXPORT void PrintKeySet(const KeySet& keys, const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter); + +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const Key& key, const std::string& str = "") { + PrintKey(key, str); + } + static bool Equals(const Key& key1, const Key& key2, double tol = 1e-8) { + return key1 == key2; + } +}; + +} // namespace gtsam From 128bac616cfd6753db3624cc8d87ed49aa2d898a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 20 Jun 2015 18:38:25 -0700 Subject: [PATCH 10/14] Globally replaced FastSet with KeySet. --- gtsam/base/tests/testFastContainers.cpp | 9 +++-- gtsam/base/tests/testSerializationBase.cpp | 10 +++-- gtsam/discrete/DiscreteFactorGraph.cpp | 4 +- gtsam/discrete/DiscreteFactorGraph.h | 2 +- gtsam/discrete/Potentials.h | 1 + gtsam/inference/BayesTree-inst.h | 4 +- gtsam/inference/BayesTree.h | 6 +-- gtsam/inference/BayesTreeCliqueBase-inst.h | 8 ++-- gtsam/inference/FactorGraph-inst.h | 5 ++- gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/ISAM-inst.h | 4 +- gtsam/inference/MetisIndex-inl.h | 4 +- gtsam/inference/Ordering.h | 2 +- gtsam/inference/VariableIndex.h | 5 +-- gtsam/linear/GaussianFactorGraph.cpp | 2 +- gtsam/linear/GaussianFactorGraph.h | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 28 +++++++------- gtsam/nonlinear/ISAM2-impl.h | 18 ++++----- gtsam/nonlinear/ISAM2-inl.h | 12 +++--- gtsam/nonlinear/ISAM2.cpp | 38 +++++++++---------- gtsam/nonlinear/ISAM2.h | 16 ++++---- gtsam/nonlinear/NonlinearFactorGraph.cpp | 8 ++-- gtsam/nonlinear/NonlinearFactorGraph.h | 2 +- gtsam/symbolic/SymbolicFactor-inst.h | 4 +- .../discrete/tests/testLoopyBelief.cpp | 2 +- gtsam_unstable/linear/QPSolver.h | 2 +- .../nonlinear/BatchFixedLagSmoother.cpp | 6 +-- .../nonlinear/BatchFixedLagSmoother.h | 2 +- .../nonlinear/ConcurrentBatchFilter.cpp | 6 +-- .../nonlinear/ConcurrentBatchSmoother.cpp | 2 +- .../ConcurrentFilteringAndSmoothing.cpp | 8 ++-- .../ConcurrentFilteringAndSmoothing.h | 2 +- .../nonlinear/ConcurrentIncrementalFilter.cpp | 6 +-- .../ConcurrentIncrementalSmoother.cpp | 2 +- .../tests/testConcurrentBatchSmoother.cpp | 2 +- .../testConcurrentIncrementalSmootherDL.cpp | 2 +- .../testConcurrentIncrementalSmootherGN.cpp | 2 +- gtsam_unstable/slam/MultiProjectionFactor.h | 4 +- .../slam/tests/testMultiProjectionFactor.cpp | 2 +- matlab.h | 8 ++-- tests/testGaussianISAM2.cpp | 4 +- tests/testMarginals.cpp | 2 +- tests/testNonlinearFactorGraph.cpp | 4 +- 43 files changed, 133 insertions(+), 131 deletions(-) diff --git a/gtsam/base/tests/testFastContainers.cpp b/gtsam/base/tests/testFastContainers.cpp index 464624bd4..19870fdeb 100644 --- a/gtsam/base/tests/testFastContainers.cpp +++ b/gtsam/base/tests/testFastContainers.cpp @@ -7,13 +7,14 @@ * @author Alex Cunningham */ -#include +#include +#include +#include #include #include -#include -#include +#include using namespace boost::assign; using namespace gtsam; @@ -21,7 +22,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST( testFastContainers, KeySet ) { - FastVector init_vector; + FastVector init_vector; init_vector += 2, 3, 4, 5; FastSet actSet(init_vector); diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp index fceb2f4b4..1db28bcc8 100644 --- a/gtsam/base/tests/testSerializationBase.cpp +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -16,6 +16,8 @@ * @date Feb 7, 2012 */ +#include + #include #include #include @@ -60,10 +62,10 @@ TEST (Serialization, FastMap) { /* ************************************************************************* */ TEST (Serialization, FastSet) { - FastSet set; - set.insert(1.0); - set.insert(2.0); - set.insert(3.0); + KeySet set; + set.insert(1); + set.insert(2); + set.insert(3); EXPECT(equality(set)); EXPECT(equalityXML(set)); diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 77be1d277..c2128c776 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -39,8 +39,8 @@ namespace gtsam { } /* ************************************************************************* */ - FastSet DiscreteFactorGraph::keys() const { - FastSet keys; + KeySet DiscreteFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); return keys; diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index c5b11adf1..cdfd7d522 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -120,7 +120,7 @@ public: } /** Return the set of variables involved in the factors (set union) */ - FastSet keys() const; + KeySet keys() const; /** return product of all factors as a single factor */ DecisionTreeFactor product() const; diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 978781d63..1078b4c61 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 281648409..3a3e1317c 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -344,7 +344,7 @@ namespace gtsam { gttic(Full_root_factoring); boost::shared_ptr p_C1_B; { FastVector C1_minus_B; { - FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); + KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C1_minus_B_set.erase(j); } C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); @@ -356,7 +356,7 @@ namespace gtsam { } boost::shared_ptr p_C2_B; { FastVector C2_minus_B; { - FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); + KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Key j, *B->conditional()) { C2_minus_B_set.erase(j); } C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index ff50c9781..4d68acb5b 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -19,13 +19,13 @@ #pragma once -#include - -#include +#include #include #include #include +#include + namespace gtsam { // Forward declarations diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 274886c21..256ff983d 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -44,8 +44,8 @@ namespace gtsam { FastVector BayesTreeCliqueBase::separator_setminus_B(const derived_ptr& B) const { - FastSet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents()); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B; std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); @@ -58,8 +58,8 @@ namespace gtsam { const derived_ptr& B, const FactorGraphType& p_Cp_B) const { gttic(shortcut_indices); - FastSet allKeys = p_Cp_B.keys(); - FastSet indicesB(B->conditional()->begin(), B->conditional()->end()); + KeySet allKeys = p_Cp_B.keys(); + KeySet indicesB(B->conditional()->begin(), B->conditional()->end()); FastVector S_setminus_B = separator_setminus_B(B); FastVector keep; // keep = S\B intersect allKeys (S_setminus_B is already sorted) diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 8c19d4ff4..c629d336a 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -28,6 +28,7 @@ #include #include +#include // for cout :-( namespace gtsam { @@ -72,8 +73,8 @@ namespace gtsam { /* ************************************************************************* */ template - FastSet FactorGraph::keys() const { - FastSet allKeys; + KeySet FactorGraph::keys() const { + KeySet allKeys; BOOST_FOREACH(const sharedFactor& factor, factors_) if (factor) allKeys.insert(factor->begin(), factor->end()); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 4d5428e5c..e97860eaa 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -332,7 +332,7 @@ namespace gtsam { size_t nrFactors() const; /** Potentially very slow function to return all keys involved */ - FastSet keys() const; + KeySet keys() const; /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */ inline bool exists(size_t idx) const { return idx < size() && at(idx); } diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index 7cb3d9817..8be01ac5f 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -29,7 +29,7 @@ namespace gtsam { // Remove the contaminated part of the Bayes tree BayesNetType bn; if (!this->empty()) { - const FastSet newFactorKeys = newFactors.keys(); + const KeySet newFactorKeys = newFactors.keys(); this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); } @@ -44,7 +44,7 @@ namespace gtsam { // eliminate into a Bayes net const VariableIndex varIndex(factors); - const FastSet newFactorKeys = newFactors.keys(); + const KeySet newFactorKeys = newFactors.keys(); const Ordering constrainedOrdering = Ordering::colamdConstrainedLast(varIndex, std::vector(newFactorKeys.begin(), newFactorKeys.end())); Base bayesTree = *factors.eliminateMultifrontal(constrainedOrdering, function, varIndex); diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 7299d7c8a..06e5ddeec 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -26,8 +26,8 @@ namespace gtsam { /* ************************************************************************* */ template void MetisIndex::augment(const FactorGraph& factors) { - std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first - std::map >::iterator iAdjMapIt; + std::map > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first + std::map >::iterator iAdjMapIt; std::set keySet; /* ********** Convert to CSR format ********** */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 9de3fb66a..2d95442de 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -149,7 +149,7 @@ namespace gtsam { /// Return a natural Ordering. Typically used by iterative solvers template static Ordering Natural(const FactorGraph &fg) { - FastSet src = fg.keys(); + KeySet src = fg.keys(); std::vector keys(src.begin(), src.end()); std::stable_sort(keys.begin(), keys.end()); return Ordering(keys); diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index d4b8eeacf..f395fd4ab 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,14 +17,11 @@ #pragma once -#include +#include #include #include -#include -#include #include -#include #include #include diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index cd60a3eb9..a39b1d91e 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -47,7 +47,7 @@ namespace gtsam { /* ************************************************************************* */ GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { - FastSet keys; + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, *this) if (factor) keys.insert(factor->begin(), factor->end()); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 832114ff6..02bc95511 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -135,7 +135,7 @@ namespace gtsam { * Return the set of variables involved in the factors (computes a set * union). */ - typedef FastSet Keys; + typedef KeySet Keys; Keys keys() const; /* return a map of (Key, dimension) */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 41ee52b3a..6df1c8b10 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -84,11 +84,11 @@ void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool u } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, +void ISAM2::Impl::RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables) + KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables) { variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); BOOST_FOREACH(Key key, unusedKeys) { @@ -103,10 +103,10 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVect } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, +KeySet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; if(const double* threshold = boost::get(&relinearizeThreshold)) { @@ -131,7 +131,7 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, +void CheckRelinearizationRecursiveDouble(KeySet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { // Check the current clique for relinearization @@ -153,7 +153,7 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thresho } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, +void CheckRelinearizationRecursiveMap(KeySet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { @@ -185,11 +185,11 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, +KeySet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { - FastSet relinKeys; + KeySet relinKeys; BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); @@ -200,7 +200,7 @@ FastSet ISAM2::Impl::CheckRelinearizationPartial(const FastVector& keys, const FastSet& markedMask) +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask) { static const bool debug = false; // does the separator contain any of the variables? @@ -224,7 +224,7 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, co /* ************************************************************************* */ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) + const KeySet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions @@ -272,7 +272,7 @@ inline static void optimizeInPlace(const boost::shared_ptr& clique, /* ************************************************************************* */ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; @@ -300,7 +300,7 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector /* ************************************************************************* */ namespace internal { -void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +void updateRgProd(const boost::shared_ptr& clique, const KeySet& replacedKeys, const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need @@ -344,7 +344,7 @@ void updateRgProd(const boost::shared_ptr& clique, const FastSet& replacedKeys, +size_t ISAM2::Impl::UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd) { // Update variables diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index a8d03df13..d34480144 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -57,10 +57,10 @@ struct GTSAM_EXPORT ISAM2::Impl { /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, + static void RemoveVariables(const KeySet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, - FastSet& fixedVariables); + VectorValues& RgProd, KeySet& replacedKeys, Base::Nodes& nodes, + KeySet& fixedVariables); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -71,7 +71,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValues& delta, + static KeySet CheckRelinearizationFull(const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -85,7 +85,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const FastVector& roots, + static KeySet CheckRelinearizationPartial(const FastVector& roots, const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** @@ -103,7 +103,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, KeySet& keys, const KeySet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -119,7 +119,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void ExpmapMasked(Values& values, const VectorValues& delta, - const FastSet& mask, + const KeySet& mask, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); @@ -127,13 +127,13 @@ struct GTSAM_EXPORT ISAM2::Impl { * Update the Newton's method step point, using wildfire */ static size_t UpdateGaussNewtonDelta(const FastVector& roots, - const FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); + const KeySet& replacedKeys, VectorValues& delta, double wildfireThreshold); /** * Update the RgProd (R*g) incrementally taking into account which variables * have been recalculated in \c replacedKeys. Only used in Dogleg. */ - static size_t UpdateRgProd(const ISAM2::Roots& roots, const FastSet& replacedKeys, + static size_t UpdateRgProd(const ISAM2::Roots& roots, const KeySet& replacedKeys, const VectorValues& gradAtZero, VectorValues& RgProd); /** diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index f7c6d0345..538c66068 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -36,7 +36,7 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) + KeySet& changed, const KeySet& replaced, VectorValues& delta, size_t& count) { // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the @@ -245,8 +245,8 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) { - FastSet changed; +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { + KeySet changed; int count = 0; // starting from the root, call optimize on each conditional if(root) @@ -256,9 +256,9 @@ size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, /* ************************************************************************* */ template -size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const KeySet& keys, VectorValues& delta) { - FastSet changed; + KeySet changed; size_t count = 0; if (root) { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 00ffdccef..f7ef09773 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -194,7 +194,7 @@ FastSet ISAM2::getAffectedFactors(const FastList& keys) const { // (note that the remaining stuff is summarized in the cached factors) GaussianFactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const { gttic(getAffectedFactors); FastSet candidates = getAffectedFactors(affectedKeys); @@ -204,7 +204,7 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastS gttic(affectedKeysSet); // for fast lookup below - FastSet affectedKeysSet; + KeySet affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); gttoc(affectedKeysSet); @@ -260,9 +260,9 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } /* ************************************************************************* */ -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& relinKeys, +boost::shared_ptr ISAM2::recalculate(const KeySet& markedKeys, const KeySet& relinKeys, const vector& observedKeys, - const FastSet& unusedIndices, + const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result) { @@ -326,7 +326,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + boost::shared_ptr affectedKeysSet(new KeySet()); // Will return this result if(affectedKeys.size() >= theta_.size() * batchThreshold) { @@ -566,17 +566,17 @@ ISAM2Result ISAM2::update( variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); // Compute unused keys and indices - FastSet unusedKeys; - FastSet unusedIndices; + KeySet unusedKeys; + KeySet unusedIndices; { // Get keys from removed factors and new factors, and compute unused keys, // i.e., keys that are empty now and do not appear in the new factors. - FastSet removedAndEmpty; + KeySet removedAndEmpty; BOOST_FOREACH(Key key, removeFactors.keys()) { if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } - FastSet newFactorSymbKeys = newFactors.keys(); + KeySet newFactorSymbKeys = newFactors.keys(); std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end())); @@ -602,10 +602,10 @@ ISAM2Result ISAM2::update( gttic(gather_involved_keys); // 3. Mark linear update - FastSet markedKeys = newFactors.keys(); // Get keys from new factors + KeySet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors + KeySet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys @@ -632,7 +632,7 @@ ISAM2Result ISAM2::update( gttoc(gather_involved_keys); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - FastSet relinKeys; + KeySet relinKeys; if (relinearizeThisStep) { gttic(gather_relinearize_keys); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. @@ -659,7 +659,7 @@ ISAM2Result ISAM2::update( result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys - FastSet markedRelinMask; + KeySet markedRelinMask; BOOST_FOREACH(const Key key, relinKeys) markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); @@ -674,7 +674,7 @@ ISAM2Result ISAM2::update( // Relin involved keys for detailed results if(params_.enableDetailedResults) { - FastSet involvedRelinKeys; + KeySet involvedRelinKeys; BOOST_FOREACH(const sharedClique& root, roots_) Impl::FindAll(root, involvedRelinKeys, markedRelinMask); BOOST_FOREACH(Key key, involvedRelinKeys) { @@ -726,7 +726,7 @@ ISAM2Result ISAM2::update( gttic(recalculate); // 8. Redo top of Bayes tree - boost::shared_ptr > replacedKeys; + boost::shared_ptr replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); @@ -758,7 +758,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, boost::optional&> deletedFactorsIndices) { // Convert to ordered set - FastSet leafKeys(leafKeysList.begin(), leafKeysList.end()); + KeySet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. @@ -769,7 +769,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, FastSet factorIndicesToRemove; // Keep track of variables removed in subtrees - FastSet leafKeysRemoved; + KeySet leafKeysRemoved; // Remove each variable and its subtrees BOOST_FOREACH(Key j, leafKeys) { @@ -881,7 +881,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Reeliminate the linear graph to get the marginal and discard the conditional - const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); + const KeySet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); FastVector cliqueFrontalsToEliminate; std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::back_inserter(cliqueFrontalsToEliminate)); @@ -957,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList, deletedFactorsIndices->assign(factorIndicesToRemove.begin(), factorIndicesToRemove.end()); // Remove the marginalized variables - Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + Impl::RemoveVariables(KeySet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, nodes_, fixedVariables_); } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5f5554e91..fdc0021e5 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -460,7 +460,7 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable FastSet deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable KeySet deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -476,7 +476,7 @@ protected: /** Set of variables that are involved with linear factors from marginalized * variables and thus cannot have their linearization points changed. */ - FastSet fixedVariables_; + KeySet fixedVariables_; int update_count_; ///< Counter incremented every update(), used to determine periodic relinearization @@ -614,7 +614,7 @@ public: const VariableIndex& getVariableIndex() const { return variableIndex_; } /** Access the nonlinear variable index */ - const FastSet& getFixedVariables() const { return fixedVariables_; } + const KeySet& getFixedVariables() const { return fixedVariables_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -641,11 +641,11 @@ public: protected: FastSet getAffectedFactors(const FastList& keys) const; - GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const KeySet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - virtual boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const std::vector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + virtual boost::shared_ptr recalculate(const KeySet& markedKeys, const KeySet& relinKeys, + const std::vector& observedKeys, const KeySet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 @@ -666,11 +666,11 @@ template<> struct traits : public Testable {}; /// @return The number of variables that were solved for template size_t optimizeWildfire(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); template size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const FastSet& replaced, VectorValues& delta); + double threshold, const KeySet& replaced, VectorValues& delta); /// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix) template diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index f23418d2a..298ccf4b7 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -70,7 +70,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, stm << " size=\"" << formatting.figureWidthInches << "," << formatting.figureHeightInches << "\";\n\n"; - FastSet keys = this->keys(); + KeySet keys = this->keys(); // Local utility function to extract x and y coordinates struct { boost::optional operator()( @@ -144,7 +144,7 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, if (formatting.mergeSimilarFactors) { // Remove duplicate factors - FastSet > structure; + std::set > structure; BOOST_FOREACH(const sharedFactor& factor, *this){ if(factor) { vector factorKeys = factor->keys(); @@ -234,8 +234,8 @@ double NonlinearFactorGraph::error(const Values& c) const { } /* ************************************************************************* */ -FastSet NonlinearFactorGraph::keys() const { - FastSet keys; +KeySet NonlinearFactorGraph::keys() const { + KeySet keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 169d12455..ecd3b9b56 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -106,7 +106,7 @@ namespace gtsam { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys as an ordered set - ordering is by key value */ - FastSet keys() const; + KeySet keys() const; /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& c) const; diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index 56850e991..c917b322e 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -38,7 +38,7 @@ namespace gtsam EliminateSymbolic(const FactorGraph& factors, const Ordering& keys) { // Gather all keys - FastSet allKeys; + KeySet allKeys; BOOST_FOREACH(const boost::shared_ptr& factor, factors) { allKeys.insert(factor->begin(), factor->end()); } @@ -50,7 +50,7 @@ namespace gtsam } // Sort frontal keys - FastSet frontals(keys); + KeySet frontals(keys); const size_t nFrontals = keys.size(); // Build a key vector with the frontals followed by the separator diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index b579364b6..37a4fe5a4 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -193,7 +193,7 @@ private: // add the belief factor for each neighbor variable to this star graph // also record the factor index for later modification - FastSet neighbors = star->keys(); + KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; BOOST_FOREACH(Key neighbor, neighbors) { diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index f7a575f8c..68713f965 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -48,7 +48,7 @@ class QPSolver { GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. VariableIndex costVariableIndex_, equalityVariableIndex_, inequalityVariableIndex_; - FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph + KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph public: /// Constructor diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 563da4a9f..9d4249af4 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -463,7 +463,7 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, +void BatchFixedLagSmoother::PrintKeySet(const gtsam::KeySet& keys, const std::string& label) { std::cout << label; BOOST_FOREACH(gtsam::Key key, keys) { @@ -531,13 +531,13 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); // Get the set of all keys involved in the factor graph - FastSet allKeys(graph.keys()); + KeySet allKeys(graph.keys()); if (debug) PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys - FastSet remainingKeys; + KeySet remainingKeys; std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); if (debug) diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 89da3d7e5..605f3a5e8 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -161,7 +161,7 @@ protected: private: /** Private methods for printing debug information */ static void PrintKeySet(const std::set& keys, const std::string& label); - static void PrintKeySet(const gtsam::FastSet& keys, const std::string& label); + static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label); static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index fcaae9626..1d913d61f 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -221,7 +221,7 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& smootherSumm if(debug) { PrintNonlinearFactorGraph(smootherSummarization_, "ConcurrentBatchFilter::synchronize ", "Updated Smoother Summarization:", DefaultKeyFormatter); } // Find the set of new separator keys - FastSet newSeparatorKeys; + KeySet newSeparatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -573,7 +573,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { } // Calculate the set of new separator keys: AffectedKeys + PreviousSeparatorKeys - KeysToMove - FastSet newSeparatorKeys = removedFactors.keys(); + KeySet newSeparatorKeys = removedFactors.keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { newSeparatorKeys.insert(key_value.key); } @@ -584,7 +584,7 @@ void ConcurrentBatchFilter::moveSeparator(const FastList& keysToMove) { if(debug) { PrintKeys(newSeparatorKeys, "ConcurrentBatchFilter::synchronize ", "New Separator Keys:", DefaultKeyFormatter); } // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys = newSeparatorKeys; + KeySet shortcutKeys = newSeparatorKeys; BOOST_FOREACH(Key key, smootherSummarization_.keys()) { shortcutKeys.insert(key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 0f6515056..14c312f9d 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -371,7 +371,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 3b6b884f6..b893860cc 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -52,16 +52,16 @@ namespace internal { /* ************************************************************************* */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction) { // Calculate the set of RootKeys = AllKeys \Intersect RemainingKeys - FastSet rootKeys; - FastSet allKeys(graph.keys()); + KeySet rootKeys; + KeySet allKeys(graph.keys()); std::set_intersection(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(rootKeys, rootKeys.end())); // Calculate the set of MarginalizeKeys = AllKeys - RemainingKeys - FastSet marginalizeKeys; + KeySet marginalizeKeys; std::set_difference(allKeys.begin(), allKeys.end(), remainingKeys.begin(), remainingKeys.end(), std::inserter(marginalizeKeys, marginalizeKeys.end())); if(marginalizeKeys.size() == 0) { diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index fb4b78fc2..b8a9941ad 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -152,7 +152,7 @@ namespace internal { /** Calculate the marginal on the specified keys, returning a set of LinearContainerFactors. * Unlike other GTSAM functions with similar purposes, this version can operate on disconnected graphs. */ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, const Values& theta, - const FastSet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); + const KeySet& remainingKeys, const GaussianFactorGraph::Eliminate& eliminateFunction); } diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index 9742aefd7..cf3155602 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -184,7 +184,7 @@ void ConcurrentIncrementalFilter::synchronize(const NonlinearFactorGraph& smooth previousSmootherSummarization_ = smootherSummarization; // Find the set of new separator keys - const FastSet& newSeparatorKeys = isam2_.getFixedVariables(); + const KeySet& newSeparatorKeys = isam2_.getFixedVariables(); // Use the shortcut to calculate an updated marginal on the current separator // Combine just the shortcut and the previousSmootherSummarization @@ -312,7 +312,7 @@ std::vector ConcurrentIncrementalFilter::FindAdjacentFactors(const ISAM2 void ConcurrentIncrementalFilter::updateShortcut(const NonlinearFactorGraph& removedFactors) { // Calculate the set of shortcut keys: NewSeparatorKeys + OldSeparatorKeys - FastSet shortcutKeys; + KeySet shortcutKeys; BOOST_FOREACH(size_t slot, currentSmootherSummarizationSlots_) { const NonlinearFactor::shared_ptr& factor = isam2_.getFactorsUnsafe().at(slot); if(factor) { @@ -343,7 +343,7 @@ NonlinearFactorGraph ConcurrentIncrementalFilter::calculateFilterSummarization() // variables that result from marginalizing out all of the other variables // Find the set of current separator keys - const FastSet& separatorKeys = isam2_.getFixedVariables(); + const KeySet& separatorKeys = isam2_.getFixedVariables(); // Find all cliques that contain any separator variables std::set separatorCliques; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp index 16a336695..b87409aae 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.cpp @@ -245,7 +245,7 @@ void ConcurrentIncrementalSmoother::updateSmootherSummarization() { } // Get the set of separator keys - gtsam::FastSet separatorKeys; + gtsam::KeySet separatorKeys; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, separatorValues_) { separatorKeys.insert(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp index 51c2a55a2..558654367 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp @@ -559,7 +559,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 ) ordering = smoother.getOrdering(); // I'm really hoping this is an acceptable ordering... GaussianFactorGraph::shared_ptr linearFactors = allFactors.linearize(allValues); - FastSet eliminateKeys = linearFactors->keys(); + KeySet eliminateKeys = linearFactors->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { eliminateKeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index 5f91527e6..22dd0ccaa 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -579,7 +579,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) { allkeys.erase(key_value.key); } diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 9708eedb5..c372577ca 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -581,7 +581,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 ) // GaussianSequentialSolver GSS = GaussianSequentialSolver(*LinFactorGraph); // GaussianBayesNet::shared_ptr GBNsptr = GSS.eliminate(); - FastSet allkeys = LinFactorGraph->keys(); + KeySet allkeys = LinFactorGraph->keys(); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, filterSeparatorValues) allkeys.erase(key_value.key); std::vector variables(allkeys.begin(), allkeys.end()); diff --git a/gtsam_unstable/slam/MultiProjectionFactor.h b/gtsam_unstable/slam/MultiProjectionFactor.h index da60c2c31..f1487f562 100644 --- a/gtsam_unstable/slam/MultiProjectionFactor.h +++ b/gtsam_unstable/slam/MultiProjectionFactor.h @@ -70,7 +70,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), verboseCheirality_(false) { @@ -91,7 +91,7 @@ namespace gtsam { * @param body_P_sensor is the transform from body to sensor frame (default identity) */ MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model, - FastSet poseKeys, Key pointKey, const boost::shared_ptr& K, + KeySet poseKeys, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, boost::optional body_P_sensor = boost::none) : Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor), diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 2794e5707..ca91d4cb5 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -69,7 +69,7 @@ TEST( MultiProjectionFactor, create ){ n_measPixel << 10, 10, 10, 10, 10, 10; const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); - FastSet views; + KeySet views; views.insert(x1); views.insert(x2); views.insert(x3); diff --git a/matlab.h b/matlab.h index 349cdeea4..a215c6e07 100644 --- a/matlab.h +++ b/matlab.h @@ -71,16 +71,16 @@ FastVector createKeyVector(string s, const Vector& I) { } // Create a KeySet from indices -FastSet createKeySet(const Vector& I) { - FastSet set; +KeySet createKeySet(const Vector& I) { + KeySet set; for (int i = 0; i < I.size(); i++) set.insert(I[i]); return set; } // Create a KeySet from indices using symbol -FastSet createKeySet(string s, const Vector& I) { - FastSet set; +KeySet createKeySet(string s, const Vector& I) { + KeySet set; char c = s[0]; for (int i = 0; i < I.size(); i++) set.insert(symbol(c, I[i])); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 4e14d49b3..7922c14d1 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -186,12 +186,12 @@ done: // Permuted permuted(permutation, values); // // // After permutation, the indices above the threshold are 2 and 2 -// FastSet expected; +// KeySet expected; // expected.insert(2); // expected.insert(3); // // // Indices checked by CheckRelinearization -// FastSet actual = Impl::CheckRelinearization(permuted, 0.1); +// KeySet actual = Impl::CheckRelinearization(permuted, 0.1); // // EXPECT(assert_equal(expected, actual)); //} diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index 0af4c4a57..bd202e991 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -226,7 +226,7 @@ TEST(Marginals, order) { vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); Marginals marginals(fg, vals); - FastSet set = fg.keys(); + KeySet set = fg.keys(); FastVector keys(set.begin(), set.end()); JointMarginal joint = marginals.jointMarginalCovariance(keys); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index f398a33fe..b81a3113b 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -66,9 +66,9 @@ TEST( NonlinearFactorGraph, error ) TEST( NonlinearFactorGraph, keys ) { NonlinearFactorGraph fg = createNonlinearFactorGraph(); - FastSet actual = fg.keys(); + KeySet actual = fg.keys(); LONGS_EQUAL(3, (long)actual.size()); - FastSet::const_iterator it = actual.begin(); + KeySet::const_iterator it = actual.begin(); LONGS_EQUAL((long)L(1), (long)*(it++)); LONGS_EQUAL((long)X(1), (long)*(it++)); LONGS_EQUAL((long)X(2), (long)*(it++)); From 47495c8f46addc6212334a1802c784006f7fcae4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Jun 2015 18:14:20 -0700 Subject: [PATCH 11/14] Included config where needed --- examples/SolverComparer.cpp | 1 + examples/TimeTBB.cpp | 2 ++ gtsam/base/ThreadsafeException.h | 3 +++ gtsam/base/debug.cpp | 2 ++ gtsam/base/timing.h | 1 + gtsam/base/treeTraversal-inst.h | 1 + gtsam/base/types.h | 2 ++ gtsam/geometry/Unit3.cpp | 1 + gtsam/nonlinear/ISAM2-impl.cpp | 2 ++ gtsam/nonlinear/NonlinearFactorGraph.cpp | 14 ++++++++------ 10 files changed, 23 insertions(+), 6 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 0393affe1..f8f1411a3 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -44,6 +44,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #include #include diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index a35980836..de472b10c 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -17,6 +17,8 @@ #include #include +#include // for GTSAM_USE_TBB + #include #include #include diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index 3bdd42608..d464e9f21 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -19,6 +19,8 @@ #pragma once +#include // for GTSAM_USE_TBB + #include #include #include @@ -27,6 +29,7 @@ #ifdef GTSAM_USE_TBB #include #include +#include #endif namespace gtsam { diff --git a/gtsam/base/debug.cpp b/gtsam/base/debug.cpp index 6abc98642..1c4d08dcd 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,6 +17,8 @@ */ #include +#include // for GTSAM_USE_TBB + #ifdef GTSAM_USE_TBB #include #endif diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index a904c5f08..d0bca4a9d 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -19,6 +19,7 @@ #include #include +#include // for GTSAM_USE_TBB #include #include diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 3edd7d076..ab2792a89 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -22,6 +22,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #include #include diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 69e4e4192..84c94e73d 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -22,6 +22,8 @@ #include #include #include +#include // for GTSAM_USE_TBB + #include #ifdef GTSAM_USE_TBB diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index cc3865b8e..a74e39f47 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -21,6 +21,7 @@ #include #include #include +#include // for GTSAM_USE_TBB #ifdef __clang__ # pragma clang diagnostic push diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 6df1c8b10..4e8c0e303 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -19,6 +19,8 @@ #include #include // for selective linearization thresholds #include +#include // for GTSAM_USE_TBB + #include #include diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 298ccf4b7..827aaa7d8 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -17,22 +17,24 @@ * @author Christian Potthast */ -#include -#include -#include #include #include -#include -#include #include -#include #include #include +#include +#include +#include +#include // for GTSAM_USE_TBB #ifdef GTSAM_USE_TBB # include #endif +#include +#include +#include + using namespace std; namespace gtsam { From 3ca9cb8022978d3306163394d50208efdb52edab Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 22 Jun 2015 11:43:30 -0400 Subject: [PATCH 12/14] this should fix the MKL linking problem --- CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index a77173ba0..fd11a6733 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -179,6 +179,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL) set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL include_directories(${MKL_INCLUDE_DIR}) list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES}) + + # --no-as-needed is required with gcc according to the MKL link advisor + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed") + endif() else() set(GTSAM_USE_EIGEN_MKL 0) set(EIGEN_USE_MKL_ALL 0) From c4e0a109490bf8bae19843b5f3ada441db395466 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 22 Jun 2015 12:00:44 -0400 Subject: [PATCH 13/14] Add gtsam/config.h include to all files which include Eigen --- gtsam/base/Matrix.h | 2 ++ gtsam/base/OptionalJacobian.h | 2 +- gtsam/nonlinear/internal/ExecutionTrace.h | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index caee2071c..41ffa27b5 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -22,6 +22,8 @@ #pragma once #include +#include // Configuration from CMake + #include #include #include diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 7055a287a..d0e2297ef 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -18,7 +18,7 @@ */ #pragma once - +#include // Configuration from CMake #include #ifndef OPTIONALJACOBIAN_NOBOOST diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 37ce4dfd5..45817a3f9 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -17,7 +17,7 @@ */ #pragma once - +#include // Configuration from CMake #include #include #include From da726aa21f87d16f684c8434db83ea5707040553 Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Mon, 22 Jun 2015 12:02:40 -0400 Subject: [PATCH 14/14] remove config.h include as global_includes.h and config.h are redundant --- examples/TimeTBB.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/TimeTBB.cpp b/examples/TimeTBB.cpp index de472b10c..602a00593 100644 --- a/examples/TimeTBB.cpp +++ b/examples/TimeTBB.cpp @@ -17,7 +17,6 @@ #include #include -#include // for GTSAM_USE_TBB #include #include