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

Initial AprilTag support (#458)

Browse Source 
(Very) beta AprilTag support in PhotonVision. Disables Picam GPU acceleration until we can debug auto exposure in the MMAL driver.

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Chris Gerth <gerth2@users.noreply.github.com>
Co-authored-by: Chris <chrisgerth010592@gmail.com>
Co-authored-by: mdurrani808 <mdurrani808@gmail.com>
This commit is contained in:
shueja-personal
2022-09-28 18:21:41 -07:00
committed by GitHub
parent a3bcd3ac4f
commit a764ace7f2
114 changed files with 21488 additions and 10851 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
photonlib-java-examples/src/main/java/org/photonlib/examples/simposeest/robot/DrivetrainPoseEstimator.java
View File

@@ -29,6 +29,7 @@ import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
@@ -86,8 +87,12 @@ public class DrivetrainPoseEstimator {
var res = cam.getLatestResult();
if (res.hasTargets()) {
double imageCaptureTime = Timer.getFPGATimestamp() - res.getLatencyMillis();
Transform2d camToTargetTrans = res.getBestTarget().getCameraToTarget();
Pose2d camPose = Constants.kFarTargetPose.transformBy(camToTargetTrans.inverse());
Transform3d camToTargetTrans = res.getBestTarget().getCameraToTarget();
var transform =
new Transform2d(
camToTargetTrans.getTranslation().toTranslation2d(),
camToTargetTrans.getRotation().toRotation2d());
Pose2d camPose = Constants.kFarTargetPose.transformBy(transform.inverse());
m_poseEstimator.addVisionMeasurement(
camPose.transformBy(Constants.kCameraToRobot), imageCaptureTime);
}