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

Merge pull request #2027 from borglab/city10000-py 

Improvements to HybridCity10000 python script
release/4.3a0
Varun Agrawal 2025-03-05 09:48:24 -05:00 committed by GitHub
parent c6a22301b7 ce6b1461e1
commit a89b88c2a0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 214 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/discrete/DiscreteValues.cpp
View File

@ -145,6 +145,11 @@ string DiscreteValues::html(const KeyFormatter& keyFormatter,
}
/* ************************************************************************ */
void PrintDiscreteValues(const DiscreteValues& values, const std::string& s,
const KeyFormatter& keyFormatter) {
values.print(s, keyFormatter);
}
string markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter,
const DiscreteValues::Names& names) {
return values.markdown(keyFormatter, names);

5
gtsam/discrete/DiscreteValues.h
View File

@ -188,6 +188,11 @@ inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
return DiscreteValues::CartesianProduct(keys);
}
/// Free version of print for wrapper
void GTSAM_EXPORT
PrintDiscreteValues(const DiscreteValues& values, const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/// Free version of markdown.
std::string GTSAM_EXPORT
markdown(const DiscreteValues& values,

11
gtsam/discrete/discrete.i
View File

@ -22,6 +22,11 @@ class DiscreteKeys {
// DiscreteValues is added in specializations/discrete.h as a std::map
std::vector<gtsam::DiscreteValues> cartesianProduct(
const gtsam::DiscreteKeys& keys);
void PrintDiscreteValues(
const gtsam::DiscreteValues& values, const std::string& s = "",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
string markdown(
const gtsam::DiscreteValues& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
@ -472,9 +477,9 @@ class DiscreteSearchSolution {
};
class DiscreteSearch {
static DiscreteSearch FromFactorGraph(const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::Ordering& ordering,
bool buildJunctionTree = false);
static gtsam::DiscreteSearch FromFactorGraph(
const gtsam::DiscreteFactorGraph& factorGraph,
const gtsam::Ordering& ordering, bool buildJunctionTree = false);
DiscreteSearch(const gtsam::DiscreteEliminationTree& etree);
DiscreteSearch(const gtsam::DiscreteJunctionTree& junctionTree);

4
gtsam/hybrid/hybrid.i
View File

@ -152,7 +152,11 @@ class HybridBayesNet {
gtsam::HybridGaussianFactorGraph toFactorGraph(
const gtsam::VectorValues& measurements) const;
gtsam::GaussianBayesNet choose(const gtsam::DiscreteValues& assignment) const;
gtsam::HybridValues optimize() const;
gtsam::VectorValues optimize(const gtsam::DiscreteValues& assignment) const;
gtsam::HybridValues sample(const gtsam::HybridValues& given) const;
gtsam::HybridValues sample() const;

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

@ -15,6 +15,7 @@ import time
import numpy as np
from gtsam.symbol_shorthand import L, M, X
from matplotlib import pyplot as plt
import gtsam
from gtsam import (BetweenFactorPose2, HybridNonlinearFactor,
@ -28,6 +29,30 @@ def parse_arguments():
parser.add_argument("--data_file",
help="The path to the City10000 data file",
default="T1_city10000_04.txt")
parser.add_argument(
"--max_loop_count",
"-l",
type=int,
default=10000,
help="The maximum number of loops to run over the dataset")
parser.add_argument(
"--update_frequency",
"-u",
type=int,
default=3,
help="After how many steps to run the smoother update.")
parser.add_argument(
"--max_num_hypotheses",
"-m",
type=int,
default=10,
help="The maximum number of hypotheses to keep at any time.")
parser.add_argument(
"--plot_hypotheses",
"-p",
action="store_true",
help="Plot all hypotheses. NOTE: This is exponential, use with caution."
)
return parser.parse_args()
@ -39,7 +64,7 @@ prior_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([0.0001, 0.0001, 0.0001]))
pose_noise_model = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0]))
np.asarray([1.0 / 20.0, 1.0 / 20.0, 1.0 / 100.0]))
pose_noise_constant = pose_noise_model.negLogConstant()
@ -60,13 +85,16 @@ class City10000Dataset:
"""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]]:
def parse_line(self,
line: str) -> tuple[list[Pose2], tuple[int, int], bool]:
"""Parse line from file"""
parts = self.read_line(line)
key_s = int(parts[1])
key_t = int(parts[3])
is_ambiguous_loop = bool(int(parts[4]))
num_measurements = int(parts[5])
pose_array = [Pose2()] * num_measurements
@ -76,7 +104,7 @@ class City10000Dataset:
rad = float(parts[8 + 3 * i])
pose_array[i] = Pose2(x, y, rad)
return pose_array, (key_s, key_t)
return pose_array, (key_s, key_t), is_ambiguous_loop
def next(self):
"""Read and parse the next line."""
@ -84,7 +112,67 @@ class City10000Dataset:
if line:
return self.parse_line(line)
else:
return None, None
return None, None, None
def plot_all_results(ground_truth,
all_results,
iters=0,
estimate_color=(0.1, 0.1, 0.9, 0.4),
estimate_label="Hybrid Factor Graphs",
text="",
filename="city10000_results.svg"):
"""Plot the City10000 estimates against the ground truth.
Args:
ground_truth: The ground truth trajectory as xy values.
all_results (List[Tuple(np.ndarray, str)]): All the estimates trajectory as xy values,
as well as assginment strings.
estimate_color (tuple, optional): The color to use for the graph of estimates.
Defaults to (0.1, 0.1, 0.9, 0.4).
estimate_label (str, optional): Label for the estimates, used in the legend.
Defaults to "Hybrid Factor Graphs".
"""
if len(all_results) == 1:
fig, axes = plt.subplots(1, 1)
axes = [axes]
else:
fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2)
axes = axes.flatten()
for i, (estimates, s, prob) in enumerate(all_results):
ax = axes[i]
ax.axis('equal')
ax.axis((-75.0, 100.0, -75.0, 75.0))
gt = ground_truth[:estimates.shape[0]]
ax.plot(gt[:, 0],
gt[:, 1],
'--',
linewidth=1,
color=(0.1, 0.7, 0.1, 0.5),
label="Ground Truth")
ax.plot(estimates[:, 0],
estimates[:, 1],
'-',
linewidth=1,
color=estimate_color,
label=estimate_label)
# ax.legend()
ax.set_title(f"P={prob:.3f}\n{s}", fontdict={'fontsize': 10})
fig.suptitle(f"After {iters} iterations")
num_chunks = int(np.ceil(len(text) / 90))
text = "\n".join(text[i * 60:(i + 1) * 60] for i in range(num_chunks))
fig.text(0.5,
0.015,
s=text,
wrap=True,
horizontalalignment='center',
fontsize=12)
fig.savefig(filename, format="svg")
class Experiment:
@ -93,10 +181,11 @@ class Experiment:
def __init__(self,
filename: str,
marginal_threshold: float = 0.9999,
max_loop_count: int = 8000,
max_loop_count: int = 150,
update_frequency: int = 3,
max_num_hypotheses: int = 10,
relinearization_frequency: int = 10):
relinearization_frequency: int = 10,
plot_hypotheses: bool = False):
self.dataset_ = City10000Dataset(filename)
self.max_loop_count = max_loop_count
self.update_frequency = update_frequency
@ -108,6 +197,8 @@ class Experiment:
self.all_factors_ = HybridNonlinearFactorGraph()
self.initial_ = Values()
self.plot_hypotheses = plot_hypotheses
def hybrid_loop_closure_factor(self, loop_counter, key_s, key_t,
measurement: Pose2):
"""
@ -147,7 +238,7 @@ class Experiment:
after_update = time.time()
return after_update - before_update
def reInitialize(self) -> float:
def reinitialize(self) -> float:
"""Re-linearize, solve ALL, and re-initialize smoother."""
print(f"================= Re-Initialize: {self.all_factors_.size()}")
before_update = time.time()
@ -191,7 +282,7 @@ class Experiment:
start_time = time.time()
while index < self.max_loop_count:
pose_array, keys = self.dataset_.next()
pose_array, keys, is_ambiguous_loop = self.dataset_.next()
if pose_array is None:
break
key_s = keys[0]
@ -200,6 +291,7 @@ class Experiment:
num_measurements = len(pose_array)
# Take the first one as the initial estimate
# odom_pose = pose_array[np.random.choice(num_measurements)]
odom_pose = pose_array[0]
if key_s == key_t - 1:
# Odometry factor
@ -224,8 +316,14 @@ class Experiment:
self.initial_.atPose2(X(key_s)) * odom_pose)
else:
# Loop closure
loop_factor = self.hybrid_loop_closure_factor(
loop_count, key_s, key_t, odom_pose)
if is_ambiguous_loop:
loop_factor = self.hybrid_loop_closure_factor(
loop_count, key_s, key_t, odom_pose)
else:
loop_factor = BetweenFactorPose2(X(key_s), X(key_t),
odom_pose,
pose_noise_model)
# print loop closure event keys:
print(f"Loop closure: {key_s} {key_t}")
@ -240,7 +338,7 @@ class Experiment:
update_count += 1
if update_count % self.relinearization_frequency == 0:
self.reInitialize()
self.reinitialize()
# Record timing for odometry edges only
if key_s == key_t - 1:
@ -271,8 +369,85 @@ class Experiment:
total_time = end_time - start_time
print(f"Total time: {total_time} seconds")
# self.save_results(result, key_t + 1, time_list)
if self.plot_hypotheses:
# Get all the discrete values
discrete_keys = gtsam.DiscreteKeys()
for key in delta.discrete().keys():
# TODO Get cardinality from DiscreteFactor
discrete_keys.push_back((key, 2))
print("plotting all hypotheses")
self.plot_all_hypotheses(discrete_keys, key_t + 1, index)
def plot_all_hypotheses(self, discrete_keys, num_poses, num_iters=0):
"""Plot all possible hypotheses."""
# Get ground truth
gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"),
delimiter=" ")
dkeys = gtsam.DiscreteKeys()
for i in range(discrete_keys.size()):
key, cardinality = discrete_keys.at(i)
if key not in self.smoother_.fixedValues().keys():
dkeys.push_back((key, cardinality))
fixed_values_str = " ".join(
f"{gtsam.DefaultKeyFormatter(k)}:{v}"
for k, v in self.smoother_.fixedValues().items())
all_assignments = gtsam.cartesianProduct(dkeys)
all_results = []
for assignment in all_assignments:
result = gtsam.Values()
gbn = self.smoother_.hybridBayesNet().choose(assignment)
# Check to see if the GBN has any nullptrs, if it does it is null overall
is_invalid_gbn = False
for i in range(gbn.size()):
if gbn.at(i) is None:
is_invalid_gbn = True
break
if is_invalid_gbn:
continue
delta = self.smoother_.hybridBayesNet().optimize(assignment)
result.insert_or_assign(self.initial_.retract(delta))
poses = np.zeros((num_poses, 3))
for i in range(num_poses):
pose = result.atPose2(X(i))
poses[i] = np.asarray((pose.x(), pose.y(), pose.theta()))
assignment_string = " ".join([
f"{gtsam.DefaultKeyFormatter(k)}={v}"
for k, v in assignment.items()
])
conditional = self.smoother_.hybridBayesNet().at(
self.smoother_.hybridBayesNet().size() - 1).asDiscrete()
discrete_values = self.smoother_.fixedValues()
for k, v in assignment.items():
discrete_values[k] = v
if conditional is None:
probability = 1.0
else:
probability = conditional.evaluate(discrete_values)
all_results.append((poses, assignment_string, probability))
plot_all_results(gt,
all_results,
iters=num_iters,
text=fixed_values_str,
filename=f"city10000_results_{num_iters}.svg")
def save_results(self, result, final_key, time_list):
"""Save results to file."""
# Write results to file
self.write_result(result, key_t + 1, "Hybrid_City10000.txt")
self.write_result(result, final_key, "Hybrid_City10000.txt")
# Write timing info to file
self.write_timing_info(time_list=time_list)
@ -312,7 +487,11 @@ def main():
"""Main runner"""
args = parse_arguments()
experiment = Experiment(gtsam.findExampleDataFile(args.data_file))
experiment = Experiment(gtsam.findExampleDataFile(args.data_file),
max_loop_count=args.max_loop_count,
update_frequency=args.update_frequency,
max_num_hypotheses=args.max_num_hypotheses,
plot_hypotheses=args.plot_hypotheses)
experiment.run()