SLAM Module
===========

The SLAM (Simultaneous Localization and Mapping) module estimates the vehicle's pose (position and orientation) while simultaneously building a map of cone landmarks on the track. This module is critical for autonomous navigation, providing both the vehicle state and track map to downstream planning and control systems.

Overview
--------

Our system implements GraphSLAM, a full SLAM approach that optimizes the entire trajectory and map jointly. We provide two distinct implementations that differ in how they handle sensor fusion:

1. **Early Stage Fusion**: Receives pre-fused cone observations from the perception module
2. **Late Stage Fusion (MWEKF)**: Receives separate camera and LiDAR observations and fuses them within SLAM

.. toctree::
   :maxdepth: 2
   :caption: SLAM Implementations

   slam/overview
   slam/early_stage_fusion
   slam/late_stage_fusion
   slam/configuration

Published Topics
----------------

- ``/slam/state`` (``feb_msgs/State``): Vehicle state (x, y, velocity, heading, lap_count)
- ``/slam/map/global`` (``feb_msgs/Map``): Complete cone map (published after first lap)
- ``/slam/map/local`` (``feb_msgs/Map``): Local subset of cones near vehicle
- ``/slam/conemap`` (``sensor_msgs/PointCloud``): Visualization of cone map (if enabled)
- ``/slam/pose`` (``geometry_msgs/PoseStamped``): Vehicle pose for RViz (if enabled)
- ``/slam/guessed_positions`` (``sensor_msgs/PointCloud``): Historical vehicle positions (if enabled)

Subscribed Topics
-----------------

**Early Stage Fusion:**

- ``/fusion/cones`` or ``/perception_cones`` (``eufs_msgs/ConeArrayWithCovariance`` or ``feb_msgs/ConesCartesian``): Fused cone observations
- ``/imu`` (``sensor_msgs/Imu``): Orientation and acceleration
- ``/odometry/wheelspeeds`` or ``/ground_truth/wheel_speeds`` (``Float64MultiArray`` or ``eufs_msgs/WheelSpeedsStamped``): Wheel speed measurements

**Late Stage Fusion (MWEKF):**

- ``/camera/cones`` (``feb_msgs/ConesCartesian``): Camera-only cone detections
- ``/lidar/cones`` (``feb_msgs/ConesCartesian``): LiDAR-only cone detections
- ``/imu`` (``sensor_msgs/Imu``): Orientation measurements
- ``/cmd`` (``ackermann_msgs/AckermannDriveStamped``): MPC control commands
- ``/odometry/wheelspeeds`` or ``/ground_truth/wheel_speeds``: Wheel speed measurements

API Reference
-------------

For detailed implementation documentation including class docstrings and method descriptions,
see the source code in ``slam_ws/graphslam_global/graphslam_global/``.

GraphSLAM Solver
~~~~~~~~~~~~~~~~

.. autoclass:: GraphSLAMSolve.GraphSLAMSolve
   :members:
   :undoc-members:
   :show-inheritance:

Utility Functions
~~~~~~~~~~~~~~~~~

.. automodule:: utility_functions
   :members:
   :undoc-members:
   :show-inheritance: