generate a single plot with subplots
							parent
							
								
									ebfb94c4fd
								
							
						
					
					
						commit
						6919bae1b7
					
				|  | @ -112,45 +112,44 @@ class City10000Dataset: | |||
|             return None, None | ||||
| 
 | ||||
| 
 | ||||
| def plot_estimates(gt, | ||||
|                    estimates, | ||||
|                    fignum: int, | ||||
|                    estimate_color=(0.1, 0.1, 0.9, 0.4), | ||||
|                    estimate_label="Hybrid Factor Graphs", | ||||
|                    text="graph"): | ||||
| def plot_all_results(ground_truth, | ||||
|                      all_results, | ||||
|                      estimate_color=(0.1, 0.1, 0.9, 0.4), | ||||
|                      estimate_label="Hybrid Factor Graphs"): | ||||
|     """Plot the City10000 estimates against the ground truth. | ||||
| 
 | ||||
|     Args: | ||||
|         estimates (np.ndarray): The estimates trajectory as xy values. | ||||
|         fignum (int): The figure number for multiple plots. | ||||
|         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". | ||||
|     """ | ||||
|     fig = plt.figure(fignum) | ||||
|     ax = fig.gca() | ||||
|     ax.axis('equal') | ||||
|     ax.axis((-75.0, 75.0, -75.0, 75.0)) | ||||
|     print(len(all_results)) | ||||
|     fig, axes = plt.subplots(int(np.ceil(len(all_results) / 2)), 2) | ||||
|     for i, (estimates, text) in enumerate(all_results): | ||||
|         ax = axes[i] | ||||
|         ax.axis('equal') | ||||
|         ax.axis((-75.0, 75.0, -75.0, 75.0)) | ||||
| 
 | ||||
|     gt = gt[: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() | ||||
|     fig.text(0.1, 0.03, text) | ||||
|         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.text(0.0, 100.0, text) | ||||
| 
 | ||||
|     filename = f"city10000_{text.replace(' ', '_')}.svg" | ||||
|     fig.savefig(filename, format="svg") | ||||
|     fig.savefig("city10000_results.svg", format="svg") | ||||
| 
 | ||||
| 
 | ||||
| class Experiment: | ||||
|  | @ -269,7 +268,8 @@ 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[np.random.choice(num_measurements)] | ||||
|             odom_pose = pose_array[0] | ||||
|             if key_s == key_t - 1: | ||||
|                 # Odometry factor | ||||
|                 if num_measurements > 1: | ||||
|  | @ -358,14 +358,16 @@ class Experiment: | |||
|         gt = np.loadtxt(gtsam.findExampleDataFile("ISAM2_GT_city10000.txt"), | ||||
|                         delimiter=" ") | ||||
| 
 | ||||
|         # Get all possible assignments | ||||
|         if discrete_keys.size() > 5: | ||||
|             print("Too many discrete keys to plot all hypotheses. Exiting.") | ||||
|             exit(0) | ||||
|         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)) | ||||
| 
 | ||||
|         all_assignments = gtsam.cartesianProduct(discrete_keys) | ||||
|         all_assignments = gtsam.cartesianProduct(dkeys) | ||||
| 
 | ||||
|         for idx, assignment in enumerate(all_assignments): | ||||
|         all_results = [] | ||||
|         for assignment in all_assignments: | ||||
|             result = gtsam.Values() | ||||
|             gbn = self.smoother_.hybridBayesNet().choose(assignment) | ||||
| 
 | ||||
|  | @ -381,21 +383,18 @@ class Experiment: | |||
|             delta = self.smoother_.hybridBayesNet().optimize(assignment) | ||||
|             result.insert_or_assign(self.initial_.retract(delta)) | ||||
| 
 | ||||
|             poses = [] | ||||
|             poses = np.zeros((num_poses, 3)) | ||||
|             for i in range(num_poses): | ||||
|                 pose = result.atPose2(X(i)) | ||||
|                 poses.append((pose.x(), pose.y(), pose.theta())) | ||||
|             poses = np.asarray(poses) | ||||
|                 poses[i] = np.asarray((pose.x(), pose.y(), pose.theta())) | ||||
| 
 | ||||
|             assignment_string = " ".join([ | ||||
|                 f"{gtsam.DefaultKeyFormatter(k)}={v}" | ||||
|                 for k, v in assignment.items() | ||||
|             ]) | ||||
|             all_results.append((poses, assignment_string)) | ||||
| 
 | ||||
|             plot_estimates(gt, | ||||
|                            estimates=poses, | ||||
|                            fignum=idx, | ||||
|                            text=assignment_string) | ||||
|         plot_all_results(gt, all_results) | ||||
| 
 | ||||
|     def save_results(self, result, final_key, time_list): | ||||
|         """Save results to file.""" | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue