Trajectory saving

[anathonic]

Title

Add TrajectoryWriter Class for Saving Trajectories in Multiple Formats

Description

This pull request introduces a new TrajectoryWriter class to facilitate saving robot trajectories in three different formats: TUM (The Open Mapping format), KITTI (KITTI Odometry format), and EuRoC (EuRoC MAV format). It also updates the config.ini file to include new settings for configuring trajectory saving options and integrates this functionality into the main SLAM script.

Changes Introduced:

• Added the TrajectoryWriter class to handle saving trajectories in the following formats:

  • TUM (The Open Mapping format)
  • KITTI (KITTI Odometry format)
  • EuRoC (EuRoC MAV format)

• Updated config.ini with new settings under the [SAVE_TRAJECTORY] section:

  [SAVE_TRAJECTORY]
  save_trajectory=True
  format_type=tum
  filename=kitti00_trajectory.txt
  
  

##Testing Confirmation:

The TrajectoryWriter class has been tested with both video and folder types. The EuRoC (MAV0) dataset and the KITTI (00) dataset. All tests confirmed successful trajectory saving and accurate comparisons. For a comparison of trajectories collected from pySLAM and ORB-SLAM3, including the EuRoC MAV0 dataset, see the trajectory comparison notebook.