### Build FAST-LIVO2 Package

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Steps to build the FAST-LIVO2 catkin package, including installing prerequisites, Sophus, and vikit.

```bash
sudo apt install ros-noetic-pcl-ros ros-noetic-cv-bridge ros-noetic-image-transport

git clone https://github.com/strasdat/Sophus.git
cd Sophus && git checkout a621ff
mkdir build && cd build && cmake .. && make && sudo make install

cd ~/catkin_ws/src
git clone https://github.com/xuankuzcr/rpg_vikit.git

git clone https://github.com/hku-mars/FAST-LIVO2
cd ~/catkin_ws && catkin_make
source ~/catkin_ws/devel/setup.bash
```

--------------------------------

### Run FAST-LIVO2 Mapping Example

Source: https://github.com/hku-mars/fast-livo2/blob/main/README.md

Launch the mapping node using the provided launch file and play a downloaded ROS bag file to test the system.

```bash
roslaunch fast_livo mapping_avia.launch
rosbag play YOUR_DOWNLOADED.bag
```

--------------------------------

### Clone and Build FAST-LIVO2

Source: https://github.com/hku-mars/fast-livo2/blob/main/README.md

Clone the FAST-LIVO2 repository into your catkin workspace and build the project. Remember to source the setup file after building.

```bash
cd ~/catkin_ws/src
git clone https://github.com/hku-mars/FAST-LIVO2
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
```

--------------------------------

### Install Sophus Library

Source: https://github.com/hku-mars/fast-livo2/blob/main/README.md

Clone, checkout a specific version, build, and install the Sophus library. Ensure you use the correct version as specified.

```bash
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build && cd build && cmake ..
make
sudo make install
```

--------------------------------

### Livox Avia Sensor Configuration

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Example YAML configuration for a sensor suite including Livox Avia LiDAR and a pinhole camera. This file sets common parameters, extrinsic calibration, time offsets, and sensor-specific preprocessing, VIO, IMU, LIO, and PCD saving settings.

```yaml
# config/avia.yaml  — Livox Avia + pinhole camera example
common:
  img_topic:  "/left_camera/image"
  lid_topic:  "/livox/lidar"
  imu_topic:  "/livox/imu"
  img_en:     1
  lidar_en:   1

extrin_calib:
  # IMU-to-LiDAR translation (metres) and rotation (row-major 3×3)
  extrinsic_T: [0.04165, 0.02326, -0.0284]
  extrinsic_R: [1, 0, 0,  0, 1, 0,  0, 0, 1]
  # LiDAR-to-Camera rotation and translation
  Rcl: [ 0.00610193, -0.999863, -0.0154172,
        -0.00615449,  0.0153796,-0.999863,
         0.999962,    0.00619598,-0.0060598]
  Pcl: [0.0194384, 0.104689, -0.0251952]

time_offset:
  imu_time_offset:  0.0   # seconds
  img_time_offset:  0.1   # seconds (camera lag relative to LiDAR)

preprocess:
  lidar_type:       1     # 1 = Livox Avia
  point_filter_num: 1
  filter_size_surf: 0.1
  blind:            0.8   # ignore points closer than 0.8 m

vio:
  max_iterations:      5
  outlier_threshold:   1000
  img_point_cov:       100
  patch_size:          8
  patch_pyrimid_level: 4
  exposure_estimate_en: true

imu:
  imu_en:       true
  acc_cov:      0.5
  gyr_cov:      0.3
  b_acc_cov:    0.0001
  b_gyr_cov:    0.0001

lio:
  max_iterations: 5
  voxel_size:     0.5
  max_layer:      2
  max_points_num: 50
  beam_err:       0.05
  dept_err:       0.02
  min_eigen_value: 0.0025

pcd_save:
  pcd_save_en:      false
  colmap_output_en: false
  filter_size_pcd:  0.15
  interval:         -1    # -1 = single file for all frames

```

--------------------------------

