I sent the opencv calibration matrices. we convert them here:
var cx = cameraMatrix.get(0, 2)[0];
var cy = cameraMatrix.get(1, 2)[0];
var fx = cameraMatrix.get(0, 0)[0];
var fy = cameraMatrix.get(1, 1)[0];
You’ll probably also wanna undistort corners, as we do here: photonvision/AprilTagPoseEstimatorPipe.java at master · PhotonVision/photonvision · GitHub
So fx = 699.3778103158814, fy = 677.716, cx = 345.61, cy=207.13
I don’t have the time to work on it, so your contribution would be very welcome! We’re targeting a release Friday night so if we can have it ready for that, that would be awesome!
Regarding calibration, agreed a simple method we could put in the docs would be great (we could still point folks to PV as well for a more “automated” approach–particularly since a team could simply run it on desktop to get the camera parameters?).
My recollection is that PhotonVision uses the opencv.calibratecamera routine, and saves the entire matrix.
The matrix is
fx s cx
0 fy cy
0 0 1
so for Matt’s example, fx = 699.3778…, cx = 345.605…, fy = 677.716…, cy = 207.127.
Ref: Dissecting the Camera Matrix, Part 3: The Intrinsic Matrix ←
Issue #4929. I can get a Java version done. Someone else will need to help the poor C teams out (or else wait another week for that).
I don’t care about the C teams; they made their choice. The C++ teams on the other hand…
It’s all rock 'n roll to me.
I think I can get the C++ in there “tonight” also.
Here is some Python code that uses an attached Webcam to detect Apriltags using the robotpy-apriltag module. To use this you’ll need robotpy and opencv installed. It may be helpful for those using other languages, as the function calls would be the same or similar.
Experimenting with a locally-printed “ID 1” apriltag, I am little disappointed in the robustness of the detection both in terms of a sensitivity to distance (too close, too far) and especially in terms of angle. You’ll note that even after detection, if you don’t filter for a significant “Decision Margin”, you will get erroneous detections. Hopefully that’s just a matter of tuning the detection parameters.
Here is code for detecting AprilTags on the roboRIO in C++. Probably slower than a coprocessor, but faster than having nothing.
Now that we got AprilTag detection running really well on the RoboRIO, the next step is to do the same thing but with LimeLight. We have installed PhotonVision image on the LimeLight. We can use our web browser and connect to the LimeLight and saw it detected the AprilTag. Everything looks good. But we need to have our code to communicate with LimeLight to get the detected info. We have LimeLight code from previous season that basically communicating with LimeLight via Network Table. So, I figure we can do similarly. Looking at our code, it is fetching the values through Network Table keys. So the question is: what are the Network Table keys for AprilTag? Opening ShuffleBoard and peeking into the Network Table, I saw the “limelight” table but it has only two entries: LEDMode and Pipeline. That’s it. Do I have to do something to enable LimeLight to export AprilTag info to the Network Table? I also found a photonLib vendor library. Do I need to install that and use PhotonLib to access the LimeLight instead of doing our old way of accessing the Network Table directly? If I should be using PhotonLib, is there any sample code on this? I looked and found none.
If you’re using PhotonVision, I reccomend looking at PhotonLib, it’s a little different than how Limelight does it but more convenient for some users. We don’t support pure NT implementations because we send data for each tag. PhotonLib: Robot Code Interface - PhotonVision Docs
Ok, we got PhotonLib working nicely now. The only remaining issue is that I can’t seem to show the limelight camera stream in shuffleboard. I looked at the PhotonCamera methods and they don’t have anything about enabling a stream. Suggestions?
Never mind. I figured that out. Everything is working nicely now.
Re-reading the instructions on installing PhotonVision on a LimeLight, I just realized I have missed a step: “Download the hardwareConfig.json file for the version of your Limelight”. I know we have LimeLight 2 but I am not sure if it’s a 2 or 2+. Does anyone know how to tell them apart?
I believe the 2+ has a red dot on it somewhere? Note that this isn’t necessary if you will be using the LEDs (you don’t need the LEDs for AprilTags)
It’s not the LED we worry about. It’s the FOV. Hmm wait, 2 and 2+ has the same FOV. So, 2 and 2+ is just LED difference? We do plan to use the same LimeLight to detect retro-reflective tape. Therefore, we still need the LEDs. I guess I can just try importing one of the config and check if LED works. If it doesn’t, then I switch to the other config and try again. Woudl that work?
I’ve extended the @fovea1959 contribution to include producing the pose of the robot in the field frame of reference.
The tag pose is retrieved. The proper orientation of the tag facing the camera (rotated 180 degrees) and conversion from EDN to NWU is performed correctly (patched the incorrect WPILib conversion of transforms). All components are combined to form the robot pose in the field.
Tags with any arbitrary pose (location and rotation on the field) work and not just at the ends of the long axis (X) of the field. The AdvantageScope display of rotated tags appears correctly but I was unable to compare those with LimeLight since I don’t know how to enter a tag rotation into LL. Otherwise the example code and LL yield similar results for tags at the ends of the field. [Caution that my test setup wasn’t good enough to verify results at great accuracy. Also, I saw a comment that the tag pose is set at its center but the camera to tag estimator is pointing at a corner of the tag as the object origin and that could introduce a few inches of error. I did not verify any of this theoretically and I cannot get 3 inches of precision on my setup to measure errors.]
My development project which includes some other stuff (custom tag layout, refactored variable names, 3-D box around highlighting the AprilTag, output of the robot and tags poses for AdvantageScope) is for now at AprilTag to Robot Pose .
I was unable to figure out how to accurately and efficiently use the the WPILib homography matrix to compute the 3-D box highlighting the AprilTag so I recomputed the Rotation and Translation of the tag using the OpenCV SolvePnP (it produces nearly identical results to WPILib but reveals the single best R and T vectors which I didn’t know how to determine from the WPILib H matrix).
I’m not recommending this extended example over LimeLight or PhotonVision but for simple, low speed tag usage and a roboRIO v2 this could suffice for some. And it isn’t a bad example for helping to train students on part of what’s behind LimeLIght and PhotonVision.
[Sorry - Java only and no other languages forthcoming from me.]
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc.robot;
import edu.wpi.first.apriltag.AprilTagDetection;
import edu.wpi.first.apriltag.AprilTagDetector;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.apriltag.AprilTagPoseEstimator;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.CvSink;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.UsbCamera;
import edu.wpi.first.math.ComputerVisionUtil;
import edu.wpi.first.math.geometry.CoordinateSystem;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.IntegerArrayPublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.TimedRobot;
import java.io.IOException;
import java.util.ArrayList;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
/**
* This is a demo program showing the detection of AprilTags. The image is acquired from the USB
* camera, then any detected AprilTags are marked up on the image, transformed to the robot on
* the field pose and sent to the dashboard.
*
* <p>Be aware that the performance on this is much worse than a coprocessor solution!
*/
public class Robot extends TimedRobot {
@Override
public void robotInit() {
var visionThread = new Thread(() -> apriltagVisionThreadProc());
visionThread.setDaemon(true);
visionThread.start();
}
void apriltagVisionThreadProc() {
var detector = new AprilTagDetector();
// look for tag16h5, don't correct any error bits
detector.addFamily("tag16h5", 0);
// Positions of AprilTags
AprilTagFieldLayout aprilTagFieldLayout;
try {
aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile);
} catch (IOException e) {
e.printStackTrace();
aprilTagFieldLayout = null;
}
// Set up Pose Estimator - parameters are for a Microsoft Lifecam HD-3000
// (https://www.chiefdelphi.com/t/wpilib-apriltagdetector-sample-code/421411/21)
var poseEstConfig =
new AprilTagPoseEstimator.Config(
0.1524, 699.3778103158814, 677.7161226393544, 345.6059345433618, 207.12741326228522);
var estimator = new AprilTagPoseEstimator(poseEstConfig);
// Get the UsbCamera from CameraServer
UsbCamera camera = CameraServer.startAutomaticCapture();
// Set the resolution
camera.setResolution(640, 480);
// Get a CvSink. This will capture Mats from the camera
CvSink cvSink = CameraServer.getVideo();
// Setup a CvSource. This will send images back to the Dashboard
CvSource outputStream = CameraServer.putVideo("Detected", 640, 480);
// Mats are very memory expensive. Lets reuse these.
var mat = new Mat();
var grayMat = new Mat();
// Instantiate once
ArrayList<Long> tags = new ArrayList<>();
var outlineColor = new Scalar(0, 255, 0);
var crossColor = new Scalar(0, 0, 255);
// We'll output to NT
NetworkTable tagsTable = NetworkTableInstance.getDefault().getTable("apriltags");
IntegerArrayPublisher pubTags = tagsTable.getIntegerArrayTopic("tags").publish();
// This cannot be 'true'. The program will never exit if it is. This
// lets the robot stop this thread when restarting robot code or
// deploying.
while (!Thread.interrupted()) {
// Tell the CvSink to grab a frame from the camera and put it
// in the source mat. If there is an error notify the output.
if (cvSink.grabFrame(mat) == 0) {
// Send the output the error.
outputStream.notifyError(cvSink.getError());
// skip the rest of the current iteration
continue;
}
Imgproc.cvtColor(mat, grayMat, Imgproc.COLOR_RGB2GRAY);
AprilTagDetection[] detections = detector.detect(grayMat);
// have not seen any tags yet
tags.clear();
for (AprilTagDetection detection : detections) {
Pose3d tagInFieldFrame; // pose from WPILib resource
if(aprilTagFieldLayout.getTagPose(detection.getId()).isPresent() && detection.getDecisionMargin() > 50.) // margin < 20 seems bad; margin > 120 are good
{
tagInFieldFrame = aprilTagFieldLayout.getTagPose(detection.getId()).get();
}
else
{
System.out.println("bad id " + detection.getId() + " " + detection.getDecisionMargin());
continue;
}
// remember we saw this tag
tags.add((long) detection.getId());
// draw lines around the tag
for (var i = 0; i <= 3; i++) {
var j = (i + 1) % 4;
var pt1 = new Point(detection.getCornerX(i), detection.getCornerY(i));
var pt2 = new Point(detection.getCornerX(j), detection.getCornerY(j));
Imgproc.line(mat, pt1, pt2, outlineColor, 2);
}
// mark the center of the tag
var cx = detection.getCenterX();
var cy = detection.getCenterY();
var ll = 10;
Imgproc.line(mat, new Point(cx - ll, cy), new Point(cx + ll, cy), crossColor, 2);
Imgproc.line(mat, new Point(cx, cy - ll), new Point(cx, cy + ll), crossColor, 2);
// identify the tag
Imgproc.putText(
mat,
Integer.toString(detection.getId()),
new Point(cx + ll, cy),
Imgproc.FONT_HERSHEY_SIMPLEX,
1,
crossColor,
3);
// determine pose
Transform3d pose = estimator.estimate(detection);
// These transformations are required for the correct robot pose.
// They arise from the tag facing the camera thus Pi radians rotated or CCW/CW flipped from the
// mathematically described pose from the estimator that's what our eyes see. The true rotation
// has to be used to get the right robot pose.
pose = new Transform3d(
new Translation3d(
pose.getX(),
pose.getY(),
pose.getZ()),
new Rotation3d(
-pose.getRotation().getX() - Math.PI,
-pose.getRotation().getY(),
pose.getRotation().getZ() - Math.PI));
// OpenCV and WPILib estimator layout of axes is EDN and field WPILib is NWU; need x -> -y , y -> -z , z -> x and same for differential rotations
// pose = CoordinateSystem.convert(pose, CoordinateSystem.EDN(), CoordinateSystem.NWU());
// WPILib convert is wrong for transforms as of 2023.4.3 so use this patch for now
{
// corrected convert
var from = CoordinateSystem.EDN();
var to = CoordinateSystem.NWU();
pose = new Transform3d(
CoordinateSystem.convert(pose.getTranslation(), from, to),
CoordinateSystem.convert(new Rotation3d(), to, from)
.plus(CoordinateSystem.convert(pose.getRotation(), from, to)));
// end of corrected convert
}
var // transform to camera from robot chassis center at floor level
cameraInRobotFrame = new Transform3d(
// new Translation3d(0., 0., 0.),// camera at center bottom of robot zeros for test data
// new Rotation3d(0.0, Units.degreesToRadians(0.), Units.degreesToRadians(0.0))); // camera in line with robot chassis
new Translation3d(0.2, 0., 0.8),// camera in front of center of robot and above ground
new Rotation3d(0.0, Units.degreesToRadians(-30.), Units.degreesToRadians(0.0))); // camera in line with robot chassis
// y = -30 camera points up; +30 points down; sign is correct but backwards of LL
var // robot in field is the composite of 3 pieces
robotInFieldFrame = ComputerVisionUtil.objectToRobotPose(tagInFieldFrame, pose, cameraInRobotFrame);
// end transforms to get the robot pose from this vision tag pose
// put pose into dashboard
Rotation3d rot = robotInFieldFrame.getRotation();
tagsTable
.getEntry("pose_" + detection.getId())
.setDoubleArray(
new double[] {
robotInFieldFrame.getX(), robotInFieldFrame.getY(), robotInFieldFrame.getZ(), rot.getX(), rot.getY(), rot.getZ()
});
}
// put list of tags onto dashboard
pubTags.set(tags.stream().mapToLong(Long::longValue).toArray());
// Give the output stream a new image to display
outputStream.putFrame(mat);
}
pubTags.close();
detector.close();
}
}
Someone should PR a fix for that. It’s been fixed independently by like 2-3 people at this point.
Did it not work to use the AprilTagPoseEstimator class? It takes in the homography matrix and outputs a Transform3d (or a PoseEstimate which is either one or two Transform3ds due to pose ambiguity). The WPILib code is just a light wrapper around the AprilRobotics library (the homography to pose core functions are here: apriltag/common/homography.h at master · AprilRobotics/apriltag · GitHub)
A few days ago I opened convert issue. It took me too much time but I learned a lot more than I expected to from this exercise.
I used the R and T from the WPILib AprilTagPoseEstimator as one of the steps to determine the pose of the robot on the field using:
Transform3d tagInCameraFrame = estimator.estimate(detection);
I ran those R and T through the OpenCV Calib3d.projectPoints and got this box that is close but didn’t precisely hit the corners of the scene that the WPILib detector reported and is oddly skewed.
I then tried the OpenCV Calib3d.solvePnP with the same scene corners from the detector and got its R and T for projectPoints to make this box that exactly hit the corners and is pretty good on the perspective.
The values of T for the two different methods are almost identical and the R values are within roughly 0, 2, and 4 degrees of each other.
I don’t understand the differences and how such a small difference makes a big viewing difference.
It’s not a big deal since I’d never put the slow processing for a “debugging” help into production but yes I’d say the AprilTagPoseEstimator class didn’t work as well. There might be “user error” involved but I don’t know what I could have done wrong.
As I mentioned above my test setup isn’t precise to a few inches so I couldn’t say if the OpenCV solvePnP rotation is better or worse numerically or speed-wise than the WPILib version.