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

Return named type from PhotonPoseEstimator (#734)

Browse Source 
Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator
This commit is contained in:
Andrew Gasser
2023-01-14 09:06:15 -06:00
committed by GitHub
parent 073714f0bc
commit 357d8a518a
17 changed files with 1597 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java Normal file
View File

@@ -0,0 +1,47 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision;
import edu.wpi.first.math.geometry.Pose3d;
/** An estimated pose based on pipeline result */
public class EstimatedRobotPose {
/** The estimated pose */
public final Pose3d estimatedPose;
/** The estimated time the frame used to derive the robot pose was taken */
public final double timestampSeconds;
/**
* Constructs an EstimatedRobotPose
*
* @param estimatedPose estimated pose
* @param timestampSeconds timestamp of the estimate
*/
public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) {
this.estimatedPose = estimatedPose;
this.timestampSeconds = timestampSeconds;
}
}

492
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java Normal file
View File

@@ -0,0 +1,492 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.photonvision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
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.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;
import java.util.Optional;
import java.util.Set;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
/**
* The PhotonPoseEstimator class filters or combines readings from all the fiducials visible at a
* given timestamp on the field to produce a single robot in field pose, using the strategy set
* below. Example usage can be found in our apriltagExample example project.
*/
public class PhotonPoseEstimator {
/** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */
public enum PoseStrategy {
/** Choose the Pose with the lowest ambiguity. */
LOWEST_AMBIGUITY,
/** Choose the Pose which is closest to the camera height. */
CLOSEST_TO_CAMERA_HEIGHT,
/** Choose the Pose which is closest to a set Reference position. */
CLOSEST_TO_REFERENCE_POSE,
/** Choose the Pose which is closest to the last pose calculated */
CLOSEST_TO_LAST_POSE,
/** Choose the Pose with the lowest ambiguity. */
AVERAGE_BEST_TARGETS
}
private AprilTagFieldLayout fieldTags;
private PoseStrategy strategy;
private final PhotonCamera camera;
private final Transform3d robotToCamera;
private Pose3d lastPose;
private Pose3d referencePose;
private final Set<Integer> reportedErrors = new HashSet<>();
/**
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera mount positions
* (ie, robot ➔ camera).
*/
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags,
PoseStrategy strategy,
PhotonCamera camera,
Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.strategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
}
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
public AprilTagFieldLayout getFieldTags() {
return fieldTags;
}
/**
* Set the AprilTagFieldLayout being used by the PositionEstimator.
*
* @param fieldTags the AprilTagFieldLayout
*/
public void setFieldTags(AprilTagFieldLayout fieldTags) {
this.fieldTags = fieldTags;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
public PoseStrategy getStrategy() {
return strategy;
}
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
public void setStrategy(PoseStrategy strategy) {
this.strategy = strategy;
}
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
public Pose3d getReferencePose() {
return referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose3d referencePose) {
this.referencePose = referencePose;
}
/**
* Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
* strategy.
*
* @param referencePose the referencePose to set
*/
public void setReferencePose(Pose2d referencePose) {
this.referencePose = new Pose3d(referencePose);
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
*/
public void setLastPose(Pose3d lastPose) {
this.lastPose = lastPose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when using the
* <b>CLOSEST_TO_LAST_POSE</b> strategy.
*
* @param lastPose the lastPose to set
*/
public void setLastPose(Pose2d lastPose) {
this.lastPose = new Pose3d(lastPose);
}
/**
* Poll data from the configured cameras and update the estimated position of the robot. Returns
* empty if there are no cameras set or no targets were found from the cameras.
*
* @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
* pipeline results used to create the estimate
*/
public Optional<EstimatedRobotPose> update() {
if (camera == null) {
DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
return Optional.empty();
}
PhotonPipelineResult cameraResult = camera.getLatestResult();
if (!cameraResult.hasTargets()) {
return Optional.empty();
}
Optional<EstimatedRobotPose> estimatedPose;
switch (strategy) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
estimatedPose = closestToCameraHeightStrategy(cameraResult);
break;
case CLOSEST_TO_LAST_POSE:
setReferencePose(lastPose);
case CLOSEST_TO_REFERENCE_POSE:
estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
break;
case AVERAGE_BEST_TARGETS:
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
default:
DriverStation.reportError(
"[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
return Optional.empty();
}
if (estimatedPose.isEmpty()) {
lastPose = null;
}
return estimatedPose;
}
/**
* Return the estimated position of the robot with the lowest position ambiguity from a List of
* pipeline results.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
*/
private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
PhotonTrackedTarget lowestAmbiguityTarget = null;
double lowestAmbiguityScore = 10;
for (PhotonTrackedTarget target : result.targets) {
double targetPoseAmbiguity = target.getPoseAmbiguity();
// Make sure the target is a Fiducial target.
if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
lowestAmbiguityScore = targetPoseAmbiguity;
lowestAmbiguityTarget = target;
}
}
// Although there are confirmed to be targets, none of them may be fiducial
// targets.
if (lowestAmbiguityTarget == null) return Optional.empty();
int targetFiducialId = lowestAmbiguityTarget.getFiducialId();
Optional<Pose3d> targetPosition = fieldTags.getTagPose(targetFiducialId);
if (targetPosition.isEmpty()) {
reportFiducialPoseError(targetFiducialId);
return Optional.empty();
}
return Optional.of(
new EstimatedRobotPose(
targetPosition
.get()
.transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
}
/**
* Return the estimated position of the robot using the target with the lowest delta height
* difference between the estimated and actual height of the camera.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
*/
private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
double smallestHeightDifference = 10e9;
EstimatedRobotPose closestHeightTarget = null;
for (PhotonTrackedTarget target : result.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
// adding -1 to
// the initial HashSet.
if (targetFiducialId == -1) continue;
Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
if (targetPosition.isEmpty()) {
reportFiducialPoseError(target.getFiducialId());
continue;
}
double alternateTransformDelta =
Math.abs(
robotToCamera.getZ()
- targetPosition
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.getZ());
double bestTransformDelta =
Math.abs(
robotToCamera.getZ()
- targetPosition
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.getZ());
if (alternateTransformDelta < smallestHeightDifference) {
smallestHeightDifference = alternateTransformDelta;
closestHeightTarget =
new EstimatedRobotPose(
targetPosition
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
}
if (bestTransformDelta < smallestHeightDifference) {
smallestHeightDifference = bestTransformDelta;
closestHeightTarget =
new EstimatedRobotPose(
targetPosition
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds());
}
}
// Need to null check here in case none of the provided targets are fiducial.
return Optional.ofNullable(closestHeightTarget);
}
/**
* Return the estimated position of the robot using the target with the lowest delta in the vector
* magnitude between it and the reference pose.
*
* @param result pipeline result
* @param referencePose reference pose to check vector magnitude difference against.
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
*/
private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
PhotonPipelineResult result, Pose3d referencePose) {
if (referencePose == null) {
DriverStation.reportError(
"[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
false);
return Optional.empty();
}
double smallestPoseDelta = 10e9;
EstimatedRobotPose lowestDeltaPose = null;
for (PhotonTrackedTarget target : result.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
// adding -1 to
// the initial HashSet.
if (targetFiducialId == -1) continue;
Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
if (targetPosition.isEmpty()) {
reportFiducialPoseError(targetFiducialId);
continue;
}
Pose3d altTransformPosition =
targetPosition
.get()
.transformBy(target.getAlternateCameraToTarget().inverse())
.transformBy(robotToCamera.inverse());
Pose3d bestTransformPosition =
targetPosition
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse());
double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition));
double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition));
if (altDifference < smallestPoseDelta) {
smallestPoseDelta = altDifference;
lowestDeltaPose =
new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
}
if (bestDifference < smallestPoseDelta) {
smallestPoseDelta = bestDifference;
lowestDeltaPose =
new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
}
}
return Optional.ofNullable(lowestDeltaPose);
}
/**
* Return the average of the best target poses using ambiguity as weight.
*
* @param result pipeline result
* @return the estimated position of the robot in the FCS and the estimated timestamp of this
* estimation.
*/
private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
double totalAmbiguity = 0;
for (PhotonTrackedTarget target : result.targets) {
int targetFiducialId = target.getFiducialId();
// Don't report errors for non-fiducial targets. This could also be resolved by
// adding -1 to
// the initial HashSet.
if (targetFiducialId == -1) continue;
Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
if (targetPosition.isEmpty()) {
reportFiducialPoseError(targetFiducialId);
continue;
}
double targetPoseAmbiguity = target.getPoseAmbiguity();
// Pose ambiguity is 0, use that pose
if (targetPoseAmbiguity == 0) {
return Optional.of(
new EstimatedRobotPose(
targetPosition
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse()),
result.getTimestampSeconds()));
}
totalAmbiguity += 1.0 / target.getPoseAmbiguity();
estimatedRobotPoses.add(
new Pair<>(
target,
targetPosition
.get()
.transformBy(target.getBestCameraToTarget().inverse())
.transformBy(robotToCamera.inverse())));
}
// Take the average
Translation3d transform = new Translation3d();
Rotation3d rotation = new Rotation3d();
if (estimatedRobotPoses.isEmpty()) return Optional.empty();
for (Pair<PhotonTrackedTarget, Pose3d> pair : estimatedRobotPoses) {
// Total ambiguity is non-zero confirmed because if it was zero, that pose was
// returned.
double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity;
Pose3d estimatedPose = pair.getSecond();
transform = transform.plus(estimatedPose.getTranslation().times(weight));
rotation = rotation.plus(estimatedPose.getRotation().times(weight));
}
return Optional.of(
new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
}
/**
* Difference is defined as the vector magnitude between the two poses
*
* @return The absolute "difference" (>=0) between two Pose3ds.
*/
private double calculateDifference(Pose3d x, Pose3d y) {
return x.getTranslation().getDistance(y.getTranslation());
}
private void reportFiducialPoseError(int fiducialId) {
if (!reportedErrors.contains(fiducialId)) {
DriverStation.reportError(
"[PhotonPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false);
reportedErrors.add(fiducialId);
}
}
}

