Apriltag Odometry With Photonvision Help

Technical Java
1

When setting up photonvision 3d pose estimator, how should we go about setting up a Pose2d object?
The documentation for photonvision provides the following code:

    photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camera, robotToCam);


public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
    photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
    return photonPoseEstimator.update();
}

How do we use this code to get data from the pose object?

2

We’ve implemented PhotonVision pose estimation in our repo. Feel free to take a look.

3

We use the following code. Essentially, you need to first make sure that your photonvision camera has apriltags in view (otherwise you’ll get null pointers). Then, if there are targets in view, you query an update from photonpose estimator. You then can get the pose3d of the robot using update.get().estimatedPose and convert it to a pose2d. I hope this helps.

if (VisionConfig.isPhotonVisionMode) {
      var result = photonCam_1.getLatestResult();
      photon1HasTargets = result.hasTargets();
      if (photon1HasTargets) {
        var update = photonPoseEstimator.update();
        Pose3d currentPose3d = update.get().estimatedPose;
        botPose = currentPose3d.toPose2d();
        photonTimestamp = update.get().timestampSeconds;
      }
    }
1 Like
4

Optional <EstimatedRobotPose>
est =getEstimatedGlobalPose(previouspose);
if (est.isPresent()) {
EstimatedRobotPose est1 = est.get();
previouspose = est1.estimatedPose.toPose2d();
double timestampsecondsl = est1.timestampSeconds;
}

1 Like