Mathematical Robotics (C++)

C++ port of the MathematicalRobotics library for robotics algorithms.

GitHub Pages: Mathematical Robotics Atlas

Demos

Point-Line-Plane Matching

Point cloud registration with line and plane features using ICP.

Bundle Adjustment

Large-scale sparse bundle adjustment for Structure from Motion.

Gauss-Newton Optimization

Non-linear least squares circle fitting.

Robust Optimization

Circle fitting with Huber robust kernel to handle outliers.

Features

• Lie Groups: SO(2), SO(3), SE(2), SE(3) operations with exponential/logarithm maps and Jacobians
• Optimization: Gauss-Newton solver with robust kernels (L2, L1, L4, Huber, Pseudo-Huber, Cauchy, Gaussian)
• Graph Optimization: Vertex/edge-based graph solver for SLAM
• SLAM: Bundle adjustment, camera projection, reprojection edges
• IMU Preintegration: Navigation state, bias handling, covariance propagation
• IMU Factor Graph: Navigation/bias vertices and IMU preintegration factors for graph optimization
• Filtering: Extended Kalman Filter with manifold-aware operations
• Monte Carlo Filtering: Particle Filter for the same 2D state and sensor models
• Geometry: Point-line-plane fitting, Point-to-Line/Plane ICP
• IMLS: 2D implicit moving least squares normal estimation and point-to-surface queries
• Data Processing: CSV/PCD/NPY/g2o/BAL I/O, filtering, interpolation
• Kinematics: Velocity and IMU transformations between frames

Requirements

• CMake 3.16+
• C++17 compiler
• Eigen3

Optional

• Google Test (for unit tests)
• Doxygen (for documentation)
• Python 3 with numpy/matplotlib (for visualization)

Build

# Install dependencies (Ubuntu/Debian)
sudo apt-get install build-essential cmake libeigen3-dev

# Build
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)

# Run tests
ctest --output-on-failure

# Install (optional)
sudo make install

Modules

Module	Header	Description
SO2	lie_group/so2.hpp	2D rotation group
SO3	lie_group/so3.hpp	3D rotation group with Jacobians
SE2	lie_group/se2.hpp	2D rigid transformation
SE3	lie_group/se3.hpp	3D rigid transformation
Gauss-Newton	optimization/gauss_newton.hpp	Non-linear least squares
Graph Solver	graph_optimization/graph_solver.hpp	Graph-based optimization
Bundle Adjustment	slam/projection.hpp	Camera projection and BA
IMU Preintegration	imu_preintegration/preintegration.hpp	IMU integration for VIO
IMU Factor Graph	imu_preintegration/imu_factor.hpp	Navigation/bias factors on top of graph solver
EKF	filter/ekf.hpp	Extended Kalman Filter
Particle Filter	filter/particle_filter.hpp	Monte Carlo state estimation
Robot Geometry	robot_geometry/basic_geometry.hpp	Fitting and ICP
IMLS	imls/imls.hpp	2D implicit moving least squares utilities
Kinematics	kinematics/transform_velocity.hpp	Velocity transformations
IMU Transform	kinematics/transform_imu.hpp	IMU frame transformation utilities
Data Loader	data/data_loader.hpp	I/O and data processing
Math Tools	utilities/math_tools.hpp	Utility functions

Usage

Lie Groups

#include <mathematical_robotics/lie_group/so3.hpp>
#include <mathematical_robotics/lie_group/se3.hpp>
using namespace math_robotics;

// SO(3) exponential map
Eigen::Vector3d omega(0.1, 0.2, 0.3);
Eigen::Matrix3d R = SO3::exp(omega);
Eigen::Vector3d omega_recovered = SO3::log(R);

// SO(3) Jacobians
Eigen::Matrix3d Jr = SO3::rightJacobian(omega);
Eigen::Matrix3d Jl = SO3::leftJacobian(omega);

// SE(3) operations
Eigen::Matrix<double, 6, 1> xi;
xi << 1.0, 2.0, 3.0, 0.1, 0.2, 0.3;  // [translation, rotation]
Eigen::Matrix4d T = SE3::exp(xi);

Extended Kalman Filter

#include <mathematical_robotics/filter/ekf.hpp>
using namespace math_robotics;

// Initialize state and covariance
State2D initial_state(pos, rotation, velocity);
Eigen::Matrix<double, 5, 5> initial_cov = Eigen::Matrix<double, 5, 5>::Identity() * 0.1;

// Create motion and measurement models
auto motion_model = std::make_shared<Odometry2DModel>(process_noise);
auto measurement_model = std::make_shared<GPSModel>(measurement_noise);

// Create EKF
EKF ekf(initial_state, initial_cov, motion_model, measurement_model);

// Predict and correct
ekf.predict(control, dt);
ekf.correct(measurement);

IMU Preintegration

#include <mathematical_robotics/imu_preintegration/preintegration.hpp>
using namespace math_robotics;

// Initialize
IMUBias bias;
IMUNoise noise(0.01, 0.01, 0.001, 0.001);
IMUPreintegration preint(bias, noise);

// Integrate measurements
preint.integrate(accelerometer, gyroscope, dt);

// Get preintegrated measurement
auto measurement = preint.getMeasurement();

Point Cloud Processing

#include <mathematical_robotics/data/data_loader.hpp>
#include <mathematical_robotics/robot_geometry/basic_geometry.hpp>
using namespace math_robotics;

// Load point cloud
auto cloud = DataLoader::loadPointCloud("points.pcd");

// Voxel grid filtering
auto filtered = DataProcessor::voxelGridFilter(cloud, 0.1);

// Fit plane
auto result = fitPlane(points);
if (result) {
    auto [centroid, normal] = result.value();
}

Examples

cd build/examples

# Lie group operations
./demo_so3_operations
./demo_se3_operations

# Optimization
./demo_gauss_newton              # Circle fitting
./demo_gauss_newton_with_plot    # With visualization

# SLAM
./demo_slam_bundle_adjustment    # Bundle adjustment on BAL dataset
./demo_imu_preintegration        # IMU integration
./demo_imu_factor_graph          # IMU factor graph with navigation and bias states
./demo_imu_preintegration_dataset # IMU + pose + truth fusion on provided .npy dataset
./demo_pose_graph_2d             # 2D pose graph optimization on g2o data
./demo_pose_graph_3d             # 3D pose graph optimization on g2o data

# Filtering and kinematics
./demo_particle_filter           # Particle filter and EKF on noisy circular motion
./demo_transform_velocity        # 2D/3D rigid-body velocity frame transforms
./demo_transform_imu             # IMU frame transform consistency check

# Geometry
./demo_point_line_matching       # Point-line-plane fitting
./demo_point_matching_2d         # 2D point cloud matching
./demo_point_matching_3d         # 3D point cloud matching
./demo_robot_geometry            # Geometry operations
./demo_imls_2d                   # 2D IMLS surface queries

# Data processing
./demo_data_processing           # Data loading and filtering

Testing

cd build

# Run all tests
ctest --output-on-failure

# Run specific test
./tests/test_so3
./tests/test_ekf
./tests/test_particle_filter
./tests/test_imu_preintegration
./tests/test_imu_factor
./tests/test_transform_imu
./tests/test_graph_optimization
./tests/test_integration

Documentation

Generate API documentation with Doxygen:

cd build
make docs
# Open docs/html/index.html

GitHub Pages

The repository now includes a static algorithm guide site under docs/ and a GitHub Pages workflow at .github/workflows/pages.yml.

• Site URL: https://rsasaki0109.github.io/mathematical_robotics/
• Deployment: triggered on pushes to develop or main
• Source: docs/

To preview the site locally:

cd docs
python3 -m http.server 8000
# Open http://localhost:8000

If GitHub prompts for Pages configuration, set the site source to GitHub Actions.

Project Structure

mathematical_robotics/
├── include/mathematical_robotics/
│   ├── lie_group/          # SO2, SO3, SE2, SE3
│   ├── optimization/       # Gauss-Newton solver
│   ├── graph_optimization/ # Graph-based SLAM
│   ├── slam/               # Bundle adjustment
│   ├── imu_preintegration/ # IMU integration
│   ├── filter/             # EKF
│   ├── robot_geometry/     # Fitting, ICP
│   ├── kinematics/         # Velocity transforms
│   ├── utilities/          # Math tools
│   └── data/               # Data I/O
├── src/                    # Implementations
├── examples/               # Demo programs
├── tests/                  # Unit tests
├── data/                   # Sample datasets
├── scripts/                # Visualization scripts
└── imgs/                   # Generated images

License

MIT License

Acknowledgments

Based on the Python MathematicalRobotics library.