Foundations

What you need in your head before opening either repo. Each section starts from the problem, then names the math that solves it, then points at where it shows up in the code. The math itself is developed in full in the Math chapters.

1. The two problems

A mobile robot (or arm, drone, car) constantly faces two questions:

1. Estimation — given noisy sensor readings, what is the most likely state of me and the world? This is GTSAM's job. Examples: SLAM (Simultaneous Localization and Mapping), visual-inertial odometry, structure-from-motion, calibration.
2. Planning — given a known world and a goal, what sequence of states gets me there safely? This is OMPL's job. Examples: an arm reaching into a shelf, a car parking, a drone threading through trees.

They are mirror images. Estimation runs from data back to state; planning runs from goal forward to state. Both are optimization or search over a space of robot states, and that space is usually not a flat vector space: orientations live on a sphere-like curved manifold. That one fact explains a lot of the design in both libraries.

2. SLAM: the localization framework GTSAM serves

SLAM (Simultaneous Localization and Mapping) is the problem a robot faces when it has to work out where it is (localization) inside a map it is building at the same time (mapping). Neither can be solved alone. To localize you need a map, and to build the map you need to know where you were when you saw each thing. SLAM solves both at once, as one estimation problem.

2.1 The anatomy of a SLAM system

Every modern SLAM or localization framework has the same two halves:

A constraint is something like "pose 17 is 1.02 m ahead of pose 16, ±2 cm" (odometry), "landmark 5 appears at pixel (312, 88) from pose 17" (vision), "pose 17 is the same place as pose 3" (loop closure), or "between t₁₆ and t₁₇ the IMU measured this rotation and acceleration" (inertial). The back-end's job is to find the trajectory and map that best agree with all of them, weighted by how trustworthy each one is.

2.2 Localization-only versus full SLAM

• Odometry (VO / LIO / VIO): estimate motion incrementally. It drifts without bound, because errors add up.
• Localization against a prior map: the map is fixed and known; you only estimate the robot's pose. It is the same math with the map variables held constant (in GTSAM: prior factors with tiny sigmas, or NonlinearEquality).
• Full SLAM: estimate poses and map. Loop closures (recognizing a revisited place) add constraints between distant poses that let the optimizer remove accumulated drift. This is what makes SLAM more than odometry.

2.3 Filtering versus smoothing (why GTSAM looks the way it does)

Historically SLAM was solved with an EKF (extended Kalman filter). It keeps only the current pose plus the map, with a dense covariance matrix, and updates it as each measurement arrives. Its problems: the covariance is \(O(n^2)\) in the number of landmarks; linearization errors are baked in forever, because past states are gone and can't be re-linearized; and loop closures are expensive.

Smoothing (the approach GTSAM implements) keeps all the poses, or a sliding window of them, as variables, and repeatedly re-solves for all of them. The key insight is that the information (inverse covariance) matrix of the full trajectory is sparse, while the covariance matrix of a filtered state is dense. Smoothing therefore scales better and can re-linearize, which makes it more accurate. iSAM2 makes smoothing incremental, so it runs in real time like a filter. Chapter 3 (Probability & estimation) derives all of this.

Typical SLAM / localization frameworks built on GTSAM's back-end: LIO-SAM and LVI-SAM (lidar/visual-inertial), Kimera (VIO + mesh), ORB-SLAM-style loop closing with pose graphs, VINS-like VIO with CombinedImuFactor and smart projection factors, multi-robot SLAM with LabeledSymbol keys, and GNSS/INS fusion with GPSFactor + ImuFactor.

3. Math prerequisites: the map

Both libraries rest on a small number of mathematical ideas, used over and over. Each has a dedicated chapter that builds it from scratch, with derivations, worked examples, and pointers to the exact code:

Read them in order. Each chapter assumes the ones before it. Chapter 6 depends on 1 and 2 only.

3.1 Reference texts

4. CS / engineering prerequisites

Porting means reading a lot of C++. These are the language features both codebases use heavily:

• Modern C++ (C++17): both require it. Heavy use of std::shared_ptr (OMPL's ClassForward macros produce FooPtr aliases; GTSAM's shared_ptr typedefs on every class), std::function callbacks, std::optional, and move semantics.
• Templates and concepts-by-convention: GTSAM uses traits (traits<T>::Retract, Local, Dim…) to make any type a manifold or Lie group without inheritance. You must understand the traits/CRTP pattern to port GTSAM's geometry layer.
• Eigen internals: alignment (EIGEN_MAKE_ALIGNED_OPERATOR_NEW), Eigen::Ref, Map, block expressions, aliasing pitfalls.
• Virtual dispatch and plugin-style abstract bases: OMPL is classic OOP (StateSpace, Planner, StateValidityChecker are abstract; you subclass them). GTSAM mixes both: NonlinearFactor is virtual, while geometry is static and traits-based.
• Memory management: OMPL allocates states with allocState()/freeState() (manual, for speed and polymorphism). GTSAM's Values is a type-erased heterogeneous map.
• Build systems: CMake (both), vcpkg/conda/pixi packaging, and Python bindings (GTSAM: its own wrap tool generates pybind11 from .i interface files; OMPL 2.x: nanobind).
• Concurrency: GTSAM optionally uses Intel TBB for parallel elimination. OMPL uses std::thread (parallel planners, ParallelPlan) and atomic termination conditions.

5. GTSAM — what it is

GTSAM (Georgia Tech Smoothing and Mapping, Frank Dellaert's lab, BSD license) is a C++ library that solves estimation problems expressed as factor graphs.

The 30-second model. You declare unknowns (variables: poses, landmarks, velocities, biases, calibrations), each with a key such as X(1) or L(5). You add factors: each is a function of a few variables that scores how well they agree with one measurement, e.g. "odometry says \(x_2\) is 1 m ahead of \(x_1\), ±0.1 m". GTSAM finds the variable values that best satisfy all factors at once, a MAP estimate, and can tell you how uncertain each one is.

NonlinearFactorGraph graph;
auto noise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), noise);
graph.emplace_shared<BetweenFactor<Pose2>>(1, 2, Pose2(2, 0, 0), noise);   // odometry
graph.emplace_shared<BetweenFactor<Pose2>>(2, 3, Pose2(2, 0, M_PI_2), noise);

Values initial;                       // rough initial guesses
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));

Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
Marginals marginals(graph, result);   // covariances

What sets it apart. It is built around the graph structure rather than around matrices. Elimination is graph-theoretic: variable elimination on a factor graph produces a Bayes net, and with structure, a Bayes tree. That view is what makes incremental updates (iSAM2) possible: when a new measurement arrives, only the affected part of the tree is recomputed.

Typical use cases

• SLAM (2D/3D, pose-graph or landmark-based): e.g. LIO-SAM, LeGO-LOAM back-end, Kimera (MIT), ORB-SLAM variants for loop closure.
• Visual-inertial odometry (VIO): ImuFactor/CombinedImuFactor with on-manifold preintegration.
• Structure from Motion and bundle adjustment: camera models (Cal3_S2, Cal3DS2, fisheye…), GenericProjectionFactor, smart factors (landmarks marginalized implicitly), triangulation.
• Sensor calibration (camera–IMU, lidar–camera extrinsics, intrinsics).
• Navigation / GNSS fusion (GPSFactor, NavState, attitude factors).
• Motion planning as inference (GPMP2 and STEAP use GTSAM to optimize trajectories). This is the place where GTSAM and OMPL overlap.
• Hybrid / discrete inference: multi-hypothesis SLAM, mode estimation (gtsam/discrete, gtsam/hybrid).
• Certifiable estimation: Shonan rotation averaging, SDP relaxations (gtsam/sfm, gtsam/certifiable).

6. OMPL — what it is

OMPL (Open Motion Planning Library, Kavraki Lab at Rice University, BSD license) is a C++ library of sampling-based motion planners with a deliberately thin interface to the world.

The 30-second model. You describe the space (e.g. \(SE(3)\), or \(\mathbb{R}^7\) joint angles with bounds) and provide one function: isStateValid(state) → bool. OMPL doesn't know what a robot, a mesh, or a collision is. You plug those in, usually through FCL or Bullet via MoveIt. OMPL samples states, connects them with local paths it checks for validity, and builds trees or roadmaps until it connects start to goal.

auto space = std::make_shared<ob::SE2StateSpace>();
ob::RealVectorBounds bounds(2); bounds.setLow(-1); bounds.setHigh(1);
space->setBounds(bounds);

og::SimpleSetup ss(space);
ss.setStateValidityChecker([](const ob::State *s) {
    auto *se2 = s->as<ob::SE2StateSpace::StateType>();
    return std::hypot(se2->getX(), se2->getY()) > 0.3;   // avoid a disc obstacle
});
ob::ScopedState<> start(space), goal(space);
start = {-0.9, -0.9, 0.0}; goal = {0.9, 0.9, 0.0};
ss.setStartAndGoalStates(start, goal);
ss.setPlanner(std::make_shared<og::RRTConnect>(ss.getSpaceInformation()));
if (ss.solve(1.0)) ss.simplifySolution(), ss.getSolutionPath().print(std::cout);

What sets it apart. It is a large catalog of planners: RRT, RRT-Connect, RRT*, PRM, PRM*, LazyPRM, EST, KPIECE, BIT*, AIT*, EIT*, FMT*, SPARS, Lightning/Thunder experience planners, and more. All of them sit behind one Planner interface, so they can be benchmarked against each other on the same problem (ompl::tools::Benchmark, with Planner Arena for visualization). It also handles constrained planning (states on implicit manifolds, ompl/base/spaces/constraint), kinodynamic planning (ompl/control), and multilevel planning (ompl/multilevel). OMPL 2.x adds VAMP (vectorized, SIMD collision checking from the Kavraki lab), which gives very large speedups for arm planning.

Typical use cases

• Manipulator planning: OMPL is the default planner backend of MoveIt / MoveIt 2 (ROS). Most ROS arm motions you've seen came from OMPL's RRTConnect.
• Mobile robots and cars: Dubins and Reeds–Shepp state spaces for car-like kinematics; kinodynamic planning with controls.
• Drones / UAVs: \(SE(3)\) planning, optimal planners with path-length or clearance objectives.
• Constrained motion: keep a cup upright, closed-chain mechanisms, two-arm tasks.
• Research and benchmarking: a common baseline for comparing new planners.
• Task-level planning: temporal logic (LTL via spot) for control planning (ompl/control/planners/ltl).

7. How the two relate

A common real-world pipeline uses both. GTSAM estimates the robot's pose and the map, OMPL plans a collision-free path in that map, and optionally a GTSAM-based trajectory optimizer (GPMP2-style) smooths it.

8. What comes next

• Math chapters: the prerequisites above, built from first principles.
• GTSAM deep dive: the layers from Lie-group traits to iSAM2, file by file.
• OMPL deep dive: from state spaces to RRT/RRT*/BIT*, kinodynamics, and tooling.
• Porting guide: strategy, scope tiers, dependency replacement, invariants, test plan.