diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index fa2425e4d..c699db0d3 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -20,19 +20,15 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - ubuntu-20.04-gcc-7, ubuntu-20.04-gcc-9, ubuntu-20.04-clang-9, + ubuntu-22.04-gcc-11, + ubuntu-22.04-clang-14, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: ubuntu-20.04-gcc-7 - os: ubuntu-20.04 - compiler: gcc - version: "7" - - name: ubuntu-20.04-gcc-9 os: ubuntu-20.04 compiler: gcc @@ -43,14 +39,24 @@ jobs: compiler: clang version: "9" + - name: ubuntu-22.04-gcc-11 + os: ubuntu-22.04 + compiler: gcc + version: "11" + + - name: ubuntu-22.04-clang-14 + os: ubuntu-22.04 + compiler: clang + version: "14" + steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Install Dependencies run: | - # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository. - if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + # LLVM (clang) 9/14 is not in Bionic's repositories so we add the official LLVM repository. + if [ "${{ matrix.compiler }}" = "clang" ]; then # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 7b7646328..3fa3c15dd 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -14,7 +14,7 @@ jobs: GTSAM_BUILD_UNSTABLE: ${{ matrix.build_unstable }} strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. @@ -32,20 +32,14 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Install Dependencies run: | brew install cmake ninja brew install boost - if [ "${{ matrix.compiler }}" = "gcc" ]; then - brew install gcc@${{ matrix.version }} - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else - sudo xcode-select -switch /Applications/Xcode.app - echo "CC=clang" >> $GITHUB_ENV - echo "CXX=clang++" >> $GITHUB_ENV - fi + sudo xcode-select -switch /Applications/Xcode.app + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV - name: Build and Test run: bash .github/scripts/unix.sh -t diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 4eb861ecc..442e26e47 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -14,58 +14,45 @@ jobs: PYTHON_VERSION: ${{ matrix.python_version }} strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: [ - ubuntu-20.04-gcc-7, - ubuntu-20.04-gcc-9, - ubuntu-20.04-clang-9, - macOS-11-xcode-13.4.1, - ubuntu-20.04-gcc-7-tbb, - ] + name: + [ + ubuntu-20.04-gcc-9, + ubuntu-20.04-gcc-9-tbb, + ubuntu-20.04-clang-9, + macOS-11-xcode-13.4.1, + ] - build_type: [Debug, Release] + build_type: [Release] python_version: [3] include: - - name: ubuntu-20.04-gcc-7 - os: ubuntu-20.04 - compiler: gcc - version: "7" - - name: ubuntu-20.04-gcc-9 os: ubuntu-20.04 compiler: gcc version: "9" - - name: ubuntu-20.04-clang-9 + - name: ubuntu-20.04-gcc-9-tbb os: ubuntu-20.04 - compiler: clang + compiler: gcc version: "9" + flag: tbb - # NOTE temporarily added this as it is a required check. - name: ubuntu-20.04-clang-9 os: ubuntu-20.04 compiler: clang version: "9" - build_type: Debug - python_version: "3" - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode version: "13.4.1" - - name: ubuntu-20.04-gcc-7-tbb - os: ubuntu-20.04 - compiler: gcc - version: "7" - flag: tbb - steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Install (Linux) if: runner.os == 'Linux' run: | @@ -82,7 +69,7 @@ jobs: sudo apt-get -y update sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libboost-all-dev - + if [ "${{ matrix.compiler }}" = "gcc" ]; then sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV @@ -98,15 +85,9 @@ jobs: brew tap ProfFan/robotics brew install cmake ninja brew install boost - if [ "${{ matrix.compiler }}" = "gcc" ]; then - brew install gcc@${{ matrix.version }} - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else - sudo xcode-select -switch /Applications/Xcode.app - echo "CC=clang" >> $GITHUB_ENV - echo "CXX=clang++" >> $GITHUB_ENV - fi + sudo xcode-select -switch /Applications/Xcode.app + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 458394211..211ef3cbd 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -15,78 +15,80 @@ jobs: BOOST_VERSION: 1.67.0 strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - ubuntu-gcc-deprecated, - ubuntu-gcc-quaternions, - ubuntu-gcc-tbb, - ubuntu-cayleymap, + ubuntu-clang-deprecated, + ubuntu-clang-quaternions, + ubuntu-clang-tbb, + ubuntu-clang-cayleymap, + ubuntu-clang-system-libs, ] build_type: [Debug, Release] include: - - name: ubuntu-gcc-deprecated - os: ubuntu-20.04 - compiler: gcc - version: "9" + - name: ubuntu-clang-deprecated + os: ubuntu-22.04 + compiler: clang + version: "14" flag: deprecated - - name: ubuntu-gcc-quaternions - os: ubuntu-20.04 - compiler: gcc - version: "9" + - name: ubuntu-clang-quaternions + os: ubuntu-22.04 + compiler: clang + version: "14" flag: quaternions - - name: ubuntu-gcc-tbb - os: ubuntu-20.04 - compiler: gcc - version: "9" + - name: ubuntu-clang-tbb + os: ubuntu-22.04 + compiler: clang + version: "14" flag: tbb - - name: ubuntu-cayleymap - os: ubuntu-20.04 - compiler: gcc - version: "9" + - name: ubuntu-clang-cayleymap + os: ubuntu-22.04 + compiler: clang + version: "14" flag: cayley - - name: ubuntu-system-libs - os: ubuntu-20.04 - compiler: gcc - version: "9" - flag: system-libs + - name: ubuntu-clang-system-libs + os: ubuntu-22.04 + compiler: clang + version: "14" + flag: system steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Install (Linux) if: runner.os == 'Linux' run: | - # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository. - if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then + sudo apt-get -y update + sudo apt-get -y install software-properties-common + + # LLVM (clang) 9/14 is not in 22.04 (jammy)'s repositories so we add the official LLVM repository. + if [ "${{ matrix.compiler }}" = "clang" ]; then + # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool + # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure + # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository + # This key is not in the keystore by default for Ubuntu so we need to add it. + LLVM_KEY=15CF4D18AF4F7421 gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY - gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add - - sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main" + gpg -a --export $LLVM_KEY | sudo apt-key add - + sudo add-apt-repository "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy main" fi - sudo apt-get -y update sudo apt-get -y install cmake build-essential pkg-config libpython3-dev python3-numpy libicu-dev - if [ "${{ matrix.compiler }}" = "gcc" ]; then - sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else - sudo apt-get install -y clang-${{ matrix.version }} g++-multilib - echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV - fi + sudo apt-get install -y clang-${{ matrix.version }} g++-multilib + echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV + echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV - name: Install Boost if: runner.os == 'Linux' @@ -97,15 +99,9 @@ jobs: if: runner.os == 'macOS' run: | brew install cmake ninja boost - if [ "${{ matrix.compiler }}" = "gcc" ]; then - brew install gcc@${{ matrix.version }} - echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV - echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV - else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app - echo "CC=clang" >> $GITHUB_ENV - echo "CXX=clang++" >> $GITHUB_ENV - fi + sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + echo "CC=clang" >> $GITHUB_ENV + echo "CXX=clang++" >> $GITHUB_ENV - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' @@ -135,8 +131,11 @@ jobs: - name: Use system versions of 3rd party libraries if: matrix.flag == 'system' run: | + sudo apt-get install libeigen3-dev echo "GTSAM_USE_SYSTEM_EIGEN=ON" >> $GITHUB_ENV - echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + # TODO(dellaert): This does not work yet? + # sudo apt-get install metis + # echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV - name: Build & Test run: | diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index ef2500b46..3d4bf3faf 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -16,7 +16,7 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. @@ -94,7 +94,7 @@ jobs: echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Configuration run: | diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index e8f01a1f0..ccb0e41ed 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -87,7 +87,10 @@ if(MSVC) list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE WINDOWS_LEAN_AND_MEAN NOMINMAX - ) + ) + list_append_cache(GTSAM_COMPILE_DEFINITIONS_PUBLIC + _ENABLE_EXTENDED_ALIGNED_STORAGE + ) # Avoid literally hundreds to thousands of warnings: list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /wd4267 # warning C4267: 'initializing': conversion from 'size_t' to 'int', possible loss of data diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp index edd46b5e2..f2e3529ba 100644 --- a/examples/SFMExampleExpressions_bal.cpp +++ b/examples/SFMExampleExpressions_bal.cpp @@ -31,7 +31,6 @@ #include #include -#include #include using namespace std; @@ -50,8 +49,7 @@ int main(int argc, char* argv[]) { // Load the SfM data from file SfmData mydata = SfmData::FromBalFile(filename); - cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.numberTracks() % mydata.numberCameras(); + cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl; // Create a factor graph ExpressionFactorGraph graph; diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 3edf1f89b..9db5bd175 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -23,7 +23,6 @@ #include #include -#include #include using namespace std; @@ -45,7 +44,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfmData mydata = SfmData::FromBalFile(filename); - cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras(); + cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl; // Create a factor graph NonlinearFactorGraph graph; diff --git a/examples/SFMExample_bal_COLAMD_METIS.cpp b/examples/SFMExample_bal_COLAMD_METIS.cpp index 5134285c7..592b83418 100644 --- a/examples/SFMExample_bal_COLAMD_METIS.cpp +++ b/examples/SFMExample_bal_COLAMD_METIS.cpp @@ -26,7 +26,6 @@ #include #include -#include #include using namespace std; @@ -47,8 +46,7 @@ int main(int argc, char* argv[]) { // Load the SfM data from file SfmData mydata = SfmData::FromBalFile(filename); - cout << boost::format("read %1% tracks on %2% cameras\n") % - mydata.numberTracks() % mydata.numberCameras(); + cout << "read " << mydata.numberTracks() << " tracks on " << mydata.numberCameras() << " cameras" << endl; // Create a factor graph NonlinearFactorGraph graph; @@ -128,9 +126,9 @@ int main(int argc, char* argv[]) { cout << endl << endl; cout << "Time comparison by solving " << filename << " results:" << endl; - cout << boost::format("%1% point tracks and %2% cameras\n") % - mydata.numberTracks() % mydata.numberCameras() - << endl; + + cout << mydata.numberTracks() << " point tracks and " << mydata.numberCameras() + << " cameras" << endl; tictoc_print_(); } diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 89fa8248c..5c0f92b4e 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -51,7 +51,7 @@ public: BOOST_CONCEPT_USAGE(IsGroup) { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), + (std::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); e = traits::Identity(); e = traits::Compose(g, h); diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1b574816a..3ea5e94a8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -266,7 +266,7 @@ public: BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), + (std::is_base_of::value), "This type's trait does not assert it is a Lie group (or derived)"); // group opertations with Jacobians diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 962dc8269..55aebd10f 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -144,7 +144,7 @@ public: BOOST_CONCEPT_USAGE(IsManifold) { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), + (std::is_base_of::value), "This type's structure_category trait does not assert it as a manifold (or derived)"); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 41433ac28..3c6e64dbc 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -23,9 +23,6 @@ #include #include -#include -#include - #include #include #include @@ -128,8 +125,10 @@ bool linear_dependent(const Matrix& A, const Matrix& B, double tol) { /* ************************************************************************* */ Vector operator^(const Matrix& A, const Vector & v) { - if (A.rows()!=v.size()) throw std::invalid_argument( - boost::str(boost::format("Matrix operator^ : A.m(%d)!=v.size(%d)") % A.rows() % v.size())); + if (A.rows()!=v.size()) { + throw std::invalid_argument("Matrix operator^ : A.m(" + std::to_string(A.rows()) + ")!=v.size(" + + std::to_string(v.size()) + ")"); + } // Vector vt = v.transpose(); // Vector vtA = vt * A; // return vtA.transpose(); @@ -611,11 +610,12 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, else matrixPrinted << matrix; const std::string matrixStr = matrixPrinted.str(); - boost::tokenizer > tok(matrixStr, boost::char_separator("\n")); + // Split the matrix string into lines and indent them + std::string line; + std::istringstream iss(matrixStr); DenseIndex row = 0; - for(const std::string& line: tok) - { + while (std::getline(iss, line)) { assert(row < effectiveRows); if(row > 0) ss << padding; @@ -624,6 +624,7 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, ss << "\n"; ++ row; } + } else { ss << "Empty (" << matrix.rows() << "x" << matrix.cols() << ")"; } diff --git a/gtsam/base/ThreadsafeException.h b/gtsam/base/ThreadsafeException.h index 57e37237f..79dbe6bb1 100644 --- a/gtsam/base/ThreadsafeException.h +++ b/gtsam/base/ThreadsafeException.h @@ -46,9 +46,11 @@ private: protected: typedef std::basic_string, tbb::tbb_allocator > String; + typedef tbb::tbb_allocator Allocator; #else protected: typedef std::string String; + typedef std::allocator Allocator; #endif protected: diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 9fbd581bb..9246ad871 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -474,7 +474,7 @@ public: BOOST_CONCEPT_USAGE(IsVectorSpace) { BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::value), + (std::is_base_of::value), "This type's trait does not assert it as a vector space (or derived)"); r = p + q; r = -p; diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 17cb291f0..71cef2524 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -20,7 +20,6 @@ #include #include -#include #include using namespace std; diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 8c9934d77..8183841b2 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -71,10 +71,10 @@ typename Eigen::Matrix numericalGradient( std::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of::structure_category>::value), + static_assert( + (std::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); + static_assert(N>0, "Template argument X must be fixed-size type or N must be specified."); // Prepare a tangent vector to perturb x with, only works for fixed size typename traits::TangentVector d; @@ -111,13 +111,13 @@ typename internal::FixedSizeMatrix::type numericalDerivative11( std::function h, const X& x, double delta = 1e-5) { typedef typename internal::FixedSizeMatrix::type Matrix; - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); typedef traits TraitsY; - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type or N must be specified."); + static_assert(N>0, "Template argument X must be fixed-size type or N must be specified."); typedef traits TraitsX; // get value at x, and corresponding chart @@ -165,9 +165,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative21(const std::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::placeholders::_1, std::cref(x2)), x1, delta); @@ -194,9 +194,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(cons template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative22(std::function h, const X1& x1, const X2& x2, double delta = 1e-5) { -// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), +// static_assert( (std::is_base_of::structure_category>::value), // "Template argument X1 must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::placeholders::_1), x2, delta); @@ -226,9 +226,9 @@ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative31( std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3)), @@ -259,9 +259,9 @@ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative32( std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3)), @@ -292,9 +292,9 @@ template::dimension> typename internal::FixedSizeMatrix::type numericalDerivative33( std::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1), @@ -325,9 +325,9 @@ template::di typename internal::FixedSizeMatrix::type numericalDerivative41( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), @@ -359,9 +359,9 @@ template::di typename internal::FixedSizeMatrix::type numericalDerivative42( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), @@ -393,9 +393,9 @@ template::di typename internal::FixedSizeMatrix::type numericalDerivative43( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, @@ -427,9 +427,9 @@ template::di typename internal::FixedSizeMatrix::type numericalDerivative44( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X4 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -462,9 +462,9 @@ template::type numericalDerivative51( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), @@ -498,9 +498,9 @@ template::type numericalDerivative52( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), @@ -534,9 +534,9 @@ template::type numericalDerivative53( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, @@ -570,9 +570,9 @@ template::type numericalDerivative54( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -606,9 +606,9 @@ template::type numericalDerivative55( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -643,9 +643,9 @@ template::type numericalDerivative61( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::placeholders::_1, std::cref(x2), std::cref(x3), @@ -680,9 +680,9 @@ template::type numericalDerivative62( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::placeholders::_1, std::cref(x3), @@ -717,9 +717,9 @@ template::type numericalDerivative63( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::placeholders::_1, @@ -754,9 +754,9 @@ template::type numericalDerivative64( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -791,9 +791,9 @@ template::type numericalDerivative65( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -829,9 +829,9 @@ typename internal::FixedSizeMatrix::type numericalDerivative66( std::function h, const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11( std::bind(h, std::cref(x1), std::cref(x2), std::cref(x3), @@ -860,7 +860,7 @@ inline typename internal::FixedSizeMatrix::type numericalDerivative66(Y (* template inline typename internal::FixedSizeMatrix::type numericalHessian(std::function f, const X& x, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + static_assert( (std::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); typedef Eigen::Matrix::dimension, 1> VectorD; typedef std::function F; diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 72d08ad3b..9ca5e24c9 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -19,9 +19,6 @@ #include #include -#include -#include - #include #include #include @@ -78,7 +75,7 @@ size_t TimingOutline::time() const { /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { std::string formattedLabel = label_; - boost::replace_all(formattedLabel, "_", " "); + std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' '); std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" << n_ << " times, " << wall() << " wall, " << secs() << " children, min: " << min() << " max: " << max() << ")\n"; @@ -247,16 +244,14 @@ void toc(size_t id, const char *label) { if (id != current->id_) { gTimingRoot->print(); throw std::invalid_argument( - (boost::format( - "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") - % label % current->label_).str()); + "gtsam timing: Mismatched tic/toc: gttoc(\"" + std::string(label) + + "\") called when last tic was \"" + current->label_ + "\"."); } if (!current->parent_.lock()) { gTimingRoot->print(); throw std::invalid_argument( - (boost::format( - "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") - % label).str()); + "gtsam timing: Mismatched tic/toc: extra gttoc(\"" + std::string(label) + + "\"), already at the root"); } current->toc(); gCurrentTimer = current->parent_; diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 5b6f053cb..3266431ea 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -234,27 +234,27 @@ namespace gtsam { #if (_MSC_VER < 1800) -#include +#include namespace std { template inline int isfinite(T a) { - return (int)boost::math::isfinite(a); } + return (int)std::isfinite(a); } template inline int isnan(T a) { - return (int)boost::math::isnan(a); } + return (int)std::isnan(a); } template inline int isinf(T a) { - return (int)boost::math::isinf(a); } + return (int)std::isinf(a); } } #endif -#include +#include #ifndef M_PI -#define M_PI (boost::math::constants::pi()) +#define M_PI (3.14159265358979323846) #endif #ifndef M_PI_2 -#define M_PI_2 (boost::math::constants::pi() / 2.0) +#define M_PI_2 (M_PI / 2.0) #endif #ifndef M_PI_4 -#define M_PI_4 (boost::math::constants::pi() / 4.0) +#define M_PI_4 (M_PI / 4.0) #endif #endif diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 177ea5b49..56a79a38a 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -25,10 +25,10 @@ #include #include +#include using namespace std; using namespace gtsam; -using namespace boost::placeholders; namespace { noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); @@ -119,8 +119,8 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); // Check derivative - std::function)> f = boost::bind( - &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, nullptr); + std::function)> f = std::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, std::placeholders::_1, nullptr); Matrix numericalH = numericalDerivative11, 2 * N>(f, X); EXPECT(assert_equal(numericalH, actualH, 1e-9)); @@ -378,7 +378,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor) { // Test Jacobian Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), fx, _1, nullptr), X); + std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } @@ -410,7 +410,7 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) { VecD vecd(N, points(0), 0, T); vecd(X, actualH); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&VecD::operator(), vecd, _1, nullptr), X); + std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-6)); } @@ -427,7 +427,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) { EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); Matrix expectedH = numericalDerivative11, M * N>( - boost::bind(&CompFunc::operator(), fx, _1, nullptr), X); + std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); EXPECT(assert_equal(expectedH, actualH, 1e-7)); } diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index b3f0d69b0..425ad15f9 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace gtsam { @@ -162,7 +163,9 @@ namespace gtsam { const typename Base::LabelFormatter& labelFormatter = &DefaultFormatter) const { auto valueFormatter = [](const double& v) { - return (boost::format("%4.8g") % v).str(); + std::stringstream ss; + ss << std::setw(4) << std::setprecision(8) << v; + return ss.str(); }; Base::print(s, labelFormatter, valueFormatter); } diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index f17786056..cfaa806b8 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -33,6 +32,7 @@ #include #include #include +#include namespace gtsam { @@ -192,8 +192,8 @@ namespace gtsam { ~Choice() override { #ifdef DT_DEBUG_MEMORY - std::std::cout << Node::nrNodes << " destructing (Choice) " << this->id() - << std::std::endl; + std::cout << Node::nrNodes << " destructing (Choice) " << this->id() + << std::endl; #endif } @@ -282,9 +282,9 @@ namespace gtsam { const ValueFormatter& valueFormatter) const override { std::cout << s << " Choice("; std::cout << labelFormatter(label_) << ") " << std::endl; - for (size_t i = 0; i < branches_.size(); i++) - branches_[i]->print((boost::format("%s %d") % s % i).str(), - labelFormatter, valueFormatter); + for (size_t i = 0; i < branches_.size(); i++) { + branches_[i]->print(s + " " + std::to_string(i), labelFormatter, valueFormatter); + } } /** output to graphviz (as a a graph) */ @@ -643,11 +643,8 @@ namespace gtsam { // Create a simple choice node with values as leaves. if (size != nrChoices) { std::cout << "Trying to create DD on " << begin->first << std::endl; - std::cout << boost::format( - "DecisionTree::create: expected %d values but got %d " - "instead") % - nrChoices % size - << std::endl; + std::cout << "DecisionTree::create: expected " << nrChoices + << " values but got " << size << " instead" << std::endl; throw std::invalid_argument("DecisionTree::create invalid argument"); } auto choice = std::make_shared(begin->first, endY - beginY); diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 399216b83..cb6c7761e 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -22,7 +22,6 @@ #include #include -#include #include using namespace std; @@ -79,8 +78,9 @@ namespace gtsam { const KeyFormatter& formatter) const { cout << s; cout << " f["; - for (auto&& key : keys()) - cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key); + for (auto&& key : keys()) { + cout << " (" << formatter(key) << "," << cardinality(key) << "),"; + } cout << " ]" << endl; ADT::print("", formatter); } @@ -104,13 +104,12 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( size_t nrFrontals, ADT::Binary op) const { - if (nrFrontals > size()) + if (nrFrontals > size()) { throw invalid_argument( - (boost::format( - "DecisionTreeFactor::combine: invalid number of frontal " - "keys %d, nr.keys=%d") % - nrFrontals % size()) - .str()); + "DecisionTreeFactor::combine: invalid number of frontal " + "keys " + + std::to_string(nrFrontals) + ", nr.keys=" + std::to_string(size())); + } // sum over nrFrontals keys size_t i; @@ -132,13 +131,13 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( const Ordering& frontalKeys, ADT::Binary op) const { - if (frontalKeys.size() > size()) + if (frontalKeys.size() > size()) { throw invalid_argument( - (boost::format( - "DecisionTreeFactor::combine: invalid number of frontal " - "keys %d, nr.keys=%d") % - frontalKeys.size() % size()) - .str()); + "DecisionTreeFactor::combine: invalid number of frontal " + "keys " + + std::to_string(frontalKeys.size()) + ", nr.keys=" + + std::to_string(size())); + } // sum over nrFrontals keys size_t i; @@ -193,7 +192,9 @@ namespace gtsam { /* ************************************************************************ */ static std::string valueFormatter(const double& v) { - return (boost::format("%4.2g") % v).str(); + std::stringstream ss; + ss << std::setw(4) << std::setprecision(2) << std::fixed << v; + return ss.str(); } /** output to graphviz format, stream version */ diff --git a/gtsam/discrete/DiscreteKey.cpp b/gtsam/discrete/DiscreteKey.cpp index 06ed2ca3b..79d11d8a7 100644 --- a/gtsam/discrete/DiscreteKey.cpp +++ b/gtsam/discrete/DiscreteKey.cpp @@ -17,7 +17,6 @@ */ #include -#include // for key names #include "DiscreteKey.h" namespace gtsam { @@ -26,7 +25,7 @@ namespace gtsam { DiscreteKeys::DiscreteKeys(const vector& cs) { for (size_t i = 0; i < cs.size(); i++) { - string name = boost::str(boost::format("v%1%") % i); + string name = "v" + std::to_string(i); push_back(DiscreteKey(i, cs[i])); } } diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index e3f9d9dd2..c7cb7088e 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -25,8 +25,6 @@ #include // for convert only #define DISABLE_TIMING -#include - #include #include #include @@ -81,9 +79,9 @@ void resetCounts() { } void printCounts(const string& s) { #ifndef DISABLE_TIMING - cout << boost::format("%s: %3d muls, %3d adds, %g ms.") % s % muls % adds % - (1000 * elapsed) - << endl; +cout << s << ": " << std::setw(3) << muls << " muls, " << + std::setw(3) << adds << " adds, " << 1000 * elapsed << " ms." + << endl; #endif resetCounts(); } @@ -133,7 +131,9 @@ ADT create(const Signature& signature) { ADT p(signature.discreteKeys(), signature.cpt()); static size_t count = 0; const DiscreteKey& key = signature.key(); - string DOTfile = (boost::format("CPT-%03d-%d") % ++count % key.first).str(); + std::stringstream ss; + ss << "CPT-" << std::setw(3) << std::setfill('0') << ++count << "-" << key.first; + string DOTfile = ss.str(); dot(p, DOTfile); return p; } diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 5366d7b3a..d2a94ddc3 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -26,6 +26,8 @@ #include #include +#include + using std::vector; using std::string; using std::map; @@ -50,7 +52,9 @@ struct CrazyDecisionTree : public DecisionTree { void print(const std::string& s = "") const { auto keyFormatter = [](const std::string& s) { return s; }; auto valueFormatter = [](const Crazy& v) { - return (boost::format("{%d,%4.2g}") % v.a % v.b).str(); + std::stringstream ss; + ss << "{" << v.a << "," << std::setw(4) << std::setprecision(2) << v.b << "}"; + return ss.str(); }; DecisionTree::print("", keyFormatter, valueFormatter); } @@ -86,7 +90,7 @@ struct DT : public DecisionTree { void print(const std::string& s = "") const { auto keyFormatter = [](const std::string& s) { return s; }; auto valueFormatter = [](const int& v) { - return (boost::format("%d") % v).str(); + return std::to_string(v); }; std::cout << s; Base::print("", keyFormatter, valueFormatter); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 94691357f..8a1bc1e5c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -24,8 +24,6 @@ #include #include -#include - #include using namespace std; @@ -195,7 +193,7 @@ TEST( Rot3, retract) /* ************************************************************************* */ TEST( Rot3, log) { - static const double PI = boost::math::constants::pi(); + static const double PI = std::acos(-1.0); Vector w; Rot3 R; diff --git a/gtsam/hybrid/HybridFactorGraph.cpp b/gtsam/hybrid/HybridFactorGraph.cpp index 631f8d22f..d96a890f4 100644 --- a/gtsam/hybrid/HybridFactorGraph.cpp +++ b/gtsam/hybrid/HybridFactorGraph.cpp @@ -20,8 +20,6 @@ #include #include -#include - namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index af0cea746..b8e7ff588 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -22,7 +22,6 @@ #include #include -#include #include namespace gtsam { diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index cbc949404..cc0a304a2 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -27,7 +27,6 @@ #include #include -#include #include #include #include @@ -194,7 +193,7 @@ class MixtureFactor : public HybridFactor { std::cout << "\nMixtureFactor\n"; auto valueFormatter = [](const sharedFactor& v) { if (v) { - return (boost::format("Nonlinear factor on %d keys") % v->size()).str(); + return "Nonlinear factor on " + std::to_string(v->size()) + " keys"; } else { return std::string("nullptr"); } diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 8b72370fc..fe5c53e36 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -209,9 +209,13 @@ struct EliminationData { // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid // putting orphan subtrees in the index - they'll already be in the index of the ISAM2 // object they're added to. - for (const Key& j: myData.bayesTreeNode->conditional()->frontals()) + for (const Key& j : myData.bayesTreeNode->conditional()->frontals()) { +#ifdef GTSAM_USE_TBB + nodesIndex_.insert({j, myData.bayesTreeNode}); +#else nodesIndex_.emplace(j, myData.bayesTreeNode); - +#endif + } // Store remaining factor in parent's gathered factors if (!eliminationResult.second->empty()) { #ifdef GTSAM_USE_TBB diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index dd433ff09..8fcea0d05 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -20,7 +20,6 @@ #include #include -#include #include using namespace std; @@ -30,10 +29,12 @@ namespace gtsam { /* ************************************************************************* */ string _defaultKeyFormatter(Key key) { const Symbol asSymbol(key); - if (asSymbol.chr() > 0) + if (asSymbol.chr() > 0) { return (string) asSymbol; - else - return boost::lexical_cast(key); + } + else { + return std::to_string(key); + } } /* ************************************************************************* */ @@ -48,10 +49,12 @@ string _multirobotKeyFormatter(Key key) { return (string) asLabeledSymbol; const Symbol asSymbol(key); - if (asLabeledSymbol.chr() > 0) + if (asLabeledSymbol.chr() > 0) { return (string) asSymbol; - else - return boost::lexical_cast(key); + } + else { + return std::to_string(key); + } } /* ************************************************************************* */ diff --git a/gtsam/inference/LabeledSymbol.cpp b/gtsam/inference/LabeledSymbol.cpp index 6a1739e20..6e7440402 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -17,8 +17,6 @@ #include -#include -#include #include namespace gtsam { @@ -73,7 +71,9 @@ void LabeledSymbol::print(const std::string& s) const { /* ************************************************************************* */ LabeledSymbol::operator std::string() const { - return str(boost::format("%c%c%d") % c_ % label_ % j_); + char buffer[100]; + snprintf(buffer, 100, "%c%c%lu", c_, label_, j_); + return std::string(buffer); } /* ************************************************************************* */ diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 7b7ff1403..2956c7575 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include @@ -107,9 +105,9 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, gttic(ccolamd); int rv = ccolamd((int) nFactors, (int) nVars, (int) Alen, &A[0], &p[0], knobs, stats, &cmember[0]); - if (rv != 1) - throw runtime_error( - (boost::format("ccolamd failed with return value %1%") % rv).str()); + if (rv != 1) { + throw runtime_error("ccolamd failed with return value " + to_string(rv)); + } } // ccolamd_report(stats); diff --git a/gtsam/inference/Symbol.cpp b/gtsam/inference/Symbol.cpp index 925ba9ce3..000553d8c 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,12 +18,11 @@ #include -#include -#include - #include #include #include +#include +#include namespace gtsam { @@ -40,8 +39,8 @@ Symbol::Symbol(Key key) : Key Symbol::key() const { if (j_ > indexMask) { - boost::format msg("Symbol index is too large, j=%d, indexMask=%d"); - msg % j_ % indexMask; + std::stringstream msg; + msg << "Symbol index is too large, j=" << j_ << ", indexMask=" << indexMask; throw std::invalid_argument(msg.str()); } Key key = (Key(c_) << indexBits) | j_; @@ -57,7 +56,9 @@ bool Symbol::equals(const Symbol& expected, double tol) const { } Symbol::operator std::string() const { - return str(boost::format("%c%d") % c_ % j_); + char buffer[10]; + snprintf(buffer, 10, "%c%lu", c_, j_); + return std::string(buffer); } static Symbol make(gtsam::Key key) { return Symbol(key);} diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index a1b9e2d83..006e7d67d 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -18,7 +18,6 @@ */ #include -#include #include using namespace std; @@ -49,7 +48,9 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) /*****************************************************************************/ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator( const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); + std::string s = src; + // Convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; /* default is SBM */ diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index e78caa2d9..188c31abe 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -21,13 +21,10 @@ #include #include -#include #ifdef __GNUC__ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" #endif -#include -#include #ifdef __GNUC__ #pragma GCC diagnostic pop #endif @@ -104,22 +101,20 @@ namespace gtsam { void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const { cout << s << " p("; for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { - cout << (boost::format("%1%") % (formatter(*it))).str() - << (nrFrontals() > 1 ? " " : ""); + cout << formatter(*it) << (nrFrontals() > 1 ? " " : ""); } if (nrParents()) { cout << " |"; for (const_iterator it = beginParents(); it != endParents(); ++it) { - cout << " " << (boost::format("%1%") % (formatter(*it))).str(); + cout << " " << formatter(*it); } } cout << ")" << endl; cout << formatMatrixIndented(" R = ", R()) << endl; for (const_iterator it = beginParents() ; it != endParents() ; ++it) { - cout << formatMatrixIndented((boost::format(" S[%1%] = ")%(formatter(*it))).str(), getA(it)) - << endl; + cout << formatMatrixIndented(" S[" + formatter(*it) + "] = ", getA(it)) << endl; } cout << formatMatrixIndented(" d = ", getb(), true) << "\n"; if (nrParents() == 0) { diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp index 343396c0a..196be3402 100644 --- a/gtsam/linear/GaussianDensity.cpp +++ b/gtsam/linear/GaussianDensity.cpp @@ -17,7 +17,6 @@ */ #include -#include #include using std::cout; @@ -37,8 +36,9 @@ GaussianDensity GaussianDensity::FromMeanAndStddev(Key key, const Vector& mean, void GaussianDensity::print(const string& s, const KeyFormatter& formatter) const { cout << s << ": density on "; - for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) - cout << (boost::format("[%1%]") % (formatter(*it))).str() << " "; + for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) { + cout << "[" << formatter(*it) << "] "; + } cout << endl; gtsam::print(mean(), "mean: "); gtsam::print(covariance(), "covariance: "); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 2d16802ac..db943aa70 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -29,8 +29,6 @@ #include #include -#include - #include #include #include "gtsam/base/Vector.h" @@ -134,18 +132,14 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, /* ************************************************************************* */ namespace { -DenseIndex _getSizeHF(const Vector& m) { - return m.size(); -} - -std::vector _getSizeHFVec(const std::vector& m) { +static std::vector _getSizeHFVec(const std::vector& m) { std::vector dims; for (const Vector& v : m) { dims.push_back(v.size()); } return dims; } -} +} // namespace /* ************************************************************************* */ HessianFactor::HessianFactor(const KeyVector& js, diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 5adb1916a..c5a25c4b8 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include using namespace std; @@ -57,7 +56,8 @@ ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator( const string &src) { string s = src; - boost::algorithm::to_upper(s); + // Convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SILENT") return IterativeOptimizationParameters::SILENT; else if (s == "COMPLEXITY") diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 847ac9af0..579f6cbc2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -31,8 +31,6 @@ #include #include -#include - #include #include #include @@ -407,7 +405,7 @@ void JacobianFactor::print(const string& s, if (!s.empty()) cout << s << "\n"; for (const_iterator key = begin(); key != end(); ++key) { - cout << boost::format(" A[%1%] = ") % formatter(*key); + cout << " A[" << formatter(*key) << "] = "; cout << getA(key).format(matlabFormat()) << endl; } cout << formatMatrixIndented(" b = ", getb(), true) << "\n"; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 02e44e2a8..2751e89f2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include #include @@ -606,7 +604,7 @@ Isotropic::shared_ptr Isotropic::Variance(size_t dim, double variance, bool smar /* ************************************************************************* */ void Isotropic::print(const string& name) const { - cout << boost::format("isotropic dim=%1% sigma=%2%") % dim() % sigma_ << endl; + cout << "isotropic dim=" << dim() << " sigma=" << sigma_ << endl; } /* ************************************************************************* */ diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 84837760c..2b84e3736 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -41,7 +40,9 @@ void PreconditionerParameters::print(ostream &os) const { /***************************************************************************************/ PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); + std::string s = src; + // convert string s to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "GTSAM") return PreconditionerParameters::GTSAM; else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD; /* default is cholmod */ @@ -50,7 +51,9 @@ PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(cons /***************************************************************************************/ PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); + std::string s = src; + // convert string to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SILENT") return PreconditionerParameters::SILENT; else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY; else if (s == "ERROR") return PreconditionerParameters::ERROR; diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 8c4d3437e..af31d760b 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -25,7 +25,6 @@ #include #include -#include #include #include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION @@ -133,7 +132,8 @@ ostream &operator<<(ostream &os, const SubgraphBuilderParameters &p) { SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src) { std::string s = src; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "NATURALCHAIN") return NATURALCHAIN; else if (s == "BFS") @@ -161,7 +161,8 @@ std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton s) { SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { std::string s = src; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "EQUAL") return EQUAL; else if (s == "RHS") @@ -196,7 +197,8 @@ SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator( const std::string &src) { std::string s = src; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SKELETON") return SKELETON; // else if (s == "STRETCH") return STRETCH; // else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index ada3a298c..7302380d9 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -18,7 +18,6 @@ #include #include -#include #include namespace gtsam { @@ -29,9 +28,8 @@ namespace gtsam { if(!description_) { description_ = String( "\nIndeterminant linear system detected while working near variable\n" - + boost::lexical_cast(j_) + - + " (Symbol: " + boost::lexical_cast( - gtsam::DefaultKeyFormatter(gtsam::Symbol(j_))) + ").\n" + + std::to_string(j_) + + + " (Symbol: " + gtsam::DefaultKeyFormatter(gtsam::Symbol(j_)) + ").\n" "\n\ Thrown when a linear system is ill-posed. The most common cause for this\n\ error is having underconstrained variables. Mathematically, the system is\n\ @@ -45,22 +43,21 @@ more information."); /* ************************************************************************* */ const char* InvalidNoiseModel::what() const noexcept { if(description_.empty()) - description_ = (boost::format( - "A JacobianFactor was attempted to be constructed or modified to use a\n" - "noise model of incompatible dimension. The JacobianFactor has\n" - "dimensionality (i.e. length of error vector) %d but the provided noise\n" - "model has dimensionality %d.") % factorDims % noiseModelDims).str(); + description_ = "A JacobianFactor was attempted to be constructed or modified to use a\n" + "noise model of incompatible dimension. The JacobianFactor has\n" + "dimensionality (i.e. length of error vector) " + std::to_string(factorDims) + + " but the provided noise model has dimensionality " + std::to_string(noiseModelDims) + "."; return description_.c_str(); } /* ************************************************************************* */ const char* InvalidMatrixBlock::what() const noexcept { - if(description_.empty()) - description_ = (boost::format( - "A JacobianFactor was attempted to be constructed with a matrix block of\n" - "inconsistent dimension. The JacobianFactor has %d rows (i.e. length of\n" - "error vector) but the provided matrix block has %d rows.") - % factorRows % blockRows).str(); + if(description_.empty()) { + description_ = "A JacobianFactor was attempted to be constructed with a matrix block of\n" + "inconsistent dimension. The JacobianFactor has " + std::to_string(factorRows) + + " rows (i.e. length of error vector) but the provided matrix block has " + + std::to_string(blockRows) + " rows."; + } return description_.c_str(); } diff --git a/gtsam/navigation/tests/testGeographicLib.cpp b/gtsam/navigation/tests/testGeographicLib.cpp index c568c7445..9dcc7e4e8 100644 --- a/gtsam/navigation/tests/testGeographicLib.cpp +++ b/gtsam/navigation/tests/testGeographicLib.cpp @@ -23,7 +23,6 @@ #include #include -#include #include #include @@ -71,7 +70,8 @@ TEST( GeographicLib, UTM) { // Obtained by // http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit auto actual = UTMUPS::EncodeZone(zone, northp); - boost::to_upper(actual); + // transform to upper case + std::transform(actual.begin(), actual.end(), actual.begin(), ::toupper); EXPECT(actual=="16N"); EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2); EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2); diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 654c1ad33..8899685de 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -24,13 +24,13 @@ #include #include -#include - namespace gtsam { /* ************************************************************************* */ DoglegParams::VerbosityDL DoglegParams::verbosityDLTranslator(const std::string &verbosityDL) const { - std::string s = verbosityDL; boost::algorithm::to_upper(s); + std::string s = verbosityDL; + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SILENT") return DoglegParams::SILENT; if (s == "VERBOSE") return DoglegParams::VERBOSE; diff --git a/gtsam/nonlinear/ISAM2Params.cpp b/gtsam/nonlinear/ISAM2Params.cpp index c768ce427..ec9d5b773 100644 --- a/gtsam/nonlinear/ISAM2Params.cpp +++ b/gtsam/nonlinear/ISAM2Params.cpp @@ -16,7 +16,6 @@ */ #include -#include using namespace std; @@ -46,7 +45,8 @@ DoglegOptimizerImpl::TrustRegionAdaptationMode ISAM2DoglegParams::adaptationModeTranslator( const string& adaptationMode) const { string s = adaptationMode; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SEARCH_EACH_ITERATION") return DoglegOptimizerImpl::SEARCH_EACH_ITERATION; if (s == "ONE_STEP_PER_ITERATION") @@ -60,7 +60,8 @@ ISAM2DoglegParams::adaptationModeTranslator( ISAM2Params::Factorization ISAM2Params::factorizationTranslator( const string& str) { string s = str; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "QR") return ISAM2Params::QR; if (s == "CHOLESKY") return ISAM2Params::CHOLESKY; diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index e35340377..029f66e52 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -21,7 +21,7 @@ #include #include -#include + #include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index 26049f9da..ef594f6f3 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -27,8 +27,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 3c0624986..66f4c6c70 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -28,11 +28,10 @@ #include #include -#include - #include #include #include +#include #include #include @@ -62,7 +61,8 @@ LevenbergMarquardtOptimizer::LevenbergMarquardtOptimizer(const NonlinearFactorGr /* ************************************************************************* */ void LevenbergMarquardtOptimizer::initTime() { - startTime_ = boost::posix_time::microsec_clock::universal_time(); + // use chrono to measure time in microseconds + startTime_ = std::chrono::high_resolution_clock::now(); } /* ************************************************************************* */ @@ -104,9 +104,12 @@ inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){ if (!params_.logFile.empty()) { ofstream os(params_.logFile.c_str(), ios::app); - boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time(); + // use chrono to measure time in microseconds + auto currentTime = std::chrono::high_resolution_clock::now(); + // Get the time spent in seconds and print it + double timeSpent = std::chrono::duration_cast(currentTime - startTime_).count() / 1e6; os << /*inner iterations*/ currentState->totalNumberInnerIterations << "," - << 1e-6 * (currentTime - startTime_).total_microseconds() << "," + << timeSpent << "," << /*current error*/ currentError << "," << currentState->lambda << "," << /*outer iterations*/ currentState->iterations << endl; } @@ -218,12 +221,12 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, #else double iterationTime = lamda_iteration_timer.elapsed(); #endif - if (currentState->iterations == 0) + if (currentState->iterations == 0) { cout << "iter cost cost_change lambda success iter_time" << endl; - - cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") % currentState->iterations % - newError % costChange % currentState->lambda % systemSolvedSuccessfully % - iterationTime << endl; + } + cout << setw(4) << currentState->iterations << " " << setw(8) << newError << " " << setw(3) << setprecision(2) + << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4) + << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl; } if (step_is_successful) { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index b6b981d65..826eed5d6 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -23,7 +23,7 @@ #include #include #include -#include +#include class NonlinearOptimizerMoreOptimizationTest; @@ -36,7 +36,9 @@ class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer { protected: const LevenbergMarquardtParams params_; ///< LM parameters - boost::posix_time::ptime startTime_; + + // startTime_ is a chrono time point + std::chrono::time_point startTime_; ///< time when optimization started void initTime(); diff --git a/gtsam/nonlinear/LevenbergMarquardtParams.cpp b/gtsam/nonlinear/LevenbergMarquardtParams.cpp index 32de3ffbe..39b773173 100644 --- a/gtsam/nonlinear/LevenbergMarquardtParams.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtParams.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include @@ -31,7 +30,8 @@ namespace gtsam { LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator( const std::string &src) { std::string s = src; - boost::algorithm::to_upper(s); + // convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SILENT") return LevenbergMarquardtParams::SILENT; if (s == "SUMMARY") diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index eac65d587..7312bf6d9 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -18,7 +18,6 @@ #include #include -#include namespace gtsam { @@ -96,12 +95,12 @@ NoiseModelFactor::shared_ptr NoiseModelFactor::cloneWithNewNoiseModel( /* ************************************************************************* */ static void check(const SharedNoiseModel& noiseModel, size_t m) { - if (noiseModel && m != noiseModel->dim()) + if (noiseModel && m != noiseModel->dim()) { throw std::invalid_argument( - boost::str( - boost::format( - "NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.") - % noiseModel->dim() % m)); + "NoiseModelFactor: NoiseModel has dimension " + + std::to_string(noiseModel->dim()) + + " instead of " + std::to_string(m) + "."); + } } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 6fc532bf6..671dbe34d 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -9,7 +9,6 @@ */ #include -#include namespace gtsam { @@ -17,7 +16,8 @@ namespace gtsam { NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator( const std::string &src) { std::string s = src; - boost::algorithm::to_upper(s); + // Convert to upper case + std::transform(s.begin(), s.end(), s.begin(), ::toupper); if (s == "SILENT") return NonlinearOptimizerParams::SILENT; if (s == "ERROR") diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index be2df9056..75cfc24e0 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -338,7 +338,8 @@ namespace gtsam { // supplied \c filter function. template static bool filterHelper(const std::function filter, const ConstKeyValuePair& key_value) { - BOOST_STATIC_ASSERT((!boost::is_same::value)); + // static_assert if ValueType is type: Value + static_assert(!std::is_same::value, "ValueType must not be type: Value to use this filter"); // Filter and check the type return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); } diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index 17557ba3a..d65c99880 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -17,16 +17,17 @@ */ #pragma once + #include // Configuration from CMake #include #include #include -#include - #include + #include #include +#include namespace gtsam { namespace internal { @@ -36,8 +37,8 @@ template struct CallRecord; /// Storage type for the execution trace. /// It enforces the proper alignment in a portable way. /// Provide a traceSize() sized array of this type to traceExecution as traceStorage. -static const unsigned TraceAlignment = 32; -typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; +static const unsigned TraceAlignment = 16; // 16 bytes is the default alignment used by Eigen. +typedef std::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; template struct UseBlockIf { diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9633c5ea0..b5b8303fb 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -41,34 +41,42 @@ #include #include -#include -#include #include #include #include #include #include +#include + +#if defined(__GNUC__) && (__GNUC__ == 7) +#include +namespace fs = std::experimental::filesystem; +#else +#include +namespace fs = std::filesystem; +#endif -using namespace std; -namespace fs = boost::filesystem; using gtsam::symbol_shorthand::L; +using std::cout; +using std::endl; + #define LINESIZE 81920 namespace gtsam { /* ************************************************************************* */ -string findExampleDataFile(const string &name) { +std::string findExampleDataFile(const std::string &name) { // Search source tree and installed location - vector rootsToSearch; + std::vector rootsToSearch; // Constants below are defined by CMake, see gtsam/gtsam/CMakeLists.txt rootsToSearch.push_back(GTSAM_SOURCE_TREE_DATASET_DIR); rootsToSearch.push_back(GTSAM_INSTALLED_DATASET_DIR); // Search for filename as given, and with .graph and .txt extensions - vector namesToSearch; + std::vector namesToSearch; namesToSearch.push_back(name); namesToSearch.push_back(name + ".graph"); namesToSearch.push_back(name + ".txt"); @@ -85,7 +93,7 @@ string findExampleDataFile(const string &name) { } // If we did not return already, then we did not find the file - throw invalid_argument( + throw std::invalid_argument( "gtsam::findExampleDataFile could not find a matching " "file in\n" GTSAM_SOURCE_TREE_DATASET_DIR " or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" + @@ -94,10 +102,10 @@ string findExampleDataFile(const string &name) { } /* ************************************************************************* */ -string createRewrittenFileName(const string &name) { +std::string createRewrittenFileName(const std::string &name) { // Search source tree and installed location if (!exists(fs::path(name))) { - throw invalid_argument( + throw std::invalid_argument( "gtsam::createRewrittenFileName could not find a matching file in\n" + name); } @@ -113,17 +121,17 @@ string createRewrittenFileName(const string &name) { // Type for parser functions used in parseLines below. template using Parser = - std::function(istream &is, const string &tag)>; + std::function(std::istream &is, const std::string &tag)>; // Parse a file by calling the parse(is, tag) function for every line. // The result of calling the function is ignored, so typically parse function // works via a side effect, like adding a factor into a graph etc. template -static void parseLines(const string &filename, Parser parse) { - ifstream is(filename.c_str()); +static void parseLines(const std::string &filename, Parser parse) { + std::ifstream is(filename.c_str()); if (!is) - throw invalid_argument("parse: can not find file " + filename); - string tag; + throw std::invalid_argument("parse: can not find file " + filename); + std::string tag; while (is >> tag) { parse(is, tag); // ignore return value is.ignore(LINESIZE, '\n'); @@ -133,10 +141,10 @@ static void parseLines(const string &filename, Parser parse) { /* ************************************************************************* */ // Parse types T into a size_t-indexed map template -map parseToMap(const string &filename, Parser> parse, +std::map parseToMap(const std::string &filename, Parser> parse, size_t maxIndex) { - map result; - Parser> emplace = [&](istream &is, const string &tag) { + std::map result; + Parser> emplace = [&](std::istream &is, const std::string &tag) { if (auto t = parse(is, tag)) { if (!maxIndex || t->first <= maxIndex) result.emplace(*t); @@ -150,9 +158,9 @@ map parseToMap(const string &filename, Parser> parse, /* ************************************************************************* */ // Parse a file and push results on a vector template -static vector parseToVector(const string &filename, Parser parse) { - vector result; - Parser add = [&result, parse](istream &is, const string &tag) { +static std::vector parseToVector(const std::string &filename, Parser parse) { + std::vector result; + Parser add = [&result, parse](std::istream &is, const std::string &tag) { if (auto t = parse(is, tag)) result.push_back(*t); return std::nullopt; @@ -162,7 +170,7 @@ static vector parseToVector(const string &filename, Parser parse) { } /* ************************************************************************* */ -std::optional parseVertexPose(istream &is, const string &tag) { +std::optional parseVertexPose(std::istream &is, const std::string &tag) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { size_t id; double x, y, yaw; @@ -182,8 +190,8 @@ GTSAM_EXPORT std::map parseVariables( } /* ************************************************************************* */ -std::optional parseVertexLandmark(istream &is, - const string &tag) { +std::optional parseVertexLandmark(std::istream &is, + const std::string &tag) { if (tag == "VERTEX_XY") { size_t id; double x, y; @@ -232,7 +240,7 @@ static SharedNoiseModel createNoiseModel( // v(1)' v(3) v(4); // v(2)' v(4)' v(5) ] if (v(0) == 0.0 || v(3) == 0.0 || v(5) == 0.0) - throw runtime_error( + throw std::runtime_error( "load2D::readNoiseModel looks like this is not G2O matrix order"); M << v(0), v(1), v(2), v(1), v(3), v(4), v(2), v(4), v(5); break; @@ -244,12 +252,12 @@ static SharedNoiseModel createNoiseModel( // v(1)' v(2) v(5); // v(4)' v(5)' v(3) ] if (v(0) == 0.0 || v(2) == 0.0 || v(3) == 0.0) - throw invalid_argument( + throw std::invalid_argument( "load2D::readNoiseModel looks like this is not TORO matrix order"); M << v(0), v(1), v(4), v(1), v(2), v(5), v(4), v(5), v(3); break; default: - throw runtime_error("load2D: invalid noise format"); + throw std::runtime_error("load2D: invalid noise format"); } // Now, create a Gaussian noise model @@ -267,7 +275,7 @@ static SharedNoiseModel createNoiseModel( model = noiseModel::Gaussian::Covariance(M, smart); break; default: - throw invalid_argument("load2D: invalid noise format"); + throw std::invalid_argument("load2D: invalid noise format"); } switch (kernelFunctionType) { @@ -283,12 +291,12 @@ static SharedNoiseModel createNoiseModel( noiseModel::mEstimator::Tukey::Create(4.6851), model); break; default: - throw invalid_argument("load2D: invalid kernel function type"); + throw std::invalid_argument("load2D: invalid kernel function type"); } } /* ************************************************************************* */ -std::optional parseEdge(istream &is, const string &tag) { +std::optional parseEdge(std::istream &is, const std::string &tag) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2") || (tag == "ODOMETRY")) { @@ -316,7 +324,7 @@ template struct ParseFactor : ParseMeasurement { // We parse a measurement then convert typename std::optional::shared_ptr> - operator()(istream &is, const string &tag) { + operator()(std::istream &is, const std::string &tag) { if (auto m = ParseMeasurement::operator()(is, tag)) return std::make_shared>( m->key1(), m->key2(), m->measured(), m->noiseModel()); @@ -341,8 +349,8 @@ template <> struct ParseMeasurement { SharedNoiseModel model; // The actual parser - std::optional> operator()(istream &is, - const string &tag) { + std::optional> operator()(std::istream &is, + const std::string &tag) { auto edge = parseEdge(is, tag); if (!edge) return std::nullopt; @@ -353,7 +361,7 @@ template <> struct ParseMeasurement { // optional filter size_t id1, id2; - tie(id1, id2) = edge->first; + std::tie(id1, id2) = edge->first; if (maxIndex && (id1 > maxIndex || id2 > maxIndex)) return std::nullopt; @@ -375,7 +383,7 @@ template <> struct ParseMeasurement { std::shared_ptr createSampler(const SharedNoiseModel &model) { auto noise = std::dynamic_pointer_cast(model); if (!noise) - throw invalid_argument("gtsam::load: invalid noise model for adding noise" + throw std::invalid_argument("gtsam::load: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); return std::shared_ptr(new Sampler(noise)); } @@ -447,7 +455,7 @@ template <> struct ParseMeasurement { // The actual parser std::optional> - operator()(istream &is, const string &tag) { + operator()(std::istream &is, const std::string &tag) { if (tag != "BR" && tag != "LANDMARK") return std::nullopt; @@ -497,14 +505,14 @@ template <> struct ParseMeasurement { }; /* ************************************************************************* */ -GraphAndValues load2D(const string &filename, SharedNoiseModel model, +GraphAndValues load2D(const std::string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { // Single pass for poses and landmarks. auto initial = std::make_shared(); - Parser insert = [maxIndex, &initial](istream &is, const string &tag) { + Parser insert = [maxIndex, &initial](std::istream &is, const std::string &tag) { if (auto indexedPose = parseVertexPose(is, tag)) { if (!maxIndex || indexedPose->first <= maxIndex) initial->insert(indexedPose->first, indexedPose->second); @@ -529,7 +537,7 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, // Combine in a single parser that adds factors to `graph`, but also inserts // new variables into `initial` when needed. - Parser parse = [&](istream &is, const string &tag) { + Parser parse = [&](std::istream &is, const std::string &tag) { if (auto f = parseBetweenFactor(is, tag)) { graph->push_back(*f); @@ -560,19 +568,20 @@ GraphAndValues load2D(const string &filename, SharedNoiseModel model, parseLines(filename, parse); - return make_pair(graph, initial); + return {graph, initial}; } /* ************************************************************************* */ -GraphAndValues load2D(pair dataset, size_t maxIndex, - bool addNoise, bool smart, NoiseFormat noiseFormat, +GraphAndValues load2D(std::pair dataset, + size_t maxIndex, bool addNoise, bool smart, + NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) { return load2D(dataset.first, dataset.second, maxIndex, addNoise, smart, noiseFormat, kernelFunctionType); } /* ************************************************************************* */ -GraphAndValues load2D_robust(const string &filename, +GraphAndValues load2D_robust(const std::string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex) { return load2D(filename, model, maxIndex); @@ -581,9 +590,9 @@ GraphAndValues load2D_robust(const string &filename, /* ************************************************************************* */ void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, - const string &filename) { + const std::string &filename) { - fstream stream(filename.c_str(), fstream::out); + std::fstream stream(filename.c_str(), std::fstream::out); // save poses for (const auto &key_pose : config.extract()) { @@ -612,7 +621,7 @@ void save2D(const NonlinearFactorGraph &graph, const Values &config, } /* ************************************************************************* */ -GraphAndValues readG2o(const string &g2oFile, const bool is3D, +GraphAndValues readG2o(const std::string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType) { if (is3D) { return load3D(g2oFile); @@ -628,8 +637,8 @@ GraphAndValues readG2o(const string &g2oFile, const bool is3D, /* ************************************************************************* */ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, - const string &filename) { - fstream stream(filename.c_str(), fstream::out); + const std::string &filename) { + std::fstream stream(filename.c_str(), std::fstream::out); // Use a lambda here to more easily modify behavior in future. auto index = [](gtsam::Key key) { return Symbol(key).index(); }; @@ -674,7 +683,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, std::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); - throw invalid_argument("writeG2o: invalid noise model!"); + throw std::invalid_argument("writeG2o: invalid noise model!"); } Matrix3 Info = gaussianModel->R().transpose() * gaussianModel->R(); Pose2 pose = factor->measured(); //.inverse(); @@ -698,7 +707,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, std::dynamic_pointer_cast(model); if (!gaussianModel) { model->print("model\n"); - throw invalid_argument("writeG2o: invalid noise model!"); + throw std::invalid_argument("writeG2o: invalid noise model!"); } Matrix6 Info = gaussianModel->R().transpose() * gaussianModel->R(); const Pose3 pose3D = factor3D->measured(); @@ -729,7 +738,7 @@ void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, /* ************************************************************************* */ // parse quaternion in x,y,z,w order, and normalize to unit length -istream &operator>>(istream &is, Quaternion &q) { +std::istream &operator>>(std::istream &is, Quaternion &q) { double x, y, z, w; is >> x >> y >> z >> w; const double norm = sqrt(w * w + x * x + y * y + z * z), f = 1.0 / norm; @@ -739,7 +748,7 @@ istream &operator>>(istream &is, Quaternion &q) { /* ************************************************************************* */ // parse Rot3 from roll, pitch, yaw -istream &operator>>(istream &is, Rot3 &R) { +std::istream &operator>>(std::istream &is, Rot3 &R) { double yaw, pitch, roll; is >> roll >> pitch >> yaw; // notice order ! R = Rot3::Ypr(yaw, pitch, roll); @@ -747,20 +756,20 @@ istream &operator>>(istream &is, Rot3 &R) { } /* ************************************************************************* */ -std::optional> parseVertexPose3(istream &is, - const string &tag) { +std::optional> parseVertexPose3(std::istream &is, + const std::string &tag) { if (tag == "VERTEX3") { size_t id; double x, y, z; Rot3 R; is >> id >> x >> y >> z >> R; - return make_pair(id, Pose3(R, {x, y, z})); + return std::make_pair(id, Pose3(R, {x, y, z})); } else if (tag == "VERTEX_SE3:QUAT") { size_t id; double x, y, z; Quaternion q; is >> id >> x >> y >> z >> q; - return make_pair(id, Pose3(q, {x, y, z})); + return std::make_pair(id, Pose3(q, {x, y, z})); } else return std::nullopt; } @@ -772,13 +781,13 @@ GTSAM_EXPORT std::map parseVariables( } /* ************************************************************************* */ -std::optional> parseVertexPoint3(istream &is, - const string &tag) { +std::optional> parseVertexPoint3(std::istream &is, + const std::string &tag) { if (tag == "VERTEX_TRACKXYZ") { size_t id; double x, y, z; is >> id >> x >> y >> z; - return make_pair(id, Point3(x, y, z)); + return std::make_pair(id, Point3(x, y, z)); } else return std::nullopt; } @@ -791,7 +800,7 @@ GTSAM_EXPORT std::map parseVariables( /* ************************************************************************* */ // Parse a symmetric covariance matrix (onlyupper-triangular part is stored) -istream &operator>>(istream &is, Matrix6 &m) { +std::istream &operator>>(std::istream &is, Matrix6 &m) { for (size_t i = 0; i < 6; i++) for (size_t j = i; j < 6; j++) { is >> m(i, j); @@ -808,8 +817,8 @@ template <> struct ParseMeasurement { size_t maxIndex; // The actual parser - std::optional> operator()(istream &is, - const string &tag) { + std::optional> operator()(std::istream &is, + const std::string &tag) { if (tag != "EDGE3" && tag != "EDGE_SE3:QUAT") return std::nullopt; @@ -913,7 +922,7 @@ parseFactors(const std::string &filename, } /* ************************************************************************* */ -GraphAndValues load3D(const string &filename) { +GraphAndValues load3D(const std::string &filename) { auto graph = std::make_shared(); auto initial = std::make_shared(); @@ -922,7 +931,7 @@ GraphAndValues load3D(const string &filename) { // Single pass for variables and factors. Unlike 2D version, does *not* insert // variables into `initial` if referenced but not present. - Parser parse = [&](istream &is, const string &tag) { + Parser parse = [&](std::istream &is, const std::string &tag) { if (auto indexedPose = parseVertexPose3(is, tag)) { initial->insert(indexedPose->first, indexedPose->second); } else if (auto indexedLandmark = parseVertexPoint3(is, tag)) { @@ -934,7 +943,7 @@ GraphAndValues load3D(const string &filename) { }; parseLines(filename, parse); - return make_pair(graph, initial); + return {graph, initial}; } // Wrapper-friendly versions of parseFactors and parseFactors diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index a9a1b61fe..8591a3932 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -22,8 +22,6 @@ #include #include -#include - #include #include @@ -38,7 +36,8 @@ TEST(dataSet, findExampleDataFile) { const string expected_end = "examples/Data/example.graph"; const string actual = findExampleDataFile("example"); string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size()); - boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash + // replace all ocurrences of \\ with / use stl + std::replace(actual_end.begin(), actual_end.end(), '\\', '/'); EXPECT(assert_equal(expected_end, actual_end)); } diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index 0622c833c..3526a282d 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -21,7 +21,6 @@ #include #include -#include #include namespace gtsam { @@ -86,13 +85,13 @@ class GTSAM_UNSTABLE_EXPORT Constraint : public DiscreteFactor { /// Render as markdown table. std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override { - return (boost::format("`Constraint` on %1% variables\n") % (size())).str(); + return "`Constraint` on " + std::to_string(size()) + " variables\n"; } /// Render as html table. std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, const Names& names = {}) const override { - return (boost::format("

Constraint on %1% variables

") % (size())).str(); + return "

Constraint on " + std::to_string(size()) + " variables

"; } /// @} diff --git a/gtsam_unstable/discrete/examples/schedulingExample.cpp b/gtsam_unstable/discrete/examples/schedulingExample.cpp index dd92b295d..2b52a3e3a 100644 --- a/gtsam_unstable/discrete/examples/schedulingExample.cpp +++ b/gtsam_unstable/discrete/examples/schedulingExample.cpp @@ -12,8 +12,6 @@ #include #include -#include - #include using namespace std; @@ -171,8 +169,8 @@ void solveStaged(size_t addMutex = 2) { // remove this slot from consideration slotsAvailable[bestSlot] = 0.0; - cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(6-s) - % scheduler.slotName(bestSlot) % bestSlot % count << endl; + cout << scheduler.studentName(6 - s) << " = " << scheduler.slotName(bestSlot) << " (" << bestSlot + << "), count = " << count << endl; } tictoc_print_(); @@ -229,9 +227,8 @@ void sampleSolutions() { size_t min = *min_element(stats.begin(), stats.end()); size_t nz = count_if(stats.begin(), stats.end(), NonZero); if (nz >= 15 && max <= 2) { - cout << boost::format( - "Sampled schedule %d, min = %d, nz = %d, max = %d\n") % (n + 1) % min - % nz % max; + cout << "Sampled schedule " << (n + 1) << ", min = " << min << ", nz = " << nz + << ", max = " << max << endl; for (size_t i = 0; i < 7; i++) { cout << schedulers[i].studentName(0) << " : " << schedulers[i].slotName( slots[i]) << endl; @@ -320,9 +317,8 @@ void accomodateStudent() { DiscreteValues values; values[dkey.first] = bestSlot; size_t count = (*root)(values); - cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(0) - % scheduler.slotName(bestSlot) % bestSlot % count << endl; - + cout << scheduler.studentName(0) << " = " << scheduler.slotName(bestSlot) << " (" << bestSlot + << "), count = " << count << endl; // sample schedules for (size_t n = 0; n < 10; n++) { auto sample0 = chordal->sample(); diff --git a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp index 638fc9e58..913bfb21a 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals12.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals12.cpp @@ -12,8 +12,6 @@ #include #include -#include - #include using namespace std; @@ -193,8 +191,9 @@ void solveStaged(size_t addMutex = 2) { // remove this slot from consideration slotsAvailable[bestSlot] = 0.0; - cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(NRSTUDENTS-1-s) - % scheduler.slotName(bestSlot) % bestSlot % count << endl; + cout << scheduler.studentName(NRSTUDENTS - 1 - s) << " = " << + scheduler.slotName(bestSlot) << " (" << bestSlot + << "), count = " << count << endl; } tictoc_print_(); } @@ -234,9 +233,8 @@ void sampleSolutions() { size_t min = *min_element(stats.begin(), stats.end()); size_t nz = count_if(stats.begin(), stats.end(), NonZero); if (nz >= 15 && max <= 2) { - cout << boost::format( - "Sampled schedule %d, min = %d, nz = %d, max = %d\n") % (n + 1) % min - % nz % max; + cout << "Sampled schedule " << (n + 1) << ", min = " << min + << ", nz = " << nz << ", max = " << max << endl; for (size_t i = 0; i < NRSTUDENTS; i++) { cout << schedulers[i].studentName(0) << " : " << schedulers[i].slotName( slots[i]) << endl; diff --git a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp index 06276c516..e52e3d0c6 100644 --- a/gtsam_unstable/discrete/examples/schedulingQuals13.cpp +++ b/gtsam_unstable/discrete/examples/schedulingQuals13.cpp @@ -12,8 +12,6 @@ #include #include -#include - #include using namespace std; @@ -218,8 +216,8 @@ void solveStaged(size_t addMutex = 2) { // remove this slot from consideration slotsAvailable[bestSlot] = 0.0; - cout << boost::format("%s = %d (%d), count = %d") % scheduler.studentName(NRSTUDENTS-1-s) - % scheduler.slotName(bestSlot) % bestSlot % count << endl; + cout << scheduler.studentName(NRSTUDENTS - 1 - s) << " = " << scheduler.slotName(bestSlot) << " (" << bestSlot + << "), count = " << count << endl; } tictoc_print_(); } @@ -263,9 +261,7 @@ void sampleSolutions() { size_t min = *min_element(stats.begin(), stats.end()); size_t nz = count_if(stats.begin(), stats.end(), NonZero); if (nz >= 16 && max <= 3) { - cout << boost::format( - "Sampled schedule %d, min = %d, nz = %d, max = %d\n") % (n + 1) % min - % nz % max; + cout << "Sampled schedule " << n + 1 << ", min = " << min << ", nz = " << nz << ", max = " << max << endl; for (size_t i = 0; i < NRSTUDENTS; i++) { cout << schedulers[i].studentName(0) << " : " << schedulers[i].slotName( slots[i]) << endl; diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 1b0923774..7c769fd78 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -71,7 +71,7 @@ class LoopyBelief { void print(const std::string& s = "") const { cout << s << ":" << endl; for (const auto& [key, _] : starGraphs_) { - starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); + starGraphs_.at(key).print("Node " + std::to_string(key) + ":"); } } @@ -124,9 +124,11 @@ class LoopyBelief { val[key] = v; sum += (*beliefAtKey)(val); } + // TODO(kartikarcot): Check if this makes sense string sumFactorTable; - for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v) - sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); + for (size_t v = 0; v < allDiscreteKeys.at(key).second; ++v) { + sumFactorTable = sumFactorTable + " " + std::to_string(sum); + } DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); beliefAtKey = diff --git a/gtsam_unstable/examples/TimeOfArrivalExample.cpp b/gtsam_unstable/examples/TimeOfArrivalExample.cpp index 8d496a30e..a97aa27be 100644 --- a/gtsam_unstable/examples/TimeOfArrivalExample.cpp +++ b/gtsam_unstable/examples/TimeOfArrivalExample.cpp @@ -24,8 +24,6 @@ #include #include -#include - #include using namespace std; diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index a3f2132f8..2d404005d 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -16,7 +16,6 @@ */ #include -#include #include namespace gtsam { @@ -69,8 +68,9 @@ void LinearizedJacobianFactor::print(const std::string& s, const KeyFormatter& k std::cout << keyFormatter(key) << " "; std::cout << std::endl; - for(const_iterator key=begin(); key!=end(); ++key) - std::cout << boost::format("A[%1%]=\n")%keyFormatter(*key) << A(*key) << std::endl; + for(const_iterator key=begin(); key!=end(); ++key) { + std::cout << "A[" << keyFormatter(*key) << "]=\n" << A(*key) << std::endl; + } std::cout << "b=\n" << b() << std::endl; lin_points_.print("Linearization Point: "); diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index a7e266cb5..5b98540e2 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -19,6 +19,8 @@ #include #include +#include + using namespace gtsam; @@ -46,8 +48,8 @@ struct ProjectionChart { namespace gtsam { namespace traits { -template<> struct is_chart : public boost::true_type {}; -template<> struct dimension : public boost::integral_constant {}; +template<> struct is_chart : public std::true_type {}; +template<> struct dimension : public std::integral_constant {}; } // namespace traits } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp index bc5c553e0..b6628c138 100644 --- a/gtsam_unstable/slam/tests/testTOAFactor.cpp +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -25,7 +25,6 @@ #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/timing/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp index 35ae7d4d5..7cc66b5b3 100644 --- a/gtsam_unstable/timing/timeDSFvariants.cpp +++ b/gtsam_unstable/timing/timeDSFvariants.cpp @@ -21,8 +21,6 @@ #include #include -#include - #include #include #include @@ -31,7 +29,6 @@ using namespace std; using namespace gtsam; -using boost::format; int main(int argc, char* argv[]) { @@ -52,8 +49,7 @@ int main(int argc, char* argv[]) { volatile double fpm = 0.5; // fraction of points matched volatile size_t nm = fpm * n * np; // number of matches - cout << format("\nTesting with %1% images, %2% points, %3% matches\n") - % (int)m % (int)N % (int)nm; + cout << "\nTesting with " << (int)m << " images, " << (int)N << " points, " << (int)nm << " matches\n"; cout << "Generating " << nm << " matches" << endl; std::mt19937 rng; std::uniform_int_distribution<> rn(0, N - 1); @@ -64,7 +60,7 @@ int main(int argc, char* argv[]) { for (size_t k = 0; k < nm; k++) matches.push_back(Match(rn(rng), rn(rng))); - os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm; + os << (int)m << "," << (int)N << "," << (int)nm << ","; { // DSFBase version @@ -77,7 +73,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(dsftimeNode, dsftime); dsftime = dsftimeNode->secs(); os << dsftime << ","; - cout << format("DSFBase: %1% s") % dsftime << endl; + cout << "DSFBase: " << dsftime << " s" << endl; tictoc_reset_(); } @@ -92,7 +88,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(dsftimeNode, dsftime); dsftime = dsftimeNode->secs(); os << dsftime << endl; - cout << format("DSFMap: %1% s") % dsftime << endl; + cout << "DSFMap: " << dsftime << " s" << endl; tictoc_reset_(); } @@ -109,7 +105,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(dsftimeNode, dsftime); dsftime = dsftimeNode->secs(); os << dsftime << endl; - cout << format("DSF functional: %1% s") % dsftime << endl; + cout << "DSF functional: " << dsftime << " s" << endl; tictoc_reset_(); } @@ -126,7 +122,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(dsftimeNode, dsftime); dsftime = dsftimeNode->secs(); os << dsftime << ","; - cout << format("DSF in-place: %1% s") % dsftime << endl; + cout << "DSF in-place: " << dsftime << " s" << endl; tictoc_reset_(); } diff --git a/timing/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp index 95333fbf8..2e252e7c6 100644 --- a/timing/timeMatrixOps.cpp +++ b/timing/timeMatrixOps.cpp @@ -16,9 +16,6 @@ * @date Sep 18, 2010 */ -#include -#include - #include #include @@ -30,8 +27,6 @@ using namespace std; //namespace ublas = boost::numeric::ublas; //using namespace Eigen; -using boost::format; -using namespace boost::lambda; static std::mt19937 rng; static std::uniform_real_distribution<> uniform(-1.0, 0.0); @@ -61,10 +56,10 @@ int main(int argc, char* argv[]) { gtsam::SubMatrix top = mat.block(0, 0, n, n); gtsam::SubMatrix block = mat.block(m/4, n/4, m-m/2, n-n/2); - cout << format(" Basic: %1%x%2%\n") % (int)m % (int)n; - cout << format(" Full: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)m % 0 % (int)n; - cout << format(" Top: mat(%1%:%2%, %3%:%4%)\n") % 0 % (int)n % 0 % (int)n; - cout << format(" Block: mat(%1%:%2%, %3%:%4%)\n") % size_t(m/4) % size_t(m-m/4) % size_t(n/4) % size_t(n-n/4); + cout << " Basic: " << (int)m << "x" << (int)n << endl; + cout << " Full: mat(" << 0 << ":" << (int)m << ", " << 0 << ":" << (int)n << ")" << endl; + cout << " Top: mat(" << 0 << ":" << (int)n << ", " << 0 << ":" << (int)n << ")" << endl; + cout << " Block: mat(" << size_t(m/4) << ":" << size_t(m-m/4) << ", " << size_t(n/4) << ":" << size_t(n-n/4) << ")" << endl; cout << endl; { @@ -87,7 +82,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(basicTimeNode, basicTime); basicTime = basicTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl; + cout << " Basic: " << double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << " μs/element" << endl; gttic_(fullTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl; + cout << " Full: " << double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << " μs/element" << endl; gttic_(topTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl; + cout << " Top: " << double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << " μs/element" << endl; gttic_(blockTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl; + cout << " Block: " << double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << " μs/element" << endl; cout << endl; } @@ -145,7 +140,7 @@ int main(int argc, char* argv[]) { tictoc_getNode(basicTimeNode, basicTime); basicTime = basicTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << endl; + cout << " Basic: " << double(1000000 * basicTime / double(mat.rows()*mat.cols()*nReps)) << " μs/element" << endl; gttic_(fullTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << endl; + cout << " Full: " << double(1000000 * fullTime / double(full.rows()*full.cols()*nReps)) << " μs/element" << endl; gttic_(topTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << endl; + cout << " Top: " << double(1000000 * topTime / double(top.rows()*top.cols()*nReps)) << " μs/element" << endl; gttic_(blockTime); for(size_t rep=0; repsecs(); gtsam::tictoc_reset_(); - cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << endl; + cout << " Block: " << double(1000000 * blockTime / double(block.rows()*block.cols()*nReps)) << " μs/element" << endl; cout << endl; } { double basicTime, fullTime, topTime, blockTime; - typedef pair ij_t; - vector ijs(100000); + typedef std::pair ij_t; + std::vector ijs(100000); cout << "Row-major matrix, random assignment:" << endl; // Do a few initial assignments to let any cache effects stabilize - for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); + for (auto& ij : ijs) ij = {uniform_i(rng), uniform_j(rng)}; for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); } + for(const auto& [i, j]: ijs) { mat(i, j) = uniform(rng); } gttic_(basicTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); + for (auto& ij : ijs) ij = {uniform_i(rng), uniform_j(rng)}; for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { mat(ij.first, ij.second) = uniform(rng); } + for(const auto& [i, j]: ijs) { mat(i, j) = uniform(rng); } gttoc_(basicTime); tictoc_getNode(basicTimeNode, basicTime); basicTime = basicTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Basic: %1% mus/element") % double(1000000 * basicTime / double(ijs.size()*nReps)) << endl; + cout << " Basic: " << double(1000000 * basicTime / double(ijs.size()*nReps)) << " μs/element" << endl; gttic_(fullTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng),uniform_j(rng))); + for (auto& ij : ijs) ij = {uniform_i(rng), uniform_j(rng)}; for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { full(ij.first, ij.second) = uniform(rng); } + for(const auto& [i, j]: ijs) { full(i, j) = uniform(rng); } gttoc_(fullTime); tictoc_getNode(fullTimeNode, fullTime); fullTime = fullTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Full: %1% mus/element") % double(1000000 * fullTime / double(ijs.size()*nReps)) << endl; + cout << " Full: " << double(1000000 * fullTime / double(ijs.size()*nReps)) << " μs/element" << endl; gttic_(topTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%top.rows(),uniform_j(rng))); + for (auto& ij : ijs) ij = {uniform_i(rng) % top.rows(), uniform_j(rng)}; for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { top(ij.first, ij.second) = uniform(rng); } + for(const auto& [i, j]: ijs) { top(i, j) = uniform(rng); } gttoc_(topTime); tictoc_getNode(topTimeNode, topTime); topTime = topTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Top: %1% mus/element") % double(1000000 * topTime / double(ijs.size()*nReps)) << endl; + cout << " Top: " << double(1000000 * topTime / double(ijs.size()*nReps)) << " μs/element" << endl; gttic_(blockTime); - for_each(ijs.begin(), ijs.end(), _1 = make_pair(uniform_i(rng)%block.rows(),uniform_j(rng)%block.cols())); + for (auto& ij : ijs) + ij = {uniform_i(rng) % block.rows(), uniform_j(rng) % block.cols()}; for(size_t rep=0; rep<1000; ++rep) - for(const ij_t& ij: ijs) { block(ij.first, ij.second) = uniform(rng); } + for(const auto& [i, j]: ijs) { block(i, j) = uniform(rng); } gttoc_(blockTime); tictoc_getNode(blockTimeNode, blockTime); blockTime = blockTimeNode->secs(); gtsam::tictoc_reset_(); - cout << format(" Block: %1% mus/element") % double(1000000 * blockTime / double(ijs.size()*nReps)) << endl; + cout << " Block: " << double(1000000 * blockTime / double(ijs.size()*nReps)) << " μs/element" << endl; cout << endl; }