gtsam_points

The gtsam_points library provides a collection of GTSAM factors and optimizers designed for range-based SLAM. Key components include:

Scan Matching Factors

• ICP: IntegratedICPFactor (point-to-point) and IntegratedPointToPlaneICPFactor (point-to-plane).
• GICP: IntegratedGICPFactor (distribution-to-distribution).
• VGICP: IntegratedVGICPFactor and its GPU implementation IntegratedVGICPFactorGPU (requires -DBUILD_WITH_CUDA=ON).
• LOAM: IntegratedLOAMFactor (point-to-plane and point-to-edge).

Colored Scan Matching Factors

• IntegratedColorConsistencyFactor (Photometric ICP error).
• IntegratedColoredGICPFactor (Photometric + GICP geometric error).

Continuous-time ICP Factors

• IntegratedCT_ICPFactor and IntegratedCT_GICPFactor (Continuous Time ICP).

Bundle Adjustment Factors

• PlaneEVMFactor and EdgeEVMFactor (Eigenvalue minimization).
• LsqBundleAdjustmentFactor (EVM and EF optimal condition satisfaction).

Other Features

• Optimizers for GPU Factors: LevenbergMarquardtOptimizerExt, ISAM2Ext, IncrementalFixedLagSmootherExt.
• Nearest Neighbor Search: KdTree, IncrementalVoxelMap (iVox), IncrementalCovarianceVoxelMap, FastOccupancyGrid.
• Global Registration: PFH, FPFH, RANSAC (6DoF/4DoF), and Graduated Non-Convexity (GNC).
• Segmentation: Region Growing and Min-Cut segmentation.

To install gtsam_points from source, you must first install GTSAM (version 4.3a1 is recommended).

1. Install GTSAM:

git clone https://github.com/borglab/gtsam
cd gtsam
git checkout 4.3a1

mkdir build && cd build
cmake .. \
  -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
  -DGTSAM_BUILD_TESTS=OFF \
  -DGTSAM_WITH_TBB=OFF \
  -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF

make -j$(nproc)
sudo make install

2. Install iridescence (Optional): Required only for running demo programs.

sudo apt install -y libglm-dev libglfw3-dev libpng-dev
git clone https://github.com/koide3/iridescence --recursive
mkdir iridescence/build && cd iridescence/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install

3. Build gtsam_points:

git clone https://github.com/koide3/gtsam_points
mkdir gtsam_points/build && cd gtsam_points/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install

# Install gtsam
git clone https://github.com/borglab/gtsam
cd gtsam
git checkout 4.3a1

mkdir build && cd build
cmake .. \
  -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
  -DGTSAM_BUILD_TESTS=OFF \
  -DGTSAM_WITH_TBB=OFF \
  -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF

make -j$(nproc)
sudo make install

# [optional] Install iridescence visualization library
sudo apt install -y libglm-dev libglfw3-dev libpng-dev
git clone https://github.com/koide3/iridescence --recursive
mkdir iridescence/build && cd iridescence/build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install

## Build gtsam_points
git clone https://github.com/koide3/gtsam_points
mkdir gtsam_points/build && cd gtsam_points/build
cmake .. -DCMAKE_BUILD_TYPE=Release

make -j$(nproc)
sudo make install

The PoseInterpolator class provides a programmatic interface for trajectory interpolation. It uses a continuous-time trajectory model to fit knots to input poses and then samples both poses and IMU measurements at desired timestamps.

Key Methods

• load_input_poses(filename): Loads input poses in TUM format (timestamp x y z qx qy qz qw).
• load_timestamps(filename): Loads a list of target timestamps from a file.
• generate_timestamps_arange(start, end, step): Generates a sequence of timestamps from start to end with a given step. If start or end are negative, it uses the bounds of the input trajectory.
• interpolate(knot_interval, smoothness): Performs the spline fitting. If knot_interval is negative, it is automatically estimated from the input data.
• save_poses(filename): Saves interpolated poses to a file in TUM format.
• save_imu(filename): Saves interpolated IMU measurements (6-DOF: acc x,y,z and gyro x,y,z) to a file.
• visualize(): Opens an interactive viewer to inspect the input poses, knots, interpolated trajectory, and IMU plots.

PoseInterpolator interpolator;

// 1. Load data
interpolator.load_input_poses("input_poses.txt");
interpolator.generate_timestamps_arange(-1.0, -1.0, 0.01);

// 2. Perform interpolation
// knot_interval = -1 (auto), smoothness = 1e-3
interpolator.interpolate(-1.0, 1e-3);

// 3. Save or Visualize
interpolator.save_poses("output_poses.txt");
interpolator.save_imu("output_imu.txt");
interpolator.visualize();

The PoseInterpolator tool is a command-line utility used to interpolate trajectories from discrete poses. It can generate a continuous trajectory (spline) and derive corresponding IMU measurements (accelerations and angular velocities) at specified timestamps.

Input Formats

• Input Poses: Requires a file in TUM format: [timestamp, x, y, z, qx, qy, qz, qw].
• Timestamps: If a timestamp file is provided, it should contain one timestamp per row. If not provided, the tool generates a range of timestamps based on the input trajectory's bounds and a specified step.

CLI Options

# Example: Interpolate a trajectory with a 0.01s step and save results
./pose_interpolator --input trajectory.txt --output interpolated_poses.txt --imu_output imu.txt --t_step 0.01

# Example: Visualize the interpolation process
./pose_interpolator --input trajectory.txt --visualize