Entry 2: Estimating the Robot's Global Position

🌍 Transforming to World Coordinates

With the robot's pose relative to the tag now estimated, I can proceed to convert it into global coordinates by combining the visually-obtained information from the AprilTags with known data about the scene.

Starting from the output of the solvePnP function — as detailed in the previous entry — I obtain the robot’s position (tvec) and orientation (rvec) relative to a tag detected in the image.

After converting this pose into a 4×4 homogeneous transformation matrix (combining rotation and translation), I use the known position and orientation of the AprilTag in the world, read from the apriltags_poses.yaml file. From this, I build another 4×4 matrix representing the transformation from the tag’s frame to the world frame.

With both matrices constructed — robot to tag, and tag to world — I multiply them to obtain the transformation from the robot to the global coordinate system. From this final matrix, I extract the robot’s translation (x, y) and orientation (yaw), which is the final target of the estimation.

This estimated pose is displayed on the simulator map using GUI.showEstimatedPose, allowing for visual comparison between the estimated pose (in red) and the real one (in green).

👁️‍🗨️ Observations

After running the code, I observed the following key issues:

  1. The robot’s initial estimated position does not match its real location in the simulator. There is a clear error from the beginning in the visual localization.
  2. The red trace representing the estimated trajectory shows unnatural behavior: sharp spikes and abrupt changes in direction. This suggests instability or lack of continuity in the estimation process.
  3. The robot keeps moving even when no AprilTag is visible in the image. In such cases, solvePnP is not executed, and no update to the estimated pose occurs. As a result, the red line only reflects isolated estimations when a tag is detected, while intermediate motion based purely on odometry is ignored.

The video below illustrates all of the above:

⬅ Back to Exercise 3