Class LIO

Nested Relationships

Nested Types

Class Documentation

class LIO

RKO-LIO’s core LiDAR-inertial odometry algorithm class.

Public Functions

inline explicit LIO(const Config &config_)
void add_imu_measurement(const ImuControl &base_imu)

Add an IMU measurement in base frame.

Parameters:

base_imu – IMU measurement in base frame

void add_imu_measurement(const Sophus::SE3d &extrinsic_imu2base, const ImuControl &raw_imu)

Add an IMU measurement in IMU frame and transform it by the extrinsic calibration.

Parameters:
  • extrinsic_imu2base – Extrinsic transform from IMU to base frame

  • raw_imu – Raw IMU measurement

Vector3dVector register_scan(const Vector3dVector &scan, const TimestampVector &timestamps)

Register a scan.

Parameters:
  • scan – raw point cloud

  • timestamps – Corresponding absolute timestamps for each scan point

Returns:

Deskewed and clipped scan

Vector3dVector register_scan(const Sophus::SE3d &extrinsic_lidar2base, const Vector3dVector &scan, const TimestampVector &timestamps)

Register a scan with extrinsic pre-applied.

Parameters:
  • extrinsic_lidar2base – Extrinsic from lidar to base frame

  • scan – raw point cloud

  • timestamps – Corresponding absolute timestamps for each scan point

Returns:

Deskewed and clipped scan in the original lidar frame

void dump_results_to_disk(const std::filesystem::path &results_dir, const std::string &run_name) const

Dump trajectory results to disk.

Parameters:
  • results_dir – Directory where results are saved

  • run_name – Identifier for run/folder name

Public Members

Config config
SparseVoxelGrid map
State lidar_state

Current lidar pose/state estimate.

ImuBias imu_bias

IMU bias estimates if initialization was enabled.

Eigen::Vector3d mean_body_acceleration = Eigen::Vector3d::Zero()

Estimated body acceleration.

Eigen::Matrix3d body_acceleration_covariance = Eigen::Matrix3d::Identity()

Covariance of body acceleration estimate.

IntervalStats interval_stats
struct Config

Configuration parameters for odometry.

Public Members

bool deskew = true

Enable scan deskewing.

size_t max_iterations = 100

Maximum number of ICP iterations.

double voxel_size = 1.0

Size of voxel grid (meters)

int max_points_per_voxel = 20

Max points per voxel.

double max_range = 100.0

Max lidar range (meters)

double min_range = 1.0

Min lidar range (meters)

double convergence_criterion = 1e-5

ICP convergence threshold.

double max_correspondance_distance = 0.5

Max distance for point correspondences (meters)

int max_num_threads = 0

Thread count used for data associations, zero for automatic.

bool initialization_phase = false

True to enable initialization phase.

double max_expected_jerk = 3

Maximum expected jerk for platform (m/s³)

bool double_downsample = true

Use double downsampling.

double min_beta = 200

Minimum weight for orientation regularisation.