Pose Graph Optimization

• Also called: PGO, pose-graph SLAM, pose graph optimisation
• Category: State estimation & navigation › Localisation
• Tags: slam, state-estimation, optimization, localization, mapping, factor-graphs

A SLAM back-end technique that refines a robot's trajectory by adjusting a graph of poses so that the odometry and loop-closure constraints between them agree as closely as possible.

Definition

Pose graph optimization is the process of estimating a robot's trajectory by solving a least-squares problem over a graph. Each node in the graph is a robot pose (a position and orientation at one moment in time), and each edge is a measured relative transform between two poses, together with its uncertainty, usually expressed as an information matrix. Edges come from odometry between consecutive poses and from loop closures, where the robot recognizes a place it has visited before.

Because every measurement is noisy, the edges never agree perfectly: chaining odometry edges around a loop does not bring the robot back exactly to where a loop-closure edge says it should be. Pose graph optimization finds the set of poses that minimizes the total weighted squared error across all edges, spreading the correction over the whole trajectory in proportion to how certain each measurement is.

The problem is nonlinear because rotations do not combine additively, so it is solved iteratively with methods such as Gauss-Newton or Levenberg-Marquardt, linearizing around the current estimate at each step. The resulting linear systems are sparse, since each pose connects to only a few others, and sparse solvers make graphs with many thousands of poses tractable. Open-source libraries such as g2o, GTSAM and Ceres Solver are commonly used for this step.

Pose graph optimization is the back end of most graph-based SLAM systems. It differs from bundle adjustment in that landmarks are not kept as variables: only poses are optimized, which is faster but discards some information. Its known weakness is sensitivity to wrong loop closures, which is why robust cost functions and outlier rejection are standard additions.

Why it matters

Odometry and visual odometry drift without bound over time, and pose graph optimization is the standard way to remove that accumulated drift once loop closures are found. It turns a set of locally accurate but globally inconsistent measurements into a trajectory and map that are consistent across a whole building or site.

Key applications

• Graph-based SLAM back ends for lidar and camera-based mapping
• Removing accumulated drift from wheel, visual and visual-inertial odometry
• Multi-session mapping, merging maps recorded at different times
• Multi-robot mapping, combining trajectories from several robots into one frame

In practice

In a typical lidar or visual SLAM stack, the front end adds a new node every time the robot moves a set distance or angle and connects it to the previous node with an odometry edge. A place-recognition module proposes loop closures, which are verified geometrically (for example with ICP or feature matching) before being added as edges. The back end then runs an optimizer such as g2o or GTSAM, either in batch after each loop closure or incrementally (as with iSAM2), and the corrected poses are used to rebuild the map. Much of the tuning effort goes into the covariances assigned to each edge and into robust kernels such as Huber or Cauchy, because a single false loop closure with high confidence can bend the whole map.

Common misconceptions

• Pose graph optimization builds the map — it corrects only the poses; the map is rebuilt afterward by placing sensor data at the corrected poses
• More loop closures always help — a single incorrect loop closure can distort the whole trajectory, so closures must be verified and down-weighted with robust costs
• It is the same as bundle adjustment — bundle adjustment also optimizes landmark positions, while a pose graph keeps only poses and relative constraints

Who should know this

• SLAM and localization engineers
• Mobile robot and autonomous vehicle developers
• Computer vision engineers working on visual odometry and mapping
• Students learning probabilistic robotics and state estimation

Related technologies

• g2o
• GTSAM
• Ceres Solver
• Factor Graphs
• Bundle Adjustment
• Levenberg-Marquardt Algorithm

Related terms

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

Related projects

• LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
• oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
• Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO

Videos

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB