Merge branch 'develop' into jeffrey/isam2_marginalization

.github/workflows/build-macos.yml

@@ -25,15 +25,15 @@ 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-12-xcode-14.2,
+          macos-13-xcode-14.2,
           macos-14-xcode-15.4,
         ]
 
         build_type: [Debug, Release]
         build_unstable: [ON]
         include:
-          - name: macos-12-xcode-14.2
-            os: macos-12
+          - name: macos-13-xcode-14.2
+            os: macos-13
             compiler: xcode
             version: "14.2"
 

.github/workflows/build-python.yml

@@ -30,7 +30,7 @@ jobs:
           [
             ubuntu-20.04-gcc-9,
             ubuntu-20.04-clang-9,
-            macos-12-xcode-14.2,
+            macos-13-xcode-14.2,
             macos-14-xcode-15.4,
             windows-2022-msbuild,
           ]
@@ -48,8 +48,8 @@ jobs:
             compiler: clang
             version: "9"
 
-          - name: macos-12-xcode-14.2
-            os: macos-12
+          - name: macos-13-xcode-14.2
+            os: macos-13
             compiler: xcode
             version: "14.2"
 

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.9)
 if (POLICY CMP0082)
     cmake_policy(SET CMP0082 NEW) # install from sub-directories immediately
 endif()
@@ -12,6 +12,12 @@ if (POLICY CMP0167)
 cmake_policy(SET CMP0167 OLD) # Don't complain about boost
 endif()
 
+# allow parent project to override options
+if (POLICY CMP0077)
+  cmake_policy(SET CMP0077 NEW)
+endif(POLICY CMP0077)
+
+
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 4)
 set (GTSAM_VERSION_MINOR 3)

cmake/FindGooglePerfTools.cmake

@@ -1,42 +1,40 @@
 # -*- cmake -*-
 
-# - Find Google perftools
-# Find the Google perftools includes and libraries
-# This module defines
-#  GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc.
-#  GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools.
-# also defined for general use are
-#  TCMALLOC_LIBRARY, where to find the tcmalloc library.
-
-FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h
-/usr/local/include
-/usr/include
-)
+# - Find GPerfTools (formerly Google perftools)
+# Find the GPerfTools libraries
+# If false, do not try to use Google perftools.
+# Also defined for general use are
+# - GPERFTOOLS_TCMALLOC: where to find the tcmalloc library
+# - GPERFTOOLS_PROFILER: where to find the profiler library
 
 SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc)
-FIND_LIBRARY(TCMALLOC_LIBRARY
+find_library(GPERFTOOLS_TCMALLOC
   NAMES ${TCMALLOC_NAMES}
   PATHS /usr/lib /usr/local/lib
-  )
+)
+find_library(GPERFTOOLS_PROFILER
+   NAMES profiler
+   PATHS /usr/lib /usr/local/lib
+)
 
-IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
-    SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY})
-    SET(GOOGLE_PERFTOOLS_FOUND "YES")
-ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
-  SET(GOOGLE_PERFTOOLS_FOUND "NO")
-ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR)
+IF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
+    SET(TCMALLOC_LIBRARIES ${GPERFTOOLS_TCMALLOC})
+    SET(GPERFTOOLS_FOUND "YES")
+ELSE (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
+  SET(GPERFTOOLS_FOUND "NO")
+ENDIF (GPERFTOOLS_TCMALLOC AND GPERFTOOLS_PROFILER)
 
-IF (GOOGLE_PERFTOOLS_FOUND)
-   IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}")
-   ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY)
-ELSE (GOOGLE_PERFTOOLS_FOUND)
+IF (GPERFTOOLS_FOUND)
+   MESSAGE(STATUS "Found Gperftools: ${GPERFTOOLS_PROFILER}")
+ELSE (GPERFTOOLS_FOUND)
    IF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Google perftools library")
    ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED)
-ENDIF (GOOGLE_PERFTOOLS_FOUND)
+ENDIF (GPERFTOOLS_FOUND)
 
 MARK_AS_ADVANCED(
-  TCMALLOC_LIBRARY
-  GOOGLE_PERFTOOLS_INCLUDE_DIR
-  )
+  GPERFTOOLS_TCMALLOC
+  GPERFTOOLS_PROFILER
+)
+
+option(GTSAM_ENABLE_GPERFTOOLS                   "Enable/Disable Gperftools" OFF)

cmake/HandleAllocators.cmake

@@ -7,7 +7,7 @@ else()
     list(APPEND possible_allocators BoostPool STL)
     set(preferred_allocator STL)
 endif()
-if(GOOGLE_PERFTOOLS_FOUND)
+if(GPERFTOOLS_FOUND)
     list(APPEND possible_allocators tcmalloc)
 endif()
 

cmake/HandlePerfTools.cmake

@@ -1,4 +1,4 @@
 
 ###############################################################################
 # Find Google perftools
-find_package(GooglePerfTools)
+find_package(GooglePerfTools)

examples/EssentialViewGraphExample.cpp

@@ -0,0 +1,138 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    EssentialViewGraphExample.cpp
+ * @brief   View-graph calibration with essential matrices.
+ * @author  Frank Dellaert
+ * @date    October 2024
+ */
+
+#include <gtsam/geometry/Cal3f.h>
+#include <gtsam/geometry/EssentialMatrix.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/EdgeKey.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/sfm/TransferFactor.h>  // Contains EssentialTransferFactorK
+
+#include <vector>
+
+#include "SFMdata.h"  // For createPoints() and posesOnCircle()
+
+using namespace std;
+using namespace gtsam;
+using namespace symbol_shorthand;  // For K(symbol)
+
+// Main function
+int main(int argc, char* argv[]) {
+  // Define the camera calibration parameters
+  Cal3f cal(50.0, 50.0, 50.0);
+
+  // Create the set of 8 ground-truth landmarks
+  vector<Point3> points = createPoints();
+
+  // Create the set of 4 ground-truth poses
+  vector<Pose3> poses = posesOnCircle(4, 30);
+
+  // Calculate ground truth essential matrices, 1 and 2 poses apart
+  auto E1 = EssentialMatrix::FromPose3(poses[0].between(poses[1]));
+  auto E2 = EssentialMatrix::FromPose3(poses[0].between(poses[2]));
+
+  // Simulate measurements from each camera pose
+  std::array<std::array<Point2, 8>, 4> p;
+  for (size_t i = 0; i < 4; ++i) {
+    PinholeCamera<Cal3f> camera(poses[i], cal);
+    for (size_t j = 0; j < 8; ++j) {
+      p[i][j] = camera.project(points[j]);
+    }
+  }
+
+  // Create the factor graph
+  NonlinearFactorGraph graph;
+  using Factor = EssentialTransferFactorK<Cal3f>;
+
+  for (size_t a = 0; a < 4; ++a) {
+    size_t b = (a + 1) % 4;  // Next camera
+    size_t c = (a + 2) % 4;  // Camera after next
+
+    // Vectors to collect tuples for each factor
+    std::vector<std::tuple<Point2, Point2, Point2>> tuples1, tuples2, tuples3;
+
+    // Collect data for the three factors
+    for (size_t j = 0; j < 8; ++j) {
+      tuples1.emplace_back(p[a][j], p[b][j], p[c][j]);
+      tuples2.emplace_back(p[a][j], p[c][j], p[b][j]);
+      tuples3.emplace_back(p[c][j], p[b][j], p[a][j]);
+    }
+
+    // Add transfer factors between views a, b, and c.
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(b, c), tuples1);
+    graph.emplace_shared<Factor>(EdgeKey(a, b), EdgeKey(b, c), tuples2);
+    graph.emplace_shared<Factor>(EdgeKey(a, c), EdgeKey(a, b), tuples3);
+  }
+
+  // Formatter for printing keys
+  auto formatter = [](Key key) {
+    if (Symbol(key).chr() == 'k') {
+      return (string)Symbol(key);
+    } else {
+      EdgeKey edge(key);
+      return (std::string)edge;
+    }
+  };
+
+  graph.print("Factor Graph:\n", formatter);
+
+  // Create a delta vector to perturb the ground truth (small perturbation)
+  Vector5 delta;
+  delta << 1, 1, 1, 1, 1;
+  delta *= 1e-2;
+
+  // Create the initial estimate for essential matrices
+  Values initialEstimate;
+  for (size_t a = 0; a < 4; ++a) {
+    size_t b = (a + 1) % 4;  // Next camera
+    size_t c = (a + 2) % 4;  // Camera after next
+    initialEstimate.insert(EdgeKey(a, b), E1.retract(delta));
+    initialEstimate.insert(EdgeKey(a, c), E2.retract(delta));
+  }
+
+  // Insert initial calibrations (using K symbol)
+  for (size_t i = 0; i < 4; ++i) {
+    initialEstimate.insert(K(i), cal);
+  }
+
+  initialEstimate.print("Initial Estimates:\n", formatter);
+  graph.printErrors(initialEstimate, "Initial Errors:\n", formatter);
+
+  // Optimize the graph and print results
+  LevenbergMarquardtParams params;
+  params.setlambdaInitial(1000.0);  // Initialize lambda to a high value
+  params.setVerbosityLM("SUMMARY");
+  Values result =
+      LevenbergMarquardtOptimizer(graph, initialEstimate, params).optimize();
+
+  cout << "Initial error = " << graph.error(initialEstimate) << endl;
+  cout << "Final error = " << graph.error(result) << endl;
+
+  result.print("Final Results:\n", formatter);
+
+  E1.print("Ground Truth E1:\n");
+  E2.print("Ground Truth E2:\n");
+
+  return 0;
+}

