TagTracker for AprilTag Detection and Pose Estimation

Technical
,
1

Hey!

2129 is proud to announce our new AprilTag tracker for FRC! The project aims to both detect ApriTags, and estimate the location of the robot using pose estimation. Not only can it find where the robot is with one camera, it can combine multiple cameras to get a more accurate reading. The code is written in python and can be modified to your use case. Take a look at the github repository and give us some feedback!

If you have any questions, feel free to leave a comment, and be sure to check out the wiki on github.

6 Likes
2

Are you tracking this change in tag family? [FRC Blog] 2023 Approved Devices, Rules Preview, and Vision Target Update

1 Like
3

No, we hadn’t seen this, the fix will be published within the next 10 minutes. Thank you for bringing this up! If any other changes are made to the tags, they can be reflected in the environment.json file.

4

Mason, we are very interested in this, got it cloned, dependencies loaded, etc.

Are there instructions on running it? I see the main.py (which requires some modules not in requirements.txt), but no idea (yet) of how to access any GUI.

Can you provide some help in setting up the matrices? We think we know what we are doing with translation, but are unsure about how to handle the rotation. We are just taking a few tags to our end wall and two side walls of our practice field.

1 Like
5

Thank you so much for bringing this up! The dependencies issue has be resolved.

A wiki page is being written with detailed running instructions but hopefully this will work:

To run the project, use a python environment with the dependencies installed and run main.py, run main.py -h to get all of the flag options; the main one you may need is --no-gui. Sample JSON files are already in the project so it is recommended to edit those instead of using the flags to select other JSON files.

An OpenCV window should show up for every camera feed, displaying detection overlays when a tag is spotted. An in-progress feature is a streamer to send a compressed video back to the driver station (hence the flask_opencv_streamer).

In regards to the rotation, I do not know off hand. I will talk to my more math-loving colleague and try to get a more detailed answer. It is called a transform matrix in the game development industry if that helps.

Another things not brought up is how to get the co-processor to start at the beginning of the match. Our team uses proprietary software (written by the other programmer) that is able to manage tasks from behind the driver walls. The project is linked here.

6

