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

Cache pose calculations in PhotonPoseEstimator (#788)

Browse Source 
* Add pose caching to Java

* Refactor strategy fallthrough

* Hopefully add pose caching to C++

* Make Java switch same order as enum and C++ switch

* C++ absolute value in timestamp check

* Fix Java NPE

* Use `units::second_t` in timestamp

Co-authored-by: Matt <matthew.morley.ca@gmail.com>

* Expand Java unit test

* Copy comments into C++

* Add tests to C++

* Run format

* Update PhotonCamera.cpp

* Probably fix bad access exception

* a

* init timestamp

* Remove prints

---------

Co-authored-by: Matt <matthew.morley.ca@gmail.com>
Co-authored-by: Joseph Eng <joseng2358@gmail.com>
This commit is contained in:
Joseph Eng
2023-02-13 18:22:22 -08:00
committed by GitHub
parent 5b86360b9b
commit 545e016d04
6 changed files with 198 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
View File

@@ -24,6 +24,7 @@
#include "photonlib/PhotonPoseEstimator.h"
#include <cmath>
#include <iostream>
#include <limits>
#include <map>
@@ -39,6 +40,7 @@
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/math.h>
#include <units/time.h>
#include "photonlib/PhotonCamera.h"
@@ -64,7 +66,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
camera(std::move(cam)),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()) {}
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s) {}
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP) {
@@ -74,6 +77,9 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
"");
strategy = LOWEST_AMBIGUITY;
}
if (this->multiTagFallbackStrategy != strategy) {
InvalidatePoseCache();
}
multiTagFallbackStrategy = strategy;
}
@@ -84,6 +90,22 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
return std::nullopt;
}
// If the pose cache timestamp was set, and the result is from the same
// timestamp, return an empty result
if (poseCacheTimestamp > 0_s &&
units::math::abs(poseCacheTimestamp - result.GetTimestamp()) < 0.001_ms) {
return std::nullopt;
}
// Remember the timestamp of the current result used
poseCacheTimestamp = result.GetTimestamp();
// If no targets seen, trivial case -- return empty result
if (!result.HasTargets()) {
return std::nullopt;
}
@@ -118,11 +140,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
return std::nullopt;
}
if (!ret) {
// TODO
ret = std::nullopt;
}
return ret;