Skip to content

Commit

Permalink
Fix tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed Aug 4, 2024
1 parent 72e0ef4 commit 2ec688b
Show file tree
Hide file tree
Showing 6 changed files with 82 additions and 54 deletions.
31 changes: 16 additions & 15 deletions photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,18 +210,17 @@ PhotonPipelineResult PhotonCameraSim::Process(

std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
std::vector<TargetCorner> smallVec;

for (const auto& corner : tempCorners) {
smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
smallVec.emplace_back(static_cast<double>(corner.first),
static_cast<double>(corner.second));
}

std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
auto cornersFloat = OpenCVHelp::PointsToTargetCorners(noisyTargetCorners);

std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
Expand Down Expand Up @@ -277,17 +276,15 @@ PhotonPipelineResult PhotonCameraSim::Process(
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
OpenCVHelp::CornersToPoints(detectedCornersDouble),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
OpenCVHelp::CornersToPoints(detectedCornersDouble)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
Expand Down Expand Up @@ -316,7 +313,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}

MultiTargetPNPResult multiTagResults{};
std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;

std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
Expand All @@ -330,12 +327,16 @@ PhotonPipelineResult PhotonCameraSim::Process(
auto pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
if (pnpResult) {
multiTagResults = MultiTargetPNPResult{*pnpResult, usedIds};
}
}

heartbeatCounter++;
return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
multiTagResults};
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<double>()},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
Expand Down
49 changes: 30 additions & 19 deletions photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,15 +51,17 @@ static std::vector<frc::AprilTag> tags = {

static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};

static wpi::SmallVector<std::pair<double, double>, 4> corners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1, 2}, photon::TargetCorner{3, 4},
photon::TargetCorner{5, 6}, photon::TargetCorner{7, 8}};
static std::vector<photon::TargetCorner> detectedCorners{
photon::TargetCorner{1, 2}, photon::TargetCorner{3, 4},
photon::TargetCorner{5, 6}, photon::TargetCorner{7, 8}};

TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
Expand All @@ -83,7 +85,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));

photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
Expand Down Expand Up @@ -118,7 +121,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
// ID 0 at 3,3,3
// ID 1 at 5,5,5

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
Expand All @@ -142,7 +145,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(17_s);

photon::PhotonPoseEstimator estimator(
Expand All @@ -165,7 +169,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
Expand All @@ -189,7 +193,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags,
Expand All @@ -214,7 +219,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
Expand All @@ -238,7 +243,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));

photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
Expand All @@ -254,7 +260,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targetsThree{
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
Expand All @@ -277,7 +283,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};

cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));

// std::optional<photon::EstimatedRobotPose> estimatedPose;
Expand All @@ -298,7 +305,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
Expand All @@ -322,7 +329,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};

cameraOne.test = true;
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));

photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
Expand All @@ -345,7 +353,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
TEST(PhotonPoseEstimatorTest, PoseCache) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");

wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
Expand Down Expand Up @@ -374,7 +382,9 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
{});

// empty input, expect empty out
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000},
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));

std::optional<photon::EstimatedRobotPose> estimatedPose;
Expand All @@ -385,7 +395,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);

// Set result, and update -- expect present and timestamp to be 15
cameraOne.testResult = {{0, 0_s, 3_ms, targets}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));

