Merge branch 'hybrid/error' into hybrid/tests

.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'
+            })

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)

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},

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}")

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

gtsam/linear/GaussianBayesTree.cpp

@@ -31,18 +31,37 @@ namespace gtsam {
   template class BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>;
   template class BayesTree<GaussianBayesTreeClique>;
 
-  /* ************************************************************************* */
-  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
-
-
-
-

gtsam/linear/tests/testGaussianBayesTree.cpp

@@ -15,18 +15,18 @@
  * @author Kai Ni
  */
 
-#include <iostream>
 #include <CppUnitLite/TestHarness.h>
-
-#include <boost/assign/list_of.hpp>
-#include <boost/assign/std/list.hpp> // for operator +=
-#include <boost/assign/std/set.hpp> // for operator +=
-
 #include <gtsam/base/debug.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
+
+#include <boost/assign/list_of.hpp>
+#include <boost/assign/std/list.hpp>  // for operator +=
+#include <boost/assign/std/set.hpp>   // for operator +=
+#include <iostream>
 
 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<gtsam::GaussianBayesNet> bn = fg.eliminateSequential();
+  boost::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
+
+  // Test logDeterminant
+  EXPECT_DOUBLES_EQUAL(bn->logDeterminant(), bt->logDeterminant(), 1e-9);
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

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>) = &cross;

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
 

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}
 

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<Vector3> 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);