Merge branch 'develop' into chebyshev-improvements

.github/workflows/build-linux.yml

@@ -31,7 +31,7 @@ jobs:
           ubuntu-22.04-clang-14,
         ]
 
-        build_type: [Debug, Release]
+        build_type: [Release]
         build_unstable: [ON]
         include:
           - name: ubuntu-20.04-gcc-9

CMakeLists.txt

@@ -36,13 +36,6 @@ set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake"
 include(GtsamMakeConfigFile)
 include(GNUInstallDirs)
 
-# Load build type flags and default to Debug mode
-include(GtsamBuildTypes)
-
-# Use macros for creating tests/timing scripts
-include(GtsamTesting)
-include(GtsamPrinting)
-
 # guard against in-source builds
 if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR})
   message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
@@ -50,6 +43,13 @@ endif()
 
 include(cmake/HandleGeneralOptions.cmake)   # CMake build options
 
+# Load build type flags and default to Debug mode
+include(GtsamBuildTypes)
+
+# Use macros for creating tests/timing scripts
+include(GtsamTesting)
+include(GtsamPrinting)
+
 ############### Decide on BOOST ######################################
 # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION
 option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON)

cmake/GtsamBuildTypes.cmake

@@ -55,9 +55,6 @@ if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
       "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
 endif()
 
-# Add option for using build type postfixes to allow installing multiple build modes
-option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" ON)
-
 # Define all cache variables, to be populated below depending on the OS/compiler:
 set(GTSAM_COMPILE_OPTIONS_PRIVATE        "" CACHE INTERNAL "(Do not edit) Private compiler flags for all build configurations." FORCE)
 set(GTSAM_COMPILE_OPTIONS_PUBLIC         "" CACHE INTERNAL "(Do not edit) Public compiler flags (exported to user projects) for all build configurations."  FORCE)
@@ -82,6 +79,13 @@ set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO  "NDEBUG" CACHE STRING "(Us
 set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE         "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.")
 set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING       "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.")
 set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING          "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.")
+
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING)
+mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING)
+
 if(MSVC)
   # Common to all configurations:
   list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE
@@ -143,6 +147,13 @@ else()
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          -g -O3  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
 endif()
 
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING)
+mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING)
+
 # Enable C++17:
 if (NOT CMAKE_VERSION VERSION_LESS 3.8)
     set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.")
@@ -198,7 +209,6 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 endif()
 
 if (NOT MSVC)
-  option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
   if(GTSAM_BUILD_WITH_MARCH_NATIVE)
     # Check if Apple OS and compiler is [Apple]Clang
     if(APPLE AND (${CMAKE_CXX_COMPILER_ID} MATCHES "^(Apple)?Clang$"))

cmake/GtsamTesting.cmake

@@ -42,7 +42,7 @@ endmacro()
 # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
 #
 # Usage example:
-#   gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+#   gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
 #
 # Arguments:
 #   globPatterns:  The list of files or glob patterns from which to create examples, with
@@ -51,8 +51,9 @@ endmacro()
 #   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
 #                  an empty string "" if nothing needs to be excluded.
 #   linkLibraries: The list of libraries to link to.
-macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
+#   buildWithAll: Build examples with `make` and/or `make all`
-	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
+macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries buildWithAll)
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${buildWithAll})
 endmacro()
 
 
@@ -76,8 +77,9 @@ endmacro()
 #   excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
 #                  an empty string "" if nothing needs to be excluded.
 #   linkLibraries: The list of libraries to link to.
-macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries)
+#   buildWithAll: Build examples with `make` and/or `make all`
-	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS})
+macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries buildWithAll)
+	gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${buildWithAll})
 endmacro()
 
 
@@ -86,9 +88,8 @@ endmacro()
 # Build macros for using tests
 enable_testing()
 
+#TODO(Varun) Move to HandlePrintConfiguration.cmake. This will require additional changes.
 option(GTSAM_BUILD_TESTS                 "Enable/Disable building of tests"          ON)	
-option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
-option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
 
 # Add option for combining unit tests
 if(MSVC OR XCODE_VERSION)
@@ -123,6 +124,7 @@ add_custom_target(timing)
 # Implementations of this file's macros:
 
 macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
+	#TODO(Varun) Building of tests should not depend on global gtsam flag
 	if(GTSAM_BUILD_TESTS)
 		# Add group target if it doesn't already exist
 		if(NOT TARGET check.${groupName})

