diff --git a/.github/workflows/trigger-packaging.yml b/.github/workflows/trigger-packaging.yml new file mode 100644 index 000000000..1f24db503 --- /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' + }) diff --git a/CMakeLists.txt b/CMakeLists.txt index 39d1e4307..b81479cb8 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) diff --git a/README.md b/README.md index b32ce70e0..0493c46c6 100644 --- a/README.md +++ b/README.md @@ -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}, diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 476f4ae21..7fc33f921 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}") diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index c17631864..ba4d5c974 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 diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 13c19bce6..a83475e26 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 - - - - diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index c5601af27..18b4674b5 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);} /* ************************************************************************* */ diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 3b8ea86d3..24476cb5e 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>) = ✗ diff --git a/python/gtsam/examples/CustomFactorExample.py b/python/gtsam/examples/CustomFactorExample.py index 36c1e003d..c7ae8ad21 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 diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index b760e4eb5..a56f68176 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} diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5b27eea4d..9a4e01c46 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);