diff --git a/CMakeLists.txt b/CMakeLists.txt index da60d1432..cf5a1a962 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,6 +118,7 @@ else() if(Boost_TIMER_LIBRARY) list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY}) else() + list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.") endif() endif() diff --git a/gtsam/base/SymmetricBlockMatrix.h b/gtsam/base/SymmetricBlockMatrix.h index 14014898a..45edbf6af 100644 --- a/gtsam/base/SymmetricBlockMatrix.h +++ b/gtsam/base/SymmetricBlockMatrix.h @@ -61,32 +61,32 @@ namespace gtsam { /// Construct from a container of the sizes of each block. template - SymmetricBlockMatrix(const CONTAINER& dimensions) : + SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) : blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); assertInvariants(); } /// Construct from iterator over the sizes of each vertical block. template - SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim) : + SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) : blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); assertInvariants(); } /// Construct from a container of the sizes of each vertical block and a pre-prepared matrix. template - SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : + SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : blockStart_(0) { matrix_.resize(matrix.rows(), matrix.cols()); matrix_.triangularView() = matrix.triangularView(); - fillOffsets(dimensions.begin(), dimensions.end()); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); if(matrix_.rows() != matrix_.cols()) throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix."); if(variableColOffsets_.back() != matrix_.cols()) @@ -211,85 +211,21 @@ namespace gtsam { return variableColOffsets_[block + blockStart_]; } - //void checkRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const - //{ - // const DenseIndex i_actualStartBlock = i_startBlock + blockStart_; - // const DenseIndex i_actualEndBlock = i_endBlock + blockStart_; - // const DenseIndex j_actualStartBlock = j_startBlock + blockStart_; - // const DenseIndex j_actualEndBlock = j_endBlock + blockStart_; - // checkBlock(i_actualStartBlock); - // checkBlock(j_actualStartBlock); - // if(i_startBlock != 0 || i_endBlock != 0) { - // checkBlock(i_actualStartBlock); - // assert(i_actualEndBlock < (DenseIndex)variableColOffsets_.size()); - // } - // if(j_startBlock != 0 || j_endBlock != 0) { - // checkBlock(j_actualStartBlock); - // assert(j_actualEndBlock < (DenseIndex)variableColOffsets_.size()); - // } - //} - - //void checkRange(DenseIndex startBlock, DenseIndex endBlock) const - //{ - // const DenseIndex actualStartBlock = startBlock + blockStart_; - // const DenseIndex actualEndBlock = endBlock + blockStart_; - // checkBlock(actualStartBlock); - // if(startBlock != 0 || endBlock != 0) { - // checkBlock(actualStartBlock); - // assert(actualEndBlock < (DenseIndex)variableColOffsets_.size()); - // } - //} - - //Block rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) - //{ - // const DenseIndex i_actualStartBlock = i_startBlock + blockStart_; - // const DenseIndex i_actualEndBlock = i_endBlock + blockStart_; - // const DenseIndex j_actualStartBlock = j_startBlock + blockStart_; - // const DenseIndex j_actualEndBlock = j_endBlock + blockStart_; - - // return Block(matrix(), - // variableColOffsets_[i_actualStartBlock], - // variableColOffsets_[j_actualStartBlock], - // variableColOffsets_[i_actualEndBlock] - variableColOffsets_[i_actualStartBlock], - // variableColOffsets_[j_actualEndBlock] - variableColOffsets_[j_actualStartBlock]); - //} - - //constBlock rangeUnchecked(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const - //{ - // // Convert Block to constBlock - // const Block block = const_cast(this)->rangeUnchecked(i_startBlock, i_endBlock, j_startBlock, j_endBlock); - // return constBlock(matrix(), block.Base::Base::, block.startCol(), block.rows(), block.cols()); - //} - - //Block rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) - //{ - // const DenseIndex actualStartBlock = startBlock + blockStart_; - // const DenseIndex actualEndBlock = endBlock + blockStart_; - - // return Block(matrix(), - // variableColOffsets_[actualStartBlock], - // variableColOffsets_[actualStartBlock], - // variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock], - // variableColOffsets_[actualEndBlock] - variableColOffsets_[actualStartBlock]); - //} - - //constBlock rangeUnchecked(DenseIndex startBlock, DenseIndex endBlock) const - //{ - // // Convert Block to constBlock - // const Block block = const_cast(this)->rangeUnchecked(startBlock, endBlock); - // return constBlock(matrix(), block.startRow(), block.startCol(), block.rows(), block.cols()); - //} - template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; ++ j; } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } } friend class VerticalBlockMatrix; diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 0e45a8f0d..029f55c58 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -65,20 +65,20 @@ namespace gtsam { /** Construct from a container of the sizes of each vertical block. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height) : + VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : rowStart_(0), rowEnd_(height), blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ template - VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix) : + VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { - fillOffsets(dimensions.begin(), dimensions.end()); + fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); if(variableColOffsets_.back() != matrix_.cols()) throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); assertInvariants(); @@ -87,10 +87,10 @@ namespace gtsam { /** * Construct from iterator over the sizes of each vertical block. */ template - VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height) : + VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : rowStart_(0), rowEnd_(height), blockStart_(0) { - fillOffsets(firstBlockDim, lastBlockDim); + fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); matrix_.resize(height, variableColOffsets_.back()); assertInvariants(); } @@ -202,14 +202,19 @@ namespace gtsam { } template - void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim) { - variableColOffsets_.resize((lastBlockDim-firstBlockDim)+1); + void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { + variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0)); variableColOffsets_[0] = 0; DenseIndex j=0; for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; ++ j; } + if(appendOneDimension) + { + variableColOffsets_[j+1] = variableColOffsets_[j] + 1; + ++ j; + } } friend class SymmetricBlockMatrix; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 72124d25d..3cb41e936 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -45,10 +45,10 @@ Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { } /* ************************************************************************* */ -Rot3 Rot3::Random(boost::random::mt19937 & rng) { +Rot3 Rot3::Random(boost::mt19937 & rng) { // TODO allow any engine without including all of boost :-( Sphere2 w = Sphere2::Random(rng); - boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); + boost::uniform_real randomAngle(-M_PI,M_PI); double angle = randomAngle(rng); return rodriguez(w,angle); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 847d31d65..83232ea02 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -104,7 +104,7 @@ namespace gtsam { Rot3(const Quaternion& q); /// Random, generates a random axis, then random angle \in [-p,pi] - static Rot3 Random(boost::random::mt19937 & rng); + static Rot3 Random(boost::mt19937 & rng); /** Virtual destructor */ virtual ~Rot3() {} diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index ed2561e22..1dc64312a 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include using namespace std; @@ -44,10 +45,14 @@ Sphere2 Sphere2::FromPoint3(const Point3& point, } /* ************************************************************************* */ -Sphere2 Sphere2::Random(boost::random::mt19937 & rng) { +Sphere2 Sphere2::Random(boost::mt19937 & rng) { // TODO allow any engine without including all of boost :-( - boost::random::uniform_on_sphere randomDirection(3); - vector d = randomDirection(rng); + boost::uniform_on_sphere randomDirection(3); + // This variate_generator object is required for versions of boost somewhere + // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). + boost::variate_generator > + generator(rng, randomDirection); + vector d = generator(); Sphere2 result; result.p_ = Point3(d[0], d[1], d[2]); return result; diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index d4047f421..82c6bb3d7 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -22,23 +22,12 @@ #include #include +#include #ifndef SPHERE2_DEFAULT_COORDINATES_MODE #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM #endif -// (Cumbersome) forward declaration for random generator -namespace boost { -namespace random { -template -class mersenne_twister_engine; -typedef mersenne_twister_engine mt19937; -} -} - namespace gtsam { /// Represents a 3D point on a unit sphere. @@ -75,7 +64,7 @@ public: boost::optional H = boost::none); /// Random direction, using boost::uniform_on_sphere - static Sphere2 Random(boost::random::mt19937 & rng); + static Sphere2 Random(boost::mt19937 & rng); /// @} diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index 306d84b94..15bdec407 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -312,7 +312,7 @@ TEST(Sphere2, localCoordinates_retract_expmap) { //******************************************************************************* TEST(Sphere2, Random) { - boost::random::mt19937 rng(42); + boost::mt19937 rng(42); // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Sphere2::Random(rng).point3(); diff --git a/gtsam/linear/GaussianConditional-inl.h b/gtsam/linear/GaussianConditional-inl.h index dd0307c48..45d7ecc3b 100644 --- a/gtsam/linear/GaussianConditional-inl.h +++ b/gtsam/linear/GaussianConditional-inl.h @@ -19,9 +19,6 @@ #pragma once -#include -#include - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index d8344abe1..cfa698dca 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -42,7 +42,6 @@ #include #include #include -#include #include #include @@ -179,7 +178,7 @@ DenseIndex _getSizeHF(const Vector& m) { return m.size(); } /* ************************************************************************* */ HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, const std::vector& gs, double f) : - GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), ListOfOne((DenseIndex)1))) + GaussianFactor(js), info_(gs | br::transformed(&_getSizeHF), true) { // Get the number of variables size_t variable_count = js.size(); diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index f391d4afd..b2946b1fc 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -123,7 +122,7 @@ namespace gtsam { // matrices, then extract the number of columns e.g. dimensions in each matrix. Then joins with // a single '1' to add a dimension for the b vector. { - Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&internal::getColsJF), ListOfOne((DenseIndex)1)), b.size()); + Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true); } // Check and add terms diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index e18055edc..1468810e6 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -283,7 +283,7 @@ namespace gtsam { // Allocate matrix and copy keys in order gttic(allocate); - Ab_ = VerticalBlockMatrix(boost::join(varDims, ListOfOne((DenseIndex)1)), m); // Allocate augmented matrix + Ab_ = VerticalBlockMatrix(varDims, m, true); // Allocate augmented matrix Base::keys_.resize(orderedSlots.size()); boost::range::copy( // Get variable keys orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());