Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-20 00:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

RobotPoseEstimator Enhancements (#677)

Browse Source 
* Use List for RobotPoseEstimator constructor

* Update `RobotPoseEstimator` constructor to accept wpilib `AprilTagFieldLayout` java

* Initial cpp changes

* Java return optional from update

* Fix java test

* Clean up strategy switch

* small lint

* Actually link to vision_shared

* Fix auto optimized imports

* format

* report error

* small method changes

* format and clean up

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Nick Hadley
2023-01-02 19:22:39 -05:00
committed by GitHub
parent 971b471f92
commit 0b5256df12
7 changed files with 171 additions and 164 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp
View File

@@ -26,6 +26,7 @@
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
@@ -40,11 +41,14 @@
#include "photonlib/RobotPoseEstimator.h"
TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -122,11 +126,14 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -204,11 +211,14 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())},
};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -288,11 +298,13 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
}
TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -431,11 +443,13 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
}
TEST(RobotPoseEstimatorTest, AverageBestPoses) {
std::map<int, frc::Pose3d> aprilTags;
aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
units::meter_t(3), frc::Rotation3d())});
aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
units::meter_t(5), frc::Rotation3d())});
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
frc::Rotation3d())},
{1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
frc::Rotation3d())}};
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags =
std::make_shared<frc::AprilTagFieldLayout>(tags, 54_ft, 27_ft);
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>