function to plot trajectory
							parent
							
								
									26d6cb3d6e
								
							
						
					
					
						commit
						b3a9c7a5ef
					
				|  | @ -188,3 +188,46 @@ def plot_pose3(fignum, pose, P, axis_length=0.1): | |||
|     axes = fig.gca(projection='3d') | ||||
|     plot_pose3_on_axes(axes, pose, P=P, axis_length=axis_length) | ||||
| 
 | ||||
| def plot_trajectory(fignum, values, scale=1, marginals=None): | ||||
|     pose3Values = gtsam.allPose3s(values) | ||||
|     keys = gtsam.KeyVector(pose3Values.keys()) | ||||
|     lastIndex = None | ||||
| 
 | ||||
|     for i in range(keys.size()): | ||||
|         key = keys.at(i) | ||||
|         try: | ||||
|             pose = pose3Values.atPose3(key) | ||||
|         except: | ||||
|             print("Warning: no Pose3 at key: {0}".format(key)) | ||||
| 
 | ||||
|         if lastIndex is not None: | ||||
|             lastKey = keys.at(lastIndex) | ||||
|             try: | ||||
|                 lastPose = pose3Values.atPose3(lastKey) | ||||
|             except: | ||||
|                 print("Warning: no Pose3 at key: {0}".format(lastKey)) | ||||
|                 pass | ||||
| 
 | ||||
|             if marginals: | ||||
|                 P = marginals.marginalCovariance(lastKey) | ||||
|             else: | ||||
|                 P = None | ||||
| 
 | ||||
|             plot_pose3(fignum, lastPose, P, scale) | ||||
| 
 | ||||
|         lastIndex = i | ||||
| 
 | ||||
|     # Draw final pose | ||||
|     if lastIndex is not None: | ||||
|         lastKey = keys.at(lastIndex) | ||||
|         try: | ||||
|             lastPose = pose3Values.atPose3(lastKey) | ||||
|             if marginals: | ||||
|                 P = marginals.marginalCovariance(lastKey) | ||||
|             else: | ||||
|                 P = None | ||||
| 
 | ||||
|             plot_pose3(fignum, lastPose, P, scale) | ||||
| 
 | ||||
|         except: | ||||
|             pass | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue