Mathematical Robotics (C++)

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

Features

• Lie Groups: SO(2), SO(3), SE(2), SE(3) operations
• Optimization: Gauss-Newton with robust kernels
• SLAM: Bundle adjustment, IMU preintegration
• Geometry: Point-line-plane fitting, ICP matching
• Filtering: Extended Kalman Filter
• Header-only library with Eigen3

Build

sudo apt-get install build-essential cmake libeigen3-dev
mkdir build && cd build
cmake .. && make -j$(nproc)

Usage

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

// SO(3) operations
Eigen::Vector3d omega(0.1, 0.2, 0.3);
Eigen::Matrix3d R = SO3::exp(omega);

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

Examples

cd build/examples
./demo_point_line_matching    # Point-line-plane fitting
./demo_gauss_newton          # Circle fitting optimization
./demo_imu_preintegration    # IMU processing for SLAM
./demo_slam_bundle_adjustment # Bundle adjustment

Testing

cd build && make test