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

Change pose estimator to take robotToCamera (#698)

Browse Source 
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Nick Hadley
2023-01-06 17:41:47 -05:00
committed by GitHub
parent ebef19af3d
commit dbe7464ea9
4 changed files with 57 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

36
photon-lib/src/main/java/org/photonvision/SimVisionSystem.java
View File

@@ -24,6 +24,8 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -49,7 +51,7 @@ public class SimVisionSystem {
int cameraResWidth;
int cameraResHeight;
double minTargetArea;
Transform3d cameraToRobot;
Transform3d robotToCamera;
Field2d dbgField;
FieldObject2d dbgRobot;
@@ -67,7 +69,8 @@ public class SimVisionSystem {
* @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the
* manufacturer specifications, and/or whatever is configured in the PhotonVision Setting
* page.
* @param cameraToRobot Transform to move from the camera's mount position to the robot's position
* @param robotToCamera Transform to move from the center of the robot to the camera's mount
* position
* @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and
* make it visible. Set to 9000 or more if your vision system does not rely on LED's.
* @param cameraResWidth Width of your camera's image sensor in pixels
@@ -79,12 +82,12 @@ public class SimVisionSystem {
public SimVisionSystem(
String camName,
double camDiagFOVDegrees,
Transform3d cameraToRobot,
Transform3d robotToCamera,
double maxLEDRangeMeters,
int cameraResWidth,
int cameraResHeight,
double minTargetArea) {
this.cameraToRobot = cameraToRobot;
this.robotToCamera = robotToCamera;
this.maxLEDRangeMeters = maxLEDRangeMeters;
this.cameraResWidth = cameraResWidth;
this.cameraResHeight = cameraResHeight;
@@ -119,14 +122,31 @@ public class SimVisionSystem {
;
}
/**
* Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The
* poses added will preserve the tag layout's alliance origin at the time of calling this method.
*
* @param tagLayout The field tag layout to get Apriltag poses and IDs from
*/
public void addVisionTargets(AprilTagFieldLayout tagLayout) {
for (AprilTag tag : tagLayout.getTags()) {
addSimVisionTarget(
new SimVisionTarget(
tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation
Units.inchesToMeters(6),
Units.inchesToMeters(6),
tag.ID));
}
}
/**
* Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or
* turret or some other mobile platform.
*
* @param newCameraToRobot New Transform from the robot to the camera
* @param newRobotToCamera New Transform from the robot to the camera
*/
public void moveCamera(Transform3d newCameraToRobot) {
this.cameraToRobot = newCameraToRobot;
public void moveCamera(Transform3d newRobotToCamera) {
this.robotToCamera = newRobotToCamera;
}
/**
@@ -150,7 +170,7 @@ public class SimVisionSystem {
* PhotonVision parameters.
*/
public void processFrame(Pose3d robotPoseMeters) {
Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse());
Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera);
dbgRobot.setPose(robotPoseMeters.toPose2d());
dbgCamera.setPose(cameraPose.toPose2d());