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

[photon-targeting] Move C++ targeting classes to photon-targetting (#1009)

Browse Source 
* add classes to targeting and update gradle

* rename me

* Finish cleanup

* Formatting fixes

* just use common.gradle

* Update build.gradle

* Update config.gradle

* remove typo

* simplify

* Add Packet Headers

* move simulation classes into simulation folder

* draw in dependency

* fix

* Everything working minus tests cause im lazy

* formatting fixes

REMEMBER TO REMOVE UNUSED IMPORTS, IM JUST TOO LAZY TO CHECK RN

* move packet test to targeting

* Formatting fixes

* remove TargetCorner from c++

im not 100% sure but just doing std::pair<double, double> is sufficient and shouldnt conflict with protobuf

* think i added packet

* fix namespace issue

* organize imports in photon-targeting

* Formatting fixes

* remove ctors

* fix typo

* Add PNP and Multitag packet tests

* revert TargetCorner class

* Add Test placeholders

* remove annoying print

* Reorganize build and publish process

channeling inner Thad

* add targeting as flag

* Update config.gradle

* fix issue with platform binaries not building

* Update photonlib.json.in

casing still needs to be checked

* add minimum level back

* add back UTF-8 encoding of javadoc

* simplify publish model for photon-lib

* fix windows symbol generation

* formatting fixes

* move task from main gradle to config

* Update config.gradle
This commit is contained in:
Sriman Achanta
2023-11-19 15:16:22 -05:00
committed by GitHub
parent 308fd801d4
commit 994ea1e76b
50 changed files with 1132 additions and 910 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,10 +35,13 @@
#include <wpi/SmallVector.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonPoseEstimator.h"
#include "photonlib/PhotonTrackedTarget.h"
#include "photon/PhotonCamera.h"
#include "photon/PhotonPoseEstimator.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
static std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -54,24 +57,24 @@ static std::vector<std::pair<double, double>> detectedCorners{
std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
@@ -83,8 +86,8 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(11));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -104,29 +107,29 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photonlib::PhotonCamera, frc::Transform3d>> cameras;
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -138,8 +141,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(17_s);
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
{{0_m, 0_m, 4_m}, {}});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -152,24 +155,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -181,9 +184,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(aprilTags,
photonlib::CLOSEST_TO_REFERENCE_POSE,
std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
@@ -197,24 +199,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
}
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -226,30 +228,30 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0,
frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -272,24 +274,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
}
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -301,8 +303,8 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(15));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -314,24 +316,24 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
}
TEST(PhotonPoseEstimatorTest, PoseCache) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test2");
photon::PhotonCamera cameraOne = photon::PhotonCamera("test2");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::PhotonTrackedTarget{
wpi::SmallVector<photon::PhotonTrackedTarget, 3> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0,
frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1,
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0,
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
@@ -341,8 +343,8 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
cameraOne.test = true;
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
// empty input, expect empty out
estimator.GetCamera()->testResult = {2_ms, {}};