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

Add PhotonPoseEstimator constructor without a PhotonCamera instance (#840)

Browse Source 
- Made alternate constructor for ```PhotonPoseEstimator``` that doesn't need ```PhotonCamera```
- Changed ```update``` function to take in missing cameraMatrixData and coeffsData that were previously received from PhotonCamera.
- Changed the internal ```update``` and ```multiTagPNP``` function to take in cameraMatrixData & coeffsData
- If not needed for the specified strategy then the parameters are simply not used. Also if PhotonCamera is used in constructor it instead backs up to that.

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
This commit is contained in:
Murad
2023-04-18 12:50:23 -05:00
committed by GitHub
parent 78ffa415fc
commit 2d586fe1c0
5 changed files with 127 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp
View File

@@ -257,8 +257,8 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
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(21));
estimator.GetCamera()->testResult = {2_ms, targetsThree};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(21));
estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
@@ -345,14 +345,14 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
// empty input, expect empty out
estimator.GetCamera().testResult = {2_ms, {}};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(1));
estimator.GetCamera()->testResult = {2_ms, {}};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera().testResult = {3_ms, targets};
estimator.GetCamera().testResult.SetTimestamp(units::second_t(15));
estimator.GetCamera()->testResult = {3_ms, targets};
estimator.GetCamera()->testResult.SetTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR(15, estimatedPose.value().timestamp.to<double>(), 1e-6);