mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-22 01:11:40 +00:00
Auto-generate packet dataclasses with Jinja (#1374)
This commit is contained in:
Matt
@@ -177,7 +177,11 @@ public class OpenCVTest {
|
||||
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
|
||||
var pnpSim =
|
||||
OpenCVHelp.solvePNP_SQUARE(
|
||||
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
|
||||
prop.getIntrinsics(),
|
||||
prop.getDistCoeffs(),
|
||||
target.getModel().vertices,
|
||||
targetCorners)
|
||||
.get();
|
||||
|
||||
// check solvePNP estimation accuracy
|
||||
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
|
||||
@@ -212,7 +216,11 @@ public class OpenCVTest {
|
||||
prop.getIntrinsics(), prop.getDistCoeffs(), camRt, target.getFieldVertices());
|
||||
var pnpSim =
|
||||
OpenCVHelp.solvePNP_SQPNP(
|
||||
prop.getIntrinsics(), prop.getDistCoeffs(), target.getModel().vertices, targetCorners);
|
||||
prop.getIntrinsics(),
|
||||
prop.getDistCoeffs(),
|
||||
target.getModel().vertices,
|
||||
targetCorners)
|
||||
.get();
|
||||
|
||||
// check solvePNP estimation accuracy
|
||||
assertSame(relTarget.getRotation(), pnpSim.best.getRotation());
|
||||
|
||||
@@ -37,10 +37,7 @@ class PhotonCameraTest {
|
||||
var packet = new Packet(1);
|
||||
var ret = new PhotonPipelineResult();
|
||||
packet.setData(new byte[0]);
|
||||
if (packet.getSize() < 1) {
|
||||
return;
|
||||
}
|
||||
PhotonPipelineResult.serde.pack(packet, ret);
|
||||
PhotonPipelineResult.photonStruct.pack(packet, ret);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
@@ -130,7 +130,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6));
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
|
||||
@@ -217,7 +217,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (4 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6));
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
@@ -306,7 +306,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (17 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6));
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
@@ -396,7 +396,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
@@ -478,7 +478,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6));
|
||||
|
||||
estimatedPose = estimator.update(cameraOne.result);
|
||||
pose = estimatedPose.get().estimatedPose;
|
||||
@@ -519,7 +519,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8)))));
|
||||
result.setRecieveTimestampMicros((long) (20 * 1e6));
|
||||
result.setReceiveTimestampMicros((long) (20 * 1e6));
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
@@ -529,7 +529,7 @@ class PhotonPoseEstimatorTest {
|
||||
|
||||
// Empty result, expect empty result
|
||||
cameraOne.result = new PhotonPipelineResult();
|
||||
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
|
||||
cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6));
|
||||
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
|
||||
assertFalse(estimatedPose.isPresent());
|
||||
|
||||
@@ -629,7 +629,7 @@ class PhotonPoseEstimatorTest {
|
||||
new TargetCorner(3, 4),
|
||||
new TargetCorner(5, 6),
|
||||
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
|
||||
cameraOne.result.setRecieveTimestampMicros(20 * 1000000);
|
||||
cameraOne.result.setReceiveTimestampMicros(20 * 1000000);
|
||||
|
||||
PhotonPoseEstimator estimator =
|
||||
new PhotonPoseEstimator(
|
||||
|
||||
@@ -481,11 +481,12 @@ class VisionSystemSimTest {
|
||||
visionSysSim.update(robotPose);
|
||||
var results =
|
||||
VisionEstimation.estimateCamPosePNP(
|
||||
camera.getCameraMatrix().get(),
|
||||
camera.getDistCoeffs().get(),
|
||||
camera.getLatestResult().getTargets(),
|
||||
layout,
|
||||
TargetModel.kAprilTag16h5);
|
||||
camera.getCameraMatrix().get(),
|
||||
camera.getDistCoeffs().get(),
|
||||
camera.getLatestResult().getTargets(),
|
||||
layout,
|
||||
TargetModel.kAprilTag16h5)
|
||||
.get();
|
||||
Pose3d pose = new Pose3d().plus(results.best);
|
||||
assertEquals(5, pose.getX(), .01);
|
||||
assertEquals(1, pose.getY(), .01);
|
||||
@@ -500,11 +501,12 @@ class VisionSystemSimTest {
|
||||
visionSysSim.update(robotPose);
|
||||
results =
|
||||
VisionEstimation.estimateCamPosePNP(
|
||||
camera.getCameraMatrix().get(),
|
||||
camera.getDistCoeffs().get(),
|
||||
camera.getLatestResult().getTargets(),
|
||||
layout,
|
||||
TargetModel.kAprilTag16h5);
|
||||
camera.getCameraMatrix().get(),
|
||||
camera.getDistCoeffs().get(),
|
||||
camera.getLatestResult().getTargets(),
|
||||
layout,
|
||||
TargetModel.kAprilTag16h5)
|
||||
.get();
|
||||
pose = new Pose3d().plus(results.best);
|
||||
assertEquals(5, pose.getX(), .01);
|
||||
assertEquals(1, pose.getY(), .01);
|
||||
|
||||
@@ -39,9 +39,9 @@
|
||||
#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"
|
||||
#include "photon/targeting/PnpResult.h"
|
||||
|
||||
static std::vector<frc::AprilTag> tags = {
|
||||
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
|
||||
@@ -51,31 +51,33 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
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),
|
||||
@@ -83,8 +85,9 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.test = true;
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
|
||||
|
||||
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
|
||||
frc::Transform3d{});
|
||||
@@ -93,6 +96,7 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
|
||||
for (const auto& result : cameraOne.GetAllUnreadResults()) {
|
||||
estimatedPose = estimator.Update(result);
|
||||
}
|
||||
ASSERT_TRUE(estimatedPose);
|
||||
frc::Pose3d pose = estimatedPose.value().estimatedPose;
|
||||
|
||||
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
|
||||
@@ -118,23 +122,23 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
|
||||
@@ -142,8 +146,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.test = true;
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(17_s);
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(17_s);
|
||||
|
||||
photon::PhotonPoseEstimator estimator(
|
||||
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
|
||||
@@ -152,6 +157,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
|
||||
for (const auto& result : cameraOne.GetAllUnreadResults()) {
|
||||
estimatedPose = estimator.Update(result);
|
||||
}
|
||||
ASSERT_TRUE(estimatedPose);
|
||||
|
||||
frc::Pose3d pose = estimatedPose.value().estimatedPose;
|
||||
|
||||
@@ -165,23 +171,23 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
|
||||
@@ -189,8 +195,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.test = true;
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
|
||||
|
||||
photon::PhotonPoseEstimator estimator(aprilTags,
|
||||
photon::CLOSEST_TO_REFERENCE_POSE, {});
|
||||
@@ -202,6 +209,7 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
|
||||
estimatedPose = estimator.Update(result);
|
||||
}
|
||||
|
||||
ASSERT_TRUE(estimatedPose);
|
||||
frc::Pose3d pose = estimatedPose.value().estimatedPose;
|
||||
|
||||
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
|
||||
@@ -214,23 +222,23 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
|
||||
@@ -238,8 +246,9 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.test = true;
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
|
||||
|
||||
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
|
||||
{});
|
||||
@@ -254,31 +263,32 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 0, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targetsThree, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
|
||||
|
||||
// std::optional<photon::EstimatedRobotPose> estimatedPose;
|
||||
for (const auto& result : cameraOne.GetAllUnreadResults()) {
|
||||
@@ -298,23 +308,23 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
|
||||
@@ -322,8 +332,9 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
|
||||
0.4, corners, detectedCorners}};
|
||||
|
||||
cameraOne.test = true;
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
|
||||
|
||||
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
|
||||
{});
|
||||
@@ -333,6 +344,7 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
|
||||
estimatedPose = estimator.Update(result);
|
||||
}
|
||||
|
||||
ASSERT_TRUE(estimatedPose);
|
||||
frc::Pose3d pose = estimatedPose.value().estimatedPose;
|
||||
|
||||
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
|
||||
@@ -345,23 +357,23 @@ 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,
|
||||
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1,
|
||||
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
|
||||
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},
|
||||
photon::PhotonTrackedTarget{
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1,
|
||||
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
|
||||
frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
|
||||
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
|
||||
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
|
||||
@@ -374,8 +386,10 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
|
||||
{});
|
||||
|
||||
// empty input, expect empty out
|
||||
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
|
||||
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000},
|
||||
std::vector<photon::PhotonTrackedTarget>{}, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(1));
|
||||
|
||||
std::optional<photon::EstimatedRobotPose> estimatedPose;
|
||||
for (const auto& result : cameraOne.GetAllUnreadResults()) {
|
||||
@@ -385,14 +399,15 @@ 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[0].SetRecieveTimestamp(units::second_t(15));
|
||||
cameraOne.testResult = {photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 3000}, targets, std::nullopt}};
|
||||
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
|
||||
|
||||
for (const auto& result : cameraOne.GetAllUnreadResults()) {
|
||||
estimatedPose = estimator.Update(result);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(estimatedPose);
|
||||
ASSERT_TRUE(estimatedPose);
|
||||
EXPECT_NEAR((15_s - 3_ms).to<double>(),
|
||||
estimatedPose.value().timestamp.to<double>(), 1e-6);
|
||||
|
||||
@@ -403,3 +418,15 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
|
||||
|
||||
EXPECT_FALSE(estimatedPose);
|
||||
}
|
||||
TEST(PhotonPoseEstimatorTest, CopyResult) {
|
||||
std::vector<photon::PhotonTrackedTarget> targets{};
|
||||
|
||||
auto testResult = photon::PhotonPipelineResult{
|
||||
photon::PhotonPipelineMetadata{0, 0, 2000}, targets, std::nullopt};
|
||||
testResult.SetReceiveTimestamp(units::second_t(11));
|
||||
|
||||
auto test2 = testResult;
|
||||
|
||||
EXPECT_NEAR(testResult.GetTimestamp().to<double>(),
|
||||
test2.GetTimestamp().to<double>(), 0.001);
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
@@ -460,11 +461,12 @@ 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_NEAR(5, pose2.X().to<double>(), 0.01);
|
||||
ASSERT_NEAR(1, pose2.Y().to<double>(), 0.01);
|
||||
ASSERT_TRUE(results2);
|
||||
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
|
||||
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
|
||||
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
|
||||
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
|
||||
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
|
||||
pose2.Rotation().Z().to<double>(), 0.01);
|
||||
|
||||
Reference in New Issue
Block a user