### Mesh Saved PCD with COLMAP

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Shell script command to process saved PCD files and generate mesh data using COLMAP. Requires COLMAP to be installed on the system.

```bash
# After bag replay, mesh the saved PCD with COLMAP
# (requires colmap installed)
bash scripts/colmap_output.sh
```

--------------------------------

### Get Interpolated Pixel Color

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Retrieves the interpolated pixel color for a given 3-D world point. This is useful for generating colored point cloud data.

```cpp
// Retrieve interpolated pixel colour for a 3-D point (for coloured PCD output)
V3D world_pt(1.0, 0.5, 0.2);
V3F rgb = vio->getInterpolatedPixel(vio->img_rgb, /* projected pixel */ V2D(320, 240));
```

--------------------------------

### Initialize and Configure ImuProcess for State Estimation

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Instantiates the ImuProcess module and sets crucial parameters including IMU-to-LiDAR extrinsics, process noise covariances for gyroscope and accelerometer, and biases. Optional components like IMU, gravity estimation, and bias estimation can be disabled.

```cpp
#include "IMU_Processing.h"

ImuProcessPtr p_imu(new ImuProcess());

// Set IMU-to-LiDAR extrinsics
p_imu->set_extrinsic(V3D(0.04165, 0.02326, -0.0284), M3D::Identity());

// Set process noise (match values in YAML)
p_imu->set_gyr_cov_scale(V3D(0.3, 0.3, 0.3));
p_imu->set_acc_cov_scale(V3D(0.5, 0.5, 0.5));
p_imu->set_gyr_bias_cov(V3D(1e-4, 1e-4, 1e-4));
p_imu->set_acc_bias_cov(V3D(1e-4, 1e-4, 1e-4));

// Optional: disable components
// p_imu->disable_imu();           // run LO without IMU
// p_imu->disable_gravity_est();   // fix gravity to (0,0,-9.81)
// p_imu->disable_bias_est();      // fix zero bias
```

--------------------------------

### VoxelOctoTree Initialization and Usage

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Demonstrates creating a VoxelOctoTree, inserting points with uncertainty, and accessing plane fitting results. Also shows how to find a leaf node and compute LiDAR beam covariance.

```cpp
#include "voxel_map.h"

// Create a root node: max_layer=2, current_layer=0,
//   points_size_threshold=5, max_points=50, planer_threshold=0.0025f
VoxelOctoTree* node = new VoxelOctoTree(2, 0, 5, 50, 0.0025f);

// Insert a point with uncertainty
pointWithVar pv;
pv.point_w = Eigen::Vector3d(1.2, 0.5, 0.0);
pv.var     = Eigen::Matrix3d::Identity() * 0.001;
node->UpdateOctoTree(pv);

// After >= 5 points, node->plane_ptr_->is_plane_ may be true
if (node->plane_ptr_->is_plane_)
{
  Eigen::Vector3d n = node->plane_ptr_->normal_;   // unit normal
  Eigen::Vector3d c = node->plane_ptr_->center_;   // centroid
  float d           = node->plane_ptr_->d_;        // plane offset (n·x + d = 0)
  // Use for ICP residual: residual = n.dot(point_w) + d
}

// Find the leaf node whose voxel contains a query point
Eigen::Vector3d query(1.2, 0.5, 0.01);
VoxelOctoTree* leaf = node->find_correspond(query);

// Compute LiDAR beam covariance for a body-frame point
Eigen::Vector3d pb(3.0, 0.1, 0.5);
Eigen::Matrix3d cov;
calcBodyCov(pb, /*range_inc=*/0.02f, /*degree_inc=*/0.1f, cov);
// cov is the 3×3 measurement covariance in body frame
```

--------------------------------

### Configure VIO Parameters and Initialize

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Configures various parameters for the VIO system, including patch size, iterations, and noise levels. `initializeVIO()` must be called after parameter configuration to compute necessary internal structures.

