implement City10000Dataset class

python/gtsam/examples/HybridCity10000.py

@@ -11,10 +11,15 @@ Author: Varun Agrawal
 """
 
 import argparse
+import time
 
 import numpy as np
+from gtsam.symbol_shorthand import L, M, X
 
 import gtsam
+from gtsam import (BetweenFactorPose2, HybridNonlinearFactor,
+                   HybridNonlinearFactorGraph, HybridSmoother, HybridValues,
+                   Pose2, PriorFactorPose2, Values)
 
 
 def parse_arguments():
@@ -38,6 +43,49 @@ pose_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
 pose_noise_constant = pose_noise_model.negLogConstant()
 
 
+class City10000Dataset:
+    """Class representing the City10000 dataset."""
+
+    def __init__(self, filename):
+        self.filename_ = filename
+        try:
+            f = open(self.filename_, 'r')
+            f.close()
+        except OSError:
+            print(f"Failed to open file: {self.filename_}")
+
+    def read_line(self, line: str, delimiter: str = " "):
+        """Read a `line` from the dataset, separated by the `delimiter`."""
+        return line.split(delimiter)
+
+    def parse_line(self, line: str) -> tuple[list[Pose2], tuple[int, int]]:
+        """Parse line from file"""
+        parts = self.read_line(line)
+
+        key_s = int(parts[1])
+        key_t = int(parts[3])
+
+        num_measurements = int(parts[5])
+        pose_array = [Pose2()] * num_measurements
+
+        for i in range(num_measurements):
+            x = float(parts[6 + 3 * i])
+            y = float(parts[7 + 3 * i])
+            rad = float(parts[8 + 3 * i])
+            pose_array[i] = Pose2(x, y, rad)
+
+        return pose_array, (key_s, key_t)
+
+    def next(self):
+        """Read and parse the next line."""
+        with open(self.filename_, 'w') as f:
+            line = f.readline()
+            if line:
+                yield self.parse_line(line)
+            else:
+                yield None, None
+
+
 def main():
     """Main runner"""
     args = parse_arguments()