Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-24 01:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

Run multitag on coprocessor (#816)

Browse Source
This commit is contained in:
Matt
2023-10-17 10:20:00 -04:00
committed by GitHub
parent ededc4f130
commit 47bd077bbb
72 changed files with 1708 additions and 1801 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h Normal file
View File

@@ -0,0 +1,61 @@
/*
* MIT License
*
* Copyright (c) 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 <frc/geometry/Transform3d.h>
#include <wpi/SmallVector.h>
#include "photonlib/Packet.h"
namespace photonlib {
class PNPResults {
public:
// This could be wrapped in an std::optional, but chose to do it this way to
// mirror Java
bool isValid;
frc::Transform3d best;
double bestReprojectionErr;
frc::Transform3d alt;
double altReprojectionErr;
double ambiguity;
friend Packet& operator<<(Packet& packet, const PNPResults& result);
friend Packet& operator>>(Packet& packet, PNPResults& result);
};
class MultiTargetPnpResult {
public:
PNPResults result;
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result);
friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result);
};
} // namespace photonlib

9
photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h
View File

@@ -31,6 +31,7 @@
#include <units/time.h>
#include <wpi/SmallVector.h>
#include "photonlib/MultiTargetPNPResult.h"
#include "photonlib/Packet.h"
#include "photonlib/PhotonTrackedTarget.h"
@@ -87,6 +88,13 @@ class PhotonPipelineResult {
*/
units::second_t GetTimestamp() const { return timestamp; }
/**
* Return the latest mulit-target result, as calculated on your coprocessor.
* Be sure to check getMultiTagResult().estimatedPose.isValid before using the
* pose estimate!
*/
const MultiTargetPnpResult& MultiTagResult() const { return m_pnpResults; }
/**
* Sets the timestamp in seconds
* @param timestamp The timestamp in seconds
@@ -119,6 +127,7 @@ class PhotonPipelineResult {
units::second_t latency = 0_s;
units::second_t timestamp = -1_s;
wpi::SmallVector<PhotonTrackedTarget, 10> targets;
MultiTargetPnpResult m_pnpResults;
inline static bool HAS_WARNED = false;
};
} // namespace photonlib

25
photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
View File

@@ -44,7 +44,8 @@ enum PoseStrategy {
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS,
MULTI_TAG_PNP
MULTI_TAG_PNP_ON_COPROCESSOR,
MULTI_TAG_PNP_ON_RIO,
};
struct EstimatedRobotPose {
@@ -57,11 +58,16 @@ struct EstimatedRobotPose {
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
/** The strategy actually used to produce this pose */
PoseStrategy strategy;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
std::span<const PhotonTrackedTarget> targets)
std::span<const PhotonTrackedTarget> targets,
PoseStrategy strategy_)
: estimatedPose(pose_),
timestamp(time_),
targetsUsed(targets.data(), targets.data() + targets.size()) {}
targetsUsed(targets.data(), targets.data() + targets.size()),
strategy(strategy_) {}
};
/**
@@ -260,14 +266,23 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
PhotonPipelineResult result);
/**
* Return the pose calculated by combining all tags into one on coprocessor
*
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the pose calculation using all targets in view in the same PNP()
calculation
*
* @return the estimated position of the robot in the FCS and the estimated
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
std::optional<EstimatedRobotPose> MultiTagOnRioStrategy(
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);

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

@@ -1,187 +0,0 @@
/*
* MIT License
*
* Copyright (c) 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/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include "photonlib/PhotonCamera.h"
namespace frc {
class AprilTagFieldLayout;
} // namespace frc
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
CLOSEST_TO_CAMERA_HEIGHT,
CLOSEST_TO_REFERENCE_POSE,
CLOSEST_TO_LAST_POSE,
AVERAGE_BEST_TARGETS
};
/**
* 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:
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 RobotPoseEstimator.
*
* <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 cameras.
*/
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
return aprilTags;
}
/**
* 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; }
/**
* 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;
std::vector<map_value_type> cameras;
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::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
/**
* 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();
/**
* 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();
/**
* 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