Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Aruco/Multitag 36h11 support (#981)

Browse Source 
- Aruco pipeline now infers tag width from tag family like the AprilTag pipeline
- Removes unused Aruco and 200mm AprilTag models
- `VisionEstimation.estimateCamPosePNP()` now requires a target model instead of assuming 16h5
  - Multitarget pipeline similarly infers target model of tag family now
  - `PhotonPoseEstimator` can have target model set for on-rio multitarget

---------

Co-authored-by: amquake <noleetarrr@gmail.com>
This commit is contained in:
Sriman Achanta
2023-10-29 23:02:16 -04:00
committed by GitHub
parent d61225eba3
commit 0898dfe2f7
19 changed files with 98 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
photon-targeting/src/main/java/org/photonvision/estimation/TargetModel.java
View File

@@ -37,8 +37,10 @@ public class TargetModel {
public final boolean isPlanar;
public final boolean isSpherical;
public static final TargetModel kTag16h5 =
public static final TargetModel kAprilTag16h5 =
new TargetModel(Units.inchesToMeters(6), Units.inchesToMeters(6));
public static final TargetModel kAprilTag36h11 =
new TargetModel(Units.inchesToMeters(6.5), Units.inchesToMeters(6.5));
/**
* Creates a rectangular, planar target model given the width and height. The model has four

8
photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View File

@@ -70,7 +70,8 @@ public class VisionEstimation {
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
AprilTagFieldLayout tagLayout) {
AprilTagFieldLayout tagLayout,
TargetModel tagModel) {
if (tagLayout == null
|| visTags == null
|| tagLayout.getTags().size() == 0
@@ -99,8 +100,7 @@ public class VisionEstimation {
// single-tag pnp
if (knownTags.size() == 1) {
var camToTag =
OpenCVHelp.solvePNP_SQUARE(
cameraMatrix, distCoeffs, TargetModel.kTag16h5.vertices, points);
OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
if (!camToTag.isPresent) return new PNPResults();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
@@ -118,7 +118,7 @@ public class VisionEstimation {
// multi-tag pnp
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(TargetModel.kTag16h5.getFieldVertices(tag.pose));
for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(