Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +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

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}}});
}