cmake/HandleGeneralOptions.cmake

@@ -8,6 +8,18 @@ else()
     set(GTSAM_UNSTABLE_AVAILABLE 0)
 endif()
 
+### GtsamTesting related options
+option(GTSAM_BUILD_EXAMPLES_ALWAYS       "Build examples with 'make all' (build with 'make examples' if not)"       ON)
+option(GTSAM_BUILD_TIMING_ALWAYS         "Build timing scripts with 'make all' (build with 'make timing' if not"    OFF)
+###
+
+# Add option for using build type postfixes to allow installing multiple build modes
+option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" ON)
+
+if (NOT MSVC)
+    option(GTSAM_BUILD_WITH_MARCH_NATIVE  "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF)
+endif()
+
 # Configurable Options
 if(GTSAM_UNSTABLE_AVAILABLE)
     option(GTSAM_BUILD_UNSTABLE              "Enable/Disable libgtsam_unstable"          ON)

cmake/README.html

@@ -47,9 +47,9 @@ linkLibraries: The list of libraries to link to in addition to CppUnitLite.
 </code></pre>
 </li>
 <li>
-<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)</code> Add scripts that will serve as examples of how to use the library.  A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file.  These executables will not be installed.  They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.</p>
+<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll)</code> Add scripts that will serve as examples of how to use the library.  A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file.  These executables will not be installed.  They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.</p>
 <p>Usage example:</p>
-<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
 </code></pre>
 <p>Arguments:</p>
 <pre><code>globPatterns:  The list of files or glob patterns from which to create unit tests, with
@@ -58,6 +58,7 @@ linkLibraries: The list of libraries to link to in addition to CppUnitLite.
 excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
                an empty string "" if nothing needs to be excluded.
 linkLibraries: The list of libraries to link to.
+buildWithAll: Build examples with `make` and/or `make all`
 </code></pre>
 </li>
 </ul>

cmake/README.md

@@ -52,11 +52,11 @@ Defines two useful functions for creating CTest unit tests.  Also immediately cr
                        Pass an empty string "" if nothing needs to be excluded.
         linkLibraries: The list of libraries to link to in addition to CppUnitLite.
         
