Merge branch 'borglab:develop' into develop

Author: yambati03

doc/examples.md

@@ -0,0 +1,3 @@
+# Examples
+
+This section contains python examples in interactive Python notebooks (`*.ipynb`). Python notebooks with an <img src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/> button near the top can be opened in your browser, where you can run the files yourself and make edits to play with and understand GTSAM.

doc/expressions.md

@@ -0,0 +1,113 @@
+# Expressions
+## Motivation
+GTSAM is an optimization library for objective functions expressed as a factor graph over a set of unknown variables. In the continuous case, the variables are typically vectors or elements on a manifold (such as the 3D rotation manifold). The factors compute vector-valued errors that need to be minimized, and are typically only connected to a handful of unknowns.
+
+In the continuous case, the main optimization methods we have implemented are variants of Gauss-Newton non-linear optimization or conjugate gradient methods. Let us assume there are m factors over n unknowns. For either optimization method, we need to evaluate the sparse Jacobian matrix of the entire factor graph, which is a sparse block-matrix of m block-rows and n-block columns.
+
+The sparse Jacobian is built up factor by factor, corresponding to the block-rows. A typical non-linear least-square term is $|h(x)-z|^2$ where $h(x)$ is a measurement function, which we need to be able to linearize as
+$$
+h(x) \approx h(x_0+dx)+H(x_0)dx
+$$
+Note the above is for vector unknowns, for Lie groups and manifold variables, see [doc/math.pdf](https://github.com/borglab/gtsam/blob/develop/doc/math.pdf) for details.
+
+## Expressions
+In many cases one can use GTSAM 4 Expressions to implement factors. Expressions are objects of type Expression<T>, and there are three main expression flavors:
+
+- constants, e.g., `Expression<Point2> kExpr(Point2(3,4))`
+- unknowns, e.g., `Expression<Point3> pExpr(123)` where 123 is a key.
+- functions, e.g., `Expression<double> sumExpr(h, kexpr, pExpr)`
+
+The latter case is an example of wrapping a binary measurement function `h`. To be able to wrap `h`, it needs to be able to compute its local derivatives, i.e., it has to have the signature
+```c++
+double h(const Point2& a, const Point3& b, 
+         OptionalJacobian<1, 2> Ha, OptionalJacobian<1, 3> Hb)
+```
+In this case the output type 'T' is 'double', the two arguments have type Point2 and Point3 respectively, and the two remaining arguments provide a way to compute the function Jacobians, if needed. The templated type `OptionalJacobian<M,N>` behaves very much like `std::optional<Eigen::Matrix<double,M,N>`. If an actual matrix is passed in, the function is expected to treat it as an output argument in which to write the Jacobian for the result wrp. the corresponding input argument. *The matrix to write in will be allocated before the call.*
+
+Expression constructors exist for both methods and functions with different arities. Note that an expression is templated with the output type T, not with the argument types. However, the constructor will infer the argument types from inspecting the signature of the function f, and will in this example expect two additional arguments of type Expression<Point2> and Expression<Point3>, respectively.
+
+As an example, here is the constructor declaration for wrapping unary functions:
+```c++
+template<typename A>
+Expression(typename UnaryFunction<A>::type function,
+    const Expression<A>& expression);
+```
+where (in this case) the function type is defined by
+```c++
+template<class A1>
+struct UnaryFunction {
+typedef boost::function<
+    T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
+};
+```
+## Some measurement function examples
+An example of a simple unary function is `gtsam::norm3` in [Point3.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Point3.cpp#L41):
+```c++
+double norm3(const Point3 & p, OptionalJacobian<1, 3> H = {}) {
+  double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
+  if (H) *H << p.x() / r, p.y() / r, p.z() / r;
+  return r;
+}
+```
+The key new concept here is OptionalJacobian, which acts like a std::optional: if it evaluates to true, you should write the Jacobian of the function in it. It acts as a fixed-size Eigen matrix.
+
+As we said above, expressions also support binary functions, ternary functions, and methods. An example of a binary function is 'Point3::cross':
+
+```c++
+Point3 cross(const Point3 &p, const Point3 & q,
+    OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) {
+  if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
+  if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
+  return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),  p.x() * q.y() - p.y() * q.x());
+}
+```
+Example of using cross:
+```c++
+using namespace gtsam;
+Matrix3 H1, H2;
+Point3 p(1,2,3), q(4,5,6), r = cross(p,q,H1,H2);
+```
+## Using Expressions for Inference
+The way expressions are used is by creating unknown Expressions for the unknown variables we are optimizing for:
+```c++
+Expression<Point3> x(‘x’,1);
+auto h = Expression<Point3>(& norm3, x);
+```
+For convenient creation of factors with expressions, we provide a new factor graph type `ExpressionFactorGraph`, which is just a `NonlinearFactorGraph` with an extra method addExpressionFactor(h, z, n) that takes a measurement expression h, an actual measurement z, and a measurement noise model R. With this, we can add a GTSAM nonlinear factor $|h(x)-z|^2$ to a `NonlinearFactorGraph` by
+```c++
+graph.addExpressionFactor(h, z, R)
+```
+In the above, the unknown in the example can be retrieved by the `gtsam::Symbol(‘x’,1)`, which evaluates to a uint64 identifier.
+
+## Composing Expressions
+The key coolness behind expressions, however, is that you can compose them into expression trees, as long as the leaves know how to do their own derivatives:
+```c++
+Expression<Point3> x1(‘x’1), x2(‘x’,2);
+auto h = Expression<Point3>(& cross, x1, x2);
+auto g = Expression<Point3>(& norm3, h);
+```
+Because we typedef Point3_ to Expression<Point3>, we can write this very concisely as
+```c++
+auto g = Point3_(& norm3, Point3_(& cross, x1(‘x’1), x2(‘x’,2)));
+```
+## PoseSLAM Example
+Using expressions, it is simple to quickly create a factor graph corresponding to a PoseSLAM problem, where our only measurements are relative poses between a series of unknown 2D or 3D poses. The following code snippet from [Pose2SLAMExampleExpressions.cpp](https://github.com/borglab/gtsam/blob/develop/examples/Pose2SLAMExampleExpressions.cpp) is used to create a simple Pose2 example (where the robot is moving on a plane):
+```c++
+1  ExpressionFactorGraph graph;
+2  Expression<Pose2> x1(1), x2(2), x3(3), x4(4), x5(5);
+3  graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
+4  graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0     ), model);
+5  graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
+6  graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
+7  graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
+8  graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
+```
+This is what is going on:
+- In line 1, we create an empty factor graph.
+- In line 2 we create the 5 unknown poses, of type `Expression<Pose2>`, with keys 1 to 5. These are what we will optimize over.
+- Line 3 then creates a simple factor that gives a prior on `x1` (the first argument), namely that it is at the origin `Pose2(0, 0, 0)` (the second argument), with a particular probability density given by `priorNoise` (the third argument).
+- Lines 4-7 adds factors for the odometry constraints, i.e., the movement between successive poses of the robot. The function `between(t1,t2)` is implemented in [nonlinear/expressions.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/expressions.h) and is equivalent to calling the constructor Expression<T>(traits<T>::Between, t1, t2).
+- Finally, line 8 creates a loop closure constraint between poses x2 and x5.
+
+Another good example of its use is in 
+[SFMExampleExpressions.cpp](https://github.com/borglab/gtsam/blob/develop/examples/SFMExampleExpressions.cpp).

doc/generating/gpt_generate.py

@@ -0,0 +1,138 @@
+"""
+GTSAM Copyright 2010-2025, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Author: Porter Zach
+
+This script generates interactive Python notebooks (.ipynb) that document GTSAM 
+header files. Since inserting the text of the file directly into the prompt
+might be too many tokens, it retrieves the header file content from the GTSAM 
+GitHub repository. It then sends it to OpenAI's API for processing, and saves 
+the generated documentation as a Jupyter notebook.
+
+Functions:
+    is_url_valid(url: str) -> bool:
+        Verifies that the supplied URL does not return a 404.
+
+    save_ipynb(text: str, file_path: str) -> str:
+        Saves the provided text to a single Markdown cell in a new .ipynb file.
+
+    generate_ipynb(file_path: str, openai_client):
+        Generates an interactive Python notebook for the given GTSAM header file 
+        by sending a request to OpenAI's API and saving the response.
+
+Usage:
+    Run the script with paths to GTSAM header files as arguments. For example:
+        python gpt_generate.py gtsam/geometry/Pose3.h
+"""
+
+import os
+import time
+import requests
+import argparse
+import nbformat as nbf
+from openai import OpenAI
+
+_output_folder = "output"
+_gtsam_gh_base = "https://raw.githubusercontent.com/borglab/gtsam/refs/heads/develop/"
+_asst_id = "asst_na7wYBtXyGU0x5t2RdcnpxzP"
+_request_text = "Document the file found at {}."
+
+
+def is_url_valid(url):
+    """Verify that the supplied URL does not return a 404."""
+    try:
+        response = requests.head(url, allow_redirects=True)
+        return response.status_code != 404
+    except requests.RequestException:
+        return False
+
+
+def save_ipynb(text: str, file_path: str):
+    """Save text to a single Markdown cell in a new .ipynb file."""
+    script_dir = os.path.dirname(os.path.abspath(__file__))
+    output_dir = os.path.join(script_dir, _output_folder)
+    os.makedirs(output_dir, exist_ok=True)
+    output_file = os.path.splitext(os.path.basename(file_path))[0] + ".ipynb"
+    output_full_path = os.path.join(output_dir, output_file)
+
+    nb = nbf.v4.new_notebook()
+    new_cell = nbf.v4.new_markdown_cell(text)
+    nb['cells'].append(new_cell)
+
+    with open(output_full_path, 'w', encoding='utf-8') as file:
+        nbf.write(nb, file)
+
+    return output_file
+
+
+def generate_ipynb(file_path: str, openai_client):
+    """Generate an interactive Python notebook for the given GTSAM header file.
+
+    Args:
+        file_path (str): The fully-qualified path from the root of the gtsam 
+            repository to the header file that will be documented.
+        openai_client (openai.OpenAI): The OpenAI client to use.
+    """
+    # Create the URL to get the header file from.
+    url = _gtsam_gh_base + file_path
+
+    if not is_url_valid(url):
+        print(f"{url} was not found on the server, or an error occurred.")
+        return
+
+    print(f"Sending request to OpenAI to document {url}.")
+
+    # Create a new thread and send the request
+    thread = openai_client.beta.threads.create()
+    openai_client.beta.threads.messages.create(
+        thread_id=thread.id, role="user", content=_request_text.format(url))
+
+    run = openai_client.beta.threads.runs.create(thread_id=thread.id,
+                                                 assistant_id=_asst_id)
+
+    print("Waiting for the assistant to process the request...")
+
+    # Wait for request to be processed
+    while True:
+        run_status = openai_client.beta.threads.runs.retrieve(
+            thread_id=thread.id, run_id=run.id)
+        if run_status.status == "completed":
+            break
+        time.sleep(2)
+
+    print("Request processed. Retrieving response...")
+
+    # Fetch messages
+    messages = openai_client.beta.threads.messages.list(thread_id=thread.id)
+    # Retrieve response text and strip ```markdown ... ```
+    text = messages.data[0].content[0].text.value.strip('`').strip('markdown')
+
+    # Write output to file
+    output_filename = save_ipynb(text, file_path)
+
+    print(f"Response retrieved. Find output in {output_filename}.")
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        prog="gpt_generate",
+        description=
+        "Generates .ipynb documentation files given paths to GTSAM header files."
+    )
+    parser.add_argument(
+        "file_paths",
+        nargs='+',
+        help=
+        "The paths to the header files from the root gtsam directory, e.g. 'gtsam/geometry/Pose3.h'."
+    )
+    args = parser.parse_args()
+
+    # Retrieves API key from environment variable OPENAI_API_KEY
+    client = OpenAI()
+
+    for file_path in args.file_paths:
+        generate_ipynb(file_path, client)

gtsam/user_guide.md doc/user_guide.mdgtsam/user_guide.md renamed to doc/user_guide.md

@@ -22,9 +22,9 @@
  *  - We have measurements between each pose from multiple odometry sensors
  */
 
-// This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable
+// This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM
 #include <gtsam/nonlinear/BatchFixedLagSmoother.h>
-#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
 
 // In GTSAM, measurement functions are represented as 'factors'. Several common factors
 // have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.

…ble/examples/FixedLagSmootherExample.cpp examples/FixedLagSmootherExample.cppgtsam_unstable/examples/FixedLagSmootherExample.cpp renamed to examples/FixedLagSmootherExample.cpp gtsam_unstable/examples/FixedLagSmootherExample.cpp renamed to examples/FixedLagSmootherExample.cpp

@@ -109,11 +109,10 @@ class Experiment {
     std::cout << "Smoother update: " << newFactors_.size() << std::endl;
     gttic_(SmootherUpdate);
     clock_t beforeUpdate = clock();
-    auto linearized = newFactors_.linearize(initial_);
-    smoother_.update(*linearized, initial_);
+    smoother_.update(newFactors_, initial_, maxNrHypotheses);
+    clock_t afterUpdate = clock();
     allFactors_.push_back(newFactors_);
     newFactors_.resize(0);
-    clock_t afterUpdate = clock();
     return afterUpdate - beforeUpdate;
   }
 
@@ -262,10 +261,20 @@ class Experiment {
     std::string timeFileName = "Hybrid_City10000_time.txt";
     outfileTime.open(timeFileName);
     for (auto accTime : timeList) {
-      outfileTime << accTime << std::endl;
+      outfileTime << accTime / CLOCKS_PER_SEC << std::endl;
     }
     outfileTime.close();
     std::cout << "Output " << timeFileName << " file." << std::endl;
+
+    std::ofstream timingFile;
+    std::string timingFileName = "Hybrid_City10000_timing.txt";
+    timingFile.open(timingFileName);
+    for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
+      auto p = smootherUpdateTimes.at(i);
+      timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
+    }
+    timingFile.close();
+    std::cout << "Wrote timing information to " << timingFileName << std::endl;
   }
 };
 

examples/Hybrid_City10000.cpp

@@ -74,6 +74,8 @@ class Experiment {
     // Initialize local variables
     size_t index = 0;
 
+    std::vector<std::pair<size_t, double>> smootherUpdateTimes;
+
     std::list<double> timeList;
 
     // Set up initial prior
@@ -82,10 +84,15 @@ class Experiment {
     graph_.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
 
     // Initial update
+    clock_t beforeUpdate = clock();
     isam2_.update(graph_, initial_);
+    results = isam2_.calculateBestEstimate();
+    clock_t afterUpdate = clock();
+    smootherUpdateTimes.push_back(
+        std::make_pair(index, afterUpdate - beforeUpdate));
     graph_.resize(0);
     initial_.clear();
-    results = isam2_.calculateBestEstimate();
+    index += 1;
 
     // Start main loop
     size_t keyS = 0;
@@ -127,10 +134,15 @@ class Experiment {
         index++;
       }
 
+      clock_t beforeUpdate = clock();
       isam2_.update(graph_, initial_);
+      results = isam2_.calculateBestEstimate();
+      clock_t afterUpdate = clock();
+      smootherUpdateTimes.push_back(
+          std::make_pair(index, afterUpdate - beforeUpdate));
       graph_.resize(0);
       initial_.clear();
-      results = isam2_.calculateBestEstimate();
+      index += 1;
 
       // Print loop index and time taken in processor clock ticks
       if (index % 50 == 0 && keyS != keyT - 1) {
@@ -175,6 +187,16 @@ class Experiment {
     outfileTime.close();
     std::cout << "Written cumulative time to: " << timeFileName << " file."
               << std::endl;
+
+    std::ofstream timingFile;
+    std::string timingFileName = "ISAM2_City10000_timing.txt";
+    timingFile.open(timingFileName);
+    for (size_t i = 0; i < smootherUpdateTimes.size(); i++) {
+      auto p = smootherUpdateTimes.at(i);
+      timingFile << p.first << ", " << p.second / CLOCKS_PER_SEC << std::endl;
+    }
+    timingFile.close();
+    std::cout << "Wrote timing information to " << timingFileName << std::endl;
   }
 };
 

examples/ISAM2_City10000.cpp

@@ -59,7 +59,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
+#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>  // for writeG2o
 

…s/IncrementalFixedLagSmootherExample.cpp …s/IncrementalFixedLagSmootherExample.cppgtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp renamed to examples/IncrementalFixedLagSmootherExample.cpp gtsam_unstable/examples/IncrementalFixedLagSmootherExample.cpp renamed to examples/IncrementalFixedLagSmootherExample.cpp

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 3.5)
 project(METIS)
 
 # Add flags for currect directory and below

gtsam/3rdparty/metis/CMakeLists.txt

@@ -547,7 +547,9 @@ namespace gtsam {
 /* ************************************************************************ */
 DiscreteFactor::shared_ptr DecisionTreeFactor::restrict(
     const DiscreteValues& assignment) const {
-  throw std::runtime_error("DecisionTreeFactor::restrict not implemented");
+  ADT restricted_tree = ADT::restrict(assignment);
+  return std::make_shared<DecisionTreeFactor>(this->discreteKeys(),
+                                              restricted_tree);
 }
 
   /* ************************************************************************ */