Mouse Position Tracking: Kalman Filter vs Factor Graph

A comparative implementation of real-time mouse cursor tracking using two state estimation approaches: traditional Kalman Filter and GTSAM Factor Graphs.

🎯 Overview

This repository demonstrates the fundamental differences between sequential filtering (Kalman Filter) and batch optimization (Factor Graphs) for state estimation in robotics and computer vision. Using mouse cursor tracking as an intuitive example, we show how both methods handle noisy measurements and motion prediction.

Why Mouse Tracking?

• Visual & Interactive: Real-time feedback makes concepts easy to understand
• Noisy Measurements: Simulated sensor noise demonstrates filtering effectiveness
• Nonlinear Motion: Human mouse movement is unpredictable, testing model robustness
• Low Barrier: No special hardware needed - just move your mouse!

🔬 What's Inside

Two Implementations

1. Kalman Filter (kalman_filter_tracker.cpp)

   • Classic sequential Bayesian filtering
   • Predict-update cycle at each timestep
   • Uses OpenCV's cv::KalmanFilter
   • Fast, memory-efficient, real-time
   • All the credit for this filter implementation is for Robotics with ROS

2. Factor Graph (factor_graph_tracker.cpp)

   • Graph-based batch optimization
   • Uses GTSAM's ISAM2 incremental solver
   • Custom factors for motion model and measurements

Key Features

• Constant Velocity Motion Model: Assumes smooth motion between timesteps
• Noisy Measurements: Gaussian noise added to simulate sensor uncertainty
• Real-time Visualization: Three trajectories displayed simultaneously:
  • 🟢 Green: Ground truth (actual mouse position)
  • 🔵 Blue: Estimated trajectory (filtered/optimized)
  • 🟡 Yellow: Noisy measurements (simulated sensor)
• CSV Export: Save trajectories for analysis

📊 State Space Model

Both implementations use the same state representation:

State Vector:

x = [px, py, vx, vy]ᵀ

• px, py: Position in 2D space
• vx, vy: Velocity components

Motion Model (Constant Velocity):

x_k = F * x_{k-1} + w
F = [1  0  dt  0 ]
    [0  1  0   dt]
    [0  0  1   0 ]
    [0  0  0   1 ]

where dt = 0.05s (20 Hz update rate)

Measurement Model:

z_k = H * x_k + v
H = [1  0  0  0]
    [0  1  0  0]

Only position is measured (not velocity)

🚀 Getting Started

Prerequisites

1. Opencv
2. GTSAM
3. Boost
4. Eigen

Usage

1. Launch either tracker
2. Move your mouse inside the window
3. Watch the three trajectories in real-time
4. Press q or ESC to quit
5. Results automatically saved to CSV files

🔧 Tuning Parameters

Both implementations expose key parameters for experimentation:

Noise Parameters

const double process_noise_sigma = 10.0;      // Motion model uncertainty
const double measurement_noise_sigma = 5.0;   // Sensor noise

💡 Key Insight: Higher process noise → More responsive to measurements

• Low process noise: Trusts motion model (smooth but laggy)
• High process noise: Trusts measurements (responsive but jittery)

ISAM2 Parameters (Factor Graph only)

parameters.relinearizeThreshold = 0.01;  // Only relinearize variables whose linear delta > 0.01
parameters.relinearizeSkip = 10;         // Relinearize every N updates

🧮 Implementation Details

Custom GTSAM Factors

Motion Model Factor:

class MotionModelFactor : public NoiseModelFactor2<Vector4, Vector4> {
    // Encodes: x_k = F * x_{k-1} + w
    // Provides Jacobians for optimization
};

Measurement Factor:

class MeasurementFactor : public NoiseModelFactor1<Vector4> {
    // Encodes: z = H * x + v
    // Only observes position, not velocity
};

Error Function Convention

⚠️ Important: GTSAM minimizes errors, so factors return:

error = predicted - measured  // NOT measured - predicted!

📚 Educational Value

This repo is ideal for:

• Students learning Kalman Filtering and SLAM
• Roboticists comparing estimation frameworks
• Researchers prototyping sensor fusion algorithms
• Developers exploring GTSAM's capabilities

Concepts Demonstrated

1. Bayesian Filtering: Recursive state estimation
2. Graphical Models: Factor graphs for optimization
3. Sensor Fusion: Combining predictions with measurements
4. Noise Modeling: Process vs measurement uncertainty
5. Incremental Optimization: ISAM2's efficiency tricks

📖 References

Kalman Filtering

• Thrun, S., Burgard, W., & Fox, D. (2005). "Probabilistic Robotics"
• Autonome Intelligente Systeme

Factor Graphs & GTSAM

• Dellaert, F., & Kaess, M. (2017). "Factor Graphs for Robot Perception"
• Kaess, M., et al. (2012). "iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree"
• GTSAM Documentation
• Factor Graphs for Navigation Applications: A Tutorial