parameters: (result)
docstring: VisualISAMPlot plots current state of ISAM2 object
      Author: Ellon Paiva
      Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
docstring_summary: VisualISAMPlot plots current state of ISAM2 object
      Author: Ellon Paiva
      Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
docstring_tokens: VisualISAMPlot plots current state of ISAM2 object Author : Ellon Paiva Based on MATLAB version by : Duy Nguyen Ta and Frank Dellaert
function: def visual_ISAM2_plot(result):
      """
      VisualISAMPlot plots current state of ISAM2 object
      Author: Ellon Paiva
      Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
      """
  
      # Declare an id for the figure
      fignum = 0
  
      fig = plt.figure(fignum)
      axes = fig.gca(projection='3d')
      plt.cla()
  
      # Plot points
      # Can't use data because current frame might not see all points
      # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
      # gtsam.plot_3d_points(result, [], marginals)
      gtsam_plot.plot_3d_points(fignum, result, 'rx')
  
      # Plot cameras
      i = 0
      while result.exists(X(i)):
          pose_i = result.atPose3(X(i))
          gtsam_plot.plot_pose3(fignum, pose_i, 10)
          i += 1
  
      # draw
      axes.set_xlim3d(-40, 40)
      axes.set_ylim3d(-40, 40)
      axes.set_zlim3d(-40, 40)
      plt.pause(1)
function_tokens: def visual_ISAM2_plot ( result ) : # Declare an id for the figure fignum = 0 fig = plt . figure ( fignum ) axes = fig . gca ( projection = '3d' ) plt . cla ( ) # Plot points # Can't use data because current frame might not see all points # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate()) # gtsam.plot_3d_points(result, [], marginals) gtsam_plot . plot_3d_points ( fignum , result , 'rx' ) # Plot cameras i = 0 while result . exists ( X ( i ) ) : pose_i = result . atPose3 ( X ( i ) ) gtsam_plot . plot_pose3 ( fignum , pose_i , 10 ) i += 1 # draw axes . set_xlim3d ( - 40 , 40 ) axes . set_ylim3d ( - 40 , 40 ) axes . set_zlim3d ( - 40 , 40 ) plt . pause ( 1 )