-*   `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library.  A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file.  These executables will not be installed.  They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
+*   `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll)` Add scripts that will serve as examples of how to use the library.  A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file.  These executables will not be installed.  They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled.  They may also be built with 'make examples'.
 
     Usage example:
 
-        gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+        gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
 
     Arguments:
 
@@ -66,6 +66,7 @@ Defines two useful functions for creating CTest unit tests.  Also immediately cr
         excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
                        an empty string "" if nothing needs to be excluded.
         linkLibraries: The list of libraries to link to.
+        buildWithAll: Build examples with `make` and/or `make all`
 
 ## GtsamMakeConfigFile
 

examples/CMakeLists.txt

@@ -20,4 +20,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
       )
 endif()
 
-gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS})

gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp

@@ -42,8 +42,8 @@
 #include <functional>
 #include <iostream>
 #include <iterator>
-#include <vector>
 #include <numeric>
+#include <vector>
 
 #include "Switching.h"
 #include "TinyHybridExample.h"
@@ -613,11 +613,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) {
   // Create expected decision tree with two factor graphs:
 
   // Get mixture factor:
-  auto mixture = std::dynamic_pointer_cast<GaussianMixtureFactor>(fg.at(0));
+  auto mixture = fg.at<GaussianMixtureFactor>(0);
   CHECK(mixture);
 
   // Get prior factor:
-  const auto gf = std::dynamic_pointer_cast<HybridConditional>(fg.at(1));
+  const auto gf = fg.at<HybridConditional>(1);
   CHECK(gf);
   using GF = GaussianFactor::shared_ptr;
   const GF prior = gf->asGaussian();

gtsam/inference/FactorGraph.h

@@ -310,6 +310,21 @@ class FactorGraph {
    */
   sharedFactor& at(size_t i) { return factors_.at(i); }
 
+  /** Get a specific factor by index and typecast to factor type F
+   * (this checks array bounds and may throw
+   * an exception, as opposed to operator[] which does not).
+   */
+  template <typename F>
+  std::shared_ptr<F> at(size_t i) {
+    return std::dynamic_pointer_cast<F>(factors_.at(i));
+  }
+
+  /// Const version of templated `at` method.
+  template <typename F>
+  const std::shared_ptr<F> at(size_t i) const {
+    return std::dynamic_pointer_cast<F>(factors_.at(i));
+  }
+
   /** Get a specific factor by index (this does not check array bounds, as
    * opposed to at() which does).
    */

gtsam/inference/Key.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Key.h
+ * @file Key.cpp
  * @brief
  * @author Richard Roberts
  * @author Alex Cunningham
@@ -26,6 +26,9 @@ using namespace std;
 
 namespace gtsam {
 
+/// Assign default key formatter
+KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
+
 /* ************************************************************************* */
 string _defaultKeyFormatter(Key key) {
   const Symbol asSymbol(key);

gtsam/inference/Key.h

@@ -37,10 +37,16 @@ using KeyFormatter = std::function<std::string(Key)>;
 // Helper function for DefaultKeyFormatter
 GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
 
-/// The default KeyFormatter, which is used if no KeyFormatter is passed to
+/**
-/// a nonlinear 'print' function.  Automatically detects plain integer keys
+ * The default KeyFormatter, which is used if no KeyFormatter is passed
-/// and Symbol keys.
+ * to a 'print' function.
-static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
+ *
+ * Automatically detects plain integer keys and Symbol keys.
+ * 
+ * Marked as `extern` so that it can be updated by external libraries.
+ *
+ */
+extern GTSAM_EXPORT KeyFormatter DefaultKeyFormatter;
 
 // Helper function for Multi-robot Key Formatter
 GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
@@ -124,7 +130,3 @@ struct traits<Key> {
 };
 
 } // namespace gtsam
-
-
-
-

gtsam/linear/JacobianFactor.h

@@ -312,6 +312,12 @@ namespace gtsam {
     /** Get a view of the A matrix */
     ABlock getA() { return Ab_.range(0, size()); }
 
+    /**
+     * Get a view of the A matrix for the variable
+     * pointed to by the given key.
+     */
+    ABlock getA(const Key& key) { return Ab_(find(key) - begin()); }
+
     /** Update an information matrix by adding the information corresponding to this factor
      * (used internally during elimination).
      * @param scatter A mapping from variable index to slot index in this HessianFactor

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -391,8 +391,7 @@ TEST(GaussianFactorGraph, clone) {
   EXPECT(assert_equal(init_graph, actCloned));  // Same as the original version
 
   // Apply an in-place change to init_graph and compare
-  JacobianFactor::shared_ptr jacFactor0 =
+  JacobianFactor::shared_ptr jacFactor0 = init_graph.at<JacobianFactor>(0);
-      std::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
   CHECK(jacFactor0);
   jacFactor0->getA(jacFactor0->begin()) *= 7.;
   EXPECT(assert_inequal(init_graph, exp_graph));

gtsam/linear/tests/testJacobianFactor.cpp

@@ -65,7 +65,10 @@ TEST(JacobianFactor, constructors_and_accessors)
     JacobianFactor actual(terms[0].first, terms[0].second, b, noise);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
+    // Key iterator
     EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
+    // Key
+    EXPECT(assert_equal(terms[0].second, actual.getA(terms[0].first)));
     EXPECT(assert_equal(b, expected.getb()));
     EXPECT(assert_equal(b, actual.getb()));
     EXPECT(noise == expected.get_model());
@@ -78,7 +81,10 @@ TEST(JacobianFactor, constructors_and_accessors)
       terms[1].first, terms[1].second, b, noise);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
+    // Key iterator
     EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
+    // Key
+    EXPECT(assert_equal(terms[1].second, actual.getA(terms[1].first)));
     EXPECT(assert_equal(b, expected.getb()));
     EXPECT(assert_equal(b, actual.getb()));
     EXPECT(noise == expected.get_model());
@@ -91,7 +97,10 @@ TEST(JacobianFactor, constructors_and_accessors)
       terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
     EXPECT(assert_equal(expected, actual));
     LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
+    // Key iterator
     EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
+    // Key
+    EXPECT(assert_equal(terms[2].second, actual.getA(terms[2].first)));
     EXPECT(assert_equal(b, expected.getb()));
     EXPECT(assert_equal(b, actual.getb()));
     EXPECT(noise == expected.get_model());

gtsam/nonlinear/GncOptimizer.h

@@ -65,10 +65,9 @@ class GncOptimizer {
     nfg_.resize(graph.size());
     for (size_t i = 0; i < graph.size(); i++) {
       if (graph[i]) {
-        NoiseModelFactor::shared_ptr factor = std::dynamic_pointer_cast<
+        NoiseModelFactor::shared_ptr factor = graph.at<NoiseModelFactor>(i);
-            NoiseModelFactor>(graph[i]);
+        auto robust =
-        auto robust = std::dynamic_pointer_cast<
+            std::dynamic_pointer_cast<noiseModel::Robust>(factor->noiseModel());
-            noiseModel::Robust>(factor->noiseModel());
         // if the factor has a robust loss, we remove the robust loss
         nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
       }
@@ -401,10 +400,8 @@ class GncOptimizer {
     newGraph.resize(nfg_.size());
     for (size_t i = 0; i < nfg_.size(); i++) {
       if (nfg_[i]) {
-        auto factor = std::dynamic_pointer_cast<
+        auto factor = nfg_.at<NoiseModelFactor>(i);
-            NoiseModelFactor>(nfg_[i]);
+        auto noiseModel = std::dynamic_pointer_cast<noiseModel::Gaussian>(
-        auto noiseModel =
-            std::dynamic_pointer_cast<noiseModel::Gaussian>(
             factor->noiseModel());
         if (noiseModel) {
           Matrix newInfo = weights[i] * noiseModel->information();

gtsam/nonlinear/values.i

@@ -59,10 +59,12 @@ class Values {
   // enabling serialization functionality
   void serialize() const;
 
-  // New in 4.0, we have to specialize every insert/update/at to generate
+  // New in 4.0, we have to specialize every insert/update/at
-  // wrappers Instead of the old: void insert(size_t j, const gtsam::Value&
+  // to generate wrappers.
-  // value); void update(size_t j, const gtsam::Value& val); gtsam::Value
+  // Instead of the old:
-  // at(size_t j) const;
+  // void insert(size_t j, const gtsam::Value& value);
+  // void update(size_t j, const gtsam::Value& val);
+  // gtsam::Value at(size_t j) const;
 
   // The order is important: Vector has to precede Point2/Point3 so `atVector`
   // can work for those fixed-size vectors.
@@ -99,6 +101,10 @@ class Values {
   template <T = {gtsam::Point2, gtsam::Point3}>
   void insert(size_t j, const T& val);
 
+  // The order is important: Vector has to precede Point2/Point3 so `atVector`
+  // can work for those fixed-size vectors.
+  void update(size_t j, Vector vector);
+  void update(size_t j, Matrix matrix);
   void update(size_t j, const gtsam::Point2& point2);
   void update(size_t j, const gtsam::Point3& point3);
   void update(size_t j, const gtsam::Rot2& rot2);
@@ -125,10 +131,12 @@ class Values {
   void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
-  void update(size_t j, Vector vector);
-  void update(size_t j, Matrix matrix);
   void update(size_t j, double c);
 
+  // The order is important: Vector has to precede Point2/Point3 so `atVector`
+  // can work for those fixed-size vectors.
+  void insert_or_assign(size_t j, Vector vector);
+  void insert_or_assign(size_t j, Matrix matrix);
   void insert_or_assign(size_t j, const gtsam::Point2& point2);
   void insert_or_assign(size_t j, const gtsam::Point3& point3);
   void insert_or_assign(size_t j, const gtsam::Rot2& rot2);
@@ -165,8 +173,6 @@ class Values {
   void insert_or_assign(size_t j,
                         const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
-  void insert_or_assign(size_t j, Vector vector);
-  void insert_or_assign(size_t j, Matrix matrix);
   void insert_or_assign(size_t j, double c);
 
   template <T = {gtsam::Point2,

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -373,8 +373,10 @@ TEST(ShonanAveraging2, noisyToyGraphWithHuber) {
   // test that each factor is actually robust
   for (size_t i=0; i<=4; i++) { // note: last is the Gauge factor and is not robust
     const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(
-			  std::dynamic_pointer_cast<NoiseModelFactor>(graph[i])->noiseModel());
+        graph.at<NoiseModelFactor>(i)->noiseModel());
-	  EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber)
+    // we expect the factors to be use a robust noise model
+    // (in particular, Huber)
+    EXPECT(robust);
   }
 
   // test result

gtsam/slam/expressions.h

@@ -155,10 +155,10 @@ Point2_ project2(const Expression<CAMERA>& camera_, const Expression<POINT>& p_)
 namespace internal {
 // Helper template for project3 expression below
 template <class CALIBRATION, class POINT>
-inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
+inline Point2 project6(const Pose3& x, const POINT& p, const CALIBRATION& K,
                        OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
-                       OptionalJacobian<2, 5> Dcal) {
+                       OptionalJacobian<2, CALIBRATION::dimension> Dcal) {
-  return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
+  return PinholeCamera<CALIBRATION>(x, K).project(p, Dpose, Dpoint, Dcal);
 }
 }
 

gtsam/slam/slam.i

@@ -156,6 +156,9 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 
   // enabling serialization functionality
   void serialize() const;
+
+  gtsam::TriangulationResult point() const;
+  gtsam::TriangulationResult point(const gtsam::Values& values) const;
 };
 
 typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>

gtsam/slam/tests/testDataset.cpp

@@ -97,8 +97,7 @@ TEST(dataSet, load2D) {
   auto model = noiseModel::Unit::Create(3);
   BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
                                 model);
-  BetweenFactor<Pose2>::shared_ptr actual =
+  BetweenFactor<Pose2>::shared_ptr actual = graph->at<BetweenFactor<Pose2>>(0);
-      std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
   EXPECT(assert_equal(expected, *actual));
 
   // Check binary measurements, Pose2
@@ -113,9 +112,8 @@ TEST(dataSet, load2D) {
   // // Check factor parsing
   const auto actualFactors = parseFactors<Pose2>(filename);
   for (size_t i : {0, 1, 2, 3, 4, 5}) {
-    EXPECT(assert_equal(
+    EXPECT(assert_equal(*graph->at<BetweenFactor<Pose2>>(i), *actualFactors[i],
-        *std::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
+                        1e-5));
-        *actualFactors[i], 1e-5));
   }
 
   // Check pose parsing
@@ -194,8 +192,7 @@ TEST(dataSet, readG2o3D) {
   // Check factor parsing
   const auto actualFactors = parseFactors<Pose3>(g2oFile);
   for (size_t i : {0, 1, 2, 3, 4, 5}) {
-    EXPECT(assert_equal(
+    EXPECT(assert_equal(*expectedGraph.at<BetweenFactor<Pose3>>(i),
-        *std::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
                         *actualFactors[i], 1e-5));
   }
 

gtsam_unstable/discrete/examples/CMakeLists.txt

@@ -12,4 +12,4 @@ endif()
 
 
 
-gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable")
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS})

gtsam_unstable/discrete/tests/testLoopyBelief.cpp

@@ -183,13 +183,10 @@ class LoopyBelief {
         // accumulate unary factors
         if (graph.at(factorIndex)->size() == 1) {
           if (!prodOfUnaries)
-            prodOfUnaries = std::dynamic_pointer_cast<DecisionTreeFactor>(
+            prodOfUnaries = graph.at<DecisionTreeFactor>(factorIndex);
-                graph.at(factorIndex));
           else
             prodOfUnaries = std::make_shared<DecisionTreeFactor>(
-                *prodOfUnaries *
+                *prodOfUnaries * (*graph.at<DecisionTreeFactor>(factorIndex)));
-                (*std::dynamic_pointer_cast<DecisionTreeFactor>(
-                    graph.at(factorIndex))));
         }
       }
 

gtsam_unstable/examples/CMakeLists.txt

@@ -9,4 +9,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
 endif()
 
 
-gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable")
+gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS})

gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp

@@ -87,7 +87,7 @@ TEST(NonlinearClusterTree, Clusters) {
   Ordering ordering;
   ordering.push_back(x1);
   const auto [bn, fg] = gfg->eliminatePartialSequential(ordering);
-  auto expectedFactor = std::dynamic_pointer_cast<HessianFactor>(fg->at(0));
+  auto expectedFactor = fg->at<HessianFactor>(0);
   if (!expectedFactor)
     throw std::runtime_error("Expected HessianFactor");
 

gtsam_unstable/timing/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable")
+gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable" ${GTSAM_BUILD_TIMING_ALWAYS})

matlab/CMakeLists.txt

@@ -64,6 +64,10 @@ set(ignore
     gtsam::Point3
     gtsam::CustomFactor)
 
+if (APPLE OR WIN32)
+  list(APPEND ignore gtsam::Symbol)
+endif()
+
 set(interface_files
     ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
     ${GTSAM_SOURCE_DIR}/gtsam/base/base.i

matlab/README.md

@@ -10,6 +10,14 @@ The interface generated by the wrap tool also redirects the standard output stre
 
 For instructions on updating the version of the [wrap library](https://github.com/borglab/wrap) included in GTSAM to the latest version, please refer to the [wrap README](https://github.com/borglab/wrap/blob/master/README.md#git-subtree-and-contributing)
 
+### Filename Case Sensitivity
+
+Due to the file name syntax requirements of Matlab, having the files `Symbol.m` (for the class) and `symbol.m` (for the function) together on an OS with a case-insensitive filesystem is impossible.
+
+To put it simply, for an OS like Windows or MacOS where the filesystem is case-insensitive, we cannot have two files `symbol.m` and `Symbol.m` in the same folder. When trying to write one file after another, they will end up overwriting each other. We cannot specify 2 different filenames, since Matlab's syntax prevents that.
+
+For this reason, on Windows and MacOS, we ignore the `Symbol` class and request users to use the `symbol` function exclusively for key creation.
+
 ## Ubuntu
 
 If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:

tests/testNonlinearFactor.cpp

@@ -323,7 +323,7 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel )
   // create actual
   NonlinearFactorGraph actual;
   SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2);
-  actual.push_back( std::dynamic_pointer_cast<NoiseModelFactor>(nfg[0])->cloneWithNewNoiseModel(noise2) );
+  actual.push_back(nfg.at<NoiseModelFactor>(0)->cloneWithNewNoiseModel(noise2));
 
   // check it's all good
   CHECK(assert_equal(expected, actual));

timing/CMakeLists.txt

@@ -14,6 +14,6 @@ if (NOT GTSAM_USE_BOOST_FEATURES)
       )
 endif()
 
-gtsamAddTimingGlob("*.cpp" "${excluded_scripts}" "gtsam")
+gtsamAddTimingGlob("*.cpp" "${excluded_scripts}" "gtsam" ${GTSAM_BUILD_TIMING_ALWAYS})
 
 target_link_libraries(timeGaussianFactorGraph CppUnitLite)

wrap/gtwrap/interface_parser/function.py

@@ -12,7 +12,8 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae
 
 from typing import Any, Iterable, List, Union
 
-from pyparsing import Optional, ParseResults, delimitedList  # type: ignore
+from pyparsing import (Literal, Optional, ParseResults,  # type: ignore
+                       delimitedList)
 
 from .template import Template
 from .tokens import (COMMA, DEFAULT_ARG, EQUAL, IDENT, LOPBRACK, LPAREN, PAIR,
@@ -105,8 +106,10 @@ class ReturnType:
 
     The return type can either be a single type or a pair such as <type1, type2>.
     """
+    # rule to parse optional std:: in front of `pair`
+    optional_std = Optional(Literal('std::')).suppress()
     _pair = (
-        PAIR.suppress()  #
+        optional_std + PAIR.suppress()  #
         + LOPBRACK  #
         + Type.rule("type1")  #
         + COMMA  #

wrap/gtwrap/matlab_wrapper/wrapper.py

@@ -1284,7 +1284,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                                       [instantiated_class.name])
             else:
                 # Get the full namespace
-                class_name = ".".join(instantiated_class.parent.full_namespaces()[1:])
+                class_name = ".".join(
+                    instantiated_class.parent.full_namespaces()[1:])
 
             if class_name != "":
                 class_name += '.'
@@ -1860,6 +1861,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         """
         for c in cc_content:
             if isinstance(c, list):
+                # c is a namespace
                 if len(c) == 0:
                     continue
 
@@ -1875,6 +1877,7 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                     self.generate_content(sub_content[1], path_to_folder)
 
             elif isinstance(c[1], list):
+                # c is a wrapped function
                 path_to_folder = osp.join(path, c[0])
 
                 if not osp.isdir(path_to_folder):
@@ -1882,11 +1885,13 @@ class MatlabWrapper(CheckMixin, FormatMixin):
                         os.makedirs(path_to_folder, exist_ok=True)
                     except OSError:
                         pass
+
                 for sub_content in c[1]:
                     path_to_file = osp.join(path_to_folder, sub_content[0])
                     with open(path_to_file, 'w') as f:
                         f.write(sub_content[1])
             else:
+                # c is a wrapped class
                 path_to_file = osp.join(path, c[0])
 
                 if not osp.isdir(path_to_file):
@@ -1921,6 +1926,8 @@ class MatlabWrapper(CheckMixin, FormatMixin):
         for module in modules.values():
             # Wrap the full namespace
             self.wrap_namespace(module)
+
+            # Generate the wrapping code (both C++ and .m files)
             self.generate_wrapper(module)
 
             # Generate the corresponding .m and .cpp files

wrap/tests/expected/matlab/TemplatedFunctionRot3.m

@@ -1,6 +1,6 @@
 function varargout = TemplatedFunctionRot3(varargin)
       if length(varargin) == 1 && isa(varargin{1},'gtsam.Rot3')
-        functions_wrapper(25, varargin{:});
+        functions_wrapper(26, varargin{:});
       else
         error('Arguments do not match any overload of function TemplatedFunctionRot3');
       end

wrap/tests/expected/matlab/functions_wrapper.cpp

@@ -229,7 +229,16 @@ void setPose_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
   checkArguments("setPose",nargout,nargin,0);
   setPose(gtsam::Pose3());
 }
-void TemplatedFunctionRot3_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+void EliminateDiscrete_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
+{
+  checkArguments("EliminateDiscrete",nargout,nargin,2);
+  gtsam::DiscreteFactorGraph& factors = *unwrap_shared_ptr< gtsam::DiscreteFactorGraph >(in[0], "ptr_gtsamDiscreteFactorGraph");
+  gtsam::Ordering& frontalKeys = *unwrap_shared_ptr< gtsam::Ordering >(in[1], "ptr_gtsamOrdering");
+  auto pairResult = EliminateDiscrete(factors,frontalKeys);
+  out[0] = wrap_shared_ptr(pairResult.first,"gtsam.DiscreteConditional", false);
+  out[1] = wrap_shared_ptr(pairResult.second,"gtsam.DecisionTreeFactor", false);
+}
+void TemplatedFunctionRot3_26(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {
   checkArguments("TemplatedFunctionRot3",nargout,nargin,1);
   gtsam::Rot3& t = *unwrap_shared_ptr< gtsam::Rot3 >(in[0], "ptr_gtsamRot3");
@@ -323,7 +332,10 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
       setPose_24(nargout, out, nargin-1, in+1);
       break;
     case 25:
-      TemplatedFunctionRot3_25(nargout, out, nargin-1, in+1);
+      EliminateDiscrete_25(nargout, out, nargin-1, in+1);
+      break;
+    case 26:
+      TemplatedFunctionRot3_26(nargout, out, nargin-1, in+1);
       break;
     }
   } catch(const std::exception& e) {

wrap/tests/expected/python/functions_pybind.cpp

@@ -30,6 +30,7 @@ PYBIND11_MODULE(functions_py, m_) {
     m_.def("DefaultFuncZero",[](int a, int b, double c, int d, bool e){ ::DefaultFuncZero(a, b, c, d, e);}, py::arg("a"), py::arg("b"), py::arg("c") = 0.0, py::arg("d") = 0, py::arg("e") = false);
     m_.def("DefaultFuncVector",[](const std::vector<int>& i, const std::vector<string>& s){ ::DefaultFuncVector(i, s);}, py::arg("i") = {1, 2, 3}, py::arg("s") = {"borglab", "gtsam"});
     m_.def("setPose",[](const gtsam::Pose3& pose){ ::setPose(pose);}, py::arg("pose") = gtsam::Pose3());
+    m_.def("EliminateDiscrete",[](const gtsam::DiscreteFactorGraph& factors, const gtsam::Ordering& frontalKeys){return ::EliminateDiscrete(factors, frontalKeys);}, py::arg("factors"), py::arg("frontalKeys"));
     m_.def("TemplatedFunctionRot3",[](const gtsam::Rot3& t){ ::TemplatedFunction<gtsam::Rot3>(t);}, py::arg("t"));
 
 #include "python/specializations.h"

wrap/tests/fixtures/functions.i

@@ -36,3 +36,7 @@ void DefaultFuncVector(const std::vector<int> &i = {1, 2, 3}, const std::vector<
 
 // Test for non-trivial default constructor
 void setPose(const gtsam::Pose3& pose = gtsam::Pose3());
+
+std::pair<gtsam::DiscreteConditional*, gtsam::DecisionTreeFactor*>
+EliminateDiscrete(const gtsam::DiscreteFactorGraph& factors,
+                  const gtsam::Ordering& frontalKeys);