```cpp
// Point to current EKF state (LIVMapper shares state pointers)
vio->state        = &current_state;
vio->state_propagat = &propagated_state;

// Configure VIO parameters
vio->grid_size            = 5;      // grid cell size (pixels)
vio->patch_size           = 8;      // half-patch size (pixels)
vio->patch_pyrimid_level  = 4;      // pyramid levels
vio->max_iterations       = 5;      // iEKF iterations
vio->outlier_threshold    = 1000.0; // photometric residual cap
vio->img_point_cov        = 100.0;  // image measurement noise variance
vio->normal_en            = true;   // use plane normals for patch warping
vio->exposure_estimate_en = true;   // online exposure estimation

vio->initializeVIO();  // computes Jacobian chain matrices and grid layout
```

--------------------------------

### Initialize and Configure Preprocess for LiDAR

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Instantiates the Preprocess module and sets configuration parameters such as feature extraction, LiDAR type, blind zone, and point filtering. This is typically done automatically by LIVMapper.

```cpp
#include "preprocess.h"

// Instantiate (LIVMapper does this automatically)
PreprocessPtr p_pre(new Preprocess());
p_pre->set(
  /*feat_en=*/false,   // disable feature extraction for FAST-LIVO2
  /*lid_type=*/AVIA,   // LID_TYPE enum: AVIA=1, VELO16=2, OUST64=3,
                       //               L515=4, XT32=5, PANDAR128=6, ROBOSENSE=7
  /*blind=*/0.8,       // discard points within 0.8 m
  /*pfilt_num=*/1      // keep every point (no downsampling at driver level)
);
```

--------------------------------

### Initialize and Build Voxel Map

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Loads configuration, initializes VoxelMapManager, and builds the initial map from a scan. Ensure undistorted and downsampled clouds, along with the current state, are provided.

```cpp
#include "voxel_map.h"

// Load config from ROS params (called inside LIVMapper constructor)
VoxelMapConfig cfg;
loadVoxelConfig(nh, cfg);
// cfg.max_voxel_size_   = 0.5   (metres, root voxel edge)
// cfg.max_layer_        = 2     (octree depth)
// cfg.max_points_num_   = 50    (max points per leaf before subdivision)
// cfg.planner_threshold_= 0.0025 (min eigenvalue ratio to flag as plane)
// cfg.map_sliding_en    = false
// cfg.half_map_size     = 100   (voxels)

std::unordered_map<VOXEL_LOCATION, VoxelOctoTree*> raw_map;
VoxelMapManagerPtr vmap(new VoxelMapManager(cfg, raw_map));

// Build initial map from first undistorted scan
vmap->feats_undistort_ = undistorted_cloud;
vmap->feats_down_body_ = downsampled_cloud;  // after VoxelGrid filter
vmap->state_ = current_state;
vmap->BuildVoxelMap();
```

--------------------------------

### Run FAST-LIVO2 System

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Commands to launch FAST-LIVO2 with different LiDAR configurations and replay rosbag data. Disables RViz visualization if needed.

```bash
roslaunch fast_livo mapping_avia.launch

rosbag play YOUR_DATASET.bag

roslaunch fast_livo mapping_ouster_ntu.launch

roslaunch fast_livo mapping_hesaixt32_hilti22.launch

roslaunch fast_livo mapping_avia.launch rviz:=false
```

--------------------------------

### LIVMapper Parameter Loading Reference

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

This C++ comment block outlines the key parameter groups and their corresponding YAML paths and ROS parameter server keys used by LIVMapper. It serves as a reference for understanding how configuration values are mapped.

