Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-01 02:41:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Return named type from PhotonPoseEstimator (#734)

Browse Source 
Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator
This commit is contained in:
Andrew Gasser
2023-01-14 09:06:15 -06:00
committed by GitHub
parent 073714f0bc
commit 357d8a518a
17 changed files with 1597 additions and 314 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java → photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -45,12 +45,12 @@ 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;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.TargetCorner;
class RobotPoseEstimatorTest {
class PhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;
@BeforeAll
@@ -62,24 +62,22 @@ class RobotPoseEstimatorTest {
try {
CombinedRuntimeLoader.loadLibraries(
RobotPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
PhotonPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
List<AprilTag> atList = new ArrayList<AprilTag>(2);
atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
var fl = Units.feetToMeters(54.0);
var fw = Units.feetToMeters(27.0);
aprilTags = new AprilTagFieldLayout(atList, fl, fw);
List<AprilTag> tagList = new ArrayList<AprilTag>(2);
tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
double fieldLength = Units.feetToMeters(54.0);
double fieldWidth = Units.feetToMeters(27.0);
aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
}
@Test
void testLowestAmbiguityStrategy() {
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
@@ -122,12 +120,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -147,17 +140,16 @@ class RobotPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setTimestampSeconds(11);
cameras.add(Pair.of(cameraOne, new Transform3d()));
cameras.add(Pair.of(cameraTwo, new Transform3d()));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().estimatedPose;
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(11, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(3, pose.getY(), .01);
assertEquals(2, pose.getZ(), .01);
@@ -209,12 +201,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -235,16 +222,19 @@ class RobotPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())));
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 2), new Rotation3d())));
cameraOne.result.setTimestampSeconds(4);
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
cameraOne,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(4, estimatedPose.get().timestampSeconds);
assertEquals(4, pose.getX(), .01);
assertEquals(4, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);
@@ -252,8 +242,6 @@ class RobotPoseEstimatorTest {
@Test
void closestToReferencePoseStrategy() {
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
@@ -296,12 +284,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -321,18 +304,20 @@ class RobotPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setTimestampSeconds(17);
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().getSecond());
assertEquals(17, estimatedPose.get().timestampSeconds);
assertEquals(1, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(.9, pose.getZ(), .01);
@@ -340,8 +325,6 @@ class RobotPoseEstimatorTest {
@Test
void closestToLastPose() {
ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
PhotonCameraInjector cameraOne = new PhotonCameraInjector();
cameraOne.result =
new PhotonPipelineResult(
@@ -384,12 +367,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -410,16 +388,17 @@ class RobotPoseEstimatorTest {
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_LAST_POSE, cameras);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
new PhotonPipelineResult(
@@ -462,11 +441,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))),
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -486,11 +461,12 @@ class RobotPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8)))));
cameraOne.result.setTimestampSeconds(7);
estimatedPose = estimator.update();
pose = estimatedPose.get().getFirst();
pose = estimatedPose.get().estimatedPose;
assertEquals(2, estimatedPose.get().getSecond());
assertEquals(7, estimatedPose.get().timestampSeconds);
assertEquals(.9, pose.getX(), .01);
assertEquals(1.1, pose.getY(), .01);
assertEquals(1, pose.getZ(), .01);
@@ -542,12 +518,7 @@ class RobotPoseEstimatorTest {
new TargetCorner(1, 2),
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 2 2 2 ambig .3
PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
cameraTwo.result =
new PhotonPipelineResult(
4,
List.of(
new TargetCorner(7, 8))), // 2 2 2 ambig .3
new PhotonTrackedTarget(
9.0,
-2.0,
@@ -567,16 +538,19 @@ class RobotPoseEstimatorTest {
new TargetCorner(3, 4),
new TargetCorner(5, 6),
new TargetCorner(7, 8))))); // 3 3 3 ambig .4
cameraOne.result.setTimestampSeconds(20);
cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
RobotPoseEstimator estimator =
new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().estimatedPose;
Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
Pose3d pose = estimatedPose.get().getFirst();
assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(2.15, pose.getX(), .01);
assertEquals(2.15, pose.getY(), .01);
assertEquals(2.15, pose.getZ(), .01);