For rotations, for now I would recommend looking at the Wikipedia article on rotation matrices (at https://en.wikipedia.org/wiki/Rotation_matrix). We are planning on adding easier ways to indicate rotations in the future, such as pitch, yaw, and roll. If you are willing to set up our custom driver station software, it will also be possible to adjust and view the tag positions from there in the future.
On the topic of starting the coprocessor along with the robot, we use the project called TaskManager in our 2023_Season_Projects repository to manage running the python scripts. It is also possible to set up a systemd service to run it automatically on boot. There are several existing resources for that on the internet, and we also have the services we use to run TaskManager in the Deploy folder of our repository if you want to reference a tested and working example.

7

We have had really good luck with the WPI Raspberry Pi image, it does a good job of helping with camera configuration and service startup.

Really look forward to getting help with the pose stuff.

I don’t know if you look at the WPIlib development, but there is an ApriltagsFieldLayout class that can ingest a JSON file describing the poses of all the tags on the field. Is the format the same as yours? If not, I’m willing to help bridge the gap, dollars to donuts we will get an official file for the WPIlib class at kickoff…

8

The format we went with is a map of IDs to Pose3ds, where Pose3d is serialized as a 3D translation and a quaternion. We used a quaternion instead of roll-pitch-yaw because quaternions double-cover SO(3), which are much more uniquely determined than roll-pitch-yaw. They are harder to think about though; they’re like rotation vectors in that x, y, and z represent a rotation axis, and w represents the rotation around that axis, but the components are scaled differently to get some nice mathematical properties.

In any case though, Rotation3d has constructors for rotation matrix, axis-angle, and roll-pitch-yaw representations that all convert to the internal quaternion representation.

We went with a 1:1 ID to pose mapping for simplicity with the assumption that tag IDs aren’t duplicated. If someone wants to make AprilTagFieldLayout use a list of poses instead, we would accept a PR (although it’ll conflict with #4578 for now). Methods of resolving pose ambiguity would also be a good addition (i.e., pick the best pose based on a metric like a pose measurement’s distance from the current pose estimate).

We want to get an AprilTag JSON included in the first in-season release of WPILib (the kickoff release can’t have it because we build and release it before kickoff).

4 Likes
9

Depending on the WPILib standards for JSON, we will switch our layout to match. Upon kickoff, 2129 will put out a release of the TagTracker repository using the published locations JSON.

On the topic of poses, we are working out a simple python script to convert pitch, yaw, and roll to a matrix for ease of use. Expect to see this by the end of the day today.

10

We should be able to adjust our standards to match, do you have a sample WPILib AprilTags json?

At the offseason events that we have attended we have also seen AprilTags posted at certain locations on the field, do you know of where we can find these locations to add it to our environment?

11

Here’s the one from #4578: https://github.com/pjreiniger/allwpilib/blob/april_tag_field_layout/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json based on the 2022 offseason layout. All distance units are meters.

12

Piling on, here is a file that I think is correct (I am not sure I have the rotations correct, really appreciate gentle correction if in order).

West wall is the blue alliance station, origin is SW corner.

  • target 1 is on the west wall, 1 meter north of origin, 1 meter up from floor, facing center of field (facing east)
  • target 2 is on the west wall, 2 meters north of origin, 2 meters up from floor, facing center of field (facing east)
  • target 11 is on the south wall, 1 meter east of origin, 1 meter up from floor, facing center of field (facing north)
  • target 12 is on the south wall, 2 meters north of origin, 2 meters up from floor, facing center of field (facing north)

This is magic incantation:

import java.io.*;
import java.util.*;

import org.junit.Test;

import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.apriltag.*;

public class TestGenerateJsonForTagtrackerDevelopers {
    @Test
    public void t00() throws IOException {
        AprilTagFieldLayout a = getAprilTagFieldLayout();
        a.serialize ("test_apriltags.json");
    }

    static Rotation3d r3d_westwall = new Rotation3d(0, Units.degreesToRadians(-90), 0);
    static Rotation3d r3d_southwall = new Rotation3d(0, Units.degreesToRadians(-90), Units.degreesToRadians(-90));

    static AprilTagFieldLayout getAprilTagFieldLayout() {
        List<AprilTag> rv = new ArrayList<>();
        rv.add (new AprilTag(1, new Pose3d (0, 1.0, 1.0, r3d_westwall)));
        rv.add (new AprilTag(2, new Pose3d (0, 2.0, 2.0, r3d_westwall)));
        rv.add (new AprilTag(11, new Pose3d (1.0, 0.0, 1.0, r3d_southwall)));
        rv.add (new AprilTag(12, new Pose3d (2.0, 0.0, 2.0, r3d_southwall)));
        return new AprilTagFieldLayout(rv, 20.0, 10.0);
    }
}

…and this is the (formatted) result…

{
    "tags": [
        {
            "ID": 1,
            "pose": {
                "translation": {
                    "x": 0.0,
                    "y": 1.0,
                    "z": 1.0
                },
                "rotation": {
                    "quaternion": {
                        "W": 0.7071067811865476,
                        "X": 0.0,
                        "Y": -0.7071067811865475,
                        "Z": 0.0
                    }
                }
            }
        },
        {
            "ID": 2,
            "pose": {
                "translation": {
                    "x": 0.0,
                    "y": 2.0,
                    "z": 2.0
                },
                "rotation": {
                    "quaternion": {
                        "W": 0.7071067811865476,
                        "X": 0.0,
                        "Y": -0.7071067811865475,
                        "Z": 0.0
                    }
                }
            }
        },
        {
            "ID": 11,
            "pose": {
                "translation": {
                    "x": 1.0,
                    "y": 0.0,
                    "z": 1.0
                },
                "rotation": {
                    "quaternion": {
                        "W": 0.5000000000000001,
                        "X": -0.4999999999999999,
                        "Y": -0.5,
                        "Z": -0.5
                    }
                }
            }
        },
        {
            "ID": 12,
            "pose": {
                "translation": {
                    "x": 2.0,
                    "y": 0.0,
                    "z": 2.0
                },
                "rotation": {
                    "quaternion": {
                        "W": 0.5000000000000001,
                        "X": -0.4999999999999999,
                        "Y": -0.5,
                        "Z": -0.5
                    }
                }
            }
        }
    ],
    "field": {
        "width": 20.0,
        "height": 10.0
    }
}
1 Like
13

TTMasterError.txt (11.9 KB)

From the install requirements stage.

Win10 x64 cmake version 3.24.1

14

We hadn’t had this issue before. Unfortunately, this is not an error with our code, but instead an error with the apriltags library.

You may need to configure this using linux on whatever processor you are planning on using.

15

We’ve adapted our system to not only be able to use the WPILib styling, but also any styling that your team desires. The options are:

  1. Traditional transformation matrix
  2. Quaternion and translation
  3. Euler angles and translation

For now, this code is in PR #3 until it can be properly tested for angle consistency. In the upcoming days, expect to see a full release using the published WPILib tag positions json.

1 Like
16

We’ll be sure to take a look at this and make adjustments to make this work with that system.

PR #3 has options for Euler angles. While they are slightly inferior to quaternions to to gimbel lock, they are much easier to understand. There are a few utilites in transform_matrix.py (on the pull request) that may help you understand what is going on in the transformation

17

You should fix your Wiki instructions

18

Will do. I’ve done a little more digging and it seems to relate to this issue. It is possible that the apriltags library does not support Windows x64. The documentation has been updated to reflect this issue.

Thank you for bringing this to our attention. We will try to do more testing on Windows in the future.

1 Like
19

Thanks for sharing, @Mason_V. My team is looking into different methods of localization using AprilTags, we might try and integrate this into ROS and I will post back if we do.

20

We are completely revamping the tracker to act more similarly to PhotonVision but with less UI bogging it down. We still have to make the decision whether or not to swap over to C++ for development for the higher performance ceiling.

As the current version of TagTracker was created before the PhotonVision tracker, we had it doing much more of the processing on its own. In the rework, we plan to have it send data to the RoboRio over NetworkTables exactly like Photon does so that teams can easily swap out an SD card running Photon with an SD card for TagTracker.

We plan to have much fewer graphical interface elements than PhotonVision, just basic controls with a stream through NetworkTables. This should all be integrated into the WPILibPi SD image for the write protection capabilities.

We are really exited to work on this project further and help form other teams is always greatly appreciated!