mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
Update to match new WPILib organization
This commit is contained in:
Gold856
@@ -28,7 +28,7 @@
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <wpi/deprecated.h>
|
||||
#include <wpi/util/deprecated.hpp>
|
||||
|
||||
#include "photon/PhotonUtils.h"
|
||||
#include "photon/estimation/VisionEstimation.h"
|
||||
@@ -38,7 +38,7 @@ WPI_IGNORE_DEPRECATED
|
||||
|
||||
class VisionSystemSimTest : public ::testing::Test {
|
||||
void SetUp() override {
|
||||
nt::NetworkTableInstance::GetDefault().StartServer();
|
||||
wpi::nt::NetworkTableInstance::GetDefault().StartServer();
|
||||
photon::PhotonCamera::SetVersionCheckEnabled(false);
|
||||
}
|
||||
|
||||
@@ -47,71 +47,73 @@ class VisionSystemSimTest : public ::testing::Test {
|
||||
|
||||
class VisionSystemSimTestWithParamsTest
|
||||
: public VisionSystemSimTest,
|
||||
public testing::WithParamInterface<units::degree_t> {};
|
||||
public testing::WithParamInterface<wpi::units::degree_t> {};
|
||||
class VisionSystemSimTestDistanceParamsTest
|
||||
: public VisionSystemSimTest,
|
||||
public testing::WithParamInterface<
|
||||
std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
|
||||
std::tuple<wpi::units::foot_t, wpi::units::degree_t, wpi::units::foot_t>> {};
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) {
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{15.98_m, 0_m, 2_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}});
|
||||
|
||||
// To the right, to the right
|
||||
frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m},
|
||||
wpi::math::Rotation2d{-70_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// To the right, to the right
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
|
||||
wpi::math::Rotation2d{-95_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
|
||||
wpi::math::Rotation2d{90_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// To the left, to the left
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
|
||||
wpi::math::Rotation2d{65_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// Now kick, now kick
|
||||
robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// Now kick, now kick
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
|
||||
wpi::math::Rotation2d{-5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// Now walk it by yourself
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
|
||||
wpi::math::Rotation2d{-179_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
// Now walk it by yourself
|
||||
visionSysSim.AdjustCamera(
|
||||
&cameraSim,
|
||||
frc::Transform3d{
|
||||
frc::Translation3d{},
|
||||
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
|
||||
&cameraSim, wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{},
|
||||
wpi::math::Rotation3d{
|
||||
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
}
|
||||
@@ -140,121 +142,129 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) {
|
||||
}
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{15.98_m, 0_m, 1_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
visionSysSim.AdjustCamera(
|
||||
&cameraSim,
|
||||
frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 5000_m},
|
||||
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
|
||||
&cameraSim, wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 5000_m},
|
||||
wpi::math::Rotation3d{
|
||||
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
}
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestNotVisibleVert2) {
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{15.98_m, 0_m, 2_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
frc::Transform3d robotToCamera{
|
||||
frc::Translation3d{0_m, 0_m, 1_m},
|
||||
frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}};
|
||||
wpi::math::Transform3d robotToCamera{
|
||||
wpi::math::Translation3d{0_m, 0_m, 1_m},
|
||||
wpi::math::Rotation3d{0_rad, wpi::units::radian_t{-std::numbers::pi / 4},
|
||||
0_rad}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, robotToCamera);
|
||||
cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg});
|
||||
cameraSim.prop.SetCalibration(1234, 1234, wpi::math::Rotation2d{80_deg});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m},
|
||||
frc::Rotation2d{5_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{13.98_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
}
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) {
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{15.98_m, 0_m, 1_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(20.0);
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{12_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
}
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) {
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{15.98_m, 0_m, 1_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(1.0);
|
||||
cameraSim.SetMaxSightRange(10_m);
|
||||
visionSysSim.AddVisionTargets(
|
||||
{photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
|
||||
|
||||
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
|
||||
}
|
||||
|
||||
TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
|
||||
const frc::Pose3d targetPose{
|
||||
const wpi::math::Pose3d targetPose{
|
||||
{15.98_m, 0_m, 0_m},
|
||||
frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
|
||||
wpi::math::Rotation3d{0_deg, 0_deg,
|
||||
wpi::units::radian_t{3 * std::numbers::pi / 4}}};
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(0.0);
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
|
||||
|
||||
// If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+)
|
||||
frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m},
|
||||
frc::Rotation2d{GetParam()}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m},
|
||||
wpi::math::Rotation2d{GetParam()}};
|
||||
visionSysSim.Update(robotPose);
|
||||
|
||||
const auto result = camera.GetLatestResult();
|
||||
@@ -263,26 +273,28 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
|
||||
}
|
||||
|
||||
TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
|
||||
const frc::Pose3d targetPose{
|
||||
const wpi::math::Pose3d targetPose{
|
||||
{15.98_m, 0_m, 0_m},
|
||||
frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
|
||||
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
|
||||
wpi::math::Rotation3d{0_deg, 0_deg,
|
||||
wpi::units::radian_t{3 * std::numbers::pi / 4}}};
|
||||
wpi::math::Pose2d robotPose{{10_m, 0_m},
|
||||
wpi::math::Rotation2d{GetParam() * -1.0}};
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{120_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(0.0);
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
|
||||
|
||||
robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m},
|
||||
frc::Rotation2d{-1 * GetParam()}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{10_m, 0_m},
|
||||
wpi::math::Rotation2d{-1 * GetParam()}};
|
||||
visionSysSim.AdjustCamera(
|
||||
&cameraSim,
|
||||
frc::Transform3d{
|
||||
frc::Translation3d{},
|
||||
frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}});
|
||||
wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{},
|
||||
wpi::math::Rotation3d{0_rad, wpi::units::degree_t{GetParam()}, 0_rad}});
|
||||
visionSysSim.Update(robotPose);
|
||||
|
||||
const auto result = camera.GetLatestResult();
|
||||
@@ -295,24 +307,27 @@ INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest,
|
||||
-2_deg, 5_deg, 7_deg, 10.23_deg));
|
||||
|
||||
TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) {
|
||||
units::foot_t distParam;
|
||||
units::degree_t pitchParam;
|
||||
units::foot_t heightParam;
|
||||
wpi::units::foot_t distParam;
|
||||
wpi::units::degree_t pitchParam;
|
||||
wpi::units::foot_t heightParam;
|
||||
std::tie(distParam, pitchParam, heightParam) = GetParam();
|
||||
|
||||
const frc::Pose3d targetPose{
|
||||
const wpi::math::Pose3d targetPose{
|
||||
{15.98_m, 0_m, 1_m},
|
||||
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}};
|
||||
frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}};
|
||||
frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam},
|
||||
frc::Rotation3d{0_deg, pitchParam, 0_deg}};
|
||||
wpi::math::Rotation3d{0_deg, 0_deg,
|
||||
wpi::units::radian_t{std::numbers::pi * 0.98}}};
|
||||
wpi::math::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m},
|
||||
wpi::math::Rotation3d{}};
|
||||
wpi::math::Transform3d robotToCamera{
|
||||
wpi::math::Translation3d{0_m, 0_m, heightParam},
|
||||
wpi::math::Rotation3d{0_deg, pitchParam, 0_deg}};
|
||||
photon::VisionSystemSim visionSysSim{
|
||||
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
|
||||
"wsyourdaygoingihopegoodhaveagreatrestofyourlife"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{160_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(0.0);
|
||||
visionSysSim.AdjustCamera(&cameraSim, robotToCamera);
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
@@ -325,11 +340,11 @@ TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) {
|
||||
|
||||
ASSERT_NEAR(0.0, target.GetYaw(), 0.5);
|
||||
|
||||
units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
|
||||
wpi::units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
|
||||
robotToCamera.Z(), targetPose.Z(), -pitchParam,
|
||||
units::degree_t{target.GetPitch()});
|
||||
wpi::units::degree_t{target.GetPitch()});
|
||||
ASSERT_NEAR(dist.to<double>(),
|
||||
distParam.convert<units::meters>().to<double>(), 0.25);
|
||||
distParam.convert<wpi::units::meters>().to<double>(), 0.25);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
@@ -353,69 +368,70 @@ INSTANTIATE_TEST_SUITE_P(
|
||||
std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft)));
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestMultipleTargets) {
|
||||
frc::Pose3d targetPoseL{
|
||||
frc::Translation3d{15.98_m, 2_m, 0_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
frc::Pose3d targetPoseC{
|
||||
frc::Translation3d{15.98_m, 0_m, 0_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
frc::Pose3d targetPoseR{
|
||||
frc::Translation3d{15.98_m, -2_m, 0_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPoseL{
|
||||
wpi::math::Translation3d{15.98_m, 2_m, 0_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPoseC{
|
||||
wpi::math::Translation3d{15.98_m, 0_m, 0_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPoseR{
|
||||
wpi::math::Translation3d{15.98_m, -2_m, 0_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(20.0);
|
||||
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseL.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
|
||||
targetPoseL.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 1}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseC.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
|
||||
targetPoseC.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 2}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseR.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
|
||||
targetPoseR.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 3}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseL.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
|
||||
targetPoseL.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 4}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseC.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
|
||||
targetPoseC.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 5}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseR.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
|
||||
targetPoseR.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 6}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseL.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
|
||||
targetPoseL.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 7}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseC.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
|
||||
targetPoseC.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 8}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseL.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
|
||||
targetPoseL.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 9}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseR.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
|
||||
targetPoseR.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 10}});
|
||||
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
|
||||
targetPoseL.TransformBy(frc::Transform3d{
|
||||
frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
|
||||
targetPoseL.TransformBy(wpi::math::Transform3d{
|
||||
wpi::math::Translation3d{0_m, 0_m, 0.25_m}, wpi::math::Rotation3d{}}),
|
||||
photon::kAprilTag16h5, 11}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{6_m, 0_m},
|
||||
wpi::math::Rotation2d{.25_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
photon::PhotonPipelineResult res = camera.GetLatestResult();
|
||||
ASSERT_TRUE(res.HasTargets());
|
||||
@@ -427,27 +443,31 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(20.0);
|
||||
|
||||
std::vector<frc::AprilTag> tagList;
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
0, frc::Pose3d{12_m, 3_m, 1_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
1, frc::Pose3d{12_m, 1_m, -1_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
2, frc::Pose3d{11_m, 0_m, 2_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
units::meter_t fieldLength{54};
|
||||
units::meter_t fieldWidth{27};
|
||||
frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
|
||||
frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}};
|
||||
std::vector<wpi::apriltag::AprilTag> tagList;
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
0,
|
||||
wpi::math::Pose3d{12_m, 3_m, 1_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
1,
|
||||
wpi::math::Pose3d{12_m, 1_m, -1_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
2,
|
||||
wpi::math::Pose3d{11_m, 0_m, 2_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
wpi::units::meter_t fieldLength{54};
|
||||
wpi::units::meter_t fieldWidth{27};
|
||||
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m},
|
||||
wpi::math::Rotation2d{5_deg}};
|
||||
visionSysSim.AddVisionTargets(
|
||||
{photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}});
|
||||
visionSysSim.Update(robotPose);
|
||||
@@ -464,11 +484,11 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
|
||||
auto results = photon::VisionEstimation::EstimateCamPosePNP(
|
||||
camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
|
||||
ASSERT_TRUE(results);
|
||||
frc::Pose3d pose = frc::Pose3d{} + results->best;
|
||||
wpi::math::Pose3d pose = wpi::math::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);
|
||||
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
|
||||
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
|
||||
pose.Rotation().Z().to<double>(), 0.01);
|
||||
|
||||
visionSysSim.AddVisionTargets(
|
||||
@@ -486,42 +506,47 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
|
||||
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
|
||||
camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
|
||||
ASSERT_TRUE(results2);
|
||||
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
|
||||
wpi::math::Pose3d pose2 = wpi::math::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>(),
|
||||
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
|
||||
pose2.Rotation().Z().to<double>(), 0.01);
|
||||
}
|
||||
|
||||
TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
|
||||
frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in},
|
||||
frc::Rotation3d{0_deg, -30_deg, 25.5_deg}};
|
||||
wpi::math::Transform3d robotToCamera{
|
||||
wpi::math::Translation3d{6_in, 6_in, 6_in},
|
||||
wpi::math::Rotation3d{0_deg, -30_deg, 25.5_deg}};
|
||||
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"cameraRotated"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, robotToCamera);
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(20.0);
|
||||
|
||||
std::vector<frc::AprilTag> tagList;
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
0, frc::Pose3d{12_m, 3_m, 1_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
1, frc::Pose3d{12_m, 1_m, -1_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(frc::AprilTag{
|
||||
2, frc::Pose3d{11_m, 0_m, 2_m,
|
||||
frc::Rotation3d{0_rad, 0_rad,
|
||||
units::radian_t{std::numbers::pi}}}});
|
||||
units::meter_t fieldLength{54};
|
||||
units::meter_t fieldWidth{27};
|
||||
frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
|
||||
frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}};
|
||||
std::vector<wpi::apriltag::AprilTag> tagList;
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
0,
|
||||
wpi::math::Pose3d{12_m, 3_m, 1_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
1,
|
||||
wpi::math::Pose3d{12_m, 1_m, -1_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
tagList.emplace_back(wpi::apriltag::AprilTag{
|
||||
2,
|
||||
wpi::math::Pose3d{11_m, 0_m, 2_m,
|
||||
wpi::math::Rotation3d{
|
||||
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
|
||||
wpi::units::meter_t fieldLength{54};
|
||||
wpi::units::meter_t fieldWidth{27};
|
||||
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m},
|
||||
wpi::math::Rotation2d{-5_deg}};
|
||||
visionSysSim.AddVisionTargets(
|
||||
{photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}});
|
||||
visionSysSim.Update(robotPose);
|
||||
@@ -542,12 +567,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
|
||||
auto results = photon::VisionEstimation::EstimateCamPosePNP(
|
||||
camEigen, distEigen, targets, layout, photon::kAprilTag36h11);
|
||||
ASSERT_TRUE(results);
|
||||
frc::Pose3d pose = frc::Pose3d{} + results->best;
|
||||
wpi::math::Pose3d pose = wpi::math::Pose3d{} + results->best;
|
||||
pose = pose.TransformBy(robotToCamera.Inverse());
|
||||
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);
|
||||
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
|
||||
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
|
||||
pose.Rotation().Z().to<double>(), 0.01);
|
||||
|
||||
visionSysSim.AddVisionTargets(
|
||||
@@ -565,12 +590,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
|
||||
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
|
||||
camEigen, distEigen, targets2, layout, photon::kAprilTag36h11);
|
||||
ASSERT_TRUE(results2);
|
||||
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
|
||||
wpi::math::Pose3d pose2 = wpi::math::Pose3d{} + results2->best;
|
||||
pose2 = pose2.TransformBy(robotToCamera.Inverse());
|
||||
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>(),
|
||||
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
|
||||
pose2.Rotation().Z().to<double>(), 0.01);
|
||||
}
|
||||
|
||||
@@ -578,24 +603,25 @@ TEST_F(VisionSystemSimTest, TestTagAmbiguity) {
|
||||
photon::VisionSystemSim visionSysSim{"Test"};
|
||||
photon::PhotonCamera camera{"camera"};
|
||||
photon::PhotonCameraSim cameraSim{&camera};
|
||||
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
|
||||
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
|
||||
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
|
||||
cameraSim.SetMinTargetAreaPixels(20.0);
|
||||
|
||||
frc::Pose3d targetPose{
|
||||
frc::Translation3d{2_m, 0_m, 0_m},
|
||||
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
|
||||
wpi::math::Pose3d targetPose{
|
||||
wpi::math::Translation3d{2_m, 0_m, 0_m},
|
||||
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
|
||||
visionSysSim.AddVisionTargets(
|
||||
{photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}});
|
||||
|
||||
frc::Pose2d robotPose{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{0_deg}};
|
||||
wpi::math::Pose2d robotPose{wpi::math::Translation2d{0_m, 0_m},
|
||||
wpi::math::Rotation2d{0_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
double ambiguity =
|
||||
camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
|
||||
ASSERT_TRUE(ambiguity > 0.5);
|
||||
|
||||
robotPose =
|
||||
frc::Pose2d{frc::Translation2d{-2_m, -2_m}, frc::Rotation2d{30_deg}};
|
||||
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{-2_m, -2_m},
|
||||
wpi::math::Rotation2d{30_deg}};
|
||||
visionSysSim.Update(robotPose);
|
||||
ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
|
||||
ASSERT_TRUE(0 < ambiguity && ambiguity < 0.2);
|
||||
|
||||
Reference in New Issue
Block a user