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

Add Constrained PNP Pose Strategies to C++ photonlib (#1908)

Browse Source 
This adds the two missing pose strategies from the java version of
photonlib (Constrained PNP and the Trig solve), to C++ photonlib

---------

Co-authored-by: Matthew Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
Drew Williams
2025-10-12 12:26:12 -07:00
committed by GitHub
parent 099c88e0b7
commit b89ab49d34
7 changed files with 409 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View File

@@ -42,7 +42,7 @@ static frc::Rotation3d NWU_TO_EDN{
static frc::Rotation3d EDN_TO_NWU{
(Eigen::Matrix3d() << 0, 0, 1, -1, 0, 0, 0, -1, 0).finished()};
static std::vector<cv::Point2f> GetConvexHull(
[[maybe_unused]] static std::vector<cv::Point2f> GetConvexHull(
const std::vector<cv::Point2f>& points) {
std::vector<int> outputHull{};
cv::convexHull(points, outputHull);

90
photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
View File

@@ -17,102 +17,36 @@
#pragma once
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <frc/apriltag/AprilTag.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include "OpenCVHelp.h"
#include "TargetModel.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.h"
namespace photon {
namespace VisionEstimation {
[[maybe_unused]] static std::vector<frc::AprilTag> GetVisibleLayoutTags(
std::vector<frc::AprilTag> GetVisibleLayoutTags(
const std::vector<PhotonTrackedTarget>& visTags,
const frc::AprilTagFieldLayout& layout) {
std::vector<frc::AprilTag> retVal{};
for (const auto& tag : visTags) {
int id = tag.GetFiducialId();
auto maybePose = layout.GetTagPose(id);
if (maybePose) {
retVal.emplace_back(frc::AprilTag{id, maybePose.value()});
}
}
return retVal;
}
const frc::AprilTagFieldLayout& layout);
#include <iostream>
[[maybe_unused]] static std::optional<PnpResult> EstimateCamPosePNP(
std::optional<photon::PnpResult> EstimateCamPosePNP(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
const std::vector<PhotonTrackedTarget>& visTags,
const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel) {
if (visTags.size() == 0) {
return PnpResult();
}
const frc::AprilTagFieldLayout& layout, const TargetModel& tagModel);
std::vector<photon::TargetCorner> corners{};
std::vector<frc::AprilTag> knownTags{};
for (const auto& tgt : visTags) {
int id = tgt.GetFiducialId();
auto maybePose = layout.GetTagPose(id);
if (maybePose) {
knownTags.emplace_back(frc::AprilTag{id, maybePose.value()});
auto currentCorners = tgt.GetDetectedCorners();
corners.insert(corners.end(), currentCorners.begin(),
currentCorners.end());
}
}
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return PnpResult{};
}
std::vector<cv::Point2f> points = OpenCVHelp::CornersToPoints(corners);
if (knownTags.size() == 1) {
auto camToTag = OpenCVHelp::SolvePNP_Square(cameraMatrix, distCoeffs,
tagModel.GetVertices(), points);
if (!camToTag) {
return PnpResult{};
}
frc::Pose3d bestPose =
knownTags[0].pose.TransformBy(camToTag->best.Inverse());
frc::Pose3d altPose{};
if (camToTag->ambiguity != 0) {
altPose = knownTags[0].pose.TransformBy(camToTag->alt.Inverse());
}
frc::Pose3d o{};
PnpResult result{};
result.best = frc::Transform3d{o, bestPose};
result.alt = frc::Transform3d{o, altPose};
result.ambiguity = camToTag->ambiguity;
result.bestReprojErr = camToTag->bestReprojErr;
result.altReprojErr = camToTag->altReprojErr;
return result;
} else {
std::vector<frc::Translation3d> objectTrls{};
for (const auto& tag : knownTags) {
auto verts = tagModel.GetFieldVertices(tag.pose);
objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
}
auto ret = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
points);
if (ret) {
// Invert best/alt transforms
ret->best = ret->best.Inverse();
ret->alt = ret->alt.Inverse();
}
return ret;
}
}
std::optional<photon::PnpResult> EstimateRobotPoseConstrainedSolvePNP(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,
const std::vector<photon::PhotonTrackedTarget>& visTags,
const frc::Transform3d& robot2Camera, const frc::Pose3d& robotPoseSeed,
const frc::AprilTagFieldLayout& layout, const photon::TargetModel& tagModel,
bool headingFree, frc::Rotation2d gyroTheta, double gyroErrorScaleFac);
} // namespace VisionEstimation
} // namespace photon