```cpp
// Equivalent to calling nh.param<T>(key, var, default) for every field.
// Called internally by LIVMapper constructor; shown here for reference.

// Key parameter groups and their YAML paths:
//
// common/lid_topic          -> lid_topic   (default: "/livox/lidar")
// common/imu_topic          -> imu_topic   (default: "/livox/imu")
// common/img_topic          -> img_topic   (default: "/left_camera/image")
// common/img_en             -> img_en      (1 = enabled)
// common/lidar_en           -> lidar_en    (1 = enabled)
//
// preprocess/lidar_type     -> 1=AVIA, 2=VELO16, 3=OUST64, 4=L515,
//                              5=XT32, 6=PANDAR128, 7=ROBOSENSE
// preprocess/blind          -> minimum valid range (metres)
// preprocess/point_filter_num -> keep every N-th point
//
// vio/patch_size            -> photometric patch side length (pixels)
// vio/patch_pyrimid_level   -> pyramid depth for warp search
// vio/outlier_threshold     -> photometric residual rejection threshold
// vio/exposure_estimate_en  -> online inverse-exposure estimation
//
// imu/gyr_cov / acc_cov     -> process noise for gyro / accelerometer
// imu/imu_en                -> enable IMU pre-integration
//
// lio/voxel_size            -> root voxel edge length (metres)
// lio/max_layer             -> octree subdivision depth
// lio/max_iterations        -> iEKF iterations for LIO
//
// local_map/map_sliding_en  -> enable sliding-window local map
// local_map/half_map_size   -> half-width of local map (voxels)
//
// pcd_save/pcd_save_en      -> save accumulated PCD to disk
// pcd_save/colmap_output_en -> dump COLMAP-compatible sparse reconstruction

```

--------------------------------

### LIVMapper Main Function

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

The main function for the LIVMapper ROS node, initializing the system, setting up subscribers and publishers, and running the EKF state estimation.

```cpp
#include "LIVMapper.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "laserMapping");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  LIVMapper mapper(nh);

  mapper.initializeSubscribersAndPublishers(nh, it);

  mapper.run();
  return 0;
}
```

--------------------------------

### ImuProcess::Process2

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Initializes IMU, estimates gravity and biases, propagates state, and undistorts LiDAR scans to a single pose. It requires synchronized IMU and LiDAR measurements.

```APIDOC
## ImuProcess::Process2

### Description
Initializes the IMU by estimating gravity and biases over the first `imu_int_frame` scans. It then propagates the state forward and undistorts the LiDAR scan to a single pose at the end of the scan.

### Method Signature
`void ImuProcess::Process2(const LidarMeasureGroup &lidar_meas, StatesGroup &state, PointCloudXYZI::Ptr &undistorted_cloud)`

### Parameters
#### Input `lidar_meas`
- **Type**: `LidarMeasureGroup`
- **Description**: Contains buffered IMU measurements and the raw LiDAR scan synchronized together.

#### Input/Output `state`
- **Type**: `StatesGroup`
- **Description**: The 19-DOF EKF state vector. It is updated in-place with the estimated pose, velocity, biases, and gravity.

#### Output `undistorted_cloud`
- **Type**: `PointCloudXYZI::Ptr`
- **Description**: Motion-compensated point cloud in the body frame.

### Configuration (`ImuProcess::set_*`)
- **`set_extrinsic(V3D translation, M3D rotation)`**: Sets the IMU-to-LiDAR extrinsic transformation.
- **`set_gyr_cov_scale(V3D scale)`**: Sets the scale factors for gyroscope noise covariance.
- **`set_acc_cov_scale(V3D scale)`**: Sets the scale factors for accelerometer noise covariance.
- **`set_gyr_bias_cov(V3D cov)`**: Sets the covariance for gyroscope bias.
- **`set_acc_bias_cov(V3D cov)`**: Sets the covariance for accelerometer bias.

### Optional Disabling
- **`disable_imu()`**: Runs LO without IMU data.
- **`disable_gravity_est()`**: Fixes gravity to `(0,0,-9.81)`.
- **`disable_bias_est()`**: Fixes biases to zero.

### Example Usage
```cpp
#include "IMU_Processing.h"

ImuProcessPtr p_imu(new ImuProcess());

// Set extrinsics
p_imu->set_extrinsic(V3D(0.04165, 0.02326, -0.0284), M3D::Identity());

// Set noise covariances
p_imu->set_gyr_cov_scale(V3D(0.3, 0.3, 0.3));
p_imu->set_acc_cov_scale(V3D(0.5, 0.5, 0.5));
p_imu->set_gyr_bias_cov(V3D(1e-4, 1e-4, 1e-4));
p_imu->set_acc_bias_cov(V3D(1e-4, 1e-4, 1e-4));

// Process measurements
LidarMeasureGroup lidar_meas;  // Assume this is populated
StatesGroup state;             // EKF state
PointCloudXYZI::Ptr undistorted_cloud(new PointCloudXYZI());

p_imu->Process2(lidar_meas, state, undistorted_cloud);

// After Process2, 'state' contains updated pose, velocity, biases, etc.
// 'undistorted_cloud' contains motion-compensated points.
```
```

--------------------------------

### Initialize VIO Manager and Set Extrinsics

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Initializes the VIOManager and sets the extrinsic transformations between IMU, Lidar, and Camera. This is a prerequisite for processing frames.

```cpp
#include "vio.h"

VIOManagerPtr vio(new VIOManager());

// Set extrinsics (called by LIVMapper::initializeComponents)
vio->setImuToLidarExtrinsic(
  V3D(0.04165, 0.02326, -0.0284),  // translation (metres)
  M3D::Identity()                   // rotation
);
vio->setLidarToCameraExtrinsic(cameraextrinR, cameraextrinT);
```

--------------------------------

### Clone Vikit Library

Source: https://github.com/hku-mars/fast-livo2/blob/main/README.md

Clone the rpg_vikit repository into your catkin workspace's src folder. This is a different version than used in fast-livo1.

```bash
# Different from the one used in fast-livo1
cd catkin_ws/src
git clone https://github.com/xuankuzcr/rpg_vikit.git 
```

--------------------------------

### Echoing ROS Odometry Topic

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Command to display real-time odometry messages published on the '/Odometry' ROS topic. Useful for monitoring the system's estimated pose.

```bash
# Echo real-time odometry
rostopic echo /Odometry
```

--------------------------------

### Preprocess::process

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Converts raw ROS point-cloud messages from supported LiDARs into a unified PointCloudXYZI format with per-point relative timestamps. It can be configured with various parameters like feature extraction enablement, LiDAR type, blind spot radius, and downsampling.

```APIDOC
## Preprocess::process

### Description
Converts raw ROS point-cloud messages from any supported LiDAR into a unified `PointCloudXYZI` with per-point relative timestamps encoded in the `curvature` field.

### Method Signature
`void Preprocess::process(const RawMsgType::ConstPtr &msg, PointCloudXYZI::Ptr &cloud)`

### Parameters
#### Input Message (`msg`)
- **Type**: `livox_ros_driver::CustomMsg::ConstPtr` or `sensor_msgs::PointCloud2::ConstPtr`
- **Description**: Raw point-cloud message from LiDAR.

#### Output Cloud (`cloud`)
- **Type**: `PointCloudXYZI::Ptr`
- **Description**: Unified point cloud with `curvature` field storing relative time offset.

### Configuration (`Preprocess::set`)
- **`feat_en`** (bool): Enable/disable feature extraction. Default: `false` for FAST-LIVO2.
- **`lid_type`** (LID_TYPE enum): Specifies the LiDAR sensor type (e.g., `AVIA`, `VELO16`, `OUST64`).
- **`blind`** (double): Discard points within this radius (meters). Default: `0.8`.
- **`pfilt_num`** (int): Downsampling factor. `1` means no downsampling.

### Example Usage
```cpp
#include "preprocess.h"

// Instantiate Preprocess
PreprocessPtr p_pre(new Preprocess());

// Configure Preprocess
p_pre->set(
  /*feat_en=*/false,   // disable feature extraction
  /*lid_type=*/AVIA,   // e.g., AVIA
  /*blind=*/0.8,       // discard points within 0.8 m
  /*pfilt_num=*/1      // no downsampling
);

// Callback for Livox CustomMsg
void livox_cb(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  p_pre->process(msg, cloud);
}

