From 12b35b41424d1564162fde59b47b474b23d26678 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 19 Jul 2022 18:59:58 -0400 Subject: [PATCH 1/8] remove pybind stl.h --- python/gtsam/preamble/nonlinear.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index a34e73058..d07a75f6f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -9,4 +9,4 @@ * automatic STL binding, such that the raw objects can be accessed in Python. * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. - */ \ No newline at end of file + */ From 7eb9a95c5fbf0e60813d69443e2e9ab7dcd50c58 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:35:09 -0400 Subject: [PATCH 2/8] minor fixes --- gtsam/nonlinear/NonlinearFactor.cpp | 3 ++- python/gtsam/preamble/nonlinear.h | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 3d572e970..debff54ac 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -167,8 +167,9 @@ boost::shared_ptr NoiseModelFactor::linearize( return GaussianFactor::shared_ptr( new JacobianFactor(terms, b, boost::static_pointer_cast(noiseModel_)->unit())); - else + else { return GaussianFactor::shared_ptr(new JacobianFactor(terms, b)); + } } /* ************************************************************************* */ diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index d07a75f6f..56a07cfdd 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,3 +10,5 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +#include From 31cfce0b8dfc284fb593c2e27f5c0b9c0f374ae7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 05:15:40 -0400 Subject: [PATCH 3/8] update macos CI env --- .github/workflows/build-macos.yml | 10 +++++----- .github/workflows/build-python.yml | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 462723222..7b7646328 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -19,16 +19,16 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - macOS-10.15-xcode-11.3.1, + macos-11-xcode-13.4.1, ] build_type: [Debug, Release] build_unstable: [ON] include: - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macos-11-xcode-13.4.1 + os: macos-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" steps: - name: Checkout @@ -43,7 +43,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f42939bc2..f80b9362c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: ubuntu-18.04-gcc-5, ubuntu-18.04-gcc-9, ubuntu-18.04-clang-9, - macOS-10.15-xcode-11.3.1, + macOS-11-xcode-13.4.1, ubuntu-18.04-gcc-5-tbb, ] @@ -52,10 +52,10 @@ jobs: build_type: Debug python_version: "3" - - name: macOS-10.15-xcode-11.3.1 - os: macOS-10.15 + - name: macOS-11-xcode-13.4.1 + os: macOS-11 compiler: xcode - version: "11.3.1" + version: "13.4.1" - name: ubuntu-18.04-gcc-5-tbb os: ubuntu-18.04 @@ -103,7 +103,7 @@ jobs: echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV else - sudo xcode-select -switch /Applications/Xcode_${{ matrix.version }}.app + sudo xcode-select -switch /Applications/Xcode.app echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV fi From 7edd021956e659f1b2ecba5a630682df1d7ea0d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Jul 2022 14:00:03 -0400 Subject: [PATCH 4/8] fix copying of test files in Cmake --- python/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index cba206d11..0d63c6c54 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -104,7 +104,8 @@ copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" # Hack to get python test and util files copied every time they are modified file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) - configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) + get_filename_component(test_file_name ${test_file} NAME) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file_name}" COPYONLY) endforeach() file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py") foreach(util_file ${GTSAM_PYTHON_UTIL_FILES}) From 27951b7b180f7cdfc3300fb9d8d163586fb9f305 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:17:48 -0400 Subject: [PATCH 5/8] use python 3.7 --- .github/scripts/python.sh | 2 +- .github/workflows/build-python.yml | 16 ++++++---------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 0855dbc21..b734f5919 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -59,7 +59,7 @@ PATH=$PATH:$($PYTHON -c "import site; print(site.USER_BASE)")/bin BUILD_PYBIND="ON" -sudo $PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt +$PYTHON -m pip install -r $GITHUB_WORKSPACE/python/requirements.txt mkdir $GITHUB_WORKSPACE/build cd $GITHUB_WORKSPACE/build diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f80b9362c..d391e571c 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -27,7 +27,7 @@ jobs: ] build_type: [Debug, Release] - python_version: [3] + python_version: [3.7] include: - name: ubuntu-18.04-gcc-5 os: ubuntu-18.04 @@ -44,14 +44,6 @@ jobs: compiler: clang version: "9" - # NOTE temporarily added this as it is a required check. - - name: ubuntu-18.04-clang-9 - os: ubuntu-18.04 - compiler: clang - version: "9" - build_type: Debug - python_version: "3" - - name: macOS-11-xcode-13.4.1 os: macOS-11 compiler: xcode @@ -65,7 +57,11 @@ jobs: steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 + - name: Set up Python + uses: actions/setup-python@v4 + with: + python-version: '3.7' - name: Install (Linux) if: runner.os == 'Linux' run: | From 4edcb41ad3680a9d4f6ee2f829c8ffd49b2bdba8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:05 -0400 Subject: [PATCH 6/8] remove redundant KeyVector definition --- gtsam/gtsam.i | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d4e959c3d..2671f0ef7 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -66,27 +66,6 @@ class KeySet { void serialize() const; }; -// Actually a vector -class KeyVector { - KeyVector(); - KeyVector(const gtsam::KeyVector& other); - - // Note: no print function - - // common STL methods - size_t size() const; - bool empty() const; - void clear(); - - // structure specific methods - size_t at(size_t i) const; - size_t front() const; - size_t back() const; - void push_back(size_t key) const; - - void serialize() const; -}; - // Actually a FastMap class KeyGroupMap { KeyGroupMap(); From 42bab8f3e70eee07a2ae591786f3b2a297b6784a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Jul 2022 22:19:29 -0400 Subject: [PATCH 7/8] use KeyVector for GNC inliers & outliers --- gtsam/nonlinear/GncParams.h | 12 ++++++++---- gtsam/nonlinear/nonlinear.i | 8 ++++---- python/gtsam/preamble/nonlinear.h | 2 -- python/gtsam/specializations/nonlinear.h | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index a7ccb88b7..08ae42336 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -72,8 +72,12 @@ class GTSAM_EXPORT GncParams { double relativeCostTol = 1e-5; ///< If relative cost change is below this threshold, stop iterating double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - std::vector knownInliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are inliers - std::vector knownOutliers = std::vector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers + + // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + ///< Slots in the factor graph corresponding to measurements that we know are inliers + KeyVector knownInliers = KeyVector(); + ///< Slots in the factor graph corresponding to measurements that we know are outliers + KeyVector knownOutliers = KeyVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -114,7 +118,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const std::vector& knownIn) { + void setKnownInliers(const KeyVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -125,7 +129,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const std::vector& knownOut) { + void setKnownOutliers(const KeyVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c79333236..4bb80a0d6 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -529,8 +529,8 @@ virtual class GncParams { double relativeCostTol; double weightsTol; Verbosity verbosity; - std::vector knownInliers; - std::vector knownOutliers; + gtsam::KeyVector knownInliers; + gtsam::KeyVector knownOutliers; void setLossType(const gtsam::GncLossType type); void setMaxIterations(const size_t maxIter); @@ -538,8 +538,8 @@ virtual class GncParams { void setRelativeCostTol(double value); void setWeightsTol(double value); void setVerbosityGNC(const gtsam::This::Verbosity value); - void setKnownInliers(const std::vector& knownIn); - void setKnownOutliers(const std::vector& knownOut); + void setKnownInliers(const gtsam::KeyVector& knownIn); + void setKnownOutliers(const gtsam::KeyVector& knownOut); void print(const string& str = "GncParams: ") const; enum Verbosity { diff --git a/python/gtsam/preamble/nonlinear.h b/python/gtsam/preamble/nonlinear.h index 56a07cfdd..d07a75f6f 100644 --- a/python/gtsam/preamble/nonlinear.h +++ b/python/gtsam/preamble/nonlinear.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -#include diff --git a/python/gtsam/specializations/nonlinear.h b/python/gtsam/specializations/nonlinear.h index d46ccdc66..da8842eaf 100644 --- a/python/gtsam/specializations/nonlinear.h +++ b/python/gtsam/specializations/nonlinear.h @@ -9,4 +9,4 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ From 1d51c4e64692f0792685d4d50873ba00ef61d238 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Jul 2022 07:54:05 -0400 Subject: [PATCH 8/8] Use new GncParams::IndexVector --- gtsam/nonlinear/GncParams.h | 12 +++++++----- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 08ae42336..e069d5f3d 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,11 +73,13 @@ class GTSAM_EXPORT GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - // *NOTE* We use KeyVector for inliers and outliers since it is fast + wrapping + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - KeyVector knownInliers = KeyVector(); + IndexVector knownInliers = IndexVector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers - KeyVector knownOutliers = KeyVector(); + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -118,7 +120,7 @@ class GTSAM_EXPORT GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const KeyVector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -129,7 +131,7 @@ class GTSAM_EXPORT GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const KeyVector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index c3335ce20..15d660114 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -763,7 +763,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption std::vector knownInliers; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -784,12 +784,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -813,11 +813,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -841,7 +841,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -916,11 +916,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - std::vector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - std::vector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers);