Class PoseOptimizer
Defined in File pose_optimizer.h
Class Documentation
-
class PoseOptimizer
Class for optimizing pose constraints in a trajectory using non-linear optimization.
Public Functions
-
PoseOptimizer(const std::string &osf_filename, const SolverConfig &config)
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.
-
PoseOptimizer(const std::string &osf_filename, const std::string &config_filename)
Constructor with automatic constraint loading from JSON config file.
This constructor loads trajectory data from an OSF file and automatically loads constraints from a JSON configuration file. The JSON file should contain both solver parameters and constraint definitions.
- Parameters:
osf_filename[in] – Path to the OSF file containing trajectory data.
config_filename[in] – Path to the JSON configuration file.
-
PoseOptimizer(const std::string &osf_filename, double key_frame_distance)
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.
- 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.
-
~PoseOptimizer()
Destructor for PoseOptimizer.
-
void save_config(const std::string &config_filename)
Save the current SolverConfig (including new added constraints) to a JSON file.
This function serializes the current solver configuration and all constraints to a JSON file. The resulting file can be used later with Pose Optimizer construction to restore the exact same optimization setup.
- Parameters:
config_filename[in] – The path of the JSON configuration file.
- Throws:
std::runtime_error – if the file cannot be saved.
-
uint32_t add_constraint(std::unique_ptr<Constraint> constraint)
Add a constraint to the optimization problem.
This is the main method for adding constraints to the pose optimizer. It accepts any constraint derived from Constraint. The constraint will be assigned and return a unique ID for later removal if needed.
- Parameters:
constraint[in] – A constraint object derived from Constraint.
- Throws:
std::runtime_error – if the constraint cannot be added.
- Returns:
The unique ID assigned to the constraint.
-
void remove_constraint(uint32_t constraint_id)
Remove a constraint from the pose optimization problem.
- Parameters:
constraint_id[in] – The unique ID of the constraint to remove.
- Throws:
std::runtime_error – if the constraint ID is not found.
-
bool initialize_trajectory_alignment()
Initialize trajectory alignment using average absolute constraints.
Computes a weighted average transformation matrix from the currently configured absolute pose and absolute point constraints (using their weights and Lie algebra representation) and applies it uniformly to the entire trajectory as an initial alignment step before optimization.
- Returns:
true if an alignment transform was successfully applied, false otherwise (e.g. not enough data, negligible transform).
-
double 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.
- Returns:
the current cost value.
-
void save(const std::string &osf_filename)
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_filename[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.
-
double get_key_frame_distance() const
Return the configured key-frame distance.
- Returns:
the key_frame_distance which represents the spacing in meters used when constructing the trajectory.
-
void set_solver_step_callback(std::function<void()> fn)
Set a general callback to be invoked at each solver iteration.
The callback is called from Ceres’ iteration callback during solve(). Use this to hook custom logic such as visualization. The callback is executed in the same thread that calls solve().
- Parameters:
fn[in] – A general callable. Capture what you need.
-
std::vector<std::unique_ptr<Constraint>> get_constraints() const
Get all constraints currently configured in the pose optimizer.
This function returns a copy of all constraints that are currently configured in the pose optimizer, including both constraints loaded from JSON files during construction and constraints added later via add_constraint().
- Returns:
A vector of unique_ptr to Constraint objects representing all currently configured constraints.
-
void set_constraints(std::vector<std::unique_ptr<Constraint>> constraints)
Set all constraints for the pose optimizer.
This function replaces all existing constraints with the provided set of constraints. Any constraints previously loaded from JSON files or added via add_constraint() will be removed and replaced with the new constraint set.
- Parameters:
constraints[in] – A vector of unique_ptr to Constraint objects to set as the complete constraint set.
- Throws:
std::runtime_error – if the constraints cannot be set.
-
std::shared_ptr<class Node> get_node(uint64_t timestamp) const
Get a node by its timestamp.
- Parameters:
timestamp[in] – The timestamp (nanoseconds) of the node to retrieve.
- Returns:
A shared pointer to the Node with the specified timestamp. If no node with the given timestamp exists in the trajectory, this function returns nullptr.
-
double get_cost_value() const
Get the last solver cost value.
-
PoseOptimizer(const std::string &osf_filename, const SolverConfig &config)