examples/SFMdata.h

@@ -56,7 +56,7 @@ std::vector<Point3> createPoints() {
 
 /**
  * Create a set of ground-truth poses
- *  Default values give a circular trajectory, radius 30 at pi/4 intervals,
+ * Default values give a circular trajectory, radius 30 at pi/4 intervals,
  * always facing the circle center
  */
 std::vector<Pose3> createPoses(

examples/ViewGraphExample.cpp

@@ -38,7 +38,7 @@ using namespace gtsam;
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
   // Define the camera calibration parameters
-  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
+  Cal3_S2 cal(50.0, 50.0, 0.0, 50.0, 50.0);
 
   // Create the set of 8 ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -47,13 +47,13 @@ int main(int argc, char* argv[]) {
   vector<Pose3> poses = posesOnCircle(4, 30);
 
   // Calculate ground truth fundamental matrices, 1 and 2 poses apart
-  auto F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K);
-  auto F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K);
+  auto F1 = FundamentalMatrix(cal.K(), poses[0].between(poses[1]), cal.K());
+  auto F2 = FundamentalMatrix(cal.K(), poses[0].between(poses[2]), cal.K());
 
   // Simulate measurements from each camera pose
   std::array<std::array<Point2, 8>, 4> p;
   for (size_t i = 0; i < 4; ++i) {
-    PinholeCamera<Cal3_S2> camera(poses[i], K);
+    PinholeCamera<Cal3_S2> camera(poses[i], cal);
     for (size_t j = 0; j < 8; ++j) {
       p[i][j] = camera.project(points[j]);
     }
@@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
     initialEstimate.insert(EdgeKey(a, c), F2.retract(delta));
   }
   initialEstimate.print("Initial Estimates:\n", formatter);
-    graph.printErrors(initialEstimate, "errors: ", formatter);
+  graph.printErrors(initialEstimate, "errors: ", formatter);
 
   /* Optimize the graph and print results */
   LevenbergMarquardtParams params;

gtsam/CMakeLists.txt

@@ -147,6 +147,10 @@ if (GTSAM_USE_EIGEN_MKL)
   target_include_directories(gtsam PUBLIC ${MKL_INCLUDE_DIR})
 endif()
 
+if (GTSAM_ENABLE_GPERFTOOLS AND GPERFTOOLS_FOUND)
+  target_link_libraries(gtsam PRIVATE ${GPERFTOOLS_TCMALLOC} ${GPERFTOOLS_PROFILER})
+endif()
+
 # Add includes for source directories 'BEFORE' boost and any system include
 # paths so that the compiler uses GTSAM headers in our source directory instead
 # of any previously installed GTSAM headers.

gtsam/base/Vector.h

@@ -16,6 +16,7 @@
  * @author  Frank Dellaert
  * @author  Alex Hagiopol
  * @author  Varun Agrawal
+ * @author  Fan Jiang
  */
 
 // \callgraph
@@ -193,14 +194,12 @@ GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
  */
 template<class V1, class V2>
 inline double dot(const V1 &a, const V2& b) {
-  assert (b.size()==a.size());
   return a.dot(b);
 }
 
 /** compatibility version for ublas' inner_prod() */
 template<class V1, class V2>
 inline double inner_prod(const V1 &a, const V2& b) {
-  assert (b.size()==a.size());
   return a.dot(b);
 }