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

Multi-tag pnp in robot code (#787)

Browse Source 
---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>
This commit is contained in:
Matt
2023-02-13 17:57:01 -05:00
committed by GitHub
parent a2dfe48679
commit 5b86360b9b
33 changed files with 1785 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,6 +36,9 @@
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
@@ -43,6 +46,16 @@
#include "photonlib/PhotonTrackedTarget.h"
namespace photonlib {
namespace detail {
cv::Point3d ToPoint3d(const frc::Translation3d& translation);
std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags);
frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat, PhotonCamera&& cam,
frc::Transform3d robotToCamera)
@@ -53,6 +66,17 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
FRC_ReportError(
frc::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
"");
strategy = LOWEST_AMBIGUITY;
}
multiTagFallbackStrategy = strategy;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
auto result = camera.GetLatestResult();
return Update(result);
@@ -64,6 +88,11 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
return std::nullopt;
}
return Update(result, this->strategy);
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
PhotonPipelineResult result, PoseStrategy strategy) {
std::optional<EstimatedRobotPose> ret = std::nullopt;
switch (strategy) {
@@ -83,6 +112,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
case AVERAGE_BEST_TARGETS:
ret = AverageBestTargetsStrategy(result);
break;
case ::photonlib::MULTI_TAG_PNP:
ret = MultiTagPnpStrategy(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
@@ -127,7 +159,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
fiducialPose.value()
.TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
std::optional<EstimatedRobotPose>
@@ -147,14 +179,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
frc::Pose3d const targetPose = fiducialPose.value();
units::meter_t alternativeDifference = units::math::abs(
units::meter_t const alternativeDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t bestDifference = units::math::abs(
units::meter_t const bestDifference = units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
@@ -163,14 +195,14 @@ PhotonPoseEstimator::ClosestToCameraHeightStrategy(
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
if (bestDifference < smallestHeightDifference) {
smallestHeightDifference = bestDifference;
pose = EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetTimestamp()};
result.GetTimestamp(), result.GetTargets()};
}
}
@@ -204,9 +236,9 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse());
units::meter_t alternativeDifference = units::math::abs(
units::meter_t const alternativeDifference = units::math::abs(
referencePose.Translation().Distance(altPose.Translation()));
units::meter_t bestDifference = units::math::abs(
units::meter_t const bestDifference = units::math::abs(
referencePose.Translation().Distance(bestPose.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
@@ -221,7 +253,113 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
}
}
return EstimatedRobotPose{pose, stateTimestamp};
return EstimatedRobotPose{pose, stateTimestamp, result.GetTargets()};
}
std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags) {
if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
TagCornerToObjectPoint(+3_in, +3_in, *tagPose),
TagCornerToObjectPoint(-3_in, +3_in, *tagPose)};
} else {
return std::nullopt;
}
}
cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
+translation.X().value());
}
cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY,
frc::Pose3d tagPose) {
frc::Translation3d cornerTrans =
tagPose.Translation() +
frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
return ToPoint3d(cornerTrans);
}
frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace frc;
using namespace units;
cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3
R = R.t(); // rotation of inverse
cv::Mat tvecI = -R * tvec; // translation of inverse
Vectord<3> tv;
tv[0] = +tvecI.at<double>(2, 0);
tv[1] = -tvecI.at<double>(0, 0);
tv[2] = -tvecI.at<double>(1, 0);
Vectord<3> rv;
rv[0] = +rvec.at<double>(2, 0);
rv[1] = -rvec.at<double>(0, 0);
rv[2] = +rvec.at<double>(1, 0);
return Pose3d(Translation3d(meter_t{tv[0]}, meter_t{tv[1]}, meter_t{tv[2]}),
Rotation3d(
// radian_t{rv[0]},
// radian_t{rv[1]},
// radian_t{rv[2]}
rv, radian_t{rv.norm()}));
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagPnpStrategy(
PhotonPipelineResult result) {
using namespace frc;
if (!result.HasTargets() || result.GetTargets().size() < 2) {
return Update(result, this->multiTagFallbackStrategy);
}
auto const targets = result.GetTargets();
// List of corners mapped from 3d space (meters) to the 2d camera screen
// (pixels).
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
// Add all target corners to main list of corners
for (auto target : targets) {
int id = target.GetFiducialId();
if (auto const tagCorners = detail::CalcTagCorners(id, aprilTags);
tagCorners.has_value()) {
auto const targetCorners = target.GetDetectedCorners();
for (size_t cornerIdx = 0; cornerIdx < 4; ++cornerIdx) {
imagePoints.emplace_back(targetCorners[cornerIdx].first,
targetCorners[cornerIdx].second);
objectPoints.emplace_back((*tagCorners)[cornerIdx]);
}
}
}
if (imagePoints.empty()) {
return std::nullopt;
}
// Use OpenCV ITERATIVE solver
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;
}
cv::solvePnP(objectPoints, imagePoints, camMat.value(), distCoeffs.value(),
rvec, tvec, false, cv::SOLVEPNP_SQPNP);
Pose3d const pose = detail::ToPose3d(tvec, rvec);
return photonlib::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), result.GetTimestamp(),
result.GetTargets());
}
std::optional<EstimatedRobotPose>
@@ -247,7 +385,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
return EstimatedRobotPose{
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse()),
result.GetLatency()};
result.GetTimestamp(), result.GetTargets()};
}
totalAmbiguity += 1. / target.GetPoseAmbiguity();
@@ -261,12 +399,12 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
double weight = (1. / pair.second.first) / totalAmbiguity;
double const 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()};
result.GetTimestamp(), result.GetTargets()};
}
} // namespace photonlib