Stereo Visual Odometry Example (Large Dataset)

In this notebook we build on StereoVOExample.ipynb to tackle a more realistic stereo visual-odometry problem. A robot carries a stereo camera, moves forward over a longer trajectory, and observes a large sample of landmarks. Our goals are:

1. Define a noise model and stereo calibration.
2. Assemble a factor graph of stereo-measurement factors.
3. Provide initial estimates for both camera poses and landmark positions.
4. Run Levenberg–Marquardt optimization to recover the most likely trajectory and map.
5. Analyze and visualize final poses and landmarks.

If any section may seem unclear, we recommend looking at StereoVOExample.ipynb first and coming back to this example afterwards.

Note: This example is also available in C++. If you’re interested in the C++ implementation, refer to StereoVOExample_large.cpp in the GTSAM examples directory.

1. Setup Factor Graph¶

We start by creating an empty NonlinearFactorGraph.

2. Define Camera Calibration and Measurement¶

To accurately model the stereo observations, we must define both the measurement noise model and the intrinsic calibration of the stereo camera system.

• Measurement Noise: We assume an isotropic Gaussian noise model for the stereo measurements. Since each observation consists of three pixel values $(u_L, u_R, v)$, we use Isotropic.Sigma(3, 1.0) to define a noise model with unit standard deviation in each of the three dimensions.
• Camera Calibration (Cal3_S2Stereo): We load precomputed intrinsic parameters and baseline values to construct a stereo calibration model. These values are as follows:
  • Focal lengths: $f_x=f_y=721.5377$
  • Skew: $s=0$
  • Principal point: $(u,v)=(609.5593,\,172.854)$
  • Baseline: $b=0.537150588$

These parameters define how 3D world points are projected into pixel coordinates in the left and right stereo images, and are essential for constructing GeneraicStereoFactor3D measurements in Section 5.

3. Construct Initial Pose Estimates from Odometry Measurements¶

The next step in our visual odometry pipeline is to provide an initial guess for the robot’s trajectory. GTSAM relies on good initial values for its nonlinear optimization process to converge accurately and efficiently. These initial pose estimates serve as a prior that guides the optimizer toward a globally consistent solution.

In this example, the pose data is stored in a text file, where each row corresponds to a unique camera pose. Each row contains 17 values:

• The first value is an integer index/ID that identifies the camera pose.
• The remaining 16 values encode the camera’s 3D pose as a flattened $4\times 4$ transformation matrix, stored in row-major order. This matrix represents a rigid body transformation in $SE(3)$, combining both rotation and translation.

The file contains pose data for 26 time steps along the robot’s trajectory. We load these poses and insert them into a gtsam.Values object named initial_estimates. This object functions like a dictionary, mapping symbolic keys (e.g. X(1), X(2),...) to corresponding Pose3 objects. These estimates will later be refined during optimization, but they must be reasonably close to the true trajectory to ensure successful convergence.

4. Load and Inspect Stereo Measurement Data¶

In this section, we load the raw stereo measurement data that encodes the pixel observations and 3D landmark positions captured by the robot’s stereo camera. This data provides the core geometric constraints that link camera poses to landmarks in our factor graph.

Each row in the data file represents a single stereo observation, consisting of eight values:

• The first two values are integer indices/IDs: one for the camera pose and one for the landmark.
• The next three values are the rectified stereo pixel coordinates $(u_L, u_R, v)$. These represent horizontal pixel positions in the left and right images and the sharted vertical coordinate.
• The last three values are the 3D coordinates $(X, Y, Z)$ of the observed landmark, expressed in the camera frame using stereo triangulation.

This data will be used in the next steps to build measurement factors and initialize landmark positions in the world frame.

5. Add Stereo Factors to the Factor Graph¶

With the measurement data loaded, we now translate each stereo observation into a factor that constrains the relative position of a camera pose and a landmark. These constraints form the backbone of our factor graph and are essential for computing an optimized trajectory and map.

For every observation entry (or row) in the measurement matrix:

• We create a GenericStereoFactor3D using the pixel coordinates $(u_L, u_R, v)$, the stereo calibration object K, and the noise model defined earlier.
• Each factor connects a camera pose variable X(i) and a landmark variable L(j) with the associated stereo observation.
• The stereo measurement provides enough geometric information to triangulate depth and establish spatial relationships in 3D.

This step builds up the graph structure that the optimizer will later use to refine both the robot’s trajectory and the map of landmarks.

6. Initialize Landmark Positions from Triangulated Observations¶

Now that we have added measurement factors to the graph, we need to provide an initial guess for the location of each landmark. These guesses are essential for nonlinear optimization; without them, the optimizer would have no starting point from which to refine the 3D landmark positions.

As mentioned before, each observation row includes a 3D point $(X, Y, Z)$ representing the landmark’s position in the camera frame at the time of observation. However, GTSAM operates in a global coordinate frame, so we must convert each point from the camera’s local frame to the world frame.

To do this:

• We check whether a landmark has already been initialized in initial_estimates.
• If it hasn’t, we retrieve the pose of the observing camera and apply a transformation from the camera frame to the world frame using transformFrom().
• We then insert the transformed landmark point into the initial estimate under its corresponding symbolic key.

Each landmark is initialized only once, using the pose from its first observation.

7. Fix the First Pose to Anchor the World Frame¶

Before running optimization, we fix the very first camera pose to anchor the entire system in space. Without this constraint, the entire solution would be underdetermined up to a rigid-body transformation, meaning the optimizer could slide or rotate all poses and landmarks arbitrarily while still minimizing the objective function.

Intuitively, this step establishes a reference origin for the map of the environment we’re trying to build, pinning it down so that all subsequent positions are expressed relative to that origin. As an analogy, this is much like specifying an initial condition when solving a differential equation as we need to fix a starting point in order to recover a particular solution.

To enforce this constraint, we add a NonlinearEqualityPose3 factor to the graph. It ensures that the first pose, X(1), remains exactly equal to its initial estimate and does not change during optimization. This convention is common in SLAM and structure-form-motion systems to eliminate gauge freedom and define a fixed global frame.

8. Optimize the Factor Graph with Levenberg-Marquardt¶

With the factor graph fully constructed and initial values provided for all variables, we now run nonlinear optimization to compute the most probable configuration of camera poses and landmarks. GTSAM uses the Levenberg-Marquardt algorithm, a robust and widely used solver that blends the Gauss-Newton method with gradient descent to handle nonlinearity and poor initial guesses.

We configure the optimizer using a LevenbergMarquardtParams object. One optional parameter we set here is the variable elimination ordering strategy, which influences how the underlying linear system is solved during optimization. We use "METIS", a graph partitioning strategy from the METIS software package that often improves performance by reducing fill-in during matrix factorization. While this step is not strictly necessary for small problems, it becomes valuable in larger graphs.

Once the optimizer runs, it returns a new Values object containing the optimized pose and landmark estimates. This step may take several seconds depending on the size of the graph.

9. Analyze the Results¶

With optimization complete, we now examine the quality of the resulting map and trajectory. A key benefit of using factor graphs and nonlinear optimization is that we can recover a globally consistent trajectory and landmark map even when the initial estimates are noisy or imprecise.

In this last section, we extract and visualize both:

• The initial estimates of poses and landmarks prior to optimization.
• The optimized result returned by the Levenberg-Marquardt optimizer.

To highlight the improvement, we plot both sets side-by-side in a single interactive 3D view. The viewer can rotate, zoom, and pan to inspect the geometry from different perspectives.

While the visual difference between the initial and optimized trajectory may be subtle, especially in a controlled, hallway-like environment in the case of this example, we can still measure the effectiveness of optimization quantitatively.

GTSAM provides a convenient method to evaluate the total error of the factor graph under any estimate. This cost represents the sum of squared Mahalanobis distances between the predicted and actual measurements over all factors in the graph. By computing this cost both before and after optimization, we gain a numerical understanding of how well the optimizer has improved the estimate, even in cases where the visual improvement is minor.