hololib::ApriltagLocalization Class Reference

Estimates the robot's field pose from AprilTag detections. More...

#include <ApriltagLocalization.hpp>

Classes
struct	IPPEOutput
	The two pose solutions produced by the IPPE square solver. More...

Public Member Functions
	ApriltagLocalization (pros::AIVision &visionSensor, pros::Imu &imu, std::function< hololib::Pose(bool)> odomPoseGetter, const Eigen::Matrix3f &cameraMatrix, const Eigen::Vector< float, 5 > &distCoeffs, const Eigen::Vector2f &visionSensorOffset=Eigen::Vector2f::Zero(), float visionSensorYawOffset=0.0f)
	Constructs an AprilTag localization system.
void	update (uint32_t period_ms)
	Continuously processes tag detections and updates the estimated pose.
void	startTask (uint32_t period_ms=10)
	Starts the localization task if it is not already running.
Pose	getPoseFromTag (bool useRadians=false)
	Returns the latest tag-based pose estimate.
Pose	estimateRobotPose (const IPPEOutput &ippeOutput, const Pose &odometryPose, int tagId) const
	Estimates the robot's field pose from a 6-DOF camera pose produced by IPPE.

Detailed Description

Estimates the robot's field pose from AprilTag detections.

Definition at line 15 of file ApriltagLocalization.hpp.

Constructor & Destructor Documentation

ApriltagLocalization()

hololib::ApriltagLocalization::ApriltagLocalization ( pros::AIVision &  visionSensor, pros::Imu &  imu, std::function< hololib::Pose(bool)>  odomPoseGetter, const Eigen::Matrix3f &  cameraMatrix, const Eigen::Vector< float, 5 > &  distCoeffs, const Eigen::Vector2f &  visionSensorOffset = Eigen::Vector2f::Zero(), float  visionSensorYawOffset = 0.0f  )

inline

Constructs an AprilTag localization system.

Parameters

visionSensor	AI Vision sensor used for tag detections.
imu	IMU associated with the robot.
odomPoseGetter	Callback returning the current odometry pose; its boolean argument selects radians.
cameraMatrix	Camera intrinsic matrix.
distCoeffs	Distortion coefficients ordered as k1, k2, p1, p2, k3.
visionSensorOffset	Camera offset from the robot tracking center as +x forward and +y right, in inches.
visionSensorYawOffset	Camera yaw relative to the robot, in radians.

visionSensorOffset is measured from the robot tracking center to the camera optical center in robot coordinates: +x forward and +y right. visionSensorYawOffset and returned Pose angles use the odometry convention: zero faces field +Y and positive rotation is clockwise. Angles passed to this function through Pose are radians. All translations must use the same units as FIELD_TAGS (inches).

Definition at line 54 of file ApriltagLocalization.hpp.

Member Function Documentation

update()

void hololib::ApriltagLocalization::update

(

uint32_t

period_ms

)

Continuously processes tag detections and updates the estimated pose.

Parameters

period_ms

Update period in milliseconds.

startTask()

void hololib::ApriltagLocalization::startTask

(

uint32_t

period_ms = 10

)

Starts the localization task if it is not already running.

Parameters

period_ms

Update period in milliseconds.

Example:

// Start the AprilTag localization task with a 100 ms update period.
initialize() {
    aprilTagLocalization.startTask(100);
}

getPoseFromTag()

Pose hololib::ApriltagLocalization::getPoseFromTag

(

bool

useRadians = false

)

Returns the latest tag-based pose estimate.

Parameters

useRadians

Return heading in radians when true, otherwise degrees.

Returns

The latest estimated field pose.

Example:

// Get the latest tag-based pose estimate in radians.
tagOdom.startTask(100);
while (true) {
   hololib::Pose p = tagOdom.getPoseFromTag(false);
   std::println("Tag pose: x: {}, y: {}, theta: {}", p.x, p.y, p.theta);
   pros::delay(100);
}

estimateRobotPose()

Pose hololib::ApriltagLocalization::estimateRobotPose	(	const IPPEOutput &	ippeOutput,
		const Pose &	odometryPose,
		int	tagId
	)		const

Estimates the robot's field pose from a 6-DOF camera pose produced by IPPE.

Parameters

ippeOutput	Candidate camera poses produced by IPPE.
odometryPose	Current odometry pose, with heading in radians.
tagId	Detected field tag ID.

Returns

The best matching field pose, or odometryPose if no valid match exists.