3
photon-lib/src/test/native/cpp/PacketTest.cpp
View File

@@ -42,6 +42,7 @@ TEST(PacketTest, PhotonTrackedTarget) {
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-1,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
photonlib::Packet p;
@@ -79,6 +80,7 @@ TEST(PacketTest, PhotonPipelineResult) {
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-1,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
photonlib::PhotonTrackedTarget{
3.0,
@@ -91,6 +93,7 @@ TEST(PacketTest, PhotonPipelineResult) {
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-1,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};

312
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp Normal file
View File

@@ -0,0 +1,312 @@
/*
* MIT License
*
* Copyright (c) 2022 PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <map>
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include "gtest/gtest.h"
#include "photonlib/PhotonCamera.h"
#include "photonlib/PhotonPipelineResult.h"
#include "photonlib/PhotonPoseEstimator.h"
#include "photonlib/PhotonTrackedTarget.h"
static 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())}};
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}};
TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(11));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
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())},
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photonlib::PhotonCamera, frc::Transform3d>> cameras;
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
// ID 0 at 3,3,3
// ID 1 at 5,5,5
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(17_s);
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
{{0_m, 0_m, 4_m}, {}});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(aprilTags,
photonlib::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();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(17));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::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();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
estimator.GetCamera().testResult = {2_ms, targetsThree};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(7));
estimatedPose = estimator.Update();
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
photonlib::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{
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{
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {2_ms, targets};
cameraOne.testResult.SetTimestamp(units::second_t(15));
photonlib::PhotonPoseEstimator estimator(
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
}

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

@@ -40,6 +40,11 @@
#include "photonlib/PhotonTrackedTarget.h"
#include "photonlib/RobotPoseEstimator.h"
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}};
TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
std::vector<frc::AprilTag> tags = {
{0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -61,51 +66,36 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
0,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(11));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
frc::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -119,7 +109,7 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
@@ -146,51 +136,36 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraOne->testResult.SetTimestamp(units::second_t(12));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
@@ -204,7 +179,7 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
@@ -231,51 +206,36 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(4));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(17));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -291,7 +251,7 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(4, units::unit_cast<double>(estimatedPose.second), .01);
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
@@ -317,48 +277,31 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
@@ -379,49 +322,34 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
0,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->testResult = {2_s, targetsThree};
cameraOne->testResult.SetTimestamp(units::second_t(7));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->testResult = {4_s, targetsFour};
cameraTwo->testResult.SetTimestamp(units::second_t(13));
std::vector<
std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -436,7 +364,8 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
estimatedPose = estimator.Update();
pose = estimatedPose.first;
EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
@@ -462,51 +391,36 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {
wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.0,
4.0,
0,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
0.7, corners, detectedCorners},
photonlib::PhotonTrackedTarget{
3.0,
-4.0,
9.1,
6.7,
1,
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,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.3, corners, detectedCorners}};
cameraOne->test = true;
cameraOne->testResult = {2_s, targets};
cameraOne->testResult.SetTimestamp(units::second_t(10));
wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
photonlib::PhotonTrackedTarget{
9.0,
-2.0,
19.0,
3.0,
0,
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)),
frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4,
{std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
std::pair{7, 8}}}};
0.4, corners, detectedCorners}};
cameraTwo->test = true;
cameraTwo->testResult = {4_s, targetsTwo};
cameraTwo->testResult.SetTimestamp(units::second_t(20));
cameras.push_back(std::make_pair(
cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -520,8 +434,8 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {
estimator.Update();
frc::Pose3d pose = estimatedPose.first;
EXPECT_NEAR(2.6885245901639347,
units::unit_cast<double>(estimatedPose.second), .01);
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
.01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);