// Callback for standard PointCloud2
void standard_cb(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  p_pre->process(msg, cloud);
}
```
```

--------------------------------

### Point-Cloud Coordinate Transforms

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Provides C++ functions for transforming point clouds and individual points between coordinate frames (e.g., body to world). Includes transformations for different data types and RGB information.

```cpp
// Transform entire cloud: R * cloud + t  (e.g., body→world)
PointCloudXYZI::Ptr body_cloud, world_cloud;
mapper.transformLidar(state.rot_end, state.pos_end, body_cloud, world_cloud);

// Transform a single PointType from body (LiDAR) frame to world frame
PointType p_body, p_world;
p_body.x = 1.0f; p_body.y = 0.5f; p_body.z = 0.2f;
mapper.pointBodyToWorld(p_body, p_world);

// Transform body LiDAR point to IMU frame with RGB colouring
PointType pi, po;
mapper.RGBpointBodyLidarToIMU(&pi, &po);

// Transform body point to world frame with RGB channel copy
mapper.RGBpointBodyToWorld(&pi, &po);

// Template version returning Eigen vector (used in EKF Jacobians)
Eigen::Vector3d p_in(1.0, 0.5, 0.2);
Eigen::Vector3d p_out = mapper.pointBodyToWorld(p_in);
```

--------------------------------

### Process a Frame with VIO

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Processes a single camera frame using the configured VIO system. This involves projecting map points, warping patches, and running the iterated Extended Kalman Filter (iEKF) to update the state.

```cpp
// Process one frame: retrieves sub-map, warps patches, runs iEKF
cv::Mat img;          // current greyscale image
vector<pointWithVar> pg; // downsampled points with uncertainty
std::unordered_map<VOXEL_LOCATION, VoxelOctoTree*> plane_map;
double img_timestamp = 1700000000.0;

vio->processFrame(img, pg, plane_map, img_timestamp);
// After return, vio->state->rot_end / pos_end are updated with visual correction.
```

--------------------------------

### Access and Manipulate EKF State Variables

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Demonstrates how to access individual state variables (rotation, position, velocity, biases, gravity, inverse exposure time) from the StatesGroup object and how to perform updates using manifold operations (boxplus and boxminus).

```cpp
#include "common_lib.h"

StatesGroup s;

// Access fields
Eigen::Matrix3d R  = s.rot_end;       // rotation matrix (IMU body → world)
Eigen::Vector3d p  = s.pos_end;       // position in world frame (metres)
Eigen::Vector3d v  = s.vel_end;       // velocity in world frame (m/s)
Eigen::Vector3d bg = s.bias_g;        // gyroscope bias (rad/s)
Eigen::Vector3d ba = s.bias_a;        // accelerometer bias (m/s²)
Eigen::Vector3d g  = s.gravity;       // estimated gravity vector (m/s²)
double          ie = s.inv_expo_time; // inverse exposure time (no units)

// Manifold boxplus: apply a 19-vector increment (rotation via Exp map)
Eigen::Matrix<double, 19, 1> dx = Eigen::Matrix<double, 19, 1>::Zero();
dx.block<3,1>(3,0) = Eigen::Vector3d(0.01, 0.0, 0.0); // move 1 cm in X
StatesGroup s_new = s + dx;

// Manifold boxminus: compute tangent-space difference
Eigen::Matrix<double, 19, 1> diff = s_new - s;
// diff.block<3,1>(0,0) = Log(R_new^T * R)  (rotation error)
// diff.block<3,1>(3,0) = p_new - p

// Reset position/velocity/rotation to identity/zero
s.resetpose();
```

--------------------------------

### Compute RMSE with evo_ape

Source: https://github.com/hku-mars/fast-livo2/blob/main/Log/result/ntu_viral/README.md

Use this command to calculate the Root Mean Square Error (RMSE) between ground truth and estimated trajectories. Ensure your files are in the TUM format and specify the correct ground truth and estimated pose files.

