From b925142b987af501fb86d738079e6491940f5e37 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:13:02 +0200 Subject: [PATCH 01/48] Create pose3Localizationexample.txt pose3example.txt without loop closure --- examples/Data/pose3Localizationexample.txt | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 examples/Data/pose3Localizationexample.txt diff --git a/examples/Data/pose3Localizationexample.txt b/examples/Data/pose3Localizationexample.txt new file mode 100644 index 000000000..a35005aa2 --- /dev/null +++ b/examples/Data/pose3Localizationexample.txt @@ -0,0 +1,9 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 +VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446 +VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024 +VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488 +EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 +EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000 From 3d7ce45de66bb401cdd7c124cf0b4ef0fd228a57 Mon Sep 17 00:00:00 2001 From: Martin Brossard Date: Fri, 2 Aug 2019 17:14:31 +0200 Subject: [PATCH 02/48] Create Pose3Localization.cpp Pose3Example_g2o with marginal computation. --- examples/Pose3Localization.cpp | 83 ++++++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 examples/Pose3Localization.cpp diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp new file mode 100644 index 000000000..becb9530c --- /dev/null +++ b/examples/Pose3Localization.cpp @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3Localizationexample.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the first key + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + Key firstKey = 0; + for(const Values::ConstKeyValuePair& key_value: *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + break; + } + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); // this will show info about stopping conditions + GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "final error=" <error(result)<< std::endl; + + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, result, outputFile); + std::cout << "done! " << std::endl; + } + + // Calculate and print marginal covariances for all variables + Marginals marginals(*graph, result); + for(const auto& key_value: result) { + auto p = dynamic_cast*>(&key_value.value); + if (!p) continue; + std::cout << marginals.marginalCovariance(key_value.key) << endl; + } + return 0; +} From c9bcceef2f01e9eb5e22fa172c6d4cbb6171124e Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 12 Jan 2020 11:34:02 -0500 Subject: [PATCH 03/48] add tbb to travis-ci --- .travis.yml | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/.travis.yml b/.travis.yml index 14fe66ac1..e4e1a3440 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,6 +16,7 @@ addons: - cmake - libpython-dev python-numpy - libboost-all-dev + - libtbb-dev # before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi @@ -43,7 +44,7 @@ jobs: - stage: compile os: osx compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -54,7 +55,7 @@ jobs: - stage: compile os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -65,7 +66,7 @@ jobs: - stage: compile os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -76,7 +77,7 @@ jobs: - stage: compile os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -87,7 +88,13 @@ jobs: - stage: special os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON GTSAM_WITH_TBB=OFF + script: bash .travis.sh -b +# on Linux, with GTSAM_WITH_TBB not off to make sure GTSAM still compiles/tests + - stage: special + os: linux + compiler: gcc + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b # -------- STAGE 2: TESTS ----------- # on Mac, GCC @@ -99,7 +106,7 @@ jobs: - stage: test os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -t - stage: test os: linux @@ -109,7 +116,7 @@ jobs: - stage: test os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF script: bash .travis.sh -t - stage: test os: linux From e312abdbf0a77dcbba6fd255cd51896531d7401f Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 15:22:19 -0500 Subject: [PATCH 04/48] Add More Unit Tests for Robust Noise Models --- tests/testNonlinearOptimizer.cpp | 191 +++++++++++++++++++++++++++++-- 1 file changed, 180 insertions(+), 11 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e7793552..7cf5e1e2d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -339,31 +340,199 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { } /* ************************************************************************* */ -TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { +TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; fg += PriorFactor(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(1, 2, Pose2(1,0,M_PI/2), + fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); Values init; - init.insert(0, Pose2(10,10,0)); - init.insert(1, Pose2(1,0,M_PI)); - init.insert(2, Pose2(1,1,-M_PI)); + init.insert(0, Pose2(0,0,0)); + init.insert(1, Pose2(0.961187, 0.99965, 1.1781)); Values expected; expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,0,M_PI/2)); - expected.insert(2, Pose2(1,1,M_PI)); + expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); + + LevenbergMarquardtParams lm_params; + lm_params.setRelativeErrorTol(0); + lm_params.setAbsoluteErrorTol(0); + lm_params.setMaxIterations(100); + lm_params.setlambdaUpperBound(1e10); + // lm_params.setVerbosityLM("TRYLAMBDA"); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + DoglegParams dl_params; + dl_params.setRelativeErrorTol(0); + dl_params.setAbsoluteErrorTol(0); + dl_params.setMaxIterations(100); + + // cg_params.setVerbosity("ERROR"); + + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + + EXPECT(assert_equal(expected, cg_result, 3e-2)); + EXPECT(assert_equal(expected, gn_result, 3e-2)); + EXPECT(assert_equal(expected, lm_result, 3e-2)); + EXPECT(assert_equal(expected, dl_result, 3e-2)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); + fg += BetweenFactor(0, 1, Point2(1,1.8), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,0.9), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + fg += BetweenFactor(0, 1, Point2(1,90), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), + noiseModel::Isotropic::Sigma(2,1))); + + Values init; + init.insert(0, Point2(1,1)); + init.insert(1, Point2(1,0)); + + Values expected; + expected.insert(0, Point2(0,0)); + expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); - EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, gn_result, 1e-4)); + EXPECT(assert_equal(expected, lm_result, 1e-4)); + EXPECT(assert_equal(expected, dl_result, 1e-4)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { + + NonlinearFactorGraph fg; + fg += PriorFactor(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); + fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + fg += BetweenFactor(0, 1, Pose2(0,9, 0), + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), + noiseModel::Isotropic::Sigma(3,1))); + + Values init; + init.insert(0, Pose2(0, 0, 0)); + init.insert(1, Pose2(0, 10, M_PI/4)); + + Values expected; + expected.insert(0, Pose2(0, 0, 0)); + expected.insert(1, Pose2(0, 10, 1.45212)); + + LevenbergMarquardtParams params; + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(100000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); + fg.printErrors(cg_result); + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); + + auto dl_result = DoglegOptimizer(fg, init).optimize(); + + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, gn_result, 1e-1)); + EXPECT(assert_equal(expected, lm_result, 1e-1)); + EXPECT(assert_equal(expected, dl_result, 1e-1)); +} + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, RobustMeanCalculation) { + + NonlinearFactorGraph fg; + + Values init; + + Values expected; + + auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20), + noiseModel::Isotropic::Sigma(1, 1)); + + vector pts{-10,-3,-1,1,3,10,1000}; + for(auto pt : pts) { + fg += PriorFactor(0, pt, huber); + } + + init.insert(0, 100.0); + expected.insert(0, 3.33333333); + + LevenbergMarquardtParams params; + // params.setVerbosityLM("TRYLAMBDA"); + params.setAbsoluteErrorTol(1e-20); + params.setRelativeErrorTol(1e-20); + + gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + cg_params.setErrorTol(0); + cg_params.setMaxIterations(10000); + cg_params.setRelativeErrorTol(0); + cg_params.setAbsoluteErrorTol(0); + // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); +// cg_result.print("CG: "); +// cout << fg.error(cg_result) << endl << endl << endl; + + auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); +// gn_result.print("GN: "); +// cout << fg.error(gn_result) << endl << endl << endl; + + auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); +// lm_result.print("LM: "); +// cout << fg.error(lm_result) << endl << endl << endl; + + auto dl_result = DoglegOptimizer(fg, init).optimize(); +// dl_result.print("DL: "); +// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, gn_result, tol)); + EXPECT(assert_equal(expected, lm_result, tol)); + EXPECT(assert_equal(expected, dl_result, tol)); } /* ************************************************************************* */ From 3c0671ba8de1a5fa9b115fc8cf6e5ac3b768ce41 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 1 Mar 2020 19:38:57 -0500 Subject: [PATCH 05/48] Removed commentted out and print-s --- tests/testNonlinearOptimizer.cpp | 34 ++++---------------------------- 1 file changed, 4 insertions(+), 30 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 7cf5e1e2d..4b7ca6a03 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -364,9 +364,8 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { lm_params.setAbsoluteErrorTol(0); lm_params.setMaxIterations(100); lm_params.setlambdaUpperBound(1e10); - // lm_params.setVerbosityLM("TRYLAMBDA"); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; + NonlinearConjugateGradientOptimizer::Parameters cg_params; cg_params.setErrorTol(0); cg_params.setMaxIterations(100000); cg_params.setRelativeErrorTol(0); @@ -377,8 +376,6 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { dl_params.setAbsoluteErrorTol(0); dl_params.setMaxIterations(100); - // cg_params.setVerbosity("ERROR"); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); @@ -414,19 +411,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); + NonlinearConjugateGradientOptimizer::Parameters cg_params; - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-4)); @@ -468,13 +457,9 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); - fg.printErrors(cg_result); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); - auto dl_result = DoglegOptimizer(fg, init).optimize(); EXPECT(assert_equal(expected, gn_result, 1e-1)); @@ -504,7 +489,6 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - // params.setVerbosityLM("TRYLAMBDA"); params.setAbsoluteErrorTol(1e-20); params.setRelativeErrorTol(1e-20); @@ -513,22 +497,12 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { cg_params.setMaxIterations(10000); cg_params.setRelativeErrorTol(0); cg_params.setAbsoluteErrorTol(0); - // cg_params.setVerbosity("ERROR"); + auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); -// cg_result.print("CG: "); -// cout << fg.error(cg_result) << endl << endl << endl; - auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); -// gn_result.print("GN: "); -// cout << fg.error(gn_result) << endl << endl << endl; - auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); -// lm_result.print("LM: "); -// cout << fg.error(lm_result) << endl << endl << endl; - auto dl_result = DoglegOptimizer(fg, init).optimize(); -// dl_result.print("DL: "); -// cout << fg.error(dl_result) << endl << endl << endl; + EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); From 4babfe24911eba8348ce5ff857e68637be524862 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 Mar 2020 17:39:28 -0500 Subject: [PATCH 06/48] Remove redundant params --- tests/testNonlinearOptimizer.cpp | 39 +------------------------------- 1 file changed, 1 insertion(+), 38 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 4b7ca6a03..7b5e7a0e0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -360,28 +360,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { expected.insert(1, Pose2(0.961187, 0.99965, 1.1781)); LevenbergMarquardtParams lm_params; - lm_params.setRelativeErrorTol(0); - lm_params.setAbsoluteErrorTol(0); - lm_params.setMaxIterations(100); - lm_params.setlambdaUpperBound(1e10); - NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - DoglegParams dl_params; - dl_params.setRelativeErrorTol(0); - dl_params.setAbsoluteErrorTol(0); - dl_params.setMaxIterations(100); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize(); - auto dl_result = DoglegOptimizer(fg, init, dl_params).optimize(); + auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, cg_result, 3e-2)); EXPECT(assert_equal(expected, gn_result, 3e-2)); EXPECT(assert_equal(expected, lm_result, 3e-2)); EXPECT(assert_equal(expected, dl_result, 3e-2)); @@ -411,14 +394,11 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { expected.insert(1, Point2(1,1.85)); LevenbergMarquardtParams params; - NonlinearConjugateGradientOptimizer::Parameters cg_params; - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, gn_result, 1e-4)); EXPECT(assert_equal(expected, lm_result, 1e-4)); EXPECT(assert_equal(expected, dl_result, 1e-4)); @@ -451,18 +431,11 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { expected.insert(1, Pose2(0, 10, 1.45212)); LevenbergMarquardtParams params; - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(100000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, gn_result, 1e-1)); EXPECT(assert_equal(expected, lm_result, 1e-1)); EXPECT(assert_equal(expected, dl_result, 1e-1)); @@ -489,21 +462,11 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) { expected.insert(0, 3.33333333); LevenbergMarquardtParams params; - params.setAbsoluteErrorTol(1e-20); - params.setRelativeErrorTol(1e-20); - gtsam::NonlinearConjugateGradientOptimizer::Parameters cg_params; - cg_params.setErrorTol(0); - cg_params.setMaxIterations(10000); - cg_params.setRelativeErrorTol(0); - cg_params.setAbsoluteErrorTol(0); - - auto cg_result = NonlinearConjugateGradientOptimizer(fg, init, cg_params).optimize(); auto gn_result = GaussNewtonOptimizer(fg, init).optimize(); auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize(); auto dl_result = DoglegOptimizer(fg, init).optimize(); - EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, gn_result, tol)); EXPECT(assert_equal(expected, lm_result, tol)); EXPECT(assert_equal(expected, dl_result, tol)); From 6ec13bdcd5370fbe4022a37ec61d6b9c18710662 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Sun, 12 Jan 2020 14:19:40 -0500 Subject: [PATCH 07/48] add tbb version guard to fix clang build (cherry picked from commit 9b912f6b14d2cf715d17208df35b8253d5e648e7) --- CMakeLists.txt | 5 +++++ gtsam/linear/VectorValues.cpp | 28 ++++++++++++++++++++++++++++ gtsam/linear/VectorValues.h | 6 +++++- 3 files changed, 38 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c84835d82..56250a12c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,6 +200,11 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h + if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() # all definitions and link requisites will go via imported targets: # tbb & tbbmalloc list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index f18ad9c5f..e74d3776d 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -51,7 +51,11 @@ namespace gtsam { Key key; size_t n; boost::tie(key, n) = v; +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(key, x.segment(j, n)); +#else + values_.insert(std::make_pair(key, x.segment(j, n))); +#endif j += n; } } @@ -60,7 +64,11 @@ namespace gtsam { VectorValues::VectorValues(const Vector& x, const Scatter& scatter) { size_t j = 0; for (const SlotEntry& v : scatter) { +#ifdef TBB_GREATER_EQUAL_2020 values_.emplace(v.key, x.segment(j, v.dimension)); +#else + values_.insert(std::make_pair(v.key, x.segment(j, v.dimension))); +#endif j += v.dimension; } } @@ -70,7 +78,11 @@ namespace gtsam { { VectorValues result; for(const KeyValuePair& v: other) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(v.first, Vector::Zero(v.second.size())); +#else + result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size()))); +#endif return result; } @@ -86,7 +98,11 @@ namespace gtsam { /* ************************************************************************* */ VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) { +#ifdef TBB_GREATER_EQUAL_2020 std::pair result = values_.emplace(j, value); +#else + std::pair result = values_.insert(std::make_pair(j, value)); +#endif if(!result.second) throw std::invalid_argument( "Requested to emplace variable '" + DefaultKeyFormatter(j) @@ -266,7 +282,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second + j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second + j2->second)); +#endif return result; } @@ -324,7 +344,11 @@ namespace gtsam { VectorValues result; // The result.end() hint here should result in constant-time inserts for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(j1->first, j1->second - j2->second); +#else + result.values_.insert(std::make_pair(j1->first, j1->second - j2->second)); +#endif return result; } @@ -340,7 +364,11 @@ namespace gtsam { { VectorValues result; for(const VectorValues::KeyValuePair& key_v: v) +#ifdef TBB_GREATER_EQUAL_2020 result.values_.emplace(key_v.first, a * key_v.second); +#else + result.values_.insert(std::make_pair(key_v.first, a * key_v.second)); +#endif return result; } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 46da2d5f9..eed212eda 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -198,7 +198,11 @@ namespace gtsam { * exist, it is inserted and an iterator pointing to the new element, along with 'true', is * returned. */ std::pair tryInsert(Key j, const Vector& value) { - return values_.emplace(j, value); +#ifdef TBB_GREATER_EQUAL_2020 + return values_.emplace(j, value); +#else + return values_.insert(std::make_pair(j, value)); +#endif } /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ From 73271816a6c26aab9d40de7f8011dddf5d243e30 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 12:45:21 -0400 Subject: [PATCH 08/48] make exceptions as const reference --- gtsam/nonlinear/utilities.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 867db70e0..1228cd4db 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -237,12 +237,12 @@ Values localToWorld(const Values& local, const Pose2& base, // if value is a Pose2, compose it with base pose Pose2 pose = local.at(key); world.insert(key, base.compose(pose)); - } catch (std::exception e1) { + } catch (const std::exception& e1) { try { // if value is a Point2, transform it from base pose Point2 point = local.at(key); world.insert(key, base.transformFrom(point)); - } catch (std::exception e2) { + } catch (const std::exception& e2) { // if not Pose2 or Point2, do nothing } } From d2d5ce1166d6e01101b2679020edf66899305702 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 12:46:25 -0400 Subject: [PATCH 09/48] Eigen alignment --- gtsam/nonlinear/NonlinearEquality.h | 6 ++++++ gtsam_unstable/slam/AHRS.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 4d928482e..d4eb655c3 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -175,6 +175,8 @@ public: /// @} + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -263,6 +265,8 @@ public: traits::Print(value_, "Value"); } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ @@ -327,6 +331,8 @@ public: return traits::Local(x1,x2); } + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + private: /** Serialization function */ diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index f22de48cf..35b4677d5 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -77,6 +77,8 @@ public: void print(const std::string& s = "") const; virtual ~AHRS(); + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; } /* namespace gtsam */ From c2d7df3f148894badce29fd7ee21cdaa46e9fca2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:03:10 -0400 Subject: [PATCH 10/48] make exception as reference --- gtsam/geometry/tests/testEssentialMatrix.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index c923e398b..86a498cdc 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) { Matrix actH1, actH2; try { bodyE.rotate(cRb, actH1, actH2); - } catch (exception e) { + } catch (exception& e) { } // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); From 9bef6bfded7c0c46d4706d0d23f1b3fe963c539d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:04:04 -0400 Subject: [PATCH 11/48] initialize PreintegrationParams default constructor and make serialization test more explicit --- gtsam/navigation/PreintegrationParams.h | 9 +++++++-- gtsam/navigation/tests/testImuFactorSerialization.cpp | 6 +++--- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/gtsam/navigation/PreintegrationParams.h b/gtsam/navigation/PreintegrationParams.h index 0fb54a358..962fef277 100644 --- a/gtsam/navigation/PreintegrationParams.h +++ b/gtsam/navigation/PreintegrationParams.h @@ -29,6 +29,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration Vector3 n_gravity; ///< Gravity vector in nav frame + /// Default constructor for serialization only + PreintegrationParams() + : accelerometerCovariance(I_3x3), + integrationCovariance(I_3x3), + use2ndOrderCoriolis(false), + n_gravity(0, 0, -1) {} + /// The Params constructor insists on getting the navigation frame gravity vector /// For convenience, two commonly used conventions are provided by named constructors below PreintegrationParams(const Vector3& n_gravity) @@ -60,8 +67,6 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams { bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } protected: - /// Default constructor for serialization only: uninitialized! - PreintegrationParams() {} /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index 9f9781d2c..59d0ac199 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) { ImuFactor factor(1, 2, 3, 4, 5, pim); - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); } /* ************************************************************************* */ From 0479223b3fbbdc7b5957c682cda03d5471cb8cc5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Mar 2020 20:04:33 -0400 Subject: [PATCH 12/48] suppress warning when wrapper indentation is too long --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ac85bff6..42ebe7ea4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -416,6 +416,8 @@ add_subdirectory(CppUnitLite) # Build wrap if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) + # suppress warning of cython line being too long + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") endif(GTSAM_BUILD_WRAP) # Build GTSAM library From cd809309f72e3193518ffaf01550126884d5a16e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 23 Mar 2020 08:04:17 -0400 Subject: [PATCH 13/48] suppress warning only on linux for now, need to figure out for other OSes --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 42ebe7ea4..be556d27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -417,7 +417,9 @@ add_subdirectory(CppUnitLite) if (GTSAM_BUILD_WRAP) add_subdirectory(wrap) # suppress warning of cython line being too long - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation") + endif() endif(GTSAM_BUILD_WRAP) # Build GTSAM library From 6945845868bc3598ddcbb255e9e3bae2964af378 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 25 Mar 2020 20:03:03 -0400 Subject: [PATCH 14/48] add default value of GTSAM_WITH_TBB --- .travis.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.sh b/.travis.sh index bc9029595..d57b784df 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,6 +24,7 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-ON} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ From ea8f774f8d723f053e3c53170476673e2fbdad75 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 13:38:17 -0400 Subject: [PATCH 15/48] make GTSAM_WITH_TBB=OFF default in ci --- .travis.sh | 2 +- .travis.yml | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.travis.sh b/.travis.sh index d57b784df..78351f095 100755 --- a/.travis.sh +++ b/.travis.sh @@ -24,7 +24,7 @@ function configure() -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \ -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ - -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-ON} \ + -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ diff --git a/.travis.yml b/.travis.yml index e4e1a3440..870a4bdb9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -44,7 +44,7 @@ jobs: - stage: compile os: osx compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -55,7 +55,7 @@ jobs: - stage: compile os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: osx @@ -66,7 +66,7 @@ jobs: - stage: compile os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -77,7 +77,7 @@ jobs: - stage: compile os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -b - stage: compile os: linux @@ -88,13 +88,13 @@ jobs: - stage: special os: linux compiler: clang - env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON GTSAM_WITH_TBB=OFF + env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON script: bash .travis.sh -b -# on Linux, with GTSAM_WITH_TBB not off to make sure GTSAM still compiles/tests +# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests - stage: special os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON script: bash .travis.sh -b # -------- STAGE 2: TESTS ----------- # on Mac, GCC @@ -106,7 +106,7 @@ jobs: - stage: test os: osx compiler: clang - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -t - stage: test os: linux @@ -116,7 +116,7 @@ jobs: - stage: test os: linux compiler: gcc - env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=OFF + env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF script: bash .travis.sh -t - stage: test os: linux From 88005a99a13dff3dacb726ebb9ae36b4be63df15 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 13:46:30 -0400 Subject: [PATCH 16/48] fix tbb CMakeLists indentation --- CMakeLists.txt | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 56250a12c..57c9d5f2f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -199,17 +199,17 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) - set(GTSAM_USE_TBB 1) # This will go into config.h - if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) - set(TBB_GREATER_EQUAL_2020 1) - else() - set(TBB_GREATER_EQUAL_2020 0) - endif() - # all definitions and link requisites will go via imported targets: - # tbb & tbbmalloc - list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) + set(GTSAM_USE_TBB 1) # This will go into config.h + if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + set(TBB_GREATER_EQUAL_2020 1) + else() + set(TBB_GREATER_EQUAL_2020 0) + endif() + # all definitions and link requisites will go via imported targets: + # tbb & tbbmalloc + list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc) else() - set(GTSAM_USE_TBB 0) # This will go into config.h + set(GTSAM_USE_TBB 0) # This will go into config.h endif() ############################################################################### From 80fc06cad7fbb31089e4e11709fd7c81bc98327b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 16:03:51 -0400 Subject: [PATCH 17/48] function to add tbb with debug --- .travis.sh | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/.travis.sh b/.travis.sh index 78351f095..9ff1f22cd 100755 --- a/.travis.sh +++ b/.travis.sh @@ -1,5 +1,17 @@ #!/bin/bash +# install TBB with _debug.so files +function install_tbb() +{ + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.5/tbb44_20160526oss_lin.tgz + if [ $(uname -s) == "Linux"]; then + tar -xvf tbb44_20160526oss_lin.tgz + elif [ $(uname -s) == "Linux" ]; then + tar -xvf tbb44_20160526oss_mac.tgz + fi + source tbb44_20160526oss/bin/tbbvars.sh intel64 linux auto_tbbroot +} + # common tasks before either build or test function configure() { @@ -19,6 +31,8 @@ function configure() export CXX=g++-$GCC_VERSION fi + install_tbb + # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs cmake $SOURCE_DIR \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ From 026c794a9279b3b764f9101f4e410c25793c69c1 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 26 Mar 2020 16:32:05 -0400 Subject: [PATCH 18/48] use the same tbb as xenial --- .travis.sh | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index 9ff1f22cd..5db84e1e5 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,13 +3,14 @@ # install TBB with _debug.so files function install_tbb() { - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.5/tbb44_20160526oss_lin.tgz - if [ $(uname -s) == "Linux"]; then - tar -xvf tbb44_20160526oss_lin.tgz - elif [ $(uname -s) == "Linux" ]; then - tar -xvf tbb44_20160526oss_mac.tgz + if [ $(uname -s) == "Linux" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz + tar -xvf tbb44_20151115oss_lin.tgz + elif [ $(uname -s) == "Darwin" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz + tar -xvf tbb44_20151115oss_osx.tgz fi - source tbb44_20160526oss/bin/tbbvars.sh intel64 linux auto_tbbroot + source tbb44_20151115oss/bin/tbbvars.sh intel64 linux auto_tbbroot } # common tasks before either build or test From 4a356b9bd93ab4f7076a778b073c77b104a89c40 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 18:00:47 -0400 Subject: [PATCH 19/48] improved install_tbb function --- .travis.sh | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/.travis.sh b/.travis.sh index 5db84e1e5..8fdf27bd3 100755 --- a/.travis.sh +++ b/.travis.sh @@ -5,12 +5,23 @@ function install_tbb() { if [ $(uname -s) == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz - tar -xvf tbb44_20151115oss_lin.tgz + tar -xf tbb44_20151115oss_lin.tgz elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz - tar -xvf tbb44_20151115oss_osx.tgz + tar -xf tbb44_20151115oss_osx.tgz fi - source tbb44_20151115oss/bin/tbbvars.sh intel64 linux auto_tbbroot + + # Variables needed for setting the correct library path + TBB_TARGET_ARCH="intel64" + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + library_directory="${TBB_TARGET_ARCH}/gcc4.1" + + # Set library paths + MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH + MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH + LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH + LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test From 34069a60df3cb216f74a6faabdcc2ee54e7e2d90 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 21:00:23 -0400 Subject: [PATCH 20/48] specific paths for Mac, removed libtbb from travis packages list --- .travis.sh | 28 ++++++++++++++++++---------- .travis.yml | 1 - 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/.travis.sh b/.travis.sh index 8fdf27bd3..1f681043a 100755 --- a/.travis.sh +++ b/.travis.sh @@ -6,21 +6,29 @@ function install_tbb() if [ $(uname -s) == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz tar -xf tbb44_20151115oss_lin.tgz + + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + + TBB_TARGET_ARCH="intel64" + library_directory="${TBB_TARGET_ARCH}/gcc4.1" + + # Set library paths + MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH + MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH + LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH + LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz tar -xf tbb44_20151115oss_osx.tgz + + TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + + # Set library paths + LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH"; export LIBRARY_PATH + DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH"; export DYLD_LIBRARY_PATH fi - # Variables needed for setting the correct library path - TBB_TARGET_ARCH="intel64" - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. - library_directory="${TBB_TARGET_ARCH}/gcc4.1" - - # Set library paths - MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH - MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH - LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH - LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH CPATH="${TBBROOT}/include:$CPATH"; export CPATH } diff --git a/.travis.yml b/.travis.yml index 870a4bdb9..2de325818 100644 --- a/.travis.yml +++ b/.travis.yml @@ -16,7 +16,6 @@ addons: - cmake - libpython-dev python-numpy - libboost-all-dev - - libtbb-dev # before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi From 2e34a175f7aca1bd0d8a1f11e83550b80bddfe57 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 26 Mar 2020 22:05:28 -0400 Subject: [PATCH 21/48] properly export variables --- .travis.sh | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index 1f681043a..708572ef2 100755 --- a/.travis.sh +++ b/.travis.sh @@ -13,10 +13,10 @@ function install_tbb() library_directory="${TBB_TARGET_ARCH}/gcc4.1" # Set library paths - MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}"; export MIC_LD_LIBRARY_PATH - MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}"; export MIC_LIBRARY_PATH - LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}"; export LD_LIBRARY_PATH - LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}"; export LIBRARY_PATH + export MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}" + export MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}" + export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" + export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" elif [ $(uname -s) == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz @@ -25,8 +25,8 @@ function install_tbb() TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. # Set library paths - LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH"; export LIBRARY_PATH - DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH"; export DYLD_LIBRARY_PATH + export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" + export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi CPATH="${TBBROOT}/include:$CPATH"; export CPATH From 7248b149fd0bbc7f077dc6bd5361bb280b4af6f3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 08:39:06 -0400 Subject: [PATCH 22/48] save tbb download to /tmp --- .travis.sh | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/.travis.sh b/.travis.sh index 708572ef2..e08ab5a30 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,11 +3,11 @@ # install TBB with _debug.so files function install_tbb() { - if [ $(uname -s) == "Linux" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz - tar -xf tbb44_20151115oss_lin.tgz + if [ "$(uname -s)" == "Linux" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz + tar -C /tmp -xvf /tmp/tbb442.tgz - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + TBBROOT=/tmp/tbb44_20151115oss TBB_TARGET_ARCH="intel64" library_directory="${TBB_TARGET_ARCH}/gcc4.1" @@ -18,18 +18,18 @@ function install_tbb() export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" - elif [ $(uname -s) == "Darwin" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz - tar -xf tbb44_20151115oss_osx.tgz + elif [ "$(uname -s)" == "Darwin" ]; then + wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz + tar -C /tmp -xvf /tmp/tbb442.tgz - TBBROOT=$(cd tbb44_20151115oss/bin && pwd -P)/.. + TBBROOT=/tmp/tbb44_20151115oss # Set library paths export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - CPATH="${TBBROOT}/include:$CPATH"; export CPATH + # CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test From 5dc19e074674ab3aa7cc23656491294ead0cbef8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 09:21:51 -0400 Subject: [PATCH 23/48] install tbb before install to allow propagation of env variables --- .travis.sh | 9 +++++---- .travis.yml | 5 +++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/.travis.sh b/.travis.sh index e08ab5a30..5f999e7a9 100755 --- a/.travis.sh +++ b/.travis.sh @@ -5,7 +5,7 @@ function install_tbb() { if [ "$(uname -s)" == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xvf /tmp/tbb442.tgz + tar -C /tmp -xf /tmp/tbb442.tgz TBBROOT=/tmp/tbb44_20151115oss @@ -20,7 +20,7 @@ function install_tbb() elif [ "$(uname -s)" == "Darwin" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xvf /tmp/tbb442.tgz + tar -C /tmp -xf /tmp/tbb442.tgz TBBROOT=/tmp/tbb44_20151115oss @@ -51,8 +51,6 @@ function configure() export CXX=g++-$GCC_VERSION fi - install_tbb - # GTSAM_BUILD_WITH_MARCH_NATIVE=OFF: to avoid crashes in builder VMs cmake $SOURCE_DIR \ -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \ @@ -111,4 +109,7 @@ case $1 in -t) test ;; + -tbb) + install_tbb + ;; esac diff --git a/.travis.yml b/.travis.yml index 2de325818..7e5b33ec4 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,8 +17,9 @@ addons: - libpython-dev python-numpy - libboost-all-dev -# before_install: -# - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update ; fi +before_install: + - ./.travis.sh -tbb + # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi From 99e1c42282d2cde3bf5e42e33f38c14171820d6a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 10:53:22 -0400 Subject: [PATCH 24/48] update travis.yml to install tbb during install phase, source script to add variables to current shell --- .travis.sh | 8 +++----- .travis.yml | 4 ++-- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.travis.sh b/.travis.sh index 5f999e7a9..2bf4f038c 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,12 +3,12 @@ # install TBB with _debug.so files function install_tbb() { + TBBROOT=/tmp/tbb44_20151115oss + if [ "$(uname -s)" == "Linux" ]; then wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz tar -C /tmp -xf /tmp/tbb442.tgz - TBBROOT=/tmp/tbb44_20151115oss - TBB_TARGET_ARCH="intel64" library_directory="${TBB_TARGET_ARCH}/gcc4.1" @@ -22,14 +22,12 @@ function install_tbb() wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz tar -C /tmp -xf /tmp/tbb442.tgz - TBBROOT=/tmp/tbb44_20151115oss - # Set library paths export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - # CPATH="${TBBROOT}/include:$CPATH"; export CPATH + CPATH="${TBBROOT}/include:$CPATH"; export CPATH } # common tasks before either build or test diff --git a/.travis.yml b/.travis.yml index 7e5b33ec4..893627df3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -17,13 +17,13 @@ addons: - libpython-dev python-numpy - libboost-all-dev -before_install: - - ./.travis.sh -tbb +# before_install: # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi + - source .travis.sh -tbb # We first do the compile stage specified below, then the matrix expansion specified after. stages: From ba5086f2717466f413796068c53fa337ecd73531 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 13:15:59 -0400 Subject: [PATCH 25/48] vastly improved install_tbb function which copies files correctly --- .travis.sh | 42 ++++++++++++++++++++++-------------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/.travis.sh b/.travis.sh index 2bf4f038c..10d5fd545 100755 --- a/.travis.sh +++ b/.travis.sh @@ -3,31 +3,33 @@ # install TBB with _debug.so files function install_tbb() { - TBBROOT=/tmp/tbb44_20151115oss + TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download + TBB_VERSION=4.4.2 + TBB_DIR=tbb44_20151115oss + TBB_SAVEPATH="/tmp/tbb.tgz" - if [ "$(uname -s)" == "Linux" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_lin.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xf /tmp/tbb442.tgz + if [ "$TRAVIS_OS_NAME" == "linux" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="intel64/gcc4.4" + SUDO="sudo" - TBB_TARGET_ARCH="intel64" - library_directory="${TBB_TARGET_ARCH}/gcc4.1" + elif [ "$TRAVIS_OS_NAME" == "osx" ]; then + OS_SHORT="lin" + TBB_LIB_DIR="" + SUDO="" - # Set library paths - export MIC_LD_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LD_LIBRARY_PATH}" - export MIC_LIBRARY_PATH="$TBBROOT/lib/mic:${MIC_LIBRARY_PATH}" - export LD_LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LD_LIBRARY_PATH}" - export LIBRARY_PATH="$TBBROOT/lib/$library_directory:${LIBRARY_PATH}" - - elif [ "$(uname -s)" == "Darwin" ]; then - wget https://github.com/oneapi-src/oneTBB/releases/download/4.4.2/tbb44_20151115oss_osx.tgz -O /tmp/tbb442.tgz - tar -C /tmp -xf /tmp/tbb442.tgz - - # Set library paths - export LIBRARY_PATH="${TBBROOT}/lib:$LIBRARY_PATH" - export DYLD_LIBRARY_PATH="${TBBROOT}/lib:$DYLD_LIBRARY_PATH" fi - CPATH="${TBBROOT}/include:$CPATH"; export CPATH + wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH + tar -C /tmp -xf $TBB_SAVEPATH + + TBBROOT=/tmp/$TBB_DIR + # Copy the needed files to the correct places. + # This works correctly for travis builds, instead of setting path variables. + # This is what Homebrew does to install TBB on Macs + $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/ + $SUDO cp -R $TBBROOT/include/ /usr/local/include/ + } # common tasks before either build or test From 8dc7359924176c0c575d8b1f014057ce99aaac03 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 27 Mar 2020 23:30:50 -0400 Subject: [PATCH 26/48] set makefile to not verbose to speed up travis and reduce log length --- .travis.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 10d5fd545..d1940db52 100755 --- a/.travis.sh +++ b/.travis.sh @@ -61,7 +61,7 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=ON + -DCMAKE_VERBOSE_MAKEFILE=OFF } From d608611c871035269dcc702bd13377050c8757b9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 06:56:58 -0400 Subject: [PATCH 27/48] install tbb as part of configure --- .travis.sh | 5 ++--- .travis.yml | 1 - 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.sh b/.travis.sh index d1940db52..535a72f4b 100755 --- a/.travis.sh +++ b/.travis.sh @@ -46,6 +46,8 @@ function configure() rm -fr $BUILD_DIR || true mkdir $BUILD_DIR && cd $BUILD_DIR + install_tbb + if [ ! -z "$GCC_VERSION" ]; then export CC=gcc-$GCC_VERSION export CXX=g++-$GCC_VERSION @@ -109,7 +111,4 @@ case $1 in -t) test ;; - -tbb) - install_tbb - ;; esac diff --git a/.travis.yml b/.travis.yml index 893627df3..c272cff07 100644 --- a/.travis.yml +++ b/.travis.yml @@ -23,7 +23,6 @@ addons: install: - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache ; fi - if [ "$TRAVIS_OS_NAME" == "osx" ]; then export PATH="/usr/local/opt/ccache/libexec:$PATH" ; fi - - source .travis.sh -tbb # We first do the compile stage specified below, then the matrix expansion specified after. stages: From 02ff7ae276514bb3633638416fda95e7b3100fae Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 09:56:29 -0400 Subject: [PATCH 28/48] output message for exception in debug mode --- gtsam/nonlinear/utilities.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/nonlinear/utilities.h b/gtsam/nonlinear/utilities.h index 1228cd4db..2044d091f 100644 --- a/gtsam/nonlinear/utilities.h +++ b/gtsam/nonlinear/utilities.h @@ -244,6 +244,9 @@ Values localToWorld(const Values& local, const Pose2& base, world.insert(key, base.transformFrom(point)); } catch (const std::exception& e2) { // if not Pose2 or Point2, do nothing + #ifndef NDEBUG + std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl; + #endif } } } From 35bfc8468485796e1de882c753a8777273c3682f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 28 Mar 2020 10:05:38 -0400 Subject: [PATCH 29/48] make makefile verbose again --- .travis.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.sh b/.travis.sh index 535a72f4b..9fc09a3f8 100755 --- a/.travis.sh +++ b/.travis.sh @@ -63,7 +63,7 @@ function configure() -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \ -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ - -DCMAKE_VERBOSE_MAKEFILE=OFF + -DCMAKE_VERBOSE_MAKEFILE=ON } From 4197fa3c54751a32bcaa4dabaee9955e476ea28b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 29 Mar 2020 19:11:44 -0400 Subject: [PATCH 30/48] removed graphWithPrior from all examples while keeping functionality the same --- cython/gtsam/examples/Pose2SLAMExample_g2o.py | 5 ++--- cython/gtsam/examples/Pose3SLAMExample_g2o.py | 9 ++++----- examples/Pose2SLAMExample_g2o.cpp | 5 ++--- examples/Pose2SLAMExample_lago.cpp | 12 +++++++----- examples/Pose3Localization.cpp | 12 +++++++----- examples/Pose3SLAMExample_g2o.cpp | 10 ++++++---- examples/Pose3SLAMExample_initializePose3Chordal.cpp | 10 ++++++---- .../Pose3SLAMExample_initializePose3Gradient.cpp | 10 ++++++---- 8 files changed, 40 insertions(+), 33 deletions(-) diff --git a/cython/gtsam/examples/Pose2SLAMExample_g2o.py b/cython/gtsam/examples/Pose2SLAMExample_g2o.py index 3aee7daff..83d543310 100644 --- a/cython/gtsam/examples/Pose2SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose2SLAMExample_g2o.py @@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D) assert args.kernel == "none", "Supplied kernel type is not yet implemented" # Add prior on the pose having index (key) = 0 -graphWithPrior = graph priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8)) -graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) +graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") params.setMaxIterations(maxIterations) # parameters.setRelativeErrorTol(1e-5) # Create the optimizer ... -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) # ... and optimize result = optimizer.optimize() diff --git a/cython/gtsam/examples/Pose3SLAMExample_g2o.py b/cython/gtsam/examples/Pose3SLAMExample_g2o.py index 38c5db275..25686c762 100644 --- a/cython/gtsam/examples/Pose3SLAMExample_g2o.py +++ b/cython/gtsam/examples/Pose3SLAMExample_g2o.py @@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") -graphWithPrior = graph firstKey = initial.keys().at(0) -graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) +graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds -optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) +optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") -print("initial error = ", graphWithPrior.error(initial)) -print("final error = ", graphWithPrior.error(result)) +print("initial error = ", graph.error(initial)) +print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 35f9884e1..a38dfafa6 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) { } // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graph->add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; @@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) { } std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index b83dfa1db..d6164450b 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile); // Add prior on the pose having index (key) = 0 - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8)); - graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - graphWithPrior.print(); + graph->add(PriorFactor(0, Pose2(), priorModel)); + graph->print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = lago::initialize(graphWithPrior); + Values estimateLago = lago::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { @@ -57,7 +56,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, estimateLago, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, estimateLago, outputFile); std::cout << "done! " << std::endl; } diff --git a/examples/Pose3Localization.cpp b/examples/Pose3Localization.cpp index becb9530c..05a04b353 100644 --- a/examples/Pose3Localization.cpp +++ b/examples/Pose3Localization.cpp @@ -21,8 +21,8 @@ #include #include #include -#include #include +#include using namespace std; using namespace gtsam; @@ -42,21 +42,20 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); // this will show info about stopping conditions - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -68,7 +67,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index 5066222e5..25297806e 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); // this will show info about stopping conditions - GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + GaussNewtonOptimizer optimizer(*graph, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; @@ -67,7 +66,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e90d014c0..9726f467c 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Initializing Pose3 - chordal relaxation" << std::endl; - Values initialization = InitializePose3::initialize(graphWithPrior); + Values initialization = InitializePose3::initialize(*graph); std::cout << "done!" << std::endl; if (argc < 3) { @@ -61,7 +60,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 10960cf31..000150846 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) { boost::tie(graph, initial) = readG2o(g2oFile, is3D); // Add prior on the first key - NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); Key firstKey = 0; for(const Values::ConstKeyValuePair& key_value: *initial) { std::cout << "Adding prior to g2o file " << std::endl; firstKey = key_value.key; - graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + graph->add(PriorFactor(firstKey, Pose3(), priorModel)); break; } std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; bool useGradient = true; - Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); + Values initialization = InitializePose3::initialize(*graph, *initial, useGradient); std::cout << "done!" << std::endl; std::cout << "initial error=" <error(*initial)<< std::endl; @@ -65,7 +64,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, initialization, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, initialization, outputFile); std::cout << "done! " << std::endl; } return 0; From 7fcacf90c69b73076e2e67e286e03febbd07f8d6 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 31 Mar 2020 22:22:12 -0400 Subject: [PATCH 31/48] Fix unittest when GTSAM_DEFINE_POINTS_AS_VECTORS is defined --- gtsam/geometry/tests/testPose3.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 8433bbb01..ec0450abb 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1008,7 +1008,14 @@ TEST(Pose3, print) { // Generate the expected output std::stringstream expected; Point3 translation(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';"; +#endif // reset cout to the original stream std::cout.rdbuf(oldbuf); From 8c2bd979401df08ed1a5944a6573ad05b9e5472c Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 15:45:04 -0400 Subject: [PATCH 32/48] fix comments --- gtsam/linear/GaussianBayesNet.h | 2 +- gtsam/linear/GaussianBayesTree.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 9da7a1609..3f6d69e91 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -143,7 +143,7 @@ namespace gtsam { * allocateVectorValues */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index fcc73f80e..b6f5acd52 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -106,7 +106,7 @@ namespace gtsam { * @return A VectorValues storing the gradient. */ VectorValues gradientAtZero() const; - /** Mahalanobis norm error. */ + /** 0.5 * sum of squared Mahalanobis distances. */ double error(const VectorValues& x) const; /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a From fbd5aef61a261879ea1dea3abeb8a246ede3ee13 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 15:45:58 -0400 Subject: [PATCH 33/48] deprecated Mahalanobis, add better named functions --- gtsam/linear/NoiseModel.cpp | 4 ++-- gtsam/linear/NoiseModel.h | 19 +++++++++++++++---- gtsam/linear/tests/testNoiseModel.cpp | 4 ++-- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a6ebea394..33f51e1f0 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -157,7 +157,7 @@ Vector Gaussian::unwhiten(const Vector& v) const { } /* ************************************************************************* */ -double Gaussian::Mahalanobis(const Vector& v) const { +double Gaussian::SquaredMahalanobisDistance(const Vector& v) const { // Note: for Diagonal, which does ediv_, will be correct for constraints Vector w = whiten(v); return w.dot(w); @@ -573,7 +573,7 @@ void Isotropic::print(const string& name) const { } /* ************************************************************************* */ -double Isotropic::Mahalanobis(const Vector& v) const { +double Isotropic::SquaredMahalanobisDistance(const Vector& v) const { return v.dot(v) * invsigma_ * invsigma_; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6c42fc7ba..b08207566 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -207,12 +207,23 @@ namespace gtsam { virtual Vector unwhiten(const Vector& v) const; /** - * Mahalanobis distance v'*R'*R*v = + * Squared Mahalanobis distance v'*R'*R*v = */ + virtual double SquaredMahalanobisDistance(const Vector& v) const; + + /** + * Mahalanobis distance + */ + virtual double MahalanobisDistance(const Vector& v) const { + return std::sqrt(SquaredMahalanobisDistance(v)); + } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 virtual double Mahalanobis(const Vector& v) const; +#endif inline virtual double distance(const Vector& v) const { - return Mahalanobis(v); + return SquaredMahalanobisDistance(v); } /** @@ -564,7 +575,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) const; + virtual double SquaredMahalanobisDistance(const Vector& v) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -616,7 +627,7 @@ namespace gtsam { virtual bool isUnit() const { return true; } virtual void print(const std::string& name) const; - virtual double Mahalanobis(const Vector& v) const {return v.dot(v); } + virtual double SquaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 10578627f..523a38b2b 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -68,10 +68,10 @@ TEST(NoiseModel, constructors) for(Gaussian::shared_ptr mi: m) EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened))); - // test Mahalanobis distance + // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->SquaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) From 351c6f8bccca726e3515721cde22626750fc4011 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 16:45:37 -0400 Subject: [PATCH 34/48] add implementation for deprecated Mahalanobis --- gtsam/linear/NoiseModel.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index b08207566..c4128909b 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,7 +219,9 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const; + virtual double Mahalanobis(const Vector& v) const { + return SquaredMahalanobisDistance(v); + } #endif inline virtual double distance(const Vector& v) const { From 83bd42c72fe022a74c9301eac2b2b4a5e40782df Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Apr 2020 16:52:57 -0400 Subject: [PATCH 35/48] maintain backwards compatibility of CMake --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 86234afa8..7a49a8aa6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,7 +200,7 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc) # Set up variables if we're using TBB if(TBB_FOUND AND GTSAM_WITH_TBB) set(GTSAM_USE_TBB 1) # This will go into config.h - if (${TBB_VERSION_MAJOR} GREATER_EQUAL 2020) + if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) else() set(TBB_GREATER_EQUAL_2020 0) From 4e3542a74310d098b4e3f4eba104db3d3a47d9e5 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Wed, 1 Apr 2020 18:55:23 -0400 Subject: [PATCH 36/48] renamed residual to loss, follow PR #188 --- gtsam.h | 18 +++--- gtsam/linear/LossFunctions.cpp | 16 ++--- gtsam/linear/LossFunctions.h | 33 +++++----- gtsam/linear/NoiseModel.h | 6 +- gtsam/linear/tests/testNoiseModel.cpp | 64 ++++++++++---------- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 6 files changed, 71 insertions(+), 68 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8ee778f4c..1094d9dd9 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double residual(double error) const; + double loss(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2c..8bb670a92 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::residual(double error) const { +double Fair::loss(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::residual(double error) const { +double Huber::loss(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::residual(double error) const { +double Cauchy::loss(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::residual(double error) const { +double Tukey::loss(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::residual(double error) const { +double Welsch::loss(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::residual(double error) const { +double GemanMcClure::loss(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::residual(double error) const { +double DCS::loss(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::residual(double error) const { +double L2WithDeadZone::loss(double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1f7cc1377..1b6f444e8 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Residual \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -131,7 +134,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double residual(double error) const { return error; } + double loss(double error) const { return 0.5 * error * error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -155,7 +158,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -180,7 +183,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -210,7 +213,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -235,7 +238,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -260,7 +263,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -296,7 +299,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -326,7 +329,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -359,7 +362,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double residual(double error) const override; + double loss(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index c4128909b..a4d15c0f8 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -705,11 +705,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator residual(...) function into distance(...) + // Fold the use of the m-estimator loss(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return robust_->residual(this->unweightedWhiten(v).norm()); } + { return robust_->loss(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->residual(v.norm()); } + { return robust_->loss(v.norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 523a38b2b..0290fc5d8 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index 8a0485334..ce505df5d 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.residual(x(i)); + rho(i) = model.loss(x(i)); end psi = w .* x; From 4c6feb4769b8dc2e55cfe39904baffc138ea8462 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:04:31 -0400 Subject: [PATCH 37/48] Revert "add implementation for deprecated Mahalanobis" This reverts commit 351c6f8bccca726e3515721cde22626750fc4011. --- gtsam/linear/NoiseModel.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index a4d15c0f8..73f663240 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,9 +219,7 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const { - return SquaredMahalanobisDistance(v); - } + virtual double Mahalanobis(const Vector& v) const; #endif inline virtual double distance(const Vector& v) const { From 0f713ec077c0e51cca96efa4500614f67463fc8a Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:06:31 -0400 Subject: [PATCH 38/48] renamed squaredMahalanobisDistance --- gtsam/linear/NoiseModel.cpp | 4 ++-- gtsam/linear/NoiseModel.h | 10 +++++----- gtsam/linear/tests/testNoiseModel.cpp | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 33f51e1f0..b324ea784 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -157,7 +157,7 @@ Vector Gaussian::unwhiten(const Vector& v) const { } /* ************************************************************************* */ -double Gaussian::SquaredMahalanobisDistance(const Vector& v) const { +double Gaussian::squaredMahalanobisDistance(const Vector& v) const { // Note: for Diagonal, which does ediv_, will be correct for constraints Vector w = whiten(v); return w.dot(w); @@ -573,7 +573,7 @@ void Isotropic::print(const string& name) const { } /* ************************************************************************* */ -double Isotropic::SquaredMahalanobisDistance(const Vector& v) const { +double Isotropic::squaredMahalanobisDistance(const Vector& v) const { return v.dot(v) * invsigma_ * invsigma_; } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 73f663240..bdb25bec0 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -209,13 +209,13 @@ namespace gtsam { /** * Squared Mahalanobis distance v'*R'*R*v = */ - virtual double SquaredMahalanobisDistance(const Vector& v) const; + virtual double squaredMahalanobisDistance(const Vector& v) const; /** * Mahalanobis distance */ virtual double MahalanobisDistance(const Vector& v) const { - return std::sqrt(SquaredMahalanobisDistance(v)); + return std::sqrt(squaredMahalanobisDistance(v)); } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 @@ -223,7 +223,7 @@ namespace gtsam { #endif inline virtual double distance(const Vector& v) const { - return SquaredMahalanobisDistance(v); + return squaredMahalanobisDistance(v); } /** @@ -575,7 +575,7 @@ namespace gtsam { } virtual void print(const std::string& name) const; - virtual double SquaredMahalanobisDistance(const Vector& v) const; + virtual double squaredMahalanobisDistance(const Vector& v) const; virtual Vector whiten(const Vector& v) const; virtual Vector unwhiten(const Vector& v) const; virtual Matrix Whiten(const Matrix& H) const; @@ -627,7 +627,7 @@ namespace gtsam { virtual bool isUnit() const { return true; } virtual void print(const std::string& name) const; - virtual double SquaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } + virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); } virtual Vector whiten(const Vector& v) const { return v; } virtual Vector unwhiten(const Vector& v) const { return v; } virtual Matrix Whiten(const Matrix& H) const { return H; } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 0290fc5d8..2322d8432 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -71,7 +71,7 @@ TEST(NoiseModel, constructors) // test squared Mahalanobis distance double distance = 5*5+10*10+15*15; for(Gaussian::shared_ptr mi: m) - DOUBLES_EQUAL(distance,mi->SquaredMahalanobisDistance(unwhitened),1e-9); + DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9); // test R matrix for(Gaussian::shared_ptr mi: m) From 17e8f21e75590c1c6c7120e02e138d8da3b887a0 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:08:58 -0400 Subject: [PATCH 39/48] renamed mahalanobisDistance --- gtsam/linear/NoiseModel.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index bdb25bec0..225497b42 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -214,7 +214,7 @@ namespace gtsam { /** * Mahalanobis distance */ - virtual double MahalanobisDistance(const Vector& v) const { + virtual double mahalanobisDistance(const Vector& v) const { return std::sqrt(squaredMahalanobisDistance(v)); } From 0acca9f8da665d40e3df4cf6c45c6e6734a10a31 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:24:26 -0400 Subject: [PATCH 40/48] Revert "renamed residual to loss, follow PR #188" This reverts commit 4e3542a74310d098b4e3f4eba104db3d3a47d9e5. --- gtsam.h | 18 +++--- gtsam/linear/LossFunctions.cpp | 16 ++--- gtsam/linear/LossFunctions.h | 33 +++++----- gtsam/linear/NoiseModel.h | 6 +- gtsam/linear/tests/testNoiseModel.cpp | 64 ++++++++++---------- matlab/gtsam_examples/VisualizeMEstimators.m | 2 +- 6 files changed, 68 insertions(+), 71 deletions(-) diff --git a/gtsam.h b/gtsam.h index 1094d9dd9..8ee778f4c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1458,7 +1458,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Fair: gtsam::noiseModel::mEstimator::Base { @@ -1469,7 +1469,7 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Huber: gtsam::noiseModel::mEstimator::Base { @@ -1480,7 +1480,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { @@ -1491,7 +1491,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { @@ -1502,7 +1502,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { @@ -1513,7 +1513,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { @@ -1524,7 +1524,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { @@ -1535,7 +1535,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { @@ -1546,7 +1546,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { void serializable() const; double weight(double error) const; - double loss(double error) const; + double residual(double error) const; }; }///\namespace mEstimator diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 8bb670a92..6bc737e2c 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -141,7 +141,7 @@ double Fair::weight(double error) const { return 1.0 / (1.0 + std::abs(error) / c_); } -double Fair::loss(double error) const { +double Fair::residual(double error) const { const double absError = std::abs(error); const double normalizedError = absError / c_; const double c_2 = c_ * c_; @@ -175,7 +175,7 @@ double Huber::weight(double error) const { return (absError <= k_) ? (1.0) : (k_ / absError); } -double Huber::loss(double error) const { +double Huber::residual(double error) const { const double absError = std::abs(error); if (absError <= k_) { // |x| <= k return error*error / 2; @@ -212,7 +212,7 @@ double Cauchy::weight(double error) const { return ksquared_ / (ksquared_ + error*error); } -double Cauchy::loss(double error) const { +double Cauchy::residual(double error) const { const double val = std::log1p(error * error / ksquared_); return ksquared_ * val * 0.5; } @@ -249,7 +249,7 @@ double Tukey::weight(double error) const { return 0.0; } -double Tukey::loss(double error) const { +double Tukey::residual(double error) const { double absError = std::abs(error); if (absError <= c_) { const double one_minus_xc2 = 1.0 - error*error/csquared_; @@ -285,7 +285,7 @@ double Welsch::weight(double error) const { return std::exp(-xc2); } -double Welsch::loss(double error) const { +double Welsch::residual(double error) const { const double xc2 = (error*error)/csquared_; return csquared_ * 0.5 * -std::expm1(-xc2); } @@ -318,7 +318,7 @@ double GemanMcClure::weight(double error) const { return c4/(c2error*c2error); } -double GemanMcClure::loss(double error) const { +double GemanMcClure::residual(double error) const { const double c2 = c_*c_; const double error2 = error*error; return 0.5 * (c2 * error2) / (c2 + error2); @@ -356,7 +356,7 @@ double DCS::weight(double error) const { return 1.0; } -double DCS::loss(double error) const { +double DCS::residual(double error) const { // This is the simplified version of Eq 9 from (Agarwal13icra) // after you simplify and cancel terms. const double e2 = error*error; @@ -400,7 +400,7 @@ double L2WithDeadZone::weight(double error) const { else return (k_+error)/error; } -double L2WithDeadZone::loss(double error) const { +double L2WithDeadZone::residual(double error) const { const double abs_error = std::abs(error); return (abs_error < k_) ? 0.0 : 0.5*(k_-abs_error)*(k_-abs_error); } diff --git a/gtsam/linear/LossFunctions.h b/gtsam/linear/LossFunctions.h index 1b6f444e8..1f7cc1377 100644 --- a/gtsam/linear/LossFunctions.h +++ b/gtsam/linear/LossFunctions.h @@ -36,12 +36,12 @@ namespace noiseModel { * The mEstimator name space contains all robust error functions. * It mirrors the exposition at * https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf - * which talks about minimizing \sum \rho(r_i), where \rho is a loss function of choice. + * which talks about minimizing \sum \rho(r_i), where \rho is a residual function of choice. * * To illustrate, let's consider the least-squares (L2), L1, and Huber estimators as examples: * * Name Symbol Least-Squares L1-norm Huber - * Loss \rho(x) 0.5*x^2 |x| 0.5*x^2 if |x| shared_ptr; @@ -134,7 +131,7 @@ class GTSAM_EXPORT Null : public Base { Null(const ReweightScheme reweight = Block) : Base(reweight) {} ~Null() {} double weight(double /*error*/) const { return 1.0; } - double loss(double error) const { return 0.5 * error * error; } + double residual(double error) const { return error; } void print(const std::string &s) const; bool equals(const Base & /*expected*/, double /*tol*/) const { return true; } static shared_ptr Create(); @@ -158,7 +155,7 @@ class GTSAM_EXPORT Fair : public Base { Fair(double c = 1.3998, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double c, const ReweightScheme reweight = Block); @@ -183,7 +180,7 @@ class GTSAM_EXPORT Huber : public Base { Huber(double k = 1.345, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -213,7 +210,7 @@ class GTSAM_EXPORT Cauchy : public Base { Cauchy(double k = 0.1, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -238,7 +235,7 @@ class GTSAM_EXPORT Tukey : public Base { Tukey(double c = 4.6851, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -263,7 +260,7 @@ class GTSAM_EXPORT Welsch : public Base { Welsch(double c = 2.9846, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -299,7 +296,7 @@ class GTSAM_EXPORT GemanMcClure : public Base { GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block); ~GemanMcClure() {} double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -329,7 +326,7 @@ class GTSAM_EXPORT DCS : public Base { DCS(double c = 1.0, const ReweightScheme reweight = Block); ~DCS() {} double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); @@ -362,7 +359,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base { L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block); double weight(double error) const override; - double loss(double error) const override; + double residual(double error) const override; void print(const std::string &s) const override; bool equals(const Base &expected, double tol = 1e-8) const override; static shared_ptr Create(double k, const ReweightScheme reweight = Block); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 225497b42..7c3cb1294 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -703,11 +703,11 @@ namespace gtsam { { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } inline virtual Vector unwhiten(const Vector& /*v*/) const { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } - // Fold the use of the m-estimator loss(...) function into distance(...) + // Fold the use of the m-estimator residual(...) function into distance(...) inline virtual double distance(const Vector& v) const - { return robust_->loss(this->unweightedWhiten(v).norm()); } + { return robust_->residual(this->unweightedWhiten(v).norm()); } inline virtual double distance_non_whitened(const Vector& v) const - { return robust_->loss(v.norm()); } + { return robust_->residual(v.norm()); } // TODO: these are really robust iterated re-weighting support functions virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(std::vector& A, Vector& b) const; diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 2322d8432..3f6550b9f 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -467,10 +467,10 @@ TEST(NoiseModel, robustFunctionFair) DOUBLES_EQUAL(0.3333333333333333, fair->weight(error3), 1e-8); DOUBLES_EQUAL(0.8333333333333333, fair->weight(error4), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error1), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error2), 1e-8); - DOUBLES_EQUAL(22.534692783297260, fair->loss(error3), 1e-8); - DOUBLES_EQUAL(0.441961080151135, fair->loss(error4), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error1), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error2), 1e-8); + DOUBLES_EQUAL(22.534692783297260, fair->residual(error3), 1e-8); + DOUBLES_EQUAL(0.441961080151135, fair->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionHuber) @@ -483,10 +483,10 @@ TEST(NoiseModel, robustFunctionHuber) DOUBLES_EQUAL(0.5, huber->weight(error3), 1e-8); DOUBLES_EQUAL(1.0, huber->weight(error4), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error1), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error2), 1e-8); - DOUBLES_EQUAL(37.5000, huber->loss(error3), 1e-8); - DOUBLES_EQUAL(0.5000, huber->loss(error4), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error1), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error2), 1e-8); + DOUBLES_EQUAL(37.5000, huber->residual(error3), 1e-8); + DOUBLES_EQUAL(0.5000, huber->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionCauchy) @@ -499,10 +499,10 @@ TEST(NoiseModel, robustFunctionCauchy) DOUBLES_EQUAL(0.2000, cauchy->weight(error3), 1e-8); DOUBLES_EQUAL(0.961538461538461, cauchy->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error1), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error2), 1e-8); - DOUBLES_EQUAL(20.117973905426254, cauchy->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490258914416017, cauchy->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error1), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error2), 1e-8); + DOUBLES_EQUAL(20.117973905426254, cauchy->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490258914416017, cauchy->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionGemanMcClure) @@ -514,10 +514,10 @@ TEST(NoiseModel, robustFunctionGemanMcClure) DOUBLES_EQUAL(9.80296e-5, gmc->weight(error3), 1e-8); DOUBLES_EQUAL(0.25 , gmc->weight(error4), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error1), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error2), 1e-8); - DOUBLES_EQUAL(0.495049504950495, gmc->loss(error3), 1e-8); - DOUBLES_EQUAL(0.2500, gmc->loss(error4), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error1), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error2), 1e-8); + DOUBLES_EQUAL(0.495049504950495, gmc->residual(error3), 1e-8); + DOUBLES_EQUAL(0.2500, gmc->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionWelsch) @@ -530,10 +530,10 @@ TEST(NoiseModel, robustFunctionWelsch) DOUBLES_EQUAL(0.018315638888734, welsch->weight(error3), 1e-8); DOUBLES_EQUAL(0.960789439152323, welsch->weight(error4), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error1), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error2), 1e-8); - DOUBLES_EQUAL(12.271054513890823, welsch->loss(error3), 1e-8); - DOUBLES_EQUAL(0.490132010595960, welsch->loss(error4), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error1), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error2), 1e-8); + DOUBLES_EQUAL(12.271054513890823, welsch->residual(error3), 1e-8); + DOUBLES_EQUAL(0.490132010595960, welsch->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionTukey) @@ -546,10 +546,10 @@ TEST(NoiseModel, robustFunctionTukey) DOUBLES_EQUAL(0.0, tukey->weight(error3), 1e-8); DOUBLES_EQUAL(0.9216, tukey->weight(error4), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error1), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error2), 1e-8); - DOUBLES_EQUAL(4.166666666666667, tukey->loss(error3), 1e-8); - DOUBLES_EQUAL(0.480266666666667, tukey->loss(error4), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error1), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error2), 1e-8); + DOUBLES_EQUAL(4.166666666666667, tukey->residual(error3), 1e-8); + DOUBLES_EQUAL(0.480266666666667, tukey->residual(error4), 1e-8); } TEST(NoiseModel, robustFunctionDCS) @@ -560,8 +560,8 @@ TEST(NoiseModel, robustFunctionDCS) DOUBLES_EQUAL(1.0 , dcs->weight(error1), 1e-8); DOUBLES_EQUAL(0.00039211, dcs->weight(error2), 1e-8); - DOUBLES_EQUAL(0.5 , dcs->loss(error1), 1e-8); - DOUBLES_EQUAL(0.9900990099, dcs->loss(error2), 1e-8); + DOUBLES_EQUAL(0.5 , dcs->residual(error1), 1e-8); + DOUBLES_EQUAL(0.9900990099, dcs->residual(error2), 1e-8); } TEST(NoiseModel, robustFunctionL2WithDeadZone) @@ -576,12 +576,12 @@ TEST(NoiseModel, robustFunctionL2WithDeadZone) DOUBLES_EQUAL(0.00990099009, lsdz->weight(e4), 1e-8); DOUBLES_EQUAL(0.9, lsdz->weight(e5), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e0), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e1), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e2), 1e-8); - DOUBLES_EQUAL(0.0, lsdz->loss(e3), 1e-8); - DOUBLES_EQUAL(0.00005, lsdz->loss(e4), 1e-8); - DOUBLES_EQUAL(40.5, lsdz->loss(e5), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e0), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e1), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e2), 1e-8); + DOUBLES_EQUAL(0.0, lsdz->residual(e3), 1e-8); + DOUBLES_EQUAL(0.00005, lsdz->residual(e4), 1e-8); + DOUBLES_EQUAL(40.5, lsdz->residual(e5), 1e-8); } /* ************************************************************************* */ diff --git a/matlab/gtsam_examples/VisualizeMEstimators.m b/matlab/gtsam_examples/VisualizeMEstimators.m index ce505df5d..8a0485334 100644 --- a/matlab/gtsam_examples/VisualizeMEstimators.m +++ b/matlab/gtsam_examples/VisualizeMEstimators.m @@ -48,7 +48,7 @@ function plot_m_estimator(x, model, plot_title, fig_id, filename) rho = zeros(size(x)); for i = 1:size(x, 2) w(i) = model.weight(x(i)); - rho(i) = model.loss(x(i)); + rho(i) = model.residual(x(i)); end psi = w .* x; From ae85b640a868b58ee9a5abcd4d0d979003dd2d97 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 2 Apr 2020 11:04:31 -0400 Subject: [PATCH 41/48] re-add implemntation for deprecated Mahalanobis --- gtsam/linear/NoiseModel.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 7c3cb1294..4d86e77da 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -219,7 +219,9 @@ namespace gtsam { } #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 - virtual double Mahalanobis(const Vector& v) const; + virtual double Mahalanobis(const Vector& v) const { + return squaredMahalanobisDistance(v); + } #endif inline virtual double distance(const Vector& v) const { From 8383e9e285d908140988b8dfbfcc4bfa535ba7d4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:28:46 -0400 Subject: [PATCH 42/48] Deprecated several methods and madrng mutable, so methods are const --- gtsam/linear/Sampler.cpp | 25 +++++----- gtsam/linear/Sampler.h | 79 ++++++++++++++++-------------- gtsam/linear/tests/testSampler.cpp | 18 ++++--- 3 files changed, 66 insertions(+), 56 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 9f1527bf9..57a34f364 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,6 +11,8 @@ /** * @file Sampler.cpp + * @brief sampling from a NoiseModel to generate + * @author Frank Dellaert * @author Alex Cunningham */ @@ -19,24 +21,18 @@ namespace gtsam { /* ************************************************************************* */ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) -{ -} + : model_(model), generator_(static_cast(seed)) {} /* ************************************************************************* */ Sampler::Sampler(const Vector& sigmas, int32_t seed) -: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast(seed)) -{ -} + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), + generator_(static_cast(seed)) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) -: generator_(static_cast(seed)) -{ -} +Sampler::Sampler(int32_t seed) : generator_(static_cast(seed)) {} /* ************************************************************************* */ -Vector Sampler::sampleDiagonal(const Vector& sigmas) { +Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); Vector result(d); for (size_t i = 0; i < d; i++) { @@ -55,18 +51,19 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) { } /* ************************************************************************* */ -Vector Sampler::sample() { +Vector Sampler::sample() const { assert(model_.get()); const Vector& sigmas = model_->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) { +Vector Sampler::sampleNewModel( + const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } /* ************************************************************************* */ -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index 93145c31a..b53f02d23 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @brief sampling that can be parameterized using a NoiseModel to generate samples from * @file Sampler.h - * the given distribution + * @brief sampling from a NoiseModel to generate + * @author Frank Dellaert * @author Alex Cunningham */ @@ -27,67 +27,74 @@ namespace gtsam { /** * Sampling structure that keeps internal random number generators for * diagonal distributions specified by NoiseModel - * - * This is primarily to allow for variable seeds, and does roughly the same - * thing as sample() in NoiseModel. */ class GTSAM_EXPORT Sampler { -protected: + protected: /** noiseModel created at generation */ noiseModel::Diagonal::shared_ptr model_; /** generator */ - std::mt19937_64 generator_; + mutable std::mt19937_64 generator_; -public: + public: typedef boost::shared_ptr shared_ptr; + /// @name constructors + /// @{ + /** * Create a sampler for the distribution specified by a diagonal NoiseModel * with a manually specified seed * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u); + explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, + int32_t seed = 42u); /** - * Create a sampler for a distribution specified by a vector of sigmas directly + * Create a sampler for a distribution specified by a vector of sigmas + * directly * * NOTE: do not use zero as a seed, it will break the generator */ - Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, int32_t seed = 42u); - /** - * Create a sampler without a given noisemodel - pass in to sample - * - * NOTE: do not use zero as a seed, it will break the generator - */ - Sampler(int32_t seed = 42u); + /// @} + /// @name access functions + /// @{ + + size_t dim() const { + assert(model_.get()); + return model_->dim(); + } + + Vector sigmas() const { + assert(model_.get()); + return model_->sigmas(); + } - /** access functions */ - size_t dim() const { assert(model_.get()); return model_->dim(); } - Vector sigmas() const { assert(model_.get()); return model_->sigmas(); } const noiseModel::Diagonal::shared_ptr& model() const { return model_; } - /** - * sample from distribution - * NOTE: not const due to need to update the underlying generator - */ - Vector sample(); + /// @} + /// @name basic functionality + /// @{ - /** - * Sample from noisemodel passed in as an argument, - * can be used without having initialized a model for the system. - * - * NOTE: not const due to need to update the underlying generator - */ - Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model); + /// sample from distribution + Vector sample() const; -protected: + /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 + /// @name Deprecated + /// @{ + explicit Sampler(int32_t seed = 42u); + Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; + /// @} +#endif + + protected: /** given sigmas for a diagonal model, returns a sample */ - Vector sampleDiagonal(const Vector& sigmas); - + Vector sampleDiagonal(const Vector& sigmas) const; }; -} // \namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/tests/testSampler.cpp b/gtsam/linear/tests/testSampler.cpp index bd718500e..5831d9048 100644 --- a/gtsam/linear/tests/testSampler.cpp +++ b/gtsam/linear/tests/testSampler.cpp @@ -10,8 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file testSampler + * @file testSampler.cpp + * @brief unit tests for Sampler class * @author Alex Cunningham + * @author Frank Dellaert */ #include @@ -22,14 +24,15 @@ using namespace gtsam; const double tol = 1e-5; +static const Vector3 kSigmas(1.0, 0.1, 0.0); + /* ************************************************************************* */ TEST(testSampler, basic) { - Vector sigmas = Vector3(1.0, 0.1, 0.0); - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas); + auto model = noiseModel::Diagonal::Sigmas(kSigmas); char seed = 'A'; Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1); - EXPECT(assert_equal(sigmas, sampler1.sigmas())); - EXPECT(assert_equal(sigmas, sampler2.sigmas())); + EXPECT(assert_equal(kSigmas, sampler1.sigmas())); + EXPECT(assert_equal(kSigmas, sampler2.sigmas())); EXPECT_LONGS_EQUAL(3, sampler1.dim()); EXPECT_LONGS_EQUAL(3, sampler2.dim()); Vector actual1 = sampler1.sample(); @@ -38,5 +41,8 @@ TEST(testSampler, basic) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ From 054e9f19c0cefee6cdff10bec0016c7e914394ca Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:30:34 -0400 Subject: [PATCH 43/48] Comment typos --- gtsam/linear/Sampler.cpp | 2 +- gtsam/linear/Sampler.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 57a34f364..ed204fcac 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -11,7 +11,7 @@ /** * @file Sampler.cpp - * @brief sampling from a NoiseModel to generate + * @brief sampling from a diagonal NoiseModel * @author Frank Dellaert * @author Alex Cunningham */ diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index b53f02d23..a52cde330 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -11,7 +11,7 @@ /** * @file Sampler.h - * @brief sampling from a NoiseModel to generate + * @brief sampling from a NoiseModel * @author Frank Dellaert * @author Alex Cunningham */ From 218cb5537b31a11d4fe2a066d7318fd468449fd1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 12:34:39 -0400 Subject: [PATCH 44/48] Changed argument to obviate need for cast --- gtsam/linear/Sampler.cpp | 12 ++++++------ gtsam/linear/Sampler.h | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index ed204fcac..fc58b2ba2 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -20,16 +20,16 @@ namespace gtsam { /* ************************************************************************* */ -Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed) - : model_(model), generator_(static_cast(seed)) {} +Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, + uint_fast64_t seed) + : model_(model), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(const Vector& sigmas, int32_t seed) - : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), - generator_(static_cast(seed)) {} +Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) + : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} /* ************************************************************************* */ -Sampler::Sampler(int32_t seed) : generator_(static_cast(seed)) {} +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} /* ************************************************************************* */ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { diff --git a/gtsam/linear/Sampler.h b/gtsam/linear/Sampler.h index a52cde330..54c240a2b 100644 --- a/gtsam/linear/Sampler.h +++ b/gtsam/linear/Sampler.h @@ -49,7 +49,7 @@ class GTSAM_EXPORT Sampler { * NOTE: do not use zero as a seed, it will break the generator */ explicit Sampler(const noiseModel::Diagonal::shared_ptr& model, - int32_t seed = 42u); + uint_fast64_t seed = 42u); /** * Create a sampler for a distribution specified by a vector of sigmas @@ -57,7 +57,7 @@ class GTSAM_EXPORT Sampler { * * NOTE: do not use zero as a seed, it will break the generator */ - explicit Sampler(const Vector& sigmas, int32_t seed = 42u); + explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u); /// @} /// @name access functions @@ -87,7 +87,7 @@ class GTSAM_EXPORT Sampler { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ - explicit Sampler(int32_t seed = 42u); + explicit Sampler(uint_fast64_t seed = 42u); Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const; /// @} #endif From 45cf253cecbdc8128fe8bc5a0e2b278991822285 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 13:08:36 -0400 Subject: [PATCH 45/48] Deprecated parts of cpp --- gtsam/linear/Sampler.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index fc58b2ba2..16c7e73e0 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -28,9 +28,6 @@ Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed) : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {} -/* ************************************************************************* */ -Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} - /* ************************************************************************* */ Vector Sampler::sampleDiagonal(const Vector& sigmas) const { size_t d = sigmas.size(); @@ -58,12 +55,16 @@ Vector Sampler::sample() const { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 +Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {} + Vector Sampler::sampleNewModel( const noiseModel::Diagonal::shared_ptr& model) const { assert(model.get()); const Vector& sigmas = model->sigmas(); return sampleDiagonal(sigmas); } +#endif /* ************************************************************************* */ } // namespace gtsam From d1d8543fc718197265570bc7ddcb304fd6a535d7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 4 Apr 2020 13:57:05 -0400 Subject: [PATCH 46/48] Fixed issues with deprecated Sampler methods --- gtsam.h | 6 ++---- gtsam/linear/NoiseModel.cpp | 1 - gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/slam/dataset.cpp | 6 +++--- 4 files changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam.h b/gtsam.h index 8ee778f4c..413a1bc7c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1563,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base { #include class Sampler { - //Constructors + // Constructors Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); - Sampler(int seed); - //Standard Interface + // Standard Interface size_t dim() const; Vector sigmas() const; gtsam::noiseModel::Diagonal* model() const; Vector sample(); - Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); }; #include diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index b324ea784..df99191b2 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index 5fb9f78d7..649d0fe12 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner { const Bias estimatedBias_; // Create two samplers for acceleration and omega noise - mutable Sampler gyroSampler_, accSampler_; + Sampler gyroSampler_, accSampler_; public: ScenarioRunner(const Scenario& scenario, const SharedParams& p, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 45d009b1c..052bb3343 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, is.seekg(0, ios::beg); // If asked, create a sampler with random number generator - Sampler sampler; + std::unique_ptr sampler; if (addNoise) { noiseModel::Diagonal::shared_ptr noise; if (model) @@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, throw invalid_argument( "gtsam::load2D: invalid noise model for adding noise" "(current version assumes diagonal noise model)!"); - sampler = Sampler(noise); + sampler.reset(new Sampler(noise)); } // Parse the pose constraints @@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID, model = modelInFile; if (addNoise) - l1Xl2 = l1Xl2.retract(sampler.sample()); + l1Xl2 = l1Xl2.retract(sampler->sample()); // Insert vertices if pure odometry file if (!initial->exists(id1)) From e50f79b8954fe07490389e02d48778154ae346dc Mon Sep 17 00:00:00 2001 From: kvmanohar22 Date: Sat, 4 Apr 2020 23:50:53 +0530 Subject: [PATCH 47/48] fix typo --- gtsam/navigation/NavState.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e9afcd3ac..f66e9d7a9 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -64,7 +64,7 @@ public: R_(pose.rotation()), t_(pose.translation()), v_(v) { } /// Construct from SO(3) and R^6 - NavState(const Matrix3& R, const Vector9 tv) : + NavState(const Matrix3& R, const Vector6& tv) : R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { } /// Named constructor with derivatives From 76b29b78af8658d2828959f5e69285018801f19b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 6 Apr 2020 23:31:05 +0200 Subject: [PATCH 48/48] Prefer C++11 nullptr --- examples/SFMExample_SmartFactor.cpp | 2 +- examples/SFMExample_SmartFactorPCG.cpp | 2 +- gtsam/base/OptionalJacobian.h | 26 +++++++++---------- .../treeTraversal/parallelTraversalTasks.h | 12 ++++----- gtsam/discrete/AlgebraicDecisionTree.h | 2 +- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- gtsam/geometry/tests/testOrientedPlane3.cpp | 2 +- gtsam/geometry/tests/testUnit3.cpp | 2 +- gtsam/inference/BayesTree-inst.h | 4 +-- gtsam/inference/FactorGraph-inst.h | 4 +-- gtsam/inference/FactorGraph.h | 2 +- gtsam/inference/Ordering.cpp | 4 +-- gtsam/linear/JacobianFactor.cpp | 2 +- gtsam/linear/JacobianFactor.h | 2 +- gtsam/linear/LossFunctions.cpp | 16 ++++++------ gtsam/linear/NoiseModel.cpp | 4 +-- gtsam/linear/tests/testJacobianFactor.cpp | 2 +- gtsam/navigation/AHRSFactor.cpp | 2 +- gtsam/navigation/AttitudeFactor.cpp | 4 +-- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/GPSFactor.cpp | 4 +-- gtsam/nonlinear/ISAM2Params.h | 2 +- gtsam/nonlinear/NonlinearFactorGraph.cpp | 6 ++--- gtsam/nonlinear/tests/testCallRecord.cpp | 2 +- gtsam/slam/AntiFactor.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/EssentialMatrixConstraint.cpp | 2 +- gtsam/slam/JacobianFactorSVD.h | 2 +- gtsam/slam/OrientedPlane3Factor.cpp | 2 +- gtsam/slam/PoseRotationPrior.h | 2 +- gtsam/slam/PoseTranslationPrior.h | 2 +- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/tests/testOrientedPlane3Factor.cpp | 2 +- gtsam_unstable/base/BTree.h | 4 +-- .../linear/tests/testLinearEquality.cpp | 2 +- .../nonlinear/BatchFixedLagSmoother.cpp | 6 ++--- .../nonlinear/ConcurrentBatchFilter.cpp | 4 +-- .../nonlinear/ConcurrentBatchSmoother.cpp | 4 +-- .../nonlinear/IncrementalFixedLagSmoother.cpp | 2 +- gtsam_unstable/partition/FindSeparator-inl.h | 6 ++--- gtsam_unstable/slam/BiasedGPSFactor.h | 2 +- .../slam/EquivInertialNavFactor_GlobalVel.h | 2 +- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 2 +- .../slam/GaussMarkov1stOrderFactor.h | 2 +- .../slam/InertialNavFactor_GlobalVelocity.h | 2 +- gtsam_unstable/slam/PartialPriorFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- .../slam/RelativeElevationFactor.cpp | 2 +- tests/testExtendedKalmanFilter.cpp | 8 +++--- 50 files changed, 92 insertions(+), 92 deletions(-) diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index c5545fc0c..6e28b87c9 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -117,7 +117,7 @@ int main(int argc, char* argv[]) { // The output of point() is in boost::optional, as sometimes // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 075e2a653..47d69160f 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -114,7 +114,7 @@ int main(int argc, char* argv[]) { boost::dynamic_pointer_cast(graph[j]); if (smart) { boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL + if (point) // ignore if boost::optional return nullptr landmark_result.insert(j, *point); } } diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 82a5ae7f4..602c5915e 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -55,7 +55,7 @@ private: } // Private and very dangerous constructor straight from memory - OptionalJacobian(double* data) : map_(NULL) { + OptionalJacobian(double* data) : map_(nullptr) { if (data) usurp(data); } @@ -66,25 +66,25 @@ public: /// Default constructor acts like boost::none OptionalJacobian() : - map_(NULL) { + map_(nullptr) { } /// Constructor that will usurp data of a fixed-size matrix OptionalJacobian(Jacobian& fixed) : - map_(NULL) { + map_(nullptr) { usurp(fixed.data()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version OptionalJacobian(Jacobian* fixedPtr) : - map_(NULL) { + map_(nullptr) { if (fixedPtr) usurp(fixedPtr->data()); } /// Constructor that will resize a dynamic matrix (unless already correct) OptionalJacobian(Eigen::MatrixXd& dynamic) : - map_(NULL) { + map_(nullptr) { dynamic.resize(Rows, Cols); // no malloc if correct size usurp(dynamic.data()); } @@ -93,12 +93,12 @@ public: /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - map_(NULL) { + map_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - map_(NULL) { + map_(nullptr) { if (optional) { optional->resize(Rows, Cols); usurp(optional->data()); @@ -110,11 +110,11 @@ public: /// Constructor that will usurp data of a block expression /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible // template - // OptionalJacobian(Eigen::Block block) : map_(NULL) { ?? } + // OptionalJacobian(Eigen::Block block) : map_(nullptr) { ?? } /// Return true is allocated, false if default constructor was used operator bool() const { - return map_.data() != NULL; + return map_.data() != nullptr; } /// De-reference, like boost optional @@ -173,7 +173,7 @@ public: /// Default constructor acts like boost::none OptionalJacobian() : - pointer_(NULL) { + pointer_(nullptr) { } /// Construct from pointer to dynamic matrix @@ -186,12 +186,12 @@ public: /// Constructor with boost::none just makes empty OptionalJacobian(boost::none_t /*none*/) : - pointer_(NULL) { + pointer_(nullptr) { } /// Constructor compatible with old-style derivatives OptionalJacobian(const boost::optional optional) : - pointer_(NULL) { + pointer_(nullptr) { if (optional) pointer_ = &(*optional); } @@ -199,7 +199,7 @@ public: /// Return true is allocated, false if default constructor was used operator bool() const { - return pointer_!=NULL; + return pointer_!=nullptr; } /// De-reference, like boost optional diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 9b2dae3d0..db3181b87 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -53,7 +53,7 @@ namespace gtsam { // Run the post-order visitor (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } }; @@ -88,7 +88,7 @@ namespace gtsam { { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } else { @@ -129,14 +129,14 @@ namespace gtsam { { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return NULL; + return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return NULL; + return nullptr; } } } @@ -184,8 +184,8 @@ namespace gtsam { set_ref_count(1 + (int) roots.size()); // Spawn tasks spawn_and_wait_for_all(tasks); - // Return NULL - return NULL; + // Return nullptr + return nullptr; } }; diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 041d4c206..9cc55ed6a 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -101,7 +101,7 @@ namespace gtsam { /** Create a new function splitting on a variable */ template AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) : - Super(NULL) { + Super(nullptr) { this->root_ = compose(begin, end, label); } diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index af11d4b14..e41968d6b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -71,7 +71,7 @@ namespace gtsam { for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; ss << "factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter); } } diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 54cf84ea8..849b76483 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 59b99e525..e08ad97bb 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -495,7 +495,7 @@ TEST(actualH, Serialization) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 639bcbab0..5b53a5719 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -125,7 +125,7 @@ namespace gtsam { void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { for(Key j: clique->conditional()->frontals()) nodes_[j] = clique; - if (parent_clique != NULL) { + if (parent_clique != nullptr) { clique->parent_ = parent_clique; parent_clique->children.push_back(clique); } else { @@ -430,7 +430,7 @@ namespace gtsam { template void BayesTree::removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans) { - // base case is NULL, if so we do nothing and return empties above + // base case is nullptr, if so we do nothing and return empties above if (clique) { // remove the clique from orphans in case it has been added earlier orphans->remove(clique); diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index 34ca8ab7f..df68019e1 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -55,8 +55,8 @@ bool FactorGraph::equals(const This& fg, double tol) const { // check whether the factors are the same, in same order. for (size_t i = 0; i < factors_.size(); i++) { sharedFactor f1 = factors_[i], f2 = fg.factors_[i]; - if (f1 == NULL && f2 == NULL) continue; - if (f1 == NULL || f2 == NULL) return false; + if (f1 == nullptr && f2 == nullptr) continue; + if (f1 == nullptr || f2 == nullptr) return false; if (!f1->equals(*f2, tol)) return false; } return true; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 600e3f9ed..2bc9578b2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -353,7 +353,7 @@ class FactorGraph { */ void resize(size_t size) { factors_.resize(size); } - /** delete factor without re-arranging indexes by inserting a NULL pointer + /** delete factor without re-arranging indexes by inserting a nullptr pointer */ void remove(size_t i) { factors_[i].reset(); } diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index 0875755aa..266c54ca6 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex, assert((size_t)count == variableIndex.nEntries()); - //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */ + //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */ double knobs[CCOLAMD_KNOBS]; ccolamd_set_defaults(knobs); knobs[CCOLAMD_DENSE_ROW] = -1; @@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) { int outputError; - outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0], + outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0], &iperm[0]); Ordering result; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2310d88f0..839ffe69d 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta if (!model_) { throw std::invalid_argument( - "JacobianFactor::splitConditional cannot be given a NULL noise model"); + "JacobianFactor::splitConditional cannot be given a nullptr noise model"); } if (nrFrontals > size()) { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index fad05a729..9b9d02726 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -398,7 +398,7 @@ namespace gtsam { * @param keys in some order * @param diemnsions of the variables in same order * @param m output dimension - * @param model noise model (default NULL) + * @param model noise model (default nullptr) */ template JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m, diff --git a/gtsam/linear/LossFunctions.cpp b/gtsam/linear/LossFunctions.cpp index 6bc737e2c..de2a5142d 100644 --- a/gtsam/linear/LossFunctions.cpp +++ b/gtsam/linear/LossFunctions.cpp @@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const bool Fair::equals(const Base &expected, double tol) const { const Fair* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_ ) < tol; } @@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const { bool Huber::equals(const Base &expected, double tol) const { const Huber* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } @@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const { bool Cauchy::equals(const Base &expected, double tol) const { const Cauchy* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(ksquared_ - p->ksquared_) < tol; } @@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const { bool Tukey::equals(const Base &expected, double tol) const { const Tukey* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const { bool Welsch::equals(const Base &expected, double tol) const { const Welsch* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const { bool GemanMcClure::equals(const Base &expected, double tol) const { const GemanMcClure* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const { bool DCS::equals(const Base &expected, double tol) const { const DCS* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(c_ - p->c_) < tol; } @@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const { bool L2WithDeadZone::equals(const Base &expected, double tol) const { const L2WithDeadZone* p = dynamic_cast(&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return std::abs(k_ - p->k_) < tol; } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index df99191b2..c62fe4f21 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -133,7 +133,7 @@ void Gaussian::print(const string& name) const { /* ************************************************************************* */ bool Gaussian::equals(const Base& expected, double tol) const { const Gaussian* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; if (typeid(*this) != typeid(*p)) return false; //if (!sqrt_information_) return true; // ALEX todo; return equal_with_abs_tol(R(), p->R(), sqrt(tol)); @@ -624,7 +624,7 @@ void Robust::print(const std::string& name) const { bool Robust::equals(const Base& expected, double tol) const { const Robust* p = dynamic_cast (&expected); - if (p == NULL) return false; + if (p == nullptr) return false; return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 110a1223a..21146066d 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -253,7 +253,7 @@ TEST(JacobianFactor, error) /* ************************************************************************* */ TEST(JacobianFactor, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model JacobianFactor factor(simple::terms, simple::b); Matrix jacobianExpected(3, 9); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 0d7e05515..4604a55dd 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 3c6af9332..7f335152e 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } @@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) + return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) && this->bRef_.equals(e->bRef_, tol); } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 1967e4a56..149067269 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -174,7 +174,7 @@ void CombinedImuFactor::print(const string& s, //------------------------------------------------------------------------------ bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const { const This* e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); + return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol); } //------------------------------------------------------------------------------ diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 05b7259bf..f2448c488 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } //*************************************************************************** @@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const //*************************************************************************** bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && traits::Equals(nT_, e->nT_, tol); } diff --git a/gtsam/nonlinear/ISAM2Params.h b/gtsam/nonlinear/ISAM2Params.h index 5cf962e43..c6e1001c4 100644 --- a/gtsam/nonlinear/ISAM2Params.h +++ b/gtsam/nonlinear/ISAM2Params.h @@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params { /// When you will be removing many factors, e.g. when using ISAM2 as a /// fixed-lag smoother, enable this option to add factors in the first - /// available factor slots, to avoid accumulating NULL factor slots, at the + /// available factor slots, to avoid accumulating nullptr factor slots, at the /// cost of having to search for slots every time a factor is added. bool findUnusedFactorSlots; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index d4b9fbb68..42b2d07f6 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -54,7 +54,7 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter); cout << endl; } } @@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string& for (size_t i = 0; i < factors_.size(); i++) { stringstream ss; ss << "Factor " << i << ": "; - if (factors_[i] == NULL) { - cout << "NULL" << endl; + if (factors_[i] == nullptr) { + cout << "nullptr" << endl; } else { factors_[i]->print(ss.str(), keyFormatter); cout << "error = " << factors_[i]->error(values) << endl; diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 494a59fff..c5ccc0f52 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -98,7 +98,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -internal::JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(nullptr); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index b52e115fd..9e4f7db5a 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -67,7 +67,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); + return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index f949514e3..23138b9e6 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -81,7 +81,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/EssentialMatrixConstraint.cpp b/gtsam/slam/EssentialMatrixConstraint.cpp index e27bbc8c5..5397c2e57 100644 --- a/gtsam/slam/EssentialMatrixConstraint.cpp +++ b/gtsam/slam/EssentialMatrixConstraint.cpp @@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s, bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measuredE_.equals(e->measuredE_, tol); } diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index b94714dd6..bc906d24e 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -65,7 +65,7 @@ public: Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? - // PLAIN NULL SPACE TRICK + // PLAIN nullptr SPACE TRICK // Matrix Q = Enull * Enull.transpose(); // for(const KeyMatrixZD& it: Fblocks) // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 5b0c6a6b9..56968301a 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s, bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); } diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 78030ff34..f4ce1520a 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -62,7 +62,7 @@ public: /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index c38d13b58..516582d83 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -76,7 +76,7 @@ public: /** equals specialized to this factor */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(measured_, e->measured_, tol); } /** print contents */ diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 5b085652f..8d8c67d5c 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -85,7 +85,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index ea42a1ecd..f1dbb4239 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) { /* ************************************************************************* */ int main() { - srand(time(NULL)); + srand(time(nullptr)); TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/gtsam_unstable/base/BTree.h b/gtsam_unstable/base/BTree.h index fd675d0d5..96424324b 100644 --- a/gtsam_unstable/base/BTree.h +++ b/gtsam_unstable/base/BTree.h @@ -72,7 +72,7 @@ namespace gtsam { }; // Node // We store a shared pointer to the root of the functional tree - // composed of Node classes. If root_==NULL, the tree is empty. + // composed of Node classes. If root_==nullptr, the tree is empty. typedef boost::shared_ptr sharedNode; sharedNode root_; @@ -223,7 +223,7 @@ namespace gtsam { /** Return height of the tree, 0 if empty */ size_t height() const { - return (root_ != NULL) ? root_->height_ : 0; + return (root_ != nullptr) ? root_->height_ : 0; } /** return size of the tree */ diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index fa94dd255..36a2cacd8 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -136,7 +136,7 @@ TEST(LinearEquality, error) /* ************************************************************************* */ TEST(LinearEquality, matrices_NULL) { - // Make sure everything works with NULL noise model + // Make sure everything works with nullptr noise model LinearEquality factor(simple::terms, simple::b, 0); Matrix AExpected(3, 9); diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index b0a19c6a4..777e4b2d3 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const BatchFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol); } @@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors( } else { // TODO: Throw an error?? cout << "Attempting to remove a factor from slot " << slot - << ", but it is already NULL." << endl; + << ", but it is already nullptr." << endl; } } } @@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor( cout << " " << DefaultKeyFormatter(key); } } else { - cout << " NULL"; + cout << " nullptr"; } cout << " )" << endl; } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 97df375d5..758bcfe48 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 775dccbb0..600baa9f0 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } @@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr } std::cout << ")" << std::endl; } else { - std::cout << "{ NULL }" << std::endl; + std::cout << "{ nullptr }" << std::endl; } } diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index a99360ffb..d29dfe8ca 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { const IncrementalFixedLagSmoother* e = dynamic_cast(&rhs); - return e != NULL && FixedLagSmoother::equals(*e, tol) + return e != nullptr && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); } diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index af5415469..d87a8fd42 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -83,14 +83,14 @@ namespace gtsam { namespace partition { graph_t *graph; real_t *tpwgts2; ctrl_t *ctrl; - ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL); + ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr); ctrl->iptype = METIS_IPTYPE_GROW; - //if () == NULL) + //if () == nullptr) // return METIS_ERROR_INPUT; InitRandom(ctrl->seed); - graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL); + graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr); AllocateWorkSpace(ctrl, graph); diff --git a/gtsam_unstable/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h index f2bcb7cd7..cf56afab2 100644 --- a/gtsam_unstable/slam/BiasedGPSFactor.h +++ b/gtsam_unstable/slam/BiasedGPSFactor.h @@ -66,7 +66,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e != nullptr && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 58284c3a6..f499a0f4b 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -155,7 +155,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index acca233d9..86efd10c1 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -153,7 +153,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol && (delta_angles_ - e->delta_angles_).norm() < tol diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 68c972a86..762199753 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -81,7 +81,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol); + return e != nullptr && Base::equals(*e, tol); } /** implement functions needed to derive from Factor */ diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index c3a67232a..4763c4263 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -135,7 +135,7 @@ public: /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) + return e != nullptr && Base::equals(*e, tol) && (measurement_acc_ - e->measurement_acc_).norm() < tol && (measurement_gyro_ - e->measurement_gyro_).norm() < tol && (dt_ - e->dt_) < tol diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index c71ee7abd..c6d892e93 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -100,7 +100,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && + return e != nullptr && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && this->mask_ == e->mask_; } diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 03803b5f4..bb5835f80 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -79,7 +79,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index d8649a0d5..ce9861160 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -74,7 +74,7 @@ namespace gtsam { /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL + return e != nullptr && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); diff --git a/gtsam_unstable/slam/RelativeElevationFactor.cpp b/gtsam_unstable/slam/RelativeElevationFactor.cpp index 32e8731cd..b35171041 100644 --- a/gtsam_unstable/slam/RelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/RelativeElevationFactor.cpp @@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p /* ************************************************************************* */ bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; + return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol; } /* ************************************************************************* */ diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index b3e8a3449..212a8e107 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -162,7 +162,7 @@ public: /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); + return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2()); } /** @@ -196,7 +196,7 @@ public: Vector b = -evaluateError(x1, x2, A1, A2); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, constrained)); } @@ -292,7 +292,7 @@ public: /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && Base::equals(f); + return (e != nullptr) && Base::equals(f); } /** @@ -325,7 +325,7 @@ public: Vector b = -evaluateError(x1, A1); SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); - if (constrained.get() != NULL) { + if (constrained.get() != nullptr) { return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor