Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01: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

16
photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h
View File

@@ -34,6 +34,10 @@
#include "photonlib/PhotonCamera.h"
namespace frc {
class AprilTagFieldLayout;
} // namespace frc
namespace photonlib {
enum PoseStrategy : int {
LOWEST_AMBIGUITY,
@@ -47,18 +51,18 @@ enum PoseStrategy : int {
* A managing class to determine how an estimated pose should be chosen.
*/
class RobotPoseEstimator {
public:
using map_value_type =
std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
using size_type = std::vector<map_value_type>::size_type;
public:
explicit RobotPoseEstimator(std::map<int, frc::Pose3d> aprilTags,
PoseStrategy strategy,
std::vector<map_value_type>);
explicit RobotPoseEstimator(
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
PoseStrategy strategy, std::vector<map_value_type> cameras);
std::pair<frc::Pose3d, units::millisecond_t> Update();
void SetPoseStrategy(PoseStrategy strategy);
inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
inline void SetReferencePose(frc::Pose3d referencePose) {
this->referencePose = referencePose;
@@ -79,7 +83,7 @@ class RobotPoseEstimator {
frc::Pose3d GetReferencePose() const { return referencePose; }
private:
std::map<int, frc::Pose3d> aprilTags;
std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
PoseStrategy strategy;
std::vector<map_value_type> cameras;
frc::Pose3d lastPose;