```bash
evo_ape tum eee_01_gt.txt eee_01_prism.txt -a --plot --plot_mode xyz
```

--------------------------------

### Run LIO State Estimation and Update Map

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Performs LIO state estimation and updates the voxel map with new observations. The state estimation populates `ptpl_list_` with point-to-plane residuals.

```cpp
// Run LIO state estimation (iEKF, builds ptpl_list_)
vmap->StateEstimation(state_propagat);

// Update map with newly observed points
std::vector<pointWithVar> new_points; // filled by LIVMapper
vmap->UpdateVoxelMap(new_points);
```

--------------------------------

### Plot Debug Residuals and Timing Logs

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Python script command to visualize debug residuals and timing logs. Useful for performance analysis and debugging.

```bash
# Plot debug residuals / timing logs
python3 Log/plot.py
```

--------------------------------

### Saving Point Cloud Data

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Configuration options in YAML to enable saving point cloud data. Allows specifying the frame type, voxel grid filter size, and saving interval. Enabling COLMAP output is also supported.

```yaml
# In your sensor YAML, enable any combination:
pcd_save:
  pcd_save_en:      true
  type:             0       # 0 = world frame, 1 = body/LiDAR frame
  filter_size_pcd:  0.05    # voxel-grid leaf size for saved cloud (metres)
  interval:         -1      # -1 = one file; N = new file every N scans
  colmap_output_en: true    # dump COLMAP sparse/0/ text files for meshing
```

--------------------------------

### Process IMU and LiDAR Measurements for State Estimation

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Processes a synchronized group of IMU and LiDAR measurements to update the EKF state and undistort the LiDAR point cloud. This function is called within LIVMapper::processImu.

```cpp
// Process a synchronized measurement group (called inside LIVMapper::processImu)
LidarMeasureGroup lidar_meas;  // contains buffered IMU + raw lidar scan
StatesGroup state;             // 19-DOF EKF state
PointCloudXYZI::Ptr undistorted_cloud(new PointCloudXYZI());

p_imu->Process2(lidar_meas, state, undistorted_cloud);
// After return:
//   state.rot_end  = rotation at end of scan (SO3)
//   state.pos_end  = position at end of scan (world frame, metres)
//   state.vel_end  = velocity at end of scan (m/s)
//   state.bias_g   = gyroscope bias (rad/s)
//   state.bias_a   = accelerometer bias (m/s²)
//   undistorted_cloud = motion-compensated points in body frame
```

--------------------------------

### Process Livox CustomMsg Point Cloud

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Callback function for Livox LiDARs. It processes the custom message format and converts it into a unified PointCloudXYZI format, encoding relative time offsets in the curvature field.

```cpp
// Livox CustomMsg callback (Avia, MID360, etc.)
void livox_cb(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  p_pre->process(msg, cloud);
  // cloud->points[i].curvature = relative time offset (ms) within scan
}
```

--------------------------------

### Build Point-to-Plane Residuals

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Generates a list of point-to-plane residuals for the current scan using OMP for parallel processing. The resulting `ptpl_list` contains plane normals, signed distances, and validity flags.

```cpp
// Build OMP-parallel point-to-plane residuals for current scan
std::vector<pointWithVar> pv_list;
std::vector<PointToPlane> ptpl_list;
vmap->BuildResidualListOMP(pv_list, ptpl_list);
// ptpl_list[i].normal_  = plane normal (world frame)
// ptpl_list[i].d_       = signed distance to plane
// ptpl_list[i].is_valid_= true if eigenvalue test passed
```

--------------------------------

### Saving Image Data

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

YAML configuration to enable saving image frames from the sensor. The interval parameter controls how frequently images are saved.

```yaml
image_save:
  img_save_en: true
  interval:    1            # save every N-th frame
```

--------------------------------

### Pinhole Camera Intrinsics Configuration

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

YAML configuration for pinhole camera intrinsics. This includes camera model type, resolution, scaling factor, focal lengths, principal point, and radial and tangential distortion coefficients.

```yaml
# config/camera_pinhole.yaml — pinhole intrinsics
cam_model:  Pinhole
cam_width:  1280
cam_height: 1024
scale:      0.5          # image is resized to 640×512 internally
cam_fx:     1293.56944
cam_fy:     1293.3155
cam_cx:      626.91359
cam_cy:      522.799224
cam_d0:    -0.076160     # radial distortion k1
cam_d1:     0.123001     # radial distortion k2
cam_d2:    -0.00113      # tangential p1
cam_d3:     0.000251     # tangential p2

```

--------------------------------

### Process Standard PointCloud2 Message

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Callback function for standard LiDARs (Velodyne, Ouster, etc.) that publish PointCloud2 messages. It processes the message into the unified PointCloudXYZI format.

```cpp
// Standard PointCloud2 callback (Velodyne / Ouster / Hesai / Robosense)
void standard_cb(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
  PointCloudXYZI::Ptr cloud(new PointCloudXYZI());
  p_pre->process(msg, cloud);
}
```

--------------------------------

### Enable Sliding Local Map

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Activates the sliding local map feature to bound memory usage for long trajectories. This is typically enabled via configuration files (e.g., YAML).

```cpp
// Enable sliding local map (keeps memory bounded for long trajectories)
// Set in YAML: local_map/map_sliding_en: true
vmap->mapSliding();
```

--------------------------------

### StatesGroup

Source: https://context7.com/hku-mars/fast-livo2/llms.txt

Represents the 19-DOF Extended Kalman Filter (EKF) state vector, including rotation, position, velocity, biases, and gravity. Provides methods for accessing and manipulating the state.

```APIDOC
## StatesGroup

### Description
The `StatesGroup` class represents the 19-DOF EKF state vector. This state includes rotation (SO3), position, inverse exposure time, velocity, gyroscope bias, accelerometer bias, and gravity.

### State Vector Dimensions
- Rotation (SO3): 3 dimensions
- Position: 3 dimensions
- Inverse Exposure Time: 1 dimension
- Velocity: 3 dimensions
- Gyroscope Bias: 3 dimensions
- Accelerometer Bias: 3 dimensions
- Gravity: 3 dimensions

### Accessing State Fields
```cpp
#include "common_lib.h"

StatesGroup s; // Assume 's' is populated

Eigen::Matrix3d rotation_matrix = s.rot_end;       // IMU body to world rotation
Eigen::Vector3d position = s.pos_end;              // Position in world frame (meters)
Eigen::Vector3d velocity = s.vel_end;              // Velocity in world frame (m/s)
Eigen::Vector3d gyro_bias = s.bias_g;               // Gyroscope bias (rad/s)
Eigen::Vector3d acc_bias = s.bias_a;                // Accelerometer bias (m/s^2)
Eigen::Vector3d gravity_vector = s.gravity;         // Estimated gravity vector (m/s^2)
double inverse_exposure = s.inv_expo_time;         // Inverse exposure time
```

### State Manipulation
#### Boxplus Operation (State Increment)
Applies a 19-dimensional increment `dx` to the current state `s`. Rotation is updated using the exponential map.
```cpp
Eigen::Matrix<double, 19, 1> dx = Eigen::Matrix<double, 19, 1>::Zero();
dx.block<3,1>(3,0) = Eigen::Vector3d(0.01, 0.0, 0.0); // Example: move 1 cm in X
StatesGroup s_new = s + dx;
```

#### Boxminus Operation (State Difference)
Computes the difference between two states `s_new` and `s` in the tangent space.
```cpp
Eigen::Matrix<double, 19, 1> diff = s_new - s;
// diff.block<3,1>(0,0) contains the rotation error (Log map)
// diff.block<3,1>(3,0) contains the position difference
```

#### Reset Pose
Resets the position, velocity, and rotation components of the state to identity/zero.
```cpp
s.resetpose();
```
```

=== COMPLETE CONTENT === This response contains all available snippets from this library. No additional content exists. Do not make further requests.