Using the API

This section walks through the Pose Optimizer workflow using the canonical example shipped with the SDK. Every snippet is live code, so you can copy the pattern, adjust timestamps/weights for your data, and combine constraints to refine trajectories programmatically.

Imports

import ouster.sdk.mapping as mapping

#include <ouster/mapping/pose_optimizer.h>

Construct the Optimizer

Instantiate PoseOptimizer with your SLAM-produced OSF and desired key-frame spacing.

po = mapping.PoseOptimizer(
    osf_filename=osf_filename,
    key_frame_distance=2.0,
)

ouster::sdk::mapping::PoseOptimizer po(
    /*osf_filename=*/osf_path,
    /*key_frame_distance=*/2.0
);

Pose-to-Pose Constraint

Constrains the relative transformation between two frame poses using all available point cloud data through algorithms like ICP (Iterative Closest Point) or by registering poses with external measurements like GPS. Each pose corresponds to the first valid column of a LiDAR frame.

\[\mathbf{r}_{\text{pose2pose}} = \log\left(\mathbf{T}_{\text{measured}}^{-1} \mathbf{T}_{\text{target}}\right)\]

Where \(\mathbf{T}_{\text{measured}}\) is the current relative transform, \(\mathbf{T}_{\text{target}}\) is the desired transform, and \(\log(\cdot)\) maps the matrix error to a 6‑vector.

This differs from point-to-point constraints which use specific point correspondences rather than all point cloud data.

Typical use cases:

  • enforcing odometry relationships between consecutive frames using ICP registration

  • loop-closure constraints from full frame-to-frame matching algorithms

  • GPS/INS-derived pose measurements integrated with LiDAR poses

  • maintaining temporal consistency in long trajectories using all point cloud information

Constrain two frames using a relative pose computed from entire point clouds or external sensors to limit drift over long trajectories.

pose_to_pose_constraint = mapping.PoseToPoseConstraint(
    timestamp1=np.uint64(1765338059263057992),
    timestamp2=np.uint64(1765338723963851640),
    relative_pose=np.eye(4),
    rotation_weight=1.0,
    translation_weight=np.array([1.0, 1.0, 1.0]),
)
po.add_constraint(pose_to_pose_constraint)

auto pose_to_pose_constraint =
    std::make_unique<ouster::sdk::mapping::PoseToPoseConstraint>(
    /*timestamp1=*/1765338059263057992,
    /*timestamp2=*/1765338723963851640,
    /*relative_pose=*/ouster::sdk::core::Matrix4dR::Identity(),
    /*rotation_weight=*/1.0,
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0)
);
po.add_constraint(std::move(pose_to_pose_constraint));

Automatic Loop-Closure Constraints

Use add_relative_loop_constraints() to scan the current key-frame trajectory, detect nearby revisits with a spatial hash, and add PoseToPoseConstraint loop closures automatically. The method returns the number of constraints added.

This is the same API used by ouster-cli source ... pose_optimize --auto-loop. Tune min_distance_m to avoid dense duplicate detections, cell_size_m to control the search grid resolution, and icp_score_threshold to reject weak matches.

loop_constraints_added = po.add_relative_loop_constraints(
    min_distance_m=50.0,
    cell_size_m=2.0,
    icp_score_threshold=0.6,
)

auto loop_constraints_added = po.add_relative_loop_constraints(
    /*min_distance_m=*/50.0,
    /*cell_size_m=*/2.0,
    /*icp_score_threshold=*/0.6
);

Point-to-Point Constraint

Forces two selected 3D points observed in two different frames to occupy the same 3D location. This constraint uses manually selected point correspondences between two frames, rather than using all points in the point clouds.

\[\mathbf{r}_{\text{pt2pt}} = \mathbf{T}_1 \mathbf{p}_1 - \mathbf{T}_2 \mathbf{p}_2\]

Where \(\mathbf{T}_1, \mathbf{T}_2\) are the frame poses and \(\mathbf{p}_1, \mathbf{p}_2\) are corresponding specific points identified across frames.

This differs from pose-to-pose constraints which match two selected lidar frame poses. The relative transformation between poses can be calculated using ICP algorithms, or we can match one lidar frame pose with a GPS pose.

Typical use cases:

  • loop-closure correspondences using specific landmark points in large environments

  • feature tracking of distinct geometric features across multiple frames

  • reducing drift by matching structural edges, corners, or distinctive boundaries

  • constraining poses using manually selected point correspondences

Tie explicit point correspondences together across frames to align local geometric features.

Row/col indices for point-to-point constraints are specified in the staggered image (0 <= row < frame.h, 0 <= col < frame.w).

point_to_point_constraint = mapping.PointToPointConstraint(
    timestamp1=np.uint64(1765338003762995576),
    row1=27, col1=1894, return_idx1=1,
    timestamp2=np.uint64(1765338746963563448),
    row2=29, col2=1221, return_idx2=1,
    translation_weight=np.array([0.2, 0.2, 0.2]))
po.add_constraint(point_to_point_constraint)

auto point_to_point_constraint =
    std::make_unique<ouster::sdk::mapping::PointToPointConstraint>(
        /*timestamp1=*/1765338003762995576, /*row1=*/27, /*col1=*/1894,
        /*return_idx1=*/1, /*timestamp2=*/1765338746963563448,
        /*row2=*/29, /*col2=*/1221, /*return_idx2=*/1,
        /*translation_weights=*/Eigen::Array3d(0.2, 0.2, 0.2));
po.add_constraint(std::move(point_to_point_constraint));

Absolute Point Constraint

Pin a specific return to a known global coordinate.

\[\mathbf{r}_{\text{abs\_point}} = \mathbf{T} \mathbf{p}_{\text{local}} - \mathbf{p}_{\text{world}}\]

Where \(\mathbf{T}\) is the pose at the constraint timestamp, \(\mathbf{p}_{\text{local}}\) is the frame point, and \(\mathbf{p}_{\text{world}}\) is the surveyed position.

Typical use cases:

  • surveyed landmarks, ground control points, or fiducials

  • aligning to known infrastructure corners or poles

  • tying trajectories to building layouts or survey markers

Lock a single return to an absolute coordinate to stabilize the map.

Row/col indices for absolute point constraints are specified in the staggered image (0 <= row < frame.h, 0 <= col < frame.w).

absolute_point_constraint = mapping.AbsolutePointConstraint(
    timestamp=np.uint64(1765338003762995576),
    row=27,
    col=1894,
    return_idx=1,
    absolute_position=np.array([40.0, 30.0, 10.0]),
    translation_weight=np.array([1.0, 1.0, 1.0]))
po.add_constraint(absolute_point_constraint)

auto absolute_point_constraint =
    std::make_unique<ouster::sdk::mapping::AbsolutePointConstraint>(
    /*timestamp=*/1765338003762995576, /*row=*/27, /*col=*/1894,
    /*return_idx=*/1,
    /*absolute_position=*/Eigen::Vector3d(40.0, 30.0, 10.0),
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0)
);
po.add_constraint(std::move(absolute_point_constraint));

Absolute Pose Constraint

Fix an entire 6‑DoF pose (position + orientation) to a known target frame.

\[\begin{split}\mathbf{r}_{\text{abs\_pose}} = \begin{bmatrix} \log\left(\mathbf{R}_{\text{measured}}^T \mathbf{R}_{\text{target}}\right) \\ \mathbf{t}_{\text{measured}} - \mathbf{t}_{\text{target}} \end{bmatrix}\end{split}\]

Where \(\mathbf{R}\) and \(\mathbf{t}\) are the rotation and translation components.

Typical use cases:

  • GPS waypoint anchoring

  • sensor calibration or alignment to survey control

  • constraining start/end poses in a loop to prevent drift

  • multi-session data fusion

Pin an entire 6DoF pose in place (with optional removal downstream).

abs_pose = np.eye(4)
abs_pose[:3, 3] = np.array([40.0, 30.0, 10.0])
absolute_pose_constraint = mapping.AbsolutePoseConstraint(
    timestamp=np.uint64(1765338003762995576),
    pose=abs_pose,
    rotation_weight=1.0,
    translation_weight=np.array([1.0, 1.0, 1.0]))
absolute_pose_constraint_id = po.add_constraint(absolute_pose_constraint)

ouster::sdk::core::Matrix4dR abs_pose =
    ouster::sdk::core::Matrix4dR::Identity();
abs_pose.block<3, 1>(0, 3) = Eigen::Vector3d(40.0, 30.0, 10.0);
auto absolute_pose_constraint =
    std::make_unique<ouster::sdk::mapping::AbsolutePoseConstraint>(
    /*timestamp=*/1765338003762995576,
    /*pose=*/abs_pose, /*rotation_weight=*/1.0,
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0));
auto absolute_pose_constraint_id = po.add_constraint(std::move(absolute_pose_constraint));

Remove a Constraint by ID

Once you capture a constraint identifier (e.g., the result of add_constraint), you can remove it to re-run the optimizer without that constraint.

po.remove_constraint(absolute_pose_constraint_id)

po.remove_constraint(absolute_pose_constraint_id);

Run the Optimization

Call solve() to run the constrained least-squares optimization. Each invocation advances the solver with the current constraint set; repeat as needed after adding or removing constraints.

po.solve()

po.solve();

Export a Trajectory

After solving, query timestamps/poses in your preferred sampling mode (columns or key frames) and save them to CSV/TUM using save_trajectory for downstream comparison or evaluation.

ts = po.get_timestamps(mapping.SamplingMode.COLUMNS)
poses = po.get_poses(mapping.SamplingMode.COLUMNS)
mapping.save_trajectory("loop_test_traj.csv", ts, poses)

auto ts = po.get_timestamps(ouster::sdk::mapping::SamplingMode::COLUMNS);
auto poses = po.get_poses(ouster::sdk::mapping::SamplingMode::COLUMNS);
ouster::sdk::mapping::save_trajectory("loop_test_traj.csv", ts, poses);

Save the Optimized OSF

Persist the updated poses back to an OSF so other tools (e.g., ouster-cli viz, map generation, localization) can consume the refined trajectory without rerunning the optimization.

po.save(output_osf)

po.save("po_output.osf");

Complete Example Scripts

Prefer to copy everything at once? The canonical example scripts used throughout this page are available below without the snippet boundaries, so you can run them end-to-end and tweak paths or timestamps as needed.

po = mapping.PoseOptimizer(
    osf_filename=osf_filename,
    key_frame_distance=2.0,
)
# [doc-etag-po-construct]

# [doc-stag-po-pose-to-pose]
pose_to_pose_constraint = mapping.PoseToPoseConstraint(
    timestamp1=np.uint64(1765338059263057992),
    timestamp2=np.uint64(1765338723963851640),
    relative_pose=np.eye(4),
    rotation_weight=1.0,
    translation_weight=np.array([1.0, 1.0, 1.0]),
)
po.add_constraint(pose_to_pose_constraint)
# [doc-etag-po-pose-to-pose]

# [doc-stag-po-auto-loop]
loop_constraints_added = po.add_relative_loop_constraints(
    min_distance_m=50.0,
    cell_size_m=2.0,
    icp_score_threshold=0.6,
)
# [doc-etag-po-auto-loop]

# [doc-stag-po-point-to-point]
point_to_point_constraint = mapping.PointToPointConstraint(
    timestamp1=np.uint64(1765338003762995576),
    row1=27, col1=1894, return_idx1=1,
    timestamp2=np.uint64(1765338746963563448),
    row2=29, col2=1221, return_idx2=1,
    translation_weight=np.array([0.2, 0.2, 0.2]))
po.add_constraint(point_to_point_constraint)
# [doc-etag-po-point-to-point]

# [doc-stag-po-absolute-point]
absolute_point_constraint = mapping.AbsolutePointConstraint(
    timestamp=np.uint64(1765338003762995576),
    row=27,
    col=1894,
    return_idx=1,
    absolute_position=np.array([40.0, 30.0, 10.0]),
    translation_weight=np.array([1.0, 1.0, 1.0]))
po.add_constraint(absolute_point_constraint)
# [doc-etag-po-absolute-point]

# [doc-stag-po-absolute-pose]
abs_pose = np.eye(4)
abs_pose[:3, 3] = np.array([40.0, 30.0, 10.0])
absolute_pose_constraint = mapping.AbsolutePoseConstraint(
    timestamp=np.uint64(1765338003762995576),
    pose=abs_pose,
    rotation_weight=1.0,
    translation_weight=np.array([1.0, 1.0, 1.0]))
absolute_pose_constraint_id = po.add_constraint(absolute_pose_constraint)
# [doc-etag-po-absolute-pose]

# [doc-stag-po-remove-constraint]
po.remove_constraint(absolute_pose_constraint_id)
# [doc-etag-po-remove-constraint]

# [doc-stag-po-solve]
po.solve()
# [doc-etag-po-solve]

# [doc-stag-po-save-trajectory]
ts = po.get_timestamps(mapping.SamplingMode.COLUMNS)
poses = po.get_poses(mapping.SamplingMode.COLUMNS)
mapping.save_trajectory("loop_test_traj.csv", ts, poses)
# [doc-etag-po-save-trajectory]

output_osf = os.environ.get("POSE_OPTIMIZER_OUTPUT", "po_output.osf")
# [doc-stag-po-save-osf]
po.save(output_osf)

ouster::sdk::mapping::PoseOptimizer po(
    /*osf_filename=*/osf_path,
    /*key_frame_distance=*/2.0
);
//! [doc-etag-po-construct]

//! [doc-stag-po-pose-to-pose]
auto pose_to_pose_constraint =
    std::make_unique<ouster::sdk::mapping::PoseToPoseConstraint>(
    /*timestamp1=*/1765338059263057992,
    /*timestamp2=*/1765338723963851640,
    /*relative_pose=*/ouster::sdk::core::Matrix4dR::Identity(),
    /*rotation_weight=*/1.0,
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0)
);
po.add_constraint(std::move(pose_to_pose_constraint));
//! [doc-etag-po-pose-to-pose]

//! [doc-stag-po-auto-loop]
auto loop_constraints_added = po.add_relative_loop_constraints(
    /*min_distance_m=*/50.0,
    /*cell_size_m=*/2.0,
    /*icp_score_threshold=*/0.6
);
//! [doc-etag-po-auto-loop]

(void)loop_constraints_added;

//! [doc-stag-po-point-to-point]
auto point_to_point_constraint =
    std::make_unique<ouster::sdk::mapping::PointToPointConstraint>(
        /*timestamp1=*/1765338003762995576, /*row1=*/27, /*col1=*/1894,
        /*return_idx1=*/1, /*timestamp2=*/1765338746963563448,
        /*row2=*/29, /*col2=*/1221, /*return_idx2=*/1,
        /*translation_weights=*/Eigen::Array3d(0.2, 0.2, 0.2));
po.add_constraint(std::move(point_to_point_constraint));
//! [doc-etag-po-point-to-point]

//! [doc-stag-po-absolute-point]
auto absolute_point_constraint =
    std::make_unique<ouster::sdk::mapping::AbsolutePointConstraint>(
    /*timestamp=*/1765338003762995576, /*row=*/27, /*col=*/1894,
    /*return_idx=*/1,
    /*absolute_position=*/Eigen::Vector3d(40.0, 30.0, 10.0),
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0)
);
po.add_constraint(std::move(absolute_point_constraint));
//! [doc-etag-po-absolute-point]

//! [doc-stag-po-absolute-pose]
ouster::sdk::core::Matrix4dR abs_pose =
    ouster::sdk::core::Matrix4dR::Identity();
abs_pose.block<3, 1>(0, 3) = Eigen::Vector3d(40.0, 30.0, 10.0);
auto absolute_pose_constraint =
    std::make_unique<ouster::sdk::mapping::AbsolutePoseConstraint>(
    /*timestamp=*/1765338003762995576,
    /*pose=*/abs_pose, /*rotation_weight=*/1.0,
    /*translation_weights=*/Eigen::Array3d(1.0, 1.0, 1.0));
auto absolute_pose_constraint_id = po.add_constraint(std::move(absolute_pose_constraint));
//! [doc-etag-po-absolute-pose]

//! [doc-stag-po-remove-constraint]
po.remove_constraint(absolute_pose_constraint_id);
//! [doc-etag-po-remove-constraint]

//! [doc-stag-po-solve]
po.solve();
//! [doc-etag-po-solve]

//! [doc-stag-po-save-trajectory]
auto ts = po.get_timestamps(ouster::sdk::mapping::SamplingMode::COLUMNS);
auto poses = po.get_poses(ouster::sdk::mapping::SamplingMode::COLUMNS);
ouster::sdk::mapping::save_trajectory("loop_test_traj.csv", ts, poses);
//! [doc-etag-po-save-trajectory]

//! [doc-stag-po-save-osf]
po.save("po_output.osf");
//! [doc-etag-po-save-osf]