12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix bugs

release/4.3a0
Varun Agrawal 2025-02-08 23:21:15 -05:00
parent 9efd9fc7b1
commit 425a5c692e
1 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
python/gtsam/examples/HybridCity10000.py
View File

@ -25,7 +25,7 @@ from gtsam import (BetweenFactorPose2, HybridNonlinearFactor,
def parse_arguments(): def parse_arguments():
"""Parse command line arguments""" """Parse command line arguments"""
parser = argparse.ArgumentParser() parser = argparse.ArgumentParser()
parser.add_argument("data_file", parser.add_argument("--data_file",
help="The path to the City10000 data file", help="The path to the City10000 data file",
default="T1_city10000_04.txt") default="T1_city10000_04.txt")
return parser.parse_args() return parser.parse_args()
@ -49,11 +49,13 @@ class City10000Dataset:
def __init__(self, filename): def __init__(self, filename):
self.filename_ = filename self.filename_ = filename
try: try:
f = open(self.filename_, 'r') self.f_ = open(self.filename_, 'r')
f.close()
except OSError: except OSError:
print(f"Failed to open file: {self.filename_}") print(f"Failed to open file: {self.filename_}")
def __del__(self):
self.f_.close()
def read_line(self, line: str, delimiter: str = " "): def read_line(self, line: str, delimiter: str = " "):
"""Read a `line` from the dataset, separated by the `delimiter`.""" """Read a `line` from the dataset, separated by the `delimiter`."""
return line.split(delimiter) return line.split(delimiter)
@ -78,12 +80,11 @@ class City10000Dataset:
def next(self): def next(self):
"""Read and parse the next line.""" """Read and parse the next line."""
with open(self.filename_, 'w') as f: line = self.f_.readline()
line = f.readline() if line:
if line: return self.parse_line(line)
yield self.parse_line(line) else:
else: return None, None
yield None, None
class Experiment: class Experiment:
@ -173,7 +174,7 @@ class Experiment:
# Set up initial prior # Set up initial prior
priorPose = Pose2(0, 0, 0) priorPose = Pose2(0, 0, 0)
self.self.initial_.insert(X(0), priorPose) self.initial_.insert(X(0), priorPose)
self.new_factors_.push_back( self.new_factors_.push_back(
PriorFactorPose2(X(0), priorPose, prior_noise_model)) PriorFactorPose2(X(0), priorPose, prior_noise_model))
@ -207,13 +208,13 @@ class Experiment:
m = (M(discrete_count), num_measurements) m = (M(discrete_count), num_measurements)
mixture_factor = self.hybrid_odometry_factor( mixture_factor = self.hybrid_odometry_factor(
key_s, key_t, m, pose_array) key_s, key_t, m, pose_array)
self.new_factors_.append(mixture_factor) self.new_factors_.push_back(mixture_factor)
discrete_count += 1 discrete_count += 1
number_of_hybrid_factors += 1 number_of_hybrid_factors += 1
print(f"mixture_factor: {key_s} {key_t}") print(f"mixture_factor: {key_s} {key_t}")
else: else:
self.new_factors_.add( self.new_factors_.push_back(
BetweenFactorPose2(X(key_s), X(key_t), odom_pose, BetweenFactorPose2(X(key_s), X(key_t), odom_pose,
pose_noise_model)) pose_noise_model))
@ -228,7 +229,7 @@ class Experiment:
# print loop closure event keys: # print loop closure event keys:
print(f"Loop closure: {key_s} {key_t}") print(f"Loop closure: {key_s} {key_t}")
self.new_factors_.add(loop_factor) self.new_factors_.push_back(loop_factor)
number_of_hybrid_factors += 1 number_of_hybrid_factors += 1
loop_count += 1 loop_count += 1