for (const auto& result : cameraOne.GetAllUnreadResults()) {
Expand Down
10 changes: 6 additions & 4 deletions photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -439,9 +439,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan) {
targets.push_back(tar);
}
photon::PnpResult results = photon::VisionEstimation::EstimateCamPosePNP(
auto results = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
frc::Pose3d pose = frc::Pose3d{} + results.best;
ASSERT_TRUE(results);
frc::Pose3d pose = frc::Pose3d{} + results->best;
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
Expand All @@ -460,9 +461,10 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
for (photon::PhotonTrackedTarget tar : targetSpan2) {
targets2.push_back(tar);
}
photon::PnpResult results2 = photon::VisionEstimation::EstimateCamPosePNP(
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
frc::Pose3d pose2 = frc::Pose3d{} + results2.best;
ASSERT_TRUE(results2);
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
ASSERT_NEAR(5, pose2.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
Expand Down
6 changes: 4 additions & 2 deletions photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,7 +170,8 @@ struct SerdeType<std::vector<T>> {
static constexpr std::string_view GetSchema() {
// TODO: this gets us the plain type name of T, but this is not schema JSON
// compliant!
return wpi::Demangle(typeid(T).name());
std::string name = wpi::Demangle(typeid(T).name());
return name;
}
};

Expand Down Expand Up @@ -199,7 +200,8 @@ struct SerdeType<std::optional<T>> {
static constexpr std::string_view GetSchema() {
// TODO: this gets us the plain type name of T, but this is not schema JSON
// compliant!
return wpi::Demangle(typeid(T).name());
std::string name = wpi::Demangle(typeid(T).name());
return name;
}
};

Expand Down
23 changes: 23 additions & 0 deletions photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@
#include "photon/targeting/PnpResult.h"
#include "photon/targeting/MultiTargetPNPResult.h"

#include "photon/targeting/TargetCorner.h"

namespace photon {
namespace OpenCVHelp {

Expand Down Expand Up @@ -96,6 +98,16 @@ static std::vector<cv::Point3f> RotationToRVec(
return points[0];
}

[[maybe_unused]] static std::vector<photon::TargetCorner> PointsToTargetCorners(
const std::vector<cv::Point2f>& points) {
std::vector<photon::TargetCorner> retVal;
retVal.reserve(points.size());
for (size_t i = 0; i < points.size(); i++) {
retVal.emplace_back(photon::TargetCorner{points[i].x, points[i].y});
}
return retVal;
}

[[maybe_unused]] static std::vector<std::pair<float, float>> PointsToCorners(
const std::vector<cv::Point2f>& points) {
std::vector<std::pair<float, float>> retVal;
Expand All @@ -116,6 +128,17 @@ static std::vector<cv::Point3f> RotationToRVec(
return retVal;
}

[[maybe_unused]] static std::vector<cv::Point2f> CornersToPoints(
const std::vector<photon::TargetCorner>& corners) {
std::vector<cv::Point2f> retVal;
retVal.reserve(corners.size());
for (size_t i = 0; i < corners.size(); i++) {
retVal.emplace_back(cv::Point2f{static_cast<float>(corners[i].x),
static_cast<float>(corners[i].y)});
}
return retVal;
}

[[maybe_unused]] static cv::Rect GetBoundingRect(
const std::vector<cv::Point2f>& points) {
return cv::boundingRect(points);
Expand Down
17 changes: 3 additions & 14 deletions photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ static std::optional<PnpResult> EstimateCamPosePNP(
return PnpResult();
}

std::vector<std::pair<float, float>> corners{};
std::vector<photon::TargetCorner> corners{};
std::vector<frc::AprilTag> knownTags{};

for (const auto& tgt : visTags) {
Expand Down Expand Up @@ -101,19 +101,8 @@ static std::optional<PnpResult> EstimateCamPosePNP(
auto verts = tagModel.GetFieldVertices(tag.pose);
objectTrls.insert(objectTrls.end(), verts.begin(), verts.end());
}
auto camToOrigin = OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs,
objectTrls, points);
if (!camToOrigin) {
return std::nullopt;
} else {
PnpResult ret{};
ret.best = camToOrigin.best.Inverse(),
ret.alt = camToOrigin.alt.Inverse(),
ret.ambiguity = camToOrigin.ambiguity;
ret.bestReprojErr = camToOrigin.bestReprojErr;
ret.altReprojErr = camToOrigin.altReprojErr;
return ret;
}
return OpenCVHelp::SolvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls,
points);
}
}

Expand Down

0 comments on commit 2ec688b

Please sign in to comment.