2
photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java
View File

@@ -40,6 +40,8 @@ import java.util.Set;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
/** @deprecated Use {@link PhotonPoseEstimator} */
@Deprecated
public class RobotPoseEstimator {
/**
*

270
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp Normal file
View File

@@ -0,0 +1,270 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photonlib/PhotonPoseEstimator.h"
#include <iostream>
#include <limits>
#include <map>
#include <span>
#include <string>
#include <utility>
#include <vector>
#include <frc/Errors.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat, PhotonCamera&& cam,
frc::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
camera(std::move(cam)),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
auto result = camera.GetLatestResult();
if (!result.HasTargets()) {
return std::nullopt;
}
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
case LOWEST_AMBIGUITY:
ret = LowestAmbiguityStrategy(result);
break;
case CLOSEST_TO_CAMERA_HEIGHT:
ret = ClosestToCameraHeightStrategy(result);
break;
case CLOSEST_TO_REFERENCE_POSE:
ret = ClosestToReferencePoseStrategy(result);
break;
case CLOSEST_TO_LAST_POSE:
SetReferencePose(lastPose);
ret = ClosestToReferencePoseStrategy(result);
break;
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
return std::nullopt;
}
if (!ret) {
// TODO
}
return ret;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
PhotonPipelineResult result) {
int lowestAJ = -1;
double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
lowestAJ = j;
lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
}
}
if (lowestAJ == -1) {
return std::nullopt;
}
PhotonTrackedTarget bestTarget = targets[lowestAJ];
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::nullopt;
}
return EstimatedRobotPose{
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToCameraHeightStrategy(
PhotonPipelineResult result) {
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
for (auto& target : result.GetTargets()) {
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
if (alternativeDifference < smallestHeightDifference) {
smallestHeightDifference = alternativeDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
}
}
return pose;
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::ClosestToReferencePoseStrategy(
PhotonPipelineResult result) {
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
const auto altPose =
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse());
const auto bestPose =
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse());
units::meter_t alternativeDifference = units::math::abs(
referencePose.Translation().Distance(altPose.Translation()));
units::meter_t bestDifference = units::math::abs(
referencePose.Translation().Distance(bestPose.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
pose = altPose;
stateTimestamp = result.GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = bestPose;
stateTimestamp = result.GetTimestamp();
}
}
return EstimatedRobotPose{pose, stateTimestamp};
}
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
auto targets = result.GetTargets();
for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
// Ambiguity = 0, use that pose
if (target.GetPoseAmbiguity() == 0) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetLatency()};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
}
frc::Translation3d transform = frc::Translation3d();
frc::Rotation3d rotation = frc::Rotation3d();
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
result.GetTimestamp()};
}
} // namespace photonlib

6
photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp
View File

@@ -38,7 +38,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id,
const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners)
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> detectedCorners)
: yaw(yaw),
pitch(pitch),
area(area),
@@ -47,7 +48,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
bestCameraToTarget(pose),
altCameraToTarget(alternatePose),
poseAmbiguity(ambiguity),
minAreaRectCorners(minAreaRectCorners) {}
minAreaRectCorners(minAreaRectCorners),
detectedCorners(detectedCorners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&

60
photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp
View File

@@ -54,11 +54,12 @@ RobotPoseEstimator::RobotPoseEstimator(
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
if (cameras.empty()) {
return std::make_pair(lastPose, units::millisecond_t(0));
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::millisecond_t> pair;
std::pair<frc::Pose3d, units::second_t> pair;
switch (strategy) {
case LOWEST_AMBIGUITY:
pair = LowestAmbiguityStrategy();
@@ -73,7 +74,7 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
lastPose = pair.first;
return pair;
case CLOSEST_TO_LAST_POSE:
referencePose = lastPose;
SetReferencePose(lastPose);
pair = ClosestToReferencePoseStrategy();
lastPose = pair.first;
return pair;
@@ -85,10 +86,11 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
}
return std::make_pair(lastPose, units::millisecond_t(0));
return std::make_pair(lastPose, units::second_t(0));
}
std::pair<frc::Pose3d, units::millisecond_t>
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::LowestAmbiguityStrategy() {
int lowestAI = -1;
int lowestAJ = -1;
@@ -107,7 +109,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
}
if (lowestAI == -1 || lowestAJ == -1) {
return std::make_pair(lastPose, units::millisecond_t(0));
return std::make_pair(lastPose, units::second_t(0));
}
PhotonTrackedTarget bestTarget =
@@ -119,20 +121,21 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::make_pair(lastPose, units::millisecond_t(0));
return std::make_pair(lastPose, units::second_t(0));
}
return std::make_pair(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(cameras[lowestAI].second.Inverse()),
cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
cameras[lowestAI].first->GetLatestResult().GetTimestamp());
}
std::pair<frc::Pose3d, units::millisecond_t>
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToCameraHeightStrategy() {
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::millisecond_t milli = units::millisecond_t(0);
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -161,22 +164,23 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
smallestHeightDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
milli = p.first->GetLatestResult().GetLatency() / 1000.;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
milli = p.first->GetLatestResult().GetLatency() / 1000.;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, milli);
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::millisecond_t>
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::ClosestToReferencePoseStrategy() {
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::millisecond_t milli = units::millisecond_t(0);
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -207,29 +211,32 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
smallestDifference = alternativeDifference;
pose = targetPose.TransformBy(
target.GetAlternateCameraToTarget().Inverse());
milli = p.first->GetLatestResult().GetLatency() / 1000.;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
if (bestDifference < smallestDifference) {
smallestDifference = bestDifference;
pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
milli = p.first->GetLatestResult().GetLatency() / 1000.;
stateTimestamp = p.first->GetLatestResult().GetTimestamp();
}
}
}
return std::make_pair(pose, milli);
return std::make_pair(pose, stateTimestamp);
}
std::pair<frc::Pose3d, units::millisecond_t>
std::pair<frc::Pose3d, units::second_t>
RobotPoseEstimator::AverageBestTargetsStrategy() {
std::vector<std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>>
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
units::second_t timstampSum = units::second_t(0);
for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
std::span<const PhotonTrackedTarget> targets =
p.first->GetLatestResult().GetTargets();
timstampSum += p.first->GetLatestResult().GetTimestamp();
for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
PhotonTrackedTarget target = targets[j];
std::optional<frc::Pose3d> fiducialPose =
@@ -255,20 +262,21 @@ RobotPoseEstimator::AverageBestTargetsStrategy() {
tempPoses.push_back(std::make_pair(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
std::make_pair(target.GetPoseAmbiguity(),
p.first->GetLatestResult().GetLatency() / 1000.)));
p.first->GetLatestResult().GetTimestamp())));
}
}
frc::Translation3d transform = frc::Translation3d();
frc::Rotation3d rotation = frc::Rotation3d();
units::millisecond_t latency = units::millisecond_t(0);
for (std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>& pair :
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
latency += pair.second.second * weight;
}
return std::make_pair(frc::Pose3d(transform, rotation), latency);
return std::make_pair(frc::Pose3d(transform, rotation),
timstampSum / cameras.size());
}
} // namespace photonlib

2
photon-lib/src/main/native/include/photonlib/PhotonCamera.h
View File

@@ -68,6 +68,8 @@ class PhotonCamera {
*/
explicit PhotonCamera(const std::string_view cameraName);
PhotonCamera(PhotonCamera&&) = default;
virtual ~PhotonCamera() = default;
/**

196
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h Normal file
View File

@@ -0,0 +1,196 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <map>
#include <memory>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include "photonlib/PhotonCamera.h"
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
};
struct EstimatedRobotPose {
/** The estimated pose */
frc::Pose3d estimatedPose;
/** The estimated time the frame used to derive the robot pose was taken, in
* the same timebase as the RoboRIO FPGA Timestamp */
units::second_t timestamp;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
: estimatedPose(pose_), timestamp(time_) {}
};
/**
* The PhotonPoseEstimator class filters or combines readings from all the
* fiducials visible at a given timestamp on the field to produce a single robot
* in field pose, using the strategy set below. Example usage can be found in
* our apriltagExample example project.
*/
class PhotonPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy, PhotonCamera&& camera,
frc::Transform3d robotToCamera);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; }
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Update the pose estimator. Internally grabs a new PhotonPipelineResult from
* the camera and process it.
*/
std::optional<EstimatedRobotPose> Update();
inline PhotonCamera& GetCamera() { return camera; }
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PhotonCamera camera;
frc::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
frc::Pose3d referencePose;
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
PhotonPipelineResult result);
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this
* estimation.
*/
std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
PhotonPipelineResult result);
};
} // namespace photonlib

