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

Add PhotonPoseEstimator constructor without a PhotonCamera instance (#840)

Browse Source 
- Made alternate constructor for ```PhotonPoseEstimator``` that doesn't need ```PhotonCamera```
- Changed ```update``` function to take in missing cameraMatrixData and coeffsData that were previously received from PhotonCamera.
- Changed the internal ```update``` and ```multiTagPNP``` function to take in cameraMatrixData & coeffsData
- If not needed for the specified strategy then the parameters are simply not used. Also if PhotonCamera is used in constructor it instead backs up to that.

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Murad
2023-04-18 12:50:23 -05:00
committed by GitHub
parent 78ffa415fc
commit 2d586fe1c0
5 changed files with 127 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@@ -10,6 +10,8 @@ Python/app/handlers/__pycache__/
\.vscode/
/.vs
backend/settings/
/.vscode/
# Compiled class file

63
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -26,12 +26,16 @@ package org.photonvision;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.Matrix;
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.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.ArrayList;
import java.util.HashSet;
@@ -85,14 +89,14 @@ public class PhotonPoseEstimator {
* Create a new PhotonPoseEstimator.
*
* @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects
* with respect to the FIRST field using the <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* with respect to the FIRST field using the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field
* Coordinate System</a>.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCamera
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a
* href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(
@@ -106,6 +110,14 @@ public class PhotonPoseEstimator {
this.robotToCamera = robotToCamera;
}
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.camera = null;
this.robotToCamera = robotToCamera;
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
@@ -251,7 +263,7 @@ public class PhotonPoseEstimator {
PhotonPipelineResult cameraResult = camera.getLatestResult();
return update(cameraResult);
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}
/**
@@ -263,12 +275,33 @@ public class PhotonPoseEstimator {
* pipeline results used to create the estimate
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}
/**
* Updates the estimated position of the robot. Returns empty if there are no cameras set or no
* targets were found from the cameras.
*
* @param cameraResult The latest pipeline result from the camera
* @param cameraMatrixData Camera calibration data that can be used in the case of no assigned
* PhotonCamera.
* @param coeffsData Camera calibration data that can be used in the case of no assigned
* PhotonCamera
* @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(
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrixData,
Optional<Matrix<N5, N1>> coeffsData) {
// Time in the past -- give up, since the following if expects times > 0
if (cameraResult.getTimestampSeconds() < 0) {
return Optional.empty();
}
// If the pose cache timestamp was set, and the result is from the same timestamp, return an
// If the pose cache timestamp was set, and the result is from the same
// timestamp, return an
// empty result
if (poseCacheTimestampSeconds > 0
&& Math.abs(poseCacheTimestampSeconds - cameraResult.getTimestampSeconds()) < 1e-6) {
@@ -283,11 +316,14 @@ public class PhotonPoseEstimator {
return Optional.empty();
}
return update(cameraResult, this.primaryStrategy);
return update(cameraResult, cameraMatrixData, coeffsData, this.primaryStrategy);
}
private Optional<EstimatedRobotPose> update(
PhotonPipelineResult cameraResult, PoseStrategy strat) {
PhotonPipelineResult cameraResult,
Optional<Matrix<N3, N3>> cameraMatrixData,
Optional<Matrix<N5, N1>> coeffsData,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
switch (strat) {
case LOWEST_AMBIGUITY:
@@ -307,7 +343,7 @@ public class PhotonPoseEstimator {
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP:
estimatedPose = multiTagPNPStrategy(cameraResult);
estimatedPose = multiTagPNPStrategy(cameraResult, cameraMatrixData, coeffsData);
break;
default:
DriverStation.reportError(
@@ -322,7 +358,10 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagPNPStrategy(PhotonPipelineResult result) {
private Optional<EstimatedRobotPose> multiTagPNPStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N5, N1>> distCoeffsOpt) {
// Arrays we need declared up front
var visCorners = new ArrayList<TargetCorner>();
var knownVisTags = new ArrayList<AprilTag>();
@@ -331,7 +370,7 @@ public class PhotonPoseEstimator {
if (result.getTargets().size() < 2) {
// Run fallback strategy instead
return update(result, this.multiTagFallbackStrategy);
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}
for (var target : result.getTargets()) {
@@ -352,8 +391,6 @@ public class PhotonPoseEstimator {
fieldToCamsAlt.add(tagPose.transformBy(target.getAlternateCameraToTarget().inverse()));
}
var cameraMatrixOpt = camera.getCameraMatrix();
var distCoeffsOpt = camera.getDistCoeffs();
boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
// multi-target solvePNP

47
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
View File

@@ -58,12 +58,23 @@ cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat,
frc::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
camera(nullptr),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s) {}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat, PhotonCamera&& cam,
frc::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
camera(std::move(cam)),
camera(std::make_shared<PhotonCamera>(std::move(cam))),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
@@ -84,12 +95,27 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
auto result = camera.GetLatestResult();
return Update(result);
if (!camera) {
FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!",
"");
return std::nullopt;
}
auto result = camera->GetLatestResult();
return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result) {
// If camera is null, best we can do is pass null calibration data
if (!camera) {
return Update(result, std::nullopt, std::nullopt, this->strategy);
}
return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
return std::nullopt;
@@ -110,11 +136,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return std::nullopt;
}
return Update(result, this->strategy);
return Update(result, cameraMatrixData, coeffsData, this->strategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
PhotonPipelineResult result, PoseStrategy strategy) {
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData, PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
@@ -135,7 +162,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
ret = AverageBestTargetsStrategy(result);
break;
case ::photonlib::MULTI_TAG_PNP:
ret = MultiTagPnpStrategy(result);
ret = MultiTagPnpStrategy(result, coeffsData, cameraMatrixData);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -328,11 +355,13 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
PhotonPipelineResult result) {
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs) {
using namespace frc;
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}
auto const targets = result.GetTargets();
@@ -364,8 +393,6 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
cv::Mat const rvec(3, 1, cv::DataType<double>::type);
cv::Mat const tvec(3, 1, cv::DataType<double>::type);
auto const camMat = camera.GetCameraMatrix();
auto const distCoeffs = camera.GetDistCoeffs();
if (!camMat || !distCoeffs) {
return std::nullopt;
}

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

@@ -90,6 +90,23 @@ class PhotonPoseEstimator {
PoseStrategy strategy, PhotonCamera&& camera,
frc::Transform3d robotToCamera);
/**
* 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 robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
@@ -181,14 +198,22 @@ class PhotonPoseEstimator {
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
inline PhotonCamera& GetCamera() { return camera; }
/**
* Update the pose estimator.
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData);
inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
PhotonCamera camera;
std::shared_ptr<PhotonCamera> camera;
frc::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
@@ -198,8 +223,9 @@ class PhotonPoseEstimator {
inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
std::optional<EstimatedRobotPose> Update(PhotonPipelineResult result,
PoseStrategy strategy);
std::optional<EstimatedRobotPose> Update(
PhotonPipelineResult result, std::optional<cv::Mat> cameraMatrixData,
std::optional<cv::Mat> coeffsData, PoseStrategy strategy);
/**
* Return the estimated position of the robot with the lowest position
@@ -242,7 +268,8 @@ class PhotonPoseEstimator {
timestamp of this estimation.
*/
std::optional<EstimatedRobotPose> MultiTagPnpStrategy(
PhotonPipelineResult result);
PhotonPipelineResult result, std::optional<cv::Mat> camMat,
std::optional<cv::Mat> distCoeffs);
/**
* Return the average of the best target poses using ambiguity as weight.

12
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -257,8 +257,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
estimator.GetCamera().testResult = {2_ms, targetsThree};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(21));
estimator.GetCamera()->testResult = {2_ms, targetsThree};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21));
estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
@@ -345,14 +345,14 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
// empty input, expect empty out
estimator.GetCamera().testResult = {2_ms, {}};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
estimator.GetCamera()->testResult = {2_ms, {}};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera().testResult = {3_ms, targets};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
estimator.GetCamera()->testResult = {3_ms, targets};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);