diff --git a/python/gtsam/examples/HybridCity10000.py b/python/gtsam/examples/HybridCity10000.py index 18e05125f..3f70e3c49 100644 --- a/python/gtsam/examples/HybridCity10000.py +++ b/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()