From d57fffb703770fe2f9b4588a558637fce8bb5ae9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Nov 2022 18:31:15 -0500 Subject: [PATCH 01/13] allow examples to link to gtsam_unstable --- examples/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 476f4ae215..7fc33f9210 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,4 +2,4 @@ set (excluded_examples elaboratePoint2KalmanFilter.cpp ) -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}") From 4c7dcb3efd96f2f9948d310fe8b4820758957356 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 7 Nov 2022 18:31:29 -0500 Subject: [PATCH 02/13] better error messages from Pybind --- python/gtsam/gtsam.tpl | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b760e4eb5a..a56f681766 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -7,6 +7,8 @@ * ** THIS FILE IS AUTO-GENERATED, DO NOT MODIFY! ** */ +#define PYBIND11_DETAILED_ERROR_MESSAGES + // Include relevant boost libraries required by GTSAM {include_boost} From 193ee66e8f482cffd16f34d4ac109360fa0457e4 Mon Sep 17 00:00:00 2001 From: Tae Young Kim Date: Sun, 13 Nov 2022 16:57:16 +0900 Subject: [PATCH 03/13] Remove unnecessary functions in ImuFactorsExample.cpp --- examples/ImuFactorsExample.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index c176318642..ba4d5c9744 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -267,7 +267,6 @@ int main(int argc, char* argv[]) { if (use_isam) { isam2->update(*graph, initial_values); - isam2->update(); result = isam2->calculateEstimate(); // reset the graph From c709932f9807ddc02a696ec689965cc31da8a416 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:46:38 -0500 Subject: [PATCH 04/13] Add unit test exposing GaussianBayesTree determinant bug --- gtsam/linear/tests/testGaussianBayesTree.cpp | 43 ++++++++++++++++---- 1 file changed, 36 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index c5601af271..3b960508dc 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -15,18 +15,18 @@ * @author Kai Ni */ -#include #include - -#include -#include // for operator += -#include // for operator += - #include #include -#include +#include #include #include +#include + +#include +#include // for operator += +#include // for operator += +#include using namespace boost::assign; using namespace std::placeholders; @@ -321,6 +321,35 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { EXPECT_DOUBLES_EQUAL(expectedDeterminant,actualDeterminant,expectedDeterminant*1e-6);// relative tolerance } +/* ************************************************************************* */ +/** Test to expose bug in GaussianBayesTree::logDeterminant */ +TEST(GaussianBayesTree, LogDeterminant) { + using symbol_shorthand::L; + using symbol_shorthand::X; + + // Create a factor graph that will result in a bayes tree with at least 2 + // nodes + GaussianFactorGraph fg; + Key x1 = X(1), x2 = X(2), l1 = L(1); + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), + unit2); + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg += + JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0), + unit2); + fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + + // create corresponding Bayes net and Bayes tree: + boost::shared_ptr bn = fg.eliminateSequential(); + boost::shared_ptr bt = fg.eliminateMultifrontal(); + + // Test logDeterminant + EXPECT_DOUBLES_EQUAL(bn->logDeterminant(), bt->logDeterminant(), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 3dcf9d8da810cb4225a2c242dd82de343d55cf2a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:54:03 -0500 Subject: [PATCH 05/13] fix bug in GaussianBayesTree::logDeterminant --- gtsam/linear/GaussianBayesTree.cpp | 56 +++++++++++++++++++++--------- 1 file changed, 39 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 13c19bce65..a83475e26b 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -31,18 +31,37 @@ namespace gtsam { template class BayesTreeCliqueBase; template class BayesTree; - /* ************************************************************************* */ - namespace internal - { - /* ************************************************************************* */ - double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, - double& parentSum) { - parentSum += clique->conditional() - ->R() - .diagonal() - .unaryExpr([](double x) { return log(x); }) - .sum(); - return 0; + /* ************************************************************************ */ + namespace internal { + + /** + * @brief Struct to help with traversing the Bayes Tree + * for log-determinant computation. + * Records the data which is passed to the child nodes in pre-order visit. + */ + struct LogDeterminantData { + // Use pointer so we can get the full result after tree traversal + double* logDet; + LogDeterminantData(double* logDet) + : logDet(logDet) {} + }; + /* ************************************************************************ */ + LogDeterminantData& logDeterminant( + const GaussianBayesTreeClique::shared_ptr& clique, + LogDeterminantData& parentSum) { + auto cg = clique->conditional(); + double logDet; + if (cg->get_model()) { + Vector diag = cg->R().diagonal(); + cg->get_model()->whitenInPlace(diag); + logDet = diag.unaryExpr([](double x) { return log(x); }).sum(); + } else { + logDet = + cg->R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + } + // Add the current clique's log-determinant to the overall sum + (*parentSum.logDet) += logDet; + return parentSum; } } // namespace internal @@ -87,7 +106,14 @@ namespace gtsam { return 0.0; } else { double sum = 0.0; - treeTraversal::DepthFirstForest(*this, sum, internal::logDeterminant); + // Store the log-determinant in this struct. + internal::LogDeterminantData rootData(&sum); + // No need to do anything for post-operation. + treeTraversal::no_op visitorPost; + // Limits OpenMP threads if we're mixing TBB and OpenMP + TbbOpenMPMixedScope threadLimiter; + // Traverse the GaussianBayesTree depth first and call logDeterminant on each node. + treeTraversal::DepthFirstForestParallel(*this, rootData, internal::logDeterminant, visitorPost); return sum; } } @@ -106,7 +132,3 @@ namespace gtsam { } // \namespace gtsam - - - - From 93079f32e1b2a13a9dd06433f8489882cf7a10b0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 14 Nov 2022 10:58:02 -0500 Subject: [PATCH 06/13] docstring updates --- gtsam/linear/tests/testGaussianBayesTree.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 3b960508dc..18b4674b5e 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -322,13 +322,13 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { } /* ************************************************************************* */ -/** Test to expose bug in GaussianBayesTree::logDeterminant */ +/// Test to expose bug in GaussianBayesTree::logDeterminant. TEST(GaussianBayesTree, LogDeterminant) { using symbol_shorthand::L; using symbol_shorthand::X; - // Create a factor graph that will result in a bayes tree with at least 2 - // nodes + // Create a factor graph that will result in + // a bayes tree with at least 2 nodes. GaussianFactorGraph fg; Key x1 = X(1), x2 = X(2), l1 = L(1); SharedDiagonal unit2 = noiseModel::Unit::Create(2); From 10e03063b312a7c2ca12494bba94f594a19c40a6 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Tue, 15 Nov 2022 11:46:06 -0500 Subject: [PATCH 07/13] add workflow to trigger ubuntu packaging --- .github/workflows/trigger-packaging.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/trigger-packaging.yml diff --git a/.github/workflows/trigger-packaging.yml b/.github/workflows/trigger-packaging.yml new file mode 100644 index 0000000000..1f24db5032 --- /dev/null +++ b/.github/workflows/trigger-packaging.yml @@ -0,0 +1,21 @@ +# This triggers building of packages +name: Trigger Package Builds +on: + push: + branches: + - develop +jobs: + trigger-package-build: + runs-on: ubuntu-latest + steps: + - name: Trigger Package Rebuild + uses: actions/github-script@v6 + with: + github-token: ${{ secrets.PACKAGING_REPO_ACCESS_TOKEN }} + script: | + await github.rest.actions.createWorkflowDispatch({ + owner: 'borglab-launchpad', + repo: 'gtsam-packaging', + workflow_id: 'main.yaml', + ref: 'master' + }) From 70f490b2980cd95719c0a1ce81f1f8fb81575f36 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Thu, 17 Nov 2022 17:47:26 +0100 Subject: [PATCH 08/13] Exposed normalize for Point3_ --- gtsam/slam/expressions.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 3b8ea86d37..24476cb5e2 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -48,6 +48,11 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) { return Line3_(f, wTc, wL); } +inline Point3_ normalize(const Point3_& a){ + Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize; + return Point3_(f, a); +} + inline Point3_ cross(const Point3_& a, const Point3_& b) { Point3 (*f)(const Point3 &, const Point3 &, OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = ✗ From 818417900862eb76c287b9e098d14c7118bdd51a Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Thu, 17 Nov 2022 17:59:59 +0100 Subject: [PATCH 09/13] Added test for normalize --- tests/testExpressionFactor.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5b27eea4db..9a4e01c46f 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -731,6 +731,19 @@ TEST(ExpressionFactor, variadicTemplate) { EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5); } +TEST(ExpressionFactor, normalize) { + auto model = noiseModel::Isotropic::Sigma(3, 1); + + // Create expression + const auto x = Vector3_(1); + Vector3_ f_expr = normalize(x); + + // Check derivatives + Values values; + values.insert(1, Vector3(1, 2, 3)); + ExpressionFactor factor(model, Vector3(1.0/sqrt(14), 2.0/sqrt(14), 3.0/sqrt(14)), f_expr); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} TEST(ExpressionFactor, crossProduct) { auto model = noiseModel::Isotropic::Sigma(3, 1); From de3305bcd923e21ba03cb0200a5a589e43bec421 Mon Sep 17 00:00:00 2001 From: Miguel Vega Date: Sat, 19 Nov 2022 17:28:44 +0100 Subject: [PATCH 10/13] Update README.md Suggestions to improve readme title, terminal commands and citation format. --- README.md | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index b32ce70e07..bb066a5dfa 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# README - Georgia Tech Smoothing and Mapping Library +# Georgia Tech Smoothing and Mapping Library **Important Note** @@ -31,11 +31,11 @@ In the root library folder execute: ```sh #!bash -$ mkdir build -$ cd build -$ cmake .. -$ make check (optional, runs unit tests) -$ make install +mkdir build +cd build +cmake .. +make check (optional, runs unit tests) +make install ``` Prerequisites: @@ -68,7 +68,7 @@ We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) If you are using GTSAM for academic work, please use the following citation: -``` +```bibtex @software{gtsam, author = {Frank Dellaert and Richard Roberts and Varun Agrawal and Alex Cunningham and Chris Beall and Duy-Nguyen Ta and Fan Jiang and lucacarlone and nikai and Jose Luis Blanco-Claraco and Stephen Williams and ydjian and John Lambert and Andy Melim and Zhaoyang Lv and Akshay Krishnan and Jing Dong and Gerry Chen and Krunal Chande and balderdash-devil and DiffDecisionTrees and Sungtae An and mpaluri and Ellon Paiva Mendes and Mike Bosse and Akash Patel and Ayush Baid and Paul Furgale and matthewbroadwaynavenio and roderick-koehle}, title = {borglab/gtsam}, From f9a5313ff82c20d85d80e5a5ea4038dcd3da0736 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 29 Nov 2022 00:48:03 -0500 Subject: [PATCH 11/13] Update prerelease version cmake --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 39d1e4307e..b81479cb84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ endif() set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 2) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a7") +set (GTSAM_PRERELEASE_VERSION "a8") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") if (${GTSAM_VERSION_PATCH} EQUAL 0) From 829a6698540ffcb0043de0deb5b14adb38eecb9e Mon Sep 17 00:00:00 2001 From: Miguel Vega Date: Tue, 6 Dec 2022 22:31:33 +0100 Subject: [PATCH 12/13] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index bb066a5dfa..0493c46c6e 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Georgia Tech Smoothing and Mapping Library +# README - Georgia Tech Smoothing and Mapping Library **Important Note** From 1ea655355041adf66e15c60dc6a67f157f2c3a38 Mon Sep 17 00:00:00 2001 From: ulterzlw Date: Wed, 7 Dec 2022 14:53:32 +0800 Subject: [PATCH 13/13] Fixed Jacobian in python CustomFactorExample --- python/gtsam/examples/CustomFactorExample.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 36c1e003d3..c7ae8ad21d 100644 --- a/python/gtsam/examples/CustomFactorExample.py +++ b/python/gtsam/examples/CustomFactorExample.py @@ -60,10 +60,10 @@ def error_odom(measurement: np.ndarray, this: gtsam.CustomFactor, key1 = this.keys()[0] key2 = this.keys()[1] pos1, pos2 = values.atVector(key1), values.atVector(key2) - error = measurement - (pos1 - pos2) + error = (pos2 - pos1) - measurement if jacobians is not None: - jacobians[0] = I - jacobians[1] = -I + jacobians[0] = -I + jacobians[1] = I return error