cartographer/mapping/internal/optimization/optimization_problem_interface.h

Overview

OptimizationProblemInterface defines the abstract interface shared by OptimizationProblem2D and OptimizationProblem3D. It implements the SPA (Sparse Pose Adjustment) loop-closure method: a non-linear least-squares problem over the global poses of all trajectory nodes and submaps, solved with Ceres. The interface owns all sensor data and per-trajectory bookkeeping used by the solver.

Template Interface

template <typename NodeDataType, typename SubmapDataType,
          typename RigidTransformType>
class OptimizationProblemInterface {
 public:
  using Constraint = PoseGraphInterface::Constraint;
  using LandmarkNode = PoseGraphInterface::LandmarkNode;
  ...
};

Template parameters are instantiated per dimension:

  • 2D: NodeData = NodeSpec2D, SubmapData = SubmapSpec2D, transform = Rigid2d.

  • 3D: NodeData = NodeSpec3D, SubmapData = SubmapSpec3D, transform = Rigid3d.

Adding Data

virtual void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) = 0;
virtual void AddOdometryData(int trajectory_id,
                             const sensor::OdometryData& odometry_data) = 0;
virtual void AddTrajectoryNode(int trajectory_id,
                               const NodeDataType& node_data) = 0;
virtual void InsertTrajectoryNode(const NodeId& node_id,
                                  const NodeDataType& node_data) = 0;
virtual void TrimTrajectoryNode(const NodeId& node_id) = 0;
virtual void AddSubmap(int trajectory_id,
                       const RigidTransformType& global_submap_pose) = 0;
virtual void InsertSubmap(const SubmapId& submap_id,
                          const RigidTransformType& global_submap_pose) = 0;
virtual void TrimSubmap(const SubmapId& submap_id) = 0;
virtual void SetMaxNumIterations(int32 max_num_iterations) = 0;

  • Add* appends data for a live trajectory.

  • Insert* adds already-serialized data at specific IDs (used during deserialization).

  • Trim* removes data for deleted nodes/submaps.

Solving

virtual void Solve(
    const std::vector<Constraint>& constraints,
    const std::map<int, PoseGraphInterface::TrajectoryState>&
        trajectories_state,
    const std::map<std::string, LandmarkNode>& landmark_nodes) = 0;

Optimizes global poses given the set of inter-node/submap constraints, the state of each trajectory (active/finished/deleted), and landmark observations. Implementations build a Ceres problem from constraints (via cost functions such as SpaCostFunction2D/SpaCostFunction3D), IMU and odometry residuals, and fixed-frame pose / landmark residuals, then solve it with ceres::Solve.

Accessors

virtual const MapById<NodeId, NodeDataType>& node_data() const = 0;
virtual const MapById<SubmapId, SubmapDataType>& submap_data() const = 0;
virtual const std::map<std::string, transform::Rigid3d>& landmark_data()
    const = 0;
virtual const sensor::MapByTime<sensor::ImuData>& imu_data() const = 0;
virtual const sensor::MapByTime<sensor::OdometryData>& odometry_data()
    const = 0;

node_data() and submap_data() hold the current global pose estimates; landmark_data() maps landmark IDs to global poses; imu_data()/odometry_data() store raw sensor input keyed by time.

Role in the SLAM Pipeline

The pose graph owns an OptimizationProblemInterface instance. It:

  1. Feeds every new node’s constant data, submap poses, IMU/odometry samples, and constraints into the problem.

  2. Calls Solve after each optimization run to refine all global poses.

  3. Reads the optimized node_data() and submap_data() back to update the trajectory and submap poses.

See Also