How to reach estimated X,Y,Z,Yaw,Roll and Pitch

[Sadegh-Kalami]

How to reach estimated X,Y,Z,Yaw,Roll and Pitch

[luigifreda]

did you check this https://github.com/luigifreda/pyslam/blob/master/slam.py#L705 ?

[hbonnavaud]

Wow it was the issue ... it was so simple after all thx a lot for your time. Sorry to bothered you.

[luigifreda]

Hi,
Thanks for the feedback.
Yes, you can access the current (at present time) estimates in that way (commented code
https://github.com/luigifreda/pyslam/blob/master/slam.py#L705 )
The poses are stored in the map frames and keyframes, which are continuously updated by BA.
If you access a frame/keyframe pose at a certain time during the processing, you'll get the estimate at that specific time (current estimate). Note that BA can reprocess the local map (including the keyframes and points around the frame/keyframe you considered) at any time later and readjust those estimates.
It depends on what you want to access and possibly evaluate: current (i.e. online) estimates or final estimates at the end of the trajectory.

Hope this helps.
Cheers,
Luigi

[hbonnavaud]

Hi,
I'm trying to get the estimate position without success.
I tried to plot map_state.spanning_tree[:, :3] but I don't get the same result that is plot in pangolin.
I also plot slam.tracking.predicted_pose.t but it don't shown anything similar to the trajectory ploted in pangolin.
I took a look to the commented code at https://github.com/luigifreda/pyslam/blob/master/slam.py#L700 , but I don't understood how it could help.

Can you help me? What did I missed?

[luigifreda]

Can you share your code?

[hbonnavaud]

Basically I put this block of code in the main_slam.py loop, and tryed to prind a bunch of variables, following what Viewer3D.py do.

if slam.map.num_frames() > 0:
  # Build map state
  
  ##############################################################
  # The block of code bellow is a copy of Viewer3D.py viewer_refresh().
  map = slam.map
  map_state = Viewer3DMapElement()
  
  if map.num_frames() > 0:
      map_state.cur_pose = map.get_frame(-1).Twc.copy()
  
  if slam.tracking.predicted_pose is not None:
      map_state.predicted_pose = slam.tracking.predicted_pose.inverse().matrix().copy()
  
  if slam.tracking.kf_ref is not None:
      reference_pose = slam.tracking.kf_ref.Twc.copy()
  
  num_map_keyframes = map.num_keyframes()
  keyframes = map.get_keyframes()
  if num_map_keyframes > 0:
      for kf in keyframes:
          map_state.poses.append(kf.Twc)
  map_state.poses = np.array(map_state.poses)
  
  num_map_points = map.num_points()
  if num_map_points > 0:
      for i, p in enumerate(map.get_points()):
          map_state.points.append(p.pt)
          map_state.colors.append(np.flip(p.color))
  map_state.points = np.array(map_state.points)
  map_state.colors = np.array(map_state.colors) / 256.
  
  for kf in keyframes:
      for kf_cov in kf.get_covisible_by_weight(kMinWeightForDrawingCovisibilityEdge):
          if kf_cov.kid > kf.kid:
              map_state.covisibility_graph.append([*kf.Ow, *kf_cov.Ow])
      if kf.parent is not None:
          map_state.spanning_tree.append([*kf.Ow, *kf.parent.Ow])
      for kf_loop in kf.get_loop_edges():
          if kf_loop.kid > kf.kid:
              map_state.loops.append([*kf.Ow, *kf_loop.Ow])
  ####################################################
  
  if map_state.spanning_tree:
      estimated_positions = map_state.spanning_tree
      estimated_positions = np.array(estimated_positions)
      plt.scatter(estimated_positions[:, 0], estimated_positions[:, 1], c="g")
      plt.pause(0.00001)

The thing I found wired is that the plotted values do not match the spanning tree shown in pangolin window (visible when you disable show cameras and covisibility checkboxes)

[luigifreda]

https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.scatter.html
matplotlib.pyplot.scatter is for drawing 2D points and it only considers the x and the y components (as you are doing in the code).

The spanning tree you are trying to draw in 2D is an array of 3D points. Why do you want to draw the projection of the 3D camera positions on the xy plane?

[hbonnavaud]

Because I don't care about the Z axis. Basically I want to plot the x y estimated position of the car. (I think it makes sens). I'm looking into the link to slam.py you shared above

[luigifreda]

You are assuming the Z axis is vertical. That's not correct.
This is the standard convention in computer vision when setting the camera frame
https://docs.opencv.org/4.x/pinhole_camera_model.png
https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html

Keep in mind that, in visual SLAM, the pose is always estimated w.r.t. the first camera frame, which does not necessarily have any axis aligned with the gravity acceleration direction.

[hbonnavaud]

Yes but still, scatter X and Z, X and Y, or Y and Z don't give me nothing similar to the trajectory plotted in pangolin.

[hbonnavaud]

I think there is an error in https://github.com/luigifreda/pyslam/blob/master/slam.py#L705

    def update_history(self):
        f_cur = self.map.get_frame(-1)
        self.cur_R = f_cur.pose[:3,:3].T
        self.cur_t = np.dot(-self.cur_R,f_cur.pose[:3, 3])
        if (self.init_history is True) and (self.trueX is not None):
            self.t0_est = np.array([self.cur_t[0], self.cur_t[1], self.cur_t[2]])  # starting translation
            self.t0_gt  = np.array([self.trueX, self.trueY, self.trueZ])           # starting translation
        if (self.t0_est is not None) and (self.t0_gt is not None):
            p = [self.cur_t[0]-self.t0_est[0], self.cur_t[1]-self.t0_est[1], self.cur_t[2]-self.t0_est[2]]   # the estimated traj starts at 0
            self.traj3d_est.append(p)
            self.traj3d_gt.append([self.trueX-self.t0_gt[0], self.trueY-self.t0_gt[1], self.trueZ-self.t0_gt[2]])
            self.poses.append(poseRt(self.cur_R, p))

self.init_history is always True (according to my ctrl+f, maybe I missed a file), so self.cur_t[0]-self.t0_est[0] is always equal to 0. right? What am I missing here?

[hbonnavaud]

Well, the update history function give me the same that what I had before. The estimated position projected on the XZ plan is the red curve here.

[luigifreda]

I think you may want to consider the different scales on the two 2D axes of your 2D scatter plot.

https://matplotlib.org/stable/gallery/subplots_axes_and_figures/axis_equal_demo.html

[pse0311]

Hello,

I am finding your creation very useful, thanks to you.

I'm interested in obtaining the x, y, z coordinate values stored in the Map Viewer. Could you please guide me on how to do this?

Thank you.