A robust ROS node for high-speed, jitter-free visual-inertial pose estimation. This package fuses low-frequency planar ArUco detections (~30Hz) with high-frequency IMU data (1000Hz) using a 15-DOF Error-State Kalman Filter (ESKF) and on-manifold IMU preintegration.
By tightly coupling the vision and inertial data, this estimator eliminates the "sawtooth" jitter typically associated with pure solvePnP tracking, producing a buttery-smooth TF tree ideal for real-time control.
- 15-DOF ESKF: Estimates position, velocity, orientation, and IMU biases (
ba,bg). - IMU Preintegration: Analytically integrates 1000Hz IMU data between camera frames with exact bias Jacobians.
- Zero-Order Hold Sync: Robust handling of camera-IMU time shifts to prevent noise extrapolation.
- Fractal ArUco Support: Leverages nested fractal markers for continuous tracking across diverse distances.
- Fisheye & Rectified Modes: Supports direct detection on raw distorted fisheye images or pre-rectified spaces.
- VIO Alignment: Optional subscription to global VIO (e.g., OpenVINS) to broadcast the marker in a globally consistent world frame.
- ROS (Tested on Noetic/Melodic)
- OpenCV (with Fisheye module)
- Eigen3
arucolibrary (compiled with Fractal detector support)
/imsee/image/left(sensor_msgs/Image): Mono8 camera feed./imsee/imu(sensor_msgs/Imu): High-frequency IMU stream./ov_msckf/poseimu(geometry_msgs/PoseWithCovarianceStamped): Optional OpenVINS pose for global frame alignment.
/fractal/pose(geometry_msgs/PoseStamped): Estimated pose in the local camera frame./fractal/pose_world(geometry_msgs/PoseStamped): Estimated pose aligned to the global VIO frame./fractal/debug_image/compressed(sensor_msgs/CompressedImage): HUD overlay showing tracked markers, distance, ESKF state, and covariance bounds.- TF Broadcaster: Broadcasts the transform from
camera_frametofractal_marker.
Loaded dynamically via camera_config YAML or overridden via ROS params:
detect_input_mode(string, default:"fisheye"): Set to"rectified"to undistort the image before detection.timeshift_cam_imu(double): Time offset between camera and IMU.aruco_interval(int, default:3): Throttle vision detection to save CPU (relies on IMU dead-reckoning for skipped frames).meas_sigma_t/meas_sigma_r: Measurement noise trust for the vision-based translation and rotation.
Ensure your camera.yaml is properly calibrated, particularly the T_cam_imu extrinsics and timeshift_cam_imu. The filter relies heavily on accurate IMU noise spectral densities (sigma_a, sigma_g, sigma_ba, sigma_bg) to balance the trust between the vision and inertial states.