pose_optimizer.h

Class 

class PoseOptimizer

Class for optimizing pose constraints in a trajectory using non-linear optimization.

PoseOptimizer manages trajectory optimization by minimizing errors between various constraints such as pose-to-pose, absolute pose, and point-to-point relationships. It uses the Ceres solver internally to perform the optimization.

Public Functions

PoseOptimizer(const std::string &osf_filename, const SolverConfig &config, bool fix_first_node = false)

Constructor that initializes PoseOptimizer with trajectory data and solver parameters.

This constructor loads trajectory data from an OSF file and configures the optimization problem using the provided solver config. It prepares the internal data structures needed for pose optimization and constraint management.

Parameters:

osf_filename – [in] Path to the OSF file containing trajectory data to be optimized.
config – [in] SolverConfig object specifying optimization parameters such as weights, iteration limits, convergence tolerances, and output settings.
fix_first_node – [in] Flag to fix the first node of the trajectory during the pose optimization.

PoseOptimizer(const std::string &osf_filename, double key_frame_distance, bool fix_first_node = false)

Constructor that initializes PoseOptimizer with an OSF file and a simplified configuration.

This constructor loads trajectory data from an OSF file and configures the optimization problem using default parameters, with only the node gap being customizable. The node gap controls the spatial density of optimization nodes along the trajectory. For finer control over other optimization parameters, use the constructor that accepts SolverConfig.

Parameters:

osf_filename – [in] Path to the OSF file containing trajectory data to be optimized.
key_frame_distance – [in] The distance between consecutive optimization nodes along the trajectory.
fix_first_node – [in] Flag to fix the first node of the trajectory during the pose optimization.

~PoseOptimizer(): Destructor for PoseOptimizer.

bool add_pose_to_pose_constraint(uint64_t ts1, uint64_t ts2, const Eigen::Matrix<double, 6, 1> &diff, double rotation_weight = 1.0, double translation_weight = 1.0)

Adds a pose-to-pose constraint using a predefined pose difference.

This function adds a constraint between two poses at specified timestamps, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts1 – [in] The first valid column timestamp of the first frame.
ts2 – [in] The first valid column timestamp of the second frame.
diff – [in] The pose difference between the first and second frames in a 6x1 algebraic vector.
rotation_weight – [in] Rotational weight for this constraint (0 ignores rotation difference).
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).

Returns:

True if the constraint was successfully added, false otherwise.

bool add_pose_to_pose_constraint(uint64_t ts1, uint64_t ts2, const Eigen::Matrix<double, 4, 4> &diff, double rotation_weight = 1.0, double translation_weight = 1.0)

Adds a pose-to-pose constraint using a predefined pose difference.

This function adds a constraint between two poses at specified timestamps, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts1 – [in] The first valid column timestamp of the first frame.
ts2 – [in] The first valid column timestamp of the second frame.
diff – [in] The pose difference between the first and second frames in a 4x4 transformation matrix.
rotation_weight – [in] Rotational weight for this constraint (0 ignores rotation difference).
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).

Returns:

True if the constraint was successfully added, false otherwise.

bool add_pose_to_pose_constraint(uint64_t ts1, uint64_t ts2, double rotation_weight = 1.0, double translation_weight = 1.0)

Adds a pose-to-pose constraint using the default ICP algorithm to calculate the frame difference.

This function adds a constraint between two poses at specified timestamps, using the ICP algorithm to compute the pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts1 – [in] The first valid column timestamp of the first frame.
ts2 – [in] The first valid column timestamp of the second frame.
rotation_weight – [in] Rotational weight for this constraint (0 ignores rotation difference).
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).

Returns:

True if the constraint was successfully added, false otherwise.

bool add_absolute_pose_constraint(uint64_t ts, const Eigen::Matrix<double, 6, 1> &target_pose, double rotation_weight = 1.0, double translation_weight = 1.0, const Eigen::Matrix<double, 6, 1> &diff = Eigen::Matrix<double, 6, 1>::Zero())

Adds an absolute pose constraint with a predefined pose difference for a given timestamp.

This function adds a constraint to enforce a specific pose at a given timestamp, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts – [in] The timestamp at which the absolute pose constraint is applied.
target_pose – [in] The target pose to be enforced at the given timestamp.
rotation_weight – [in] Rotational weight for this constraint (0 ignores rotation difference).
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).
diff – [in] The pose difference to be applied at the given timestamp in a 6x1 algebraic vector.

Returns:

True if the constraint was successfully added, false otherwise.

bool add_absolute_pose_constraint(uint64_t ts, const Eigen::Matrix<double, 4, 4> &target_pose, double rotation_weight = 1.0, double translation_weight = 1.0, const Eigen::Matrix<double, 4, 4> &diff = Eigen::Matrix<double, 4, 4>::Identity())

Adds an absolute pose constraint with a predefined pose difference for a given timestamp.

This function adds a constraint to enforce a specific pose at a given timestamp, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts – [in] The timestamp at which the absolute pose constraint is applied.
target_pose – [in] The target pose to be enforced at the given timestamp.
rotation_weight – [in] Rotational weight for this constraint (0 ignores rotation difference).
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).
diff – [in] The pose difference to be applied at the given timestamp in a 4x4 transformation matrix.

Returns:

True if the constraint was successfully added, false otherwise.

bool add_absolute_pose_constraint(uint64_t ts, const Eigen::Matrix<double, 6, 1> &target_pose, const std::array<double, 3> &rotation_weights = {1.0, 1.0, 1.0}, const std::array<double, 3> &translation_weights = {1.0, 1.0, 1.0}, const Eigen::Matrix<double, 6, 1> &diff = Eigen::Matrix<double, 6, 1>::Zero())

Adds an absolute pose constraint with a predefined pose difference for a given timestamp.

This function adds a constraint to enforce a specific pose at a given timestamp, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts – [in] The timestamp at which the absolute pose constraint is applied.
target_pose – [in] The target pose to be enforced at the given timestamp.
rotation_weights – [in] Rotational weight for each axis of this constraint (0 ignores rotation difference).
translation_weights – [in] Translational weight for each direction of this constraint (0 ignores translation difference).
diff – [in] The pose difference to be applied at the given timestamp in a 6x1 algebraic vector.

Returns:

True if the constraint was successfully added, false otherwise.

bool add_absolute_pose_constraint(uint64_t ts, const Eigen::Matrix<double, 4, 4> &target_pose, const std::array<double, 3> &rotation_weights = {1.0, 1.0, 1.0}, const std::array<double, 3> &translation_weights = {1.0, 1.0, 1.0}, const Eigen::Matrix<double, 4, 4> &diff = Eigen::Matrix<double, 4, 4>::Identity())

Adds an absolute pose constraint with a predefined pose difference for a given timestamp.

This function adds a constraint to enforce a specific pose at a given timestamp, using a given pose difference. The constraint is weighted by the provided rotation and translation weights.

Parameters:

ts – [in] The timestamp at which the absolute pose constraint is applied.
target_pose – [in] The target pose to be enforced at the given timestamp.
rotation_weights – [in] Rotational weight for each axis of this constraint (0 ignores rotation difference).
translation_weights – [in] Translational weight for each direction of this constraint (0 ignores translation difference).
diff – [in] The pose difference to be applied at the given timestamp in a 4x4 transformation matrix.

Returns:

True if the constraint was successfully added, false otherwise.

bool add_point_to_point_constraint(uint64_t ts1, uint32_t row1, uint32_t col1, uint32_t return_idx1, uint64_t ts2, uint32_t row2, uint32_t col2, uint32_t return_idx2, double translation_weight = 1.0)

Adds a point-to-point constraint between two points in different frames.

This function adds a constraint between two points at specified timestamps and positions in their respective frames. The constraint is weighted by the provided translation weight.

Parameters:

ts1 – [in] The first valid column timestamp of the first frame.
row1 – [in] The row index of the point in the first frame.
col1 – [in] The column index of the point in the first frame.
return_idx1 – [in] The return index of the point in the first frame. Either 1 or 2.
ts2 – [in] The first valid column timestamp of the second frame.
row2 – [in] The row index of the point in the second frame.
col2 – [in] The column index of the point in the second frame.
return_idx2 – [in] The return index of the point in the second frame. Either 1 or 2.
translation_weight – [in] Translational weight for this constraint (0 ignores translation difference).

Returns:

True if the constraint was successfully added, false otherwise.

void solve(uint32_t steps = 0)

Optimizes the trajectory using Ceres.

Runs the optimization from the current state. If a positive number of iterations is specified, it overrides the internal setting for max_num_iterations. If steps is 0, the optimizer uses the currently configured max_num_iterations value without overriding it.

Parameters:: steps – [in] Number of iterations to run. Must be >= 0. If 0, the default configured value is used.

void save(const std::string &osf_name)

Saves the optimized trajectory to an OSF file.

This function saves the current state of the optimized trajectory to an OSF file with the specified name.

Parameters:: osf_name – [in] The name of the output OSF file.

std::vector<uint64_t> get_timestamps(SamplingMode type) const

Retrieves timestamps based on the specified sampling mode.

This function returns a sequence of timestamps (in nanoseconds) extracted from the trajectory, based on the provided sampling mode.

SamplingMode::KEY_FRAMES: Returns timestamps associated with key-frame poses.
SamplingMode::COLUMNS: Returns timestamps corresponding to each column in the original LiDAR scans.

Parameters:: type – [in] Sampling mode used to determine which timestamps to return.
Throws:: std::invalid_argument – if the sampling mode is invalid.
Returns:: A vector of timestamps in ascending order.

std::vector<Eigen::Matrix<double, 4, 4>> get_poses(SamplingMode type)

Retrieves pose matrices corresponding to the selected sampling mode.

Returns a list of 4×4 homogeneous transformation matrices, each representing the pose of the sensor or platform at a specific point in time. The returned poses are aligned with the timestamps returned by get_timestamps(type).

SamplingMode::KEY_FRAMES: Returns poses at selected key frames, which typically represent major changes in motion or position.
SamplingMode::COLUMNS: Returns a pose for each column in the LiDAR scan, useful for fine-grained temporal alignment.

Parameters:: type – [in] The sampling mode: SamplingMode::KEY_FRAMES or SamplingMode::COLUMNS.
Throws:: std::invalid_argument – if an unsupported sampling mode is provided.
Returns:: A vector of 4×4 transformation matrices ordered to match the timestamps.

Structs 

struct SolverConfig

Functions 

bool ouster::mapping::save_trajectory(const std::string &filename, const std::vector<uint64_t> &timestamps, const std::vector<Eigen::Matrix<double, 4, 4>> &poses, const std::string &file_type = "csv")

Saves a trajectory into a file.

This function exports the given sequence of timestamps and corresponding 4×4 pose matrices to an output file. Supported output formats are:

”csv”: comma-separated values with columns timestamp, tx, ty, tz, qx, qy, qz, qw
”tum”: TUM trajectory format: timestamp tx ty tz qx qy qz qw

Parameters:

filename – [in] Path of the file to write.
timestamps – [in] A vector of uint64_t timestamps, one per pose.
poses – [in] A vector of 4×4 Eigen homogeneous transforms matrix, each representing the pose at the matching timestamp.
file_type – [in] Output format selector: “csv” or “tum” (default = “csv”).

Returns:

True if the file was successfully created and written; false on I/O error.

pose_optimizer.h

Class

Structs

Functions

Class 

Structs 

Functions 