Add GraphSLAM data collection loop and full linearize/reduce/solve pi…

[jsouvenir]

Adds a graphSLAM Webots controller (graphSLAM.py) that teleports the robot through rotate/move cycles while collecting synchronized odometry, landmark range-bearing, and ground-truth logs. Data collection helpers (get_odometry, get_landmark_row, rotate_step, move_forward_step) live in graphSLAM_data_collection.py, including a slew-rate limiter on rotate_step that fixes a spurious odometry velocity spike at the start of each rotation. navigation_tools.py holds shared geometry helpers split out of my_robot.py.

Implements the full offline GraphSLAM pipeline in graph_slam_calculations.py:

• graphSLAM_init: dead-reckons an initial pose trajectory and seeds landmark estimates from first sightings
• graphSLAM_linearize: builds the information matrix/vector for one Gauss-Newton correction from motion and measurement edges
• graphSLAM_reduce: eliminates landmark variables via Schur complement
• graphSLAM_solve: solves the reduced system and back-substitutes landmark corrections
• graphSLAM_run: iterates linearize/reduce/solve to convergence
• Accuracy/diagnostic helpers (graphSLAM_accuracy, print_graphSLAM_accuracy, print_landmark_triangulation_check, print_landmark_observation_spread, print_worst_pose_errors) for comparing results against ground truth

Each stage was verified against a synthetic scenario with known ground truth: linearize/reduce/solve match a full-system solve to floating-point precision, and graphSLAM_run converges landmark/pose estimates toward true positions.

Known limitation: on real Webots runs, GraphSLAM currently underperforms dead reckoning (~-31% on the current path/cycle configuration). Root cause diagnosed as insufficient landmark observability -- stretches of the path where fewer than two landmarks are simultaneously visible, or where the visible landmark(s) are seen across a narrow angular window, leave individual poses underdetermined even though the overall landmark map converges well. Not an implementation bug: linearize/reduce/solve were verified independently, and the odometry-spike issue found during investigation was fixed separately. Remaining fix requires path/rotation schedule or camera FOV changes, not optimization changes.