mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-28 02:11:40 +00:00
RobotPoseEstimator Enhancements (#677)
* 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
@@ -42,6 +42,7 @@ import edu.wpi.first.util.WPIUtilJNI;
|
||||
import java.io.IOException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.Optional;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.RobotPoseEstimator.PoseStrategy;
|
||||
@@ -138,10 +139,10 @@ class RobotPoseEstimatorTest {
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
|
||||
|
||||
Pair<Pose3d, Double> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.getFirst();
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
|
||||
assertEquals(2, estimatedPose.getSecond());
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(1, pose.getX(), .01);
|
||||
assertEquals(3, pose.getY(), .01);
|
||||
assertEquals(2, pose.getZ(), .01);
|
||||
@@ -210,10 +211,10 @@ class RobotPoseEstimatorTest {
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
|
||||
|
||||
Pair<Pose3d, Double> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.getFirst();
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
|
||||
assertEquals(2, estimatedPose.getSecond());
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(4, pose.getX(), .01);
|
||||
assertEquals(4, pose.getY(), .01);
|
||||
assertEquals(0, pose.getZ(), .01);
|
||||
@@ -283,10 +284,10 @@ class RobotPoseEstimatorTest {
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
|
||||
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
|
||||
|
||||
Pair<Pose3d, Double> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.getFirst();
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
|
||||
assertEquals(4, estimatedPose.getSecond());
|
||||
assertEquals(4, estimatedPose.get().getSecond());
|
||||
assertEquals(1, pose.getX(), .01);
|
||||
assertEquals(1.1, pose.getY(), .01);
|
||||
assertEquals(.9, pose.getZ(), .01);
|
||||
@@ -357,8 +358,8 @@ class RobotPoseEstimatorTest {
|
||||
|
||||
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
|
||||
|
||||
Pair<Pose3d, Double> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.getFirst();
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
|
||||
cameraOne.result =
|
||||
new PhotonPipelineResult(
|
||||
@@ -412,9 +413,9 @@ class RobotPoseEstimatorTest {
|
||||
new TargetCorner(7, 8)))));
|
||||
|
||||
estimatedPose = estimator.update();
|
||||
pose = estimatedPose.getFirst();
|
||||
pose = estimatedPose.get().getFirst();
|
||||
|
||||
assertEquals(2, estimatedPose.getSecond());
|
||||
assertEquals(2, estimatedPose.get().getSecond());
|
||||
assertEquals(.9, pose.getX(), .01);
|
||||
assertEquals(1.1, pose.getY(), .01);
|
||||
assertEquals(1, pose.getZ(), .01);
|
||||
@@ -483,9 +484,9 @@ class RobotPoseEstimatorTest {
|
||||
RobotPoseEstimator estimator =
|
||||
new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
|
||||
|
||||
Pair<Pose3d, Double> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.getFirst();
|
||||
assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01);
|
||||
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
|
||||
Pose3d pose = estimatedPose.get().getFirst();
|
||||
assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
|
||||
assertEquals(2.15, pose.getX(), .01);
|
||||
assertEquals(2.15, pose.getY(), .01);
|
||||
assertEquals(2.15, pose.getZ(), .01);
|
||||
|
||||
@@ -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>>
|
||||
|
||||
Reference in New Issue
Block a user