3
photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h
View File

@@ -59,7 +59,8 @@ class PhotonTrackedTarget {
double yaw, double pitch, double area, double skew, int fiducialID,
const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> corners);
const wpi::SmallVector<std::pair<double, double>, 4> corners,
const std::vector<std::pair<double, double>> detectedCorners);
/**
* Returns the target yaw (positive-left).

111
photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
View File

@@ -48,7 +48,10 @@ enum PoseStrategy : int {
};
/**
* A managing class to determine how an estimated pose should be chosen.
* The RobotPoseEstimator class filters or combines readings from all the
* fiducials visible at a given timestamp on the field to produce a single robot
* in field pose, using the strategy set below. Example usage can be found in
* our apriltagExample example project.
*/
class RobotPoseEstimator {
public:
@@ -59,44 +62,81 @@ class RobotPoseEstimator {
/**
* Create a new RobotPoseEstimator.
*
* @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs
* to Pose3ds with respect to the FIRST field.
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param cameras An ArrayList of Pairs of PhotonCameras and their respective
* Transform3ds from the center of the robot to the camera mount positions
* (ie, robot -> camera).
* Transform3ds from the center of the robot to the cameras.
*/
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
/**
* Update the estimated pose using the selected strategy.
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return The updated estimated pose and the latency in milliseconds.
* @return the AprilTagFieldLayout
*/
std::pair<frc::Pose3d, units::millisecond_t> Update();
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
return aprilTags;
}
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Set the cameras to be used by the PoseEstimator.
*
* @param cameras cameras to set.
*/
inline void SetCameras(
const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
frc::Transform3d>>& cameras) {
this->cameras = cameras;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
*
* @return the strategy
*/
PoseStrategy GetPoseStrategy() const { return strategy; }
frc::Pose3d GetLastPose() const { return lastPose; }
/**
* Set the Position Estimation Strategy used by the Position Estimator.
*
* @param strategy the strategy to set
*/
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
/**
* Return the reference position that is being used by the estimator.
*
* @return the referencePose
*/
frc::Pose3d GetReferencePose() const { return referencePose; }
/**
* Update the stored reference pose for use when using the
* CLOSEST_TO_REFERENCE_POSE strategy.
*
* @param referencePose the referencePose to set
*/
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
}
/**
* Update the stored last pose. Useful for setting the initial estimate when
* using the CLOSEST_TO_LAST_POSE strategy.
*
* @param lastPose the lastPose to set
*/
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
std::pair<frc::Pose3d, units::second_t> Update();
private:
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
PoseStrategy strategy;
@@ -104,13 +144,44 @@ class RobotPoseEstimator {
frc::Pose3d lastPose;
frc::Pose3d referencePose;
std::pair<frc::Pose3d, units::millisecond_t> LowestAmbiguityStrategy();
/**
* Return the estimated position of the robot with the lowest position
* ambiguity from a List of pipeline results.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
std::pair<frc::Pose3d, units::millisecond_t> ClosestToCameraHeightStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta height difference between the estimated and actual height of the
* camera.
*
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
std::pair<frc::Pose3d, units::millisecond_t> ClosestToReferencePoseStrategy();
/**
* Return the estimated position of the robot using the target with the lowest
* delta in the vector magnitude between it and the reference pose.
*
* @param referencePose reference pose to check vector magnitude difference
* against.
* @return the estimated position of the robot in the FCS and the estimated
* timestamp of this estimation.
*/
std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
std::pair<frc::Pose3d, units::millisecond_t> AverageBestTargetsStrategy();
/**
* Return the average of the best target poses using ambiguity as weight.
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this
* estimation.
*/
std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
};
} // namespace photonlib

1
photon-lib/src/main/native/include/photonlib/SimVisionSystem.h
View File

@@ -193,6 +193,7 @@ class SimVisionSystem {
camToTargetTransform,
// TODO ambiguity
0.0,
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
}