Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-07-05 03:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Update to match new WPILib organization

Browse Source
This commit is contained in:
Gold856
2025-12-29 16:16:56 -05:00
committed by samfreund
parent c34c854583
commit 934eed21d2
264 changed files with 3440 additions and 3299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java
View File

@@ -24,10 +24,10 @@
package org.photonvision;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.wpilib.math.geometry.Pose3d;
/** An estimated pose based on pipeline result */
public class EstimatedRobotPose {

40
photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View File

@@ -24,26 +24,6 @@
package org.photonvision;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.BooleanSubscriber;
import edu.wpi.first.networktables.DoubleArraySubscriber;
import edu.wpi.first.networktables.IntegerEntry;
import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.IntegerSubscriber;
import edu.wpi.first.networktables.MultiSubscriber;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
@@ -53,6 +33,26 @@ import org.photonvision.common.hardware.VisionLEDMode;
import org.photonvision.common.networktables.PacketSubscriber;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.timesync.TimeSyncSingleton;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.numbers.*;
import org.wpilib.math.util.Nat;
import org.wpilib.networktables.BooleanPublisher;
import org.wpilib.networktables.BooleanSubscriber;
import org.wpilib.networktables.DoubleArraySubscriber;
import org.wpilib.networktables.IntegerEntry;
import org.wpilib.networktables.IntegerPublisher;
import org.wpilib.networktables.IntegerSubscriber;
import org.wpilib.networktables.MultiSubscriber;
import org.wpilib.networktables.NetworkTable;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.PubSubOption;
import org.wpilib.networktables.StringSubscriber;
import org.wpilib.system.Timer;
import org.wpilib.util.Alert;
import org.wpilib.util.Alert.AlertType;
/** Represents a camera that is connected to PhotonVision. */
public class PhotonCamera implements AutoCloseable {

34
photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View File

@@ -24,28 +24,28 @@
package org.photonvision;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N8;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.*;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.numbers.N1;
import org.wpilib.math.numbers.N3;
import org.wpilib.math.numbers.N8;
import org.wpilib.math.util.Pair;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.camera.OpenCvLoader;
/**
* The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a

4
photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View File

@@ -24,7 +24,7 @@
package org.photonvision;
import edu.wpi.first.math.geometry.*;
import org.wpilib.math.geometry.*;
public final class PhotonUtils {
private PhotonUtils() {
@@ -39,7 +39,7 @@ public final class PhotonUtils {
* for there to exist a height differential between goal and camera. The larger this differential,
* the more accurate the distance estimate will be.
*
* <p>Units can be converted using the {@link edu.wpi.first.math.util.Units} class.
* <p>Units can be converted using the {@link org.wpilib.math.util.Units} class.
*
* @param cameraHeightMeters The physical height of the camera off the floor in meters.
* @param targetHeightMeters The physical height of the target off the floor in meters. This

29
photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View File

@@ -24,19 +24,6 @@
package org.photonvision.simulation;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.util.PixelFormat;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.RobotController;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
@@ -60,6 +47,18 @@ import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.util.Pair;
import org.wpilib.system.RobotController;
import org.wpilib.util.PixelFormat;
import org.wpilib.util.WPIUtilJNI;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
import org.wpilib.vision.camera.CvSource;
import org.wpilib.vision.camera.OpenCvLoader;
import org.wpilib.vision.camera.VideoSource.ConnectionStrategy;
import org.wpilib.vision.stream.CameraServer;
/**
* A handle for simulating {@link PhotonCamera} values. Processing simulated targets through this
@@ -295,8 +294,8 @@ public class PhotonCameraSim implements AutoCloseable {
*/
public boolean canSeeCorners(Point[] points) {
for (var point : points) {
if (MathUtil.clamp(point.x, 0, prop.getResWidth()) != point.x
|| MathUtil.clamp(point.y, 0, prop.getResHeight()) != point.y) {
if (Math.clamp(point.x, 0, prop.getResWidth()) != point.x
|| Math.clamp(point.y, 0, prop.getResHeight()) != point.y) {
return false; // point is outside of resolution
}
}

27
photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java
View File

@@ -25,19 +25,6 @@
package org.photonvision.simulation;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.Vector;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.numbers.*;
import edu.wpi.first.wpilibj.DriverStation;
import java.io.IOException;
import java.nio.file.Path;
import java.util.ArrayList;
@@ -51,6 +38,18 @@ import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.Matrix;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.linalg.Vector;
import org.wpilib.math.numbers.*;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Pair;
/**
* Calibration and performance values for this camera.
@@ -167,7 +166,7 @@ public class SimCameraProperties {
public SimCameraProperties setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) {
if (fovDiag.getDegrees() < 1 || fovDiag.getDegrees() > 179) {
fovDiag = Rotation2d.fromDegrees(MathUtil.clamp(fovDiag.getDegrees(), 1, 179));
fovDiag = Rotation2d.fromDegrees(Math.clamp(fovDiag.getDegrees(), 1, 179));
DriverStation.reportError(
"Requested invalid FOV! Clamping between (1, 179) degrees...", false);
}

14
photon-lib/src/main/java/org/photonvision/simulation/VideoSimUtil.java
View File

@@ -24,13 +24,6 @@
package org.photonvision.simulation;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.cscore.CvSource;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RawFrame;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashMap;
@@ -48,6 +41,13 @@ import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.util.Units;
import org.wpilib.util.RawFrame;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.camera.CvSource;
import org.wpilib.vision.camera.OpenCvLoader;
public class VideoSimUtil {
// Tag IDs start at 0, this should be set to 1 greater than the maximum tag ID required

18
photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View File

@@ -24,15 +24,6 @@
package org.photonvision.simulation;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.Collection;
import java.util.HashMap;
@@ -43,6 +34,15 @@ import java.util.Optional;
import java.util.Set;
import org.photonvision.PhotonCamera;
import org.photonvision.estimation.TargetModel;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.interpolation.TimeInterpolatableBuffer;
import org.wpilib.smartdashboard.Field2d;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.Timer;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on a mobile robot

4
photon-lib/src/main/java/org/photonvision/simulation/VisionTargetSim.java
View File

@@ -24,10 +24,10 @@
package org.photonvision.simulation;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Translation3d;
import java.util.List;
import org.photonvision.estimation.TargetModel;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Translation3d;
/** Describes a vision target located somewhere on the field that your vision system can detect. */
public class VisionTargetSim {

2
photon-lib/src/main/java/org/photonvision/timesync/TimeSyncSingleton.java
View File

@@ -24,9 +24,9 @@
package org.photonvision.timesync;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import org.photonvision.jni.TimeSyncServer;
import org.wpilib.util.runtime.RuntimeLoader;
/** Helper to hold a single TimeSyncServer instance with some default config */
public class TimeSyncSingleton {

79
photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp
View File

@@ -29,20 +29,21 @@
#include <string_view>
#include <vector>
#include <frc/Errors.h>
#include <frc/RobotController.h>
#include <frc/Timer.h>
#include <hal/FRCUsageReporting.h>
#include <wpi/hal/UsageReporting.h>
#include <net/TimeSyncServer.h>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <wpi/json.h>
#include <wpi/system/Errors.hpp>
#include <wpi/system/RobotController.hpp>
#include <wpi/system/Timer.hpp>
#include <wpi/system/WPILibVersion.hpp>
#include <wpi/util/json.hpp>
#include "PhotonVersion.h"
#include "photon/dataflow/structures/Packet.h"
static constexpr units::second_t WARN_DEBOUNCE_SEC = 5_s;
static constexpr units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
static constexpr wpi::units::second_t WARN_DEBOUNCE_SEC = 5_s;
static constexpr wpi::units::second_t HEARTBEAT_DEBOUNCE_SEC = 500_ms;
inline void verifyDependencies() {
if (!(std::string_view{cv::getVersionString()} ==
@@ -83,8 +84,8 @@ inline void verifyDependencies() {
">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n";
FRC_ReportWarning(bfw);
FRC_ReportError(frc::err::Error, bfw);
WPILIB_ReportWarning(bfw);
WPILIB_ReportError(wpi::err::Error, bfw);
throw new std::runtime_error(std::string{bfw});
}
}
@@ -107,7 +108,7 @@ static void InitTspServer() {
namespace photon {
constexpr const units::second_t VERSION_CHECK_INTERVAL = 5_s;
constexpr const wpi::units::second_t VERSION_CHECK_INTERVAL = 5_s;
static const std::vector<std::string_view> PHOTON_PREFIX = {"/photonvision/"};
static const std::string PHOTON_ALERT_GROUP{"PhotonAlerts"};
bool PhotonCamera::VERSION_CHECK_ENABLED = true;
@@ -120,7 +121,7 @@ static const std::string TYPE_STRING =
std::string{"photonstruct:PhotonPipelineResult:"} +
std::string{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
PhotonCamera::PhotonCamera(wpi::nt::NetworkTableInstance instance,
const std::string_view cameraName)
: mainTable(instance.GetTable("photonvision")),
rootTable(mainTable->GetSubTable(cameraName)),
@@ -163,20 +164,20 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
disconnectAlert(PHOTON_ALERT_GROUP,
std::string{"PhotonCamera '"} + std::string{cameraName} +
"' is disconnected.",
frc::Alert::AlertType::kWarning),
timesyncAlert(PHOTON_ALERT_GROUP, "", frc::Alert::AlertType::kWarning) {
wpi::Alert::AlertType::kWarning),
timesyncAlert(PHOTON_ALERT_GROUP, "", wpi::Alert::AlertType::kWarning) {
verifyDependencies();
InstanceCount++;
HAL_ReportUsage("PhotonVision/PhotonCamera", InstanceCount, "");
// The Robot class is actually created here:
// https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/frc/RobotBase.h#L33
// https://github.com/wpilibsuite/allwpilib/blob/811b1309683e930a1ce69fae818f943ff161b7a5/wpilibc/src/main/native/include/wpi/opmode/RobotBase.hpp#L33
// so we should be fine to call this from the ctor
InitTspServer();
}
PhotonCamera::PhotonCamera(const std::string_view cameraName)
: PhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
: PhotonCamera(wpi::nt::NetworkTableInstance::GetDefault(), cameraName) {}
PhotonPipelineResult PhotonCamera::GetLatestResult() {
if (test) {
@@ -190,8 +191,8 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
VerifyVersion();
// Fill the packet with latest data and populate result.
units::microsecond_t now =
units::microsecond_t(frc::RobotController::GetFPGATime());
wpi::units::microsecond_t now =
wpi::units::microsecond_t(wpi::RobotController::GetFPGATime());
const auto value = rawBytesEntry.Get();
if (!value.size()) return PhotonPipelineResult{};
@@ -223,7 +224,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
ret.reserve(changes.size());
for (size_t i = 0; i < changes.size(); i++) {
const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
const wpi::nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
if (!value.value.size() || value.time == 0) {
continue;
@@ -237,7 +238,7 @@ std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
// TODO: NT4 timestamps are still not to be trusted. But it's the best we
// can do until we can make time sync more reliable.
result.SetReceiveTimestamp(units::microsecond_t(value.time) -
result.SetReceiveTimestamp(wpi::units::microsecond_t(value.time) -
result.GetLatency());
ret.push_back(result);
@@ -261,11 +262,11 @@ void PhotonCamera::CheckTimeSyncOrWarn(photon::PhotonPipelineResult& result) {
timesyncAlert.SetText(warningText);
timesyncAlert.Set(true);
if (frc::Timer::GetFPGATimestamp() <
if (wpi::Timer::GetFPGATimestamp() <
(prevTimeSyncWarnTime + WARN_DEBOUNCE_SEC)) {
prevTimeSyncWarnTime = frc::Timer::GetFPGATimestamp();
prevTimeSyncWarnTime = wpi::Timer::GetFPGATimestamp();
FRC_ReportWarning(
WPILIB_ReportWarning(
warningText +
"\n\nCheck /photonvision/.timesync/{{COPROCESSOR_HOSTNAME}} for more "
"information.");
@@ -317,7 +318,7 @@ const std::string_view PhotonCamera::GetCameraName() const {
bool PhotonCamera::IsConnected() {
auto currentHeartbeat = heartbeatSubscriber.Get();
auto now = frc::Timer::GetFPGATimestamp();
auto now = wpi::Timer::GetFPGATimestamp();
if (currentHeartbeat < 0) {
// we have never heard from the camera
@@ -365,10 +366,10 @@ void PhotonCamera::VerifyVersion() {
return;
}
if ((frc::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
if ((wpi::Timer::GetFPGATimestamp() - lastVersionCheckTime) <
VERSION_CHECK_INTERVAL)
return;
this->lastVersionCheckTime = frc::Timer::GetFPGATimestamp();
this->lastVersionCheckTime = wpi::Timer::GetFPGATimestamp();
const std::string& versionString = versionEntry.Get("");
if (versionString.empty()) {
@@ -376,13 +377,13 @@ void PhotonCamera::VerifyVersion() {
std::vector<std::string> cameraNames =
rootTable->GetInstance().GetTable("photonvision")->GetSubTables();
if (cameraNames.empty()) {
FRC_ReportError(frc::warn::Warning,
"Could not find any PhotonVision coprocessors on "
"NetworkTables. Double check that PhotonVision is "
"running, and that your camera is connected!");
WPILIB_ReportError(wpi::warn::Warning,
"Could not find any PhotonVision coprocessors on "
"NetworkTables. Double check that PhotonVision is "
"running, and that your camera is connected!");
} else {
FRC_ReportError(
frc::warn::Warning,
WPILIB_ReportError(
wpi::warn::Warning,
"PhotonVision coprocessor at path {} not found on NetworkTables. "
"Double check that your camera names match!",
path_);
@@ -391,8 +392,8 @@ void PhotonCamera::VerifyVersion() {
for (unsigned int i = 0; i < cameraNames.size(); i++) {
cameraNameOutString += ("\n" + cameraNames[i]);
}
FRC_ReportError(
frc::warn::Warning,
WPILIB_ReportError(
wpi::warn::Warning,
"Found the following PhotonVision cameras on NetworkTables:\n{}",
cameraNameOutString);
}
@@ -400,12 +401,12 @@ void PhotonCamera::VerifyVersion() {
std::string local_uuid{SerdeType<PhotonPipelineResult>::GetSchemaHash()};
// implicit conversion here might throw an exception, so be careful of that
wpi::json remote_uuid_json =
wpi::util::json remote_uuid_json =
rawBytesEntry.GetTopic().GetProperty("message_uuid");
if (!remote_uuid_json.is_string()) {
FRC_ReportError(frc::warn::Warning,
"Cannot find property message_uuid for PhotonCamera {}",
path);
WPILIB_ReportError(
wpi::warn::Warning,
"Cannot find property message_uuid for PhotonCamera {}", path);
return;
}
std::string remote_uuid{remote_uuid_json};
@@ -429,12 +430,12 @@ void PhotonCamera::VerifyVersion() {
">>> !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>\n"
"\n\n";
FRC_ReportWarning(bfw);
WPILIB_ReportWarning(bfw);
std::string error_str = fmt::format(
"Photonlib version {} (message definition version {}) does not match "
"coprocessor version {} (message definition version {})!",
PhotonVersion::versionString, local_uuid, versionString, remote_uuid);
FRC_ReportError(frc::err::Error, "{}", error_str);
WPILIB_ReportError(wpi::err::Error, "{}", error_str);
throw std::runtime_error(error_str);
}
}

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

@@ -30,18 +30,18 @@
#include <vector>
#include <Eigen/Core>
#include <frc/Errors.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <hal/FRCUsageReporting.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/types.hpp>
#include <units/angle.h>
#include <units/math.h>
#include <units/time.h>
#include <wpi/deprecated.h>
#include <wpi/hal/UsageReporting.h>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/system/Errors.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/math.hpp>
#include <wpi/units/time.hpp>
#include "photon/PhotonCamera.h"
#include "photon/estimation/TargetModel.h"
@@ -55,37 +55,41 @@ WPI_IGNORE_DEPRECATED
namespace photon {
namespace detail {
cv::Point3d ToPoint3d(const frc::Translation3d& translation);
cv::Point3d ToPoint3d(const wpi::math::Translation3d& translation);
std::optional<std::array<cv::Point3d, 4>> CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags);
frc::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY, frc::Pose3d tagPose);
int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags);
wpi::math::Pose3d ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec);
cv::Point3d TagCornerToObjectPoint(wpi::units::meter_t cornerX,
wpi::units::meter_t cornerY,
wpi::math::Pose3d tagPose);
} // namespace detail
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
PhotonPoseEstimator::PhotonPoseEstimator(
wpi::apriltag::AprilTagFieldLayout tags,
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
lastPose(wpi::math::Pose3d()),
referencePose(wpi::math::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
headingBuffer(
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat,
frc::Transform3d robotToCamera)
PhotonPoseEstimator::PhotonPoseEstimator(
wpi::apriltag::AprilTagFieldLayout tags, PoseStrategy strat,
wpi::math::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
lastPose(wpi::math::Pose3d()),
referencePose(wpi::math::Pose3d()),
poseCacheTimestamp(-1_s),
headingBuffer(frc::TimeInterpolatableBuffer<frc::Rotation2d>(1_s)) {
headingBuffer(
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d>(1_s)) {
InstanceCount++;
HAL_ReportUsage("PhotonVision/PhotonPoseEstimator", InstanceCount, "");
}
@@ -93,8 +97,8 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
if (strategy == MULTI_TAG_PNP_ON_COPROCESSOR ||
strategy == MULTI_TAG_PNP_ON_RIO) {
FRC_ReportError(
frc::warn::Warning,
WPILIB_ReportError(
wpi::warn::Warning,
"Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity",
"");
strategy = LOWEST_AMBIGUITY;
@@ -112,15 +116,16 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
std::optional<ConstrainedSolvepnpParams> constrainedPnpParams) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
WPILIB_ReportError(wpi::warn::Warning,
"Result timestamp was reported in the past!");
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) {
wpi::units::math::abs(poseCacheTimestamp - result.GetTimestamp()) <
0.001_ms) {
return std::nullopt;
}
@@ -170,9 +175,9 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
break;
case MULTI_TAG_PNP_ON_RIO:
if (!cameraMatrixData && !cameraDistCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!",
"");
WPILIB_ReportError(
wpi::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!", "");
ret = Update(result, this->multiTagFallbackStrategy);
}
ret =
@@ -185,8 +190,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
using namespace frc;
if (!cameraMatrixData || !cameraDistCoeffs) {
FRC_ReportError(
frc::warn::Warning,
WPILib_ReportError(
wpi::warn::Warning,
"No camera calibration data provided for Constrained SolvePnP!");
ret = Update(result, this->multiTagFallbackStrategy);
break;
@@ -203,12 +208,12 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
break;
}
frc::Pose3d fieldToRobotSeed;
wpi::math::Pose3d fieldToRobotSeed;
if (result.MultiTagResult().has_value()) {
fieldToRobotSeed =
frc::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
wpi::math::Pose3d{} + (result.MultiTagResult()->estimatedPose.best +
m_robotToCamera.Inverse());
} else {
std::optional<EstimatedRobotPose> nestedUpdate =
Update(result, cameraMatrixData, cameraDistCoeffs, {},
@@ -236,8 +241,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
ret = EstimatePnpDistanceTrigSolvePose(result);
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
"");
WPILIB_ReportError(wpi::warn::Warning, "Invalid Pose Strategy selected!",
"");
ret = std::nullopt;
}
@@ -250,8 +255,8 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
bool ShouldEstimate(const PhotonPipelineResult& result) {
// Time in the past -- give up, since the following if expects times > 0
if (result.GetTimestamp() < 0_s) {
FRC_ReportError(frc::warn::Warning,
"Result timestamp was reported in the past!");
WPILib_ReportError(wpi::warn::Warning,
"Result timestamp was reported in the past!");
return false;
}
@@ -281,12 +286,12 @@ PhotonPoseEstimator::EstimateLowestAmbiguityPose(
auto& bestTarget = *foundIt;
std::optional<frc::Pose3d> fiducialPose =
std::optional<wpi::math::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
WPILIB_ReportError(wpi::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::nullopt;
}
@@ -302,28 +307,28 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestHeightDifference =
units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::meter_t smallestHeightDifference =
wpi::units::meter_t(std::numeric_limits<double>::infinity());
std::optional<EstimatedRobotPose> pose = std::nullopt;
for (auto& target : cameraResult.GetTargets()) {
std::optional<frc::Pose3d> fiducialPose =
std::optional<wpi::math::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
WPILIB_ReportError(wpi::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d const targetPose = *fiducialPose;
wpi::math::Pose3d const targetPose = *fiducialPose;
units::meter_t const alternativeDifference = units::math::abs(
wpi::units::meter_t const alternativeDifference = wpi::units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
.Z());
units::meter_t const bestDifference = units::math::abs(
wpi::units::meter_t const bestDifference = wpi::units::math::abs(
m_robotToCamera.Z() -
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
@@ -350,26 +355,26 @@ PhotonPoseEstimator::EstimateClosestToCameraHeightPose(
std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose) {
PhotonPipelineResult cameraResult, wpi::math::Pose3d referencePose) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
units::meter_t smallestDifference =
units::meter_t(std::numeric_limits<double>::infinity());
units::second_t stateTimestamp = units::second_t(0);
frc::Pose3d pose = lastPose;
wpi::units::meter_t smallestDifference =
wpi::units::meter_t(std::numeric_limits<double>::infinity());
wpi::units::second_t stateTimestamp = wpi::units::second_t(0);
wpi::math::Pose3d pose = lastPose;
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
std::optional<wpi::math::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
WPILIB_ReportError(wpi::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
wpi::math::Pose3d targetPose = fiducialPose.value();
const auto altPose =
targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
@@ -378,9 +383,9 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
.TransformBy(m_robotToCamera.Inverse());
units::meter_t const alternativeDifference = units::math::abs(
wpi::units::meter_t const alternativeDifference = wpi::units::math::abs(
referencePose.Translation().Distance(altPose.Translation()));
units::meter_t const bestDifference = units::math::abs(
wpi::units::meter_t const bestDifference = wpi::units::math::abs(
referencePose.Translation().Distance(bestPose.Translation()));
if (alternativeDifference < smallestDifference) {
smallestDifference = alternativeDifference;
@@ -400,7 +405,7 @@ PhotonPoseEstimator::EstimateClosestToReferencePose(
}
std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
int tagID, const frc::AprilTagFieldLayout& aprilTags) {
int tagID, const wpi::apriltag::AprilTagFieldLayout& aprilTags) {
if (auto tagPose = aprilTags.GetTagPose(tagID); tagPose.has_value()) {
return std::array{TagCornerToObjectPoint(-3_in, -3_in, *tagPose),
TagCornerToObjectPoint(+3_in, -3_in, *tagPose),
@@ -411,23 +416,23 @@ std::optional<std::array<cv::Point3d, 4>> detail::CalcTagCorners(
}
}
cv::Point3d detail::ToPoint3d(const frc::Translation3d& translation) {
cv::Point3d detail::ToPoint3d(const wpi::math::Translation3d& translation) {
return cv::Point3d(-translation.Y().value(), -translation.Z().value(),
+translation.X().value());
}
cv::Point3d detail::TagCornerToObjectPoint(units::meter_t cornerX,
units::meter_t cornerY,
frc::Pose3d tagPose) {
frc::Translation3d cornerTrans =
tagPose.Translation() +
frc::Translation3d(0.0_m, cornerX, cornerY).RotateBy(tagPose.Rotation());
cv::Point3d detail::TagCornerToObjectPoint(wpi::units::meter_t cornerX,
wpi::units::meter_t cornerY,
wpi::math::Pose3d tagPose) {
wpi::math::Translation3d cornerTrans =
tagPose.Translation() + wpi::math::Translation3d(0.0_m, cornerX, cornerY)
.RotateBy(tagPose.Rotation());
return ToPoint3d(cornerTrans);
}
frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace frc;
using namespace units;
wpi::math::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
using namespace wpi::math;
using namespace wpi::units;
cv::Mat R;
cv::Rodrigues(rvec, R); // R is 3x3
@@ -458,7 +463,7 @@ PhotonPoseEstimator::EstimateCoprocMultiTagPose(
const auto field2camera = cameraResult.MultiTagResult()->estimatedPose.best;
const auto fieldToRobot =
frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
wpi::math::Pose3d() + field2camera + m_robotToCamera.Inverse();
return photon::EstimatedRobotPose(fieldToRobot, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),
MULTI_TAG_PNP_ON_COPROCESSOR);
@@ -512,7 +517,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::EstimateRioMultiTagPose(
tvec, false, cv::SOLVEPNP_SQPNP);
}
const frc::Pose3d pose = detail::ToPose3d(tvec, rvec);
const wpi::math::Pose3d pose = detail::ToPose3d(tvec, rvec);
return photon::EstimatedRobotPose(
pose.TransformBy(m_robotToCamera.Inverse()), cameraResult.GetTimestamp(),
@@ -526,47 +531,50 @@ PhotonPoseEstimator::EstimatePnpDistanceTrigSolvePose(
return std::nullopt;
}
PhotonTrackedTarget bestTarget = cameraResult.GetBestTarget();
std::optional<frc::Rotation2d> headingSampleOpt =
std::optional<wpi::math::Rotation2d> headingSampleOpt =
headingBuffer.Sample(cameraResult.GetTimestamp());
if (!headingSampleOpt) {
FRC_ReportError(frc::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
WPILIB_ReportError(
wpi::warn::Warning,
"There was no heading data! Use AddHeadingData to add it!");
return std::nullopt;
}
frc::Rotation2d headingSample = headingSampleOpt.value();
wpi::math::Rotation2d headingSample = headingSampleOpt.value();
frc::Translation2d camToTagTranslation =
frc::Translation3d(
wpi::math::Translation2d camToTagTranslation =
wpi::math::Translation3d(
bestTarget.GetBestCameraToTarget().Translation().Norm(),
frc::Rotation3d(0_rad, -units::degree_t(bestTarget.GetPitch()),
-units::degree_t(bestTarget.GetYaw())))
wpi::math::Rotation3d(0_rad,
-wpi::units::degree_t(bestTarget.GetPitch()),
-wpi::units::degree_t(bestTarget.GetYaw())))
.RotateBy(m_robotToCamera.Rotation())
.ToTranslation2d()
.RotateBy(headingSample);
std::optional<frc::Pose3d> fiducialPose =
std::optional<wpi::math::Pose3d> fiducialPose =
aprilTags.GetTagPose(bestTarget.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
WPILIB_ReportError(wpi::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
bestTarget.GetFiducialId());
return std::nullopt;
}
frc::Pose2d tagPose = fiducialPose.value().ToPose2d();
wpi::math::Pose2d tagPose = fiducialPose.value().ToPose2d();
frc::Translation2d fieldToCameraTranslation =
wpi::math::Translation2d fieldToCameraTranslation =
tagPose.Translation() - camToTagTranslation;
frc::Translation2d camToRobotTranslation =
wpi::math::Translation2d camToRobotTranslation =
(-m_robotToCamera.Translation().ToTranslation2d())
.RotateBy(headingSample);
frc::Pose2d robotPose = frc::Pose2d(
wpi::math::Pose2d robotPose = wpi::math::Pose2d(
fieldToCameraTranslation + camToRobotTranslation, headingSample);
return EstimatedRobotPose{frc::Pose3d(robotPose), cameraResult.GetTimestamp(),
return EstimatedRobotPose{wpi::math::Pose3d(robotPose),
cameraResult.GetTimestamp(),
cameraResult.GetTargets(), PNP_DISTANCE_TRIG_SOLVE};
}
@@ -576,22 +584,23 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
std::vector<
std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>>
tempPoses;
double totalAmbiguity = 0;
auto targets = cameraResult.GetTargets();
for (auto& target : targets) {
std::optional<frc::Pose3d> fiducialPose =
std::optional<wpi::math::Pose3d> fiducialPose =
aprilTags.GetTagPose(target.GetFiducialId());
if (!fiducialPose) {
FRC_ReportError(frc::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
WPILIB_ReportError(wpi::warn::Warning,
"Tried to get pose of unknown April Tag: {}",
target.GetFiducialId());
continue;
}
frc::Pose3d targetPose = fiducialPose.value();
wpi::math::Pose3d targetPose = fiducialPose.value();
// Ambiguity = 0, use that pose
if (target.GetPoseAmbiguity() == 0) {
return EstimatedRobotPose{
@@ -608,17 +617,17 @@ PhotonPoseEstimator::EstimateAverageBestTargetsPose(
cameraResult.GetTimestamp())));
}
frc::Translation3d transform = frc::Translation3d();
frc::Rotation3d rotation = frc::Rotation3d();
wpi::math::Translation3d transform = wpi::math::Translation3d();
wpi::math::Rotation3d rotation = wpi::math::Rotation3d();
for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
tempPoses) {
for (std::pair<wpi::math::Pose3d, std::pair<double, wpi::units::second_t>>&
pair : tempPoses) {
double const weight = (1. / pair.second.first) / totalAmbiguity;
transform = transform + pair.first.Translation() * weight;
rotation = rotation + pair.first.Rotation() * weight;
}
return EstimatedRobotPose{frc::Pose3d(transform, rotation),
return EstimatedRobotPose{wpi::math::Pose3d(transform, rotation),
cameraResult.GetTimestamp(),
cameraResult.GetTargets(), AVERAGE_BEST_TARGETS};
}
@@ -627,8 +636,8 @@ std::optional<EstimatedRobotPose>
PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
bool headingFree, double headingScaleFactor) {
photon::PhotonCamera::DistortionMatrix distCoeffs,
wpi::math::Pose3d seedPose, bool headingFree, double headingScaleFactor) {
if (!ShouldEstimate(cameraResult)) {
return std::nullopt;
}
@@ -638,9 +647,9 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
return std::nullopt;
} else {
// If heading fixed, force rotation component
seedPose = frc::Pose3d{
seedPose = wpi::math::Pose3d{
seedPose.Translation(),
frc::Rotation3d{
wpi::math::Rotation3d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
}
}
@@ -651,7 +660,8 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
aprilTags, photon::kAprilTag36h11, headingFree,
frc::Rotation2d{
wpi::math::Rotation2d{
headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
headingScaleFactor);
@@ -659,7 +669,7 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
return std::nullopt;
}
frc::Pose3d best = frc::Pose3d{} + pnpResult->best;
wpi::math::Pose3d best = wpi::math::Pose3d{} + pnpResult->best;
return EstimatedRobotPose{best, cameraResult.GetTimestamp(),
cameraResult.GetTargets(),

81
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp
View File

@@ -29,8 +29,8 @@
#include <utility>
#include <vector>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/apriltag/AprilTagFields.hpp>
#include "photon/estimation/CameraTargetRelation.h"
#include "photon/estimation/RotTrlTransform3d.h"
@@ -40,19 +40,19 @@
namespace photon {
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera)
: PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG(),
frc::AprilTagFieldLayout::LoadField(
frc::AprilTagField::kDefaultField)) {}
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::kDefaultField)) {}
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
const SimCameraProperties& props,
const frc::AprilTagFieldLayout& tagLayout)
PhotonCameraSim::PhotonCameraSim(
PhotonCamera* camera, const SimCameraProperties& props,
const wpi::apriltag::AprilTagFieldLayout& tagLayout)
: prop{props}, cam{camera}, tagLayout{tagLayout} {
SetMinTargetAreaPixels(kDefaultMinAreaPx);
videoSimRaw =
frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
wpi::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
prop.GetResWidth(), prop.GetResHeight());
videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
videoSimProcessed = frc::CameraServer::PutVideo(
videoSimRaw.SetPixelFormat(wpi::cs::VideoMode::PixelFormat::kGray);
videoSimProcessed = wpi::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
prop.GetResHeight());
ts.subTable = cam->GetCameraTable();
@@ -62,21 +62,21 @@ PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
const SimCameraProperties& props,
double minTargetAreaPercent,
units::meter_t maxSightRange)
wpi::units::meter_t maxSightRange)
: PhotonCameraSim(camera, props) {
this->minTargetAreaPercent = minTargetAreaPercent;
this->maxSightRange = maxSightRange;
}
bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose,
bool PhotonCameraSim::CanSeeTargetPose(const wpi::math::Pose3d& camPose,
const VisionTargetSim& target) {
CameraTargetRelation rel{camPose, target.GetPose()};
return ((units::math::abs(rel.camToTargYaw.Degrees()) <
return ((wpi::units::math::abs(rel.camToTargYaw.Degrees()) <
prop.GetHorizFOV().Degrees() / 2.0) &&
(units::math::abs(rel.camToTargPitch.Degrees()) <
(wpi::units::math::abs(rel.camToTargPitch.Degrees()) <
prop.GetVertFOV().Degrees() / 2.0) &&
(!target.GetModel().GetIsPlanar() ||
units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
wpi::units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
(rel.camToTarg.Translation().Norm() <= maxSightRange));
}
bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
@@ -89,13 +89,13 @@ bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
return true;
}
std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
uint64_t now = wpi::Now();
uint64_t now = wpi::util::Now();
uint64_t timestamp{};
int iter = 0;
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
uint64_t frameTime = prop.EstSecUntilNextFrame()
.convert<units::microseconds>()
.convert<wpi::units::microseconds>()
.to<uint64_t>();
nextNTEntryTime += frameTime;
@@ -113,13 +113,13 @@ std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
}
}
PhotonPipelineResult PhotonCameraSim::Process(
units::second_t latency, const frc::Pose3d& cameraPose,
wpi::units::second_t latency, const wpi::math::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets) {
std::sort(targets.begin(), targets.end(),
[cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
units::meter_t dist1 =
wpi::units::meter_t dist1 =
t1.GetPose().Translation().Distance(cameraPose.Translation());
units::meter_t dist2 =
wpi::units::meter_t dist2 =
t2.GetPose().Translation().Distance(cameraPose.Translation());
return dist1 > dist2;
});
@@ -144,7 +144,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
continue;
}
std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
std::vector<wpi::math::Translation3d> fieldCorners = tgt.GetFieldVertices();
if (tgt.GetModel().GetIsSpherical()) {
TargetModel model = tgt.GetModel();
fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
@@ -186,11 +186,12 @@ PhotonPipelineResult PhotonCameraSim::Process(
r = i;
}
}
cv::RotatedRect rect{
cv::Point2d{center.x, center.y},
cv::Size2d{imagePoints[r].x - lc.x,
imagePoints[b].y - imagePoints[t].y},
units::radian_t{-angles[r]}.convert<units::degrees>().to<float>()};
cv::RotatedRect rect{cv::Point2d{center.x, center.y},
cv::Size2d{imagePoints[r].x - lc.x,
imagePoints[b].y - imagePoints[t].y},
wpi::units::radian_t{-angles[r]}
.convert<wpi::units::degrees>()
.to<float>()};
std::vector<cv::Point2f> points{};
rect.points(points);
@@ -210,7 +211,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
minAreaRectPts.reserve(4);
minAreaRect.points(minAreaRectPts);
cv::Point2d centerPt = minAreaRect.center;
frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
wpi::math::Rotation3d centerRot = prop.GetPixelRot(centerPt);
double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
if (!(CanSeeCorner(noisyTargetCorners) &&
@@ -250,12 +251,12 @@ PhotonPipelineResult PhotonCameraSim::Process(
std::vector<TargetCorner> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(),
-centerRot.Z().convert<wpi::units::degrees>().to<double>(),
-centerRot.Y().convert<wpi::units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<wpi::units::degrees>().to<double>(),
tgt.GetFiducialId(), classId, conf,
pnpSim ? pnpSim->best : frc::Transform3d{},
pnpSim ? pnpSim->alt : frc::Transform3d{},
pnpSim ? pnpSim->best : wpi::math::Transform3d{},
pnpSim ? pnpSim->alt : wpi::math::Transform3d{},
pnpSim ? pnpSim->ambiguity : -1, smallVec, cornersDouble);
}
@@ -288,7 +289,7 @@ PhotonPipelineResult PhotonCameraSim::Process(
videoSimRaw.PutFrame(videoSimFrameRaw);
} else {
videoSimRaw.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
if (videoSimProcEnabled) {
@@ -333,19 +334,19 @@ PhotonPipelineResult PhotonCameraSim::Process(
videoSimProcessed.PutFrame(videoSimFrameProcessed);
} else {
videoSimProcessed.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
wpi::cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
std::optional<MultiTargetPNPResult> multiTagResults = std::nullopt;
std::vector<frc::AprilTag> visibleLayoutTags =
std::vector<wpi::apriltag::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
std::vector<int16_t> usedIds{};
usedIds.resize(visibleLayoutTags.size());
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
[](const wpi::apriltag::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
auto pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
@@ -357,17 +358,17 @@ PhotonPipelineResult PhotonCameraSim::Process(
return PhotonPipelineResult{
PhotonPipelineMetadata{heartbeatCounter, 0,
units::microsecond_t{latency}.to<int64_t>(),
wpi::units::microsecond_t{latency}.to<int64_t>(),
1000000},
detectableTgts, multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
SubmitProcessedFrame(result, wpi::util::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t ReceiveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
result.GetLatency().convert<wpi::units::milliseconds>().to<double>(),
ReceiveTimestamp);
Packet newPacket{};
@@ -381,7 +382,7 @@ void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
ts.targetPitchEntry.Set(0.0, ReceiveTimestamp);
ts.targetYawEntry.Set(0.0, ReceiveTimestamp);
ts.targetAreaEntry.Set(0.0, ReceiveTimestamp);
ts.targetPoseEntry.Set(frc::Transform3d{}, ReceiveTimestamp);
ts.targetPoseEntry.Set(wpi::math::Transform3d{}, ReceiveTimestamp);
ts.targetSkewEntry.Set(0.0, ReceiveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();

86
photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp
View File

@@ -33,20 +33,20 @@
using namespace photon;
void SimCameraProperties::SetCalibration(int width, int height,
frc::Rotation2d fovDiag) {
wpi::math::Rotation2d fovDiag) {
if (fovDiag.Degrees() < 1_deg || fovDiag.Degrees() > 179_deg) {
fovDiag = frc::Rotation2d{
std::clamp<units::degree_t>(fovDiag.Degrees(), 1_deg, 179_deg)};
FRC_ReportError(
frc::err::Error,
fovDiag = wpi::math::Rotation2d{
std::clamp<wpi::units::degree_t>(fovDiag.Degrees(), 1_deg, 179_deg)};
WPILIB_ReportError(
wpi::err::Error,
"Requested invalid FOV! Clamping between (1, 179) degrees...");
}
double resDiag = std::sqrt(width * width + height * height);
double diagRatio = units::math::tan(fovDiag.Radians() / 2.0);
frc::Rotation2d fovWidth{
units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}};
frc::Rotation2d fovHeight{
units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
double diagRatio = wpi::units::math::tan(fovDiag.Radians() / 2.0);
wpi::math::Rotation2d fovWidth{
wpi::units::radian_t{std::atan(diagRatio * (width / resDiag)) * 2}};
wpi::math::Rotation2d fovHeight{
wpi::units::radian_t{std::atan(diagRatio * (height / resDiag)) * 2}};
Eigen::Matrix<double, 8, 1> newDistCoeffs =
Eigen::Matrix<double, 8, 1>::Zero();
@@ -54,8 +54,8 @@ void SimCameraProperties::SetCalibration(int width, int height,
double cx = width / 2.0 - 0.5;
double cy = height / 2.0 - 0.5;
double fx = cx / units::math::tan(fovWidth.Radians() / 2.0);
double fy = cy / units::math::tan(fovHeight.Radians() / 2.0);
double fx = cx / wpi::units::math::tan(fovWidth.Radians() / 2.0);
double fy = cy / wpi::units::math::tan(fovHeight.Radians() / 2.0);
Eigen::Matrix<double, 3, 3> newCamIntrinsics;
newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
@@ -70,29 +70,37 @@ void SimCameraProperties::SetCalibration(
camIntrinsics = newCamIntrinsics;
distCoeffs = newDistCoeffs;
std::array<frc::Translation3d, 4> p{
frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad,
(GetPixelYaw(0) +
frc::Rotation2d{units::radian_t{
-std::numbers::pi / 2.0}})
.Radians()}},
frc::Translation3d{1_m, frc::Rotation3d{0_rad, 0_rad,
(GetPixelYaw(width) +
frc::Rotation2d{units::radian_t{
std::numbers::pi / 2.0}})
.Radians()}},
frc::Translation3d{1_m, frc::Rotation3d{0_rad,
(GetPixelPitch(0) +
frc::Rotation2d{units::radian_t{
std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
frc::Translation3d{1_m, frc::Rotation3d{0_rad,
(GetPixelPitch(height) +
frc::Rotation2d{units::radian_t{
-std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
std::array<wpi::math::Translation3d, 4> p{
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad, 0_rad,
(GetPixelYaw(0) +
wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}})
.Radians()}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad, 0_rad,
(GetPixelYaw(width) +
wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}})
.Radians()}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad,
(GetPixelPitch(0) +
wpi::math::Rotation2d{wpi::units::radian_t{std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
wpi::math::Translation3d{
1_m,
wpi::math::Rotation3d{
0_rad,
(GetPixelPitch(height) +
wpi::math::Rotation2d{wpi::units::radian_t{-std::numbers::pi / 2.0}})
.Radians(),
0_rad}},
};
viewplanes.clear();
for (size_t i = 0; i < p.size(); i++) {
@@ -103,10 +111,10 @@ void SimCameraProperties::SetCalibration(
std::pair<std::optional<double>, std::optional<double>>
SimCameraProperties::GetVisibleLine(const RotTrlTransform3d& camRt,
const frc::Translation3d& a,
const frc::Translation3d& b) const {
frc::Translation3d relA = camRt.Apply(a);
frc::Translation3d relB = camRt.Apply(b);
const wpi::math::Translation3d& a,
const wpi::math::Translation3d& b) const {
wpi::math::Translation3d relA = camRt.Apply(a);
wpi::math::Translation3d relB = camRt.Apply(b);
if (relA.X() <= 0_m && relB.X() <= 0_m) {
return {std::nullopt, std::nullopt};

77
photon-lib/src/main/native/include/photon/PhotonCamera.h
View File

@@ -28,16 +28,17 @@
#include <string>
#include <vector>
#include <frc/Alert.h>
#include <networktables/BooleanTopic.h>
#include <networktables/DoubleArrayTopic.h>
#include <networktables/IntegerTopic.h>
#include <networktables/MultiSubscriber.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableInstance.h>
#include <networktables/RawTopic.h>
#include <networktables/StringTopic.h>
#include <units/time.h>
#include <wpi/nt/BooleanTopic.hpp>
#include <wpi/nt/DoubleArrayTopic.hpp>
#include <wpi/nt/DoubleTopic.hpp>
#include <wpi/nt/IntegerTopic.hpp>
#include <wpi/nt/MultiSubscriber.hpp>
#include <wpi/nt/NetworkTable.hpp>
#include <wpi/nt/NetworkTableInstance.hpp>
#include <wpi/nt/RawTopic.hpp>
#include <wpi/nt/StringTopic.hpp>
#include <wpi/units/time.hpp>
#include <wpi/util/Alert.hpp>
#include "photon/targeting/PhotonPipelineResult.h"
@@ -63,7 +64,7 @@ class PhotonCamera {
* @param cameraName The name of the camera, as seen in the UI.
* over.
*/
explicit PhotonCamera(nt::NetworkTableInstance instance,
explicit PhotonCamera(wpi::nt::NetworkTableInstance instance,
const std::string_view cameraName);
/**
@@ -203,55 +204,55 @@ class PhotonCamera {
*/
static void SetVersionCheckEnabled(bool enabled);
std::shared_ptr<nt::NetworkTable> GetCameraTable() const { return rootTable; }
std::shared_ptr<wpi::nt::NetworkTable> GetCameraTable() const { return rootTable; }
// For use in tests
bool test = false;
std::vector<PhotonPipelineResult> testResult;
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
std::shared_ptr<nt::NetworkTable> rootTable;
nt::RawSubscriber rawBytesEntry;
nt::IntegerPublisher inputSaveImgEntry;
nt::IntegerSubscriber inputSaveImgSubscriber;
nt::IntegerPublisher outputSaveImgEntry;
nt::IntegerSubscriber outputSaveImgSubscriber;
nt::IntegerPublisher pipelineIndexPub;
nt::IntegerSubscriber pipelineIndexSub;
nt::IntegerPublisher ledModePub;
nt::IntegerSubscriber ledModeSub;
nt::StringSubscriber versionEntry;
std::shared_ptr<wpi::nt::NetworkTable> mainTable;
std::shared_ptr<wpi::nt::NetworkTable> rootTable;
wpi::nt::RawSubscriber rawBytesEntry;
wpi::nt::IntegerPublisher inputSaveImgEntry;
wpi::nt::IntegerSubscriber inputSaveImgSubscriber;
wpi::nt::IntegerPublisher outputSaveImgEntry;
wpi::nt::IntegerSubscriber outputSaveImgSubscriber;
wpi::nt::IntegerPublisher pipelineIndexPub;
wpi::nt::IntegerSubscriber pipelineIndexSub;
wpi::nt::IntegerPublisher ledModePub;
wpi::nt::IntegerSubscriber ledModeSub;
wpi::nt::StringSubscriber versionEntry;
nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
nt::DoubleArraySubscriber cameraDistortionSubscriber;
wpi::nt::DoubleArraySubscriber cameraIntrinsicsSubscriber;
wpi::nt::DoubleArraySubscriber cameraDistortionSubscriber;
nt::BooleanSubscriber driverModeSubscriber;
nt::BooleanPublisher driverModePublisher;
nt::IntegerSubscriber fpsLimitSubscriber;
nt::IntegerPublisher fpsLimitPublisher;
wpi::nt::BooleanSubscriber driverModeSubscriber;
wpi::nt::BooleanPublisher driverModePublisher;
wpi::nt::IntegerSubscriber fpsLimitSubscriber;
wpi::nt::IntegerPublisher fpsLimitPublisher;
nt::IntegerSubscriber ledModeSubscriber;
wpi::nt::IntegerSubscriber ledModeSubscriber;
nt::IntegerSubscriber heartbeatSubscriber;
wpi::nt::IntegerSubscriber heartbeatSubscriber;
nt::MultiSubscriber topicNameSubscriber;
wpi::nt::MultiSubscriber topicNameSubscriber;
std::string path;
std::string cameraName;
frc::Alert disconnectAlert;
frc::Alert timesyncAlert;
wpi::Alert disconnectAlert;
wpi::Alert timesyncAlert;
private:
units::second_t lastVersionCheckTime = 0_s;
wpi::units::second_t lastVersionCheckTime = 0_s;
static bool VERSION_CHECK_ENABLED;
inline static int InstanceCount = 1;
units::second_t prevTimeSyncWarnTime = 0_s;
wpi::units::second_t prevTimeSyncWarnTime = 0_s;
int prevHeartbeatValue = -1;
units::second_t prevHeartbeatChangeTime = 0_s;
wpi::units::second_t prevHeartbeatChangeTime = 0_s;
void VerifyVersion();

77
photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
View File

@@ -24,12 +24,15 @@
#pragma once
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Rotation3d.h>
#include <frc/geometry/Transform3d.h>
#include <frc/interpolation/TimeInterpolatableBuffer.h>
#include <wpi/SmallVector.h>
#include <optional>
#include <span>
#include <wpi/util/SmallVector.hpp>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/math/interpolation/TimeInterpolatableBuffer.hpp>
#include "photon/PhotonCamera.h"
#include "photon/targeting/PhotonPipelineResult.h"
@@ -55,18 +58,18 @@ struct ConstrainedSolvepnpParams {
struct EstimatedRobotPose {
/** The estimated pose */
frc::Pose3d estimatedPose;
wpi::math::Pose3d estimatedPose;
/** The estimated time the frame used to derive the robot pose was taken, in
* the same timebase as the RoboRIO FPGA Timestamp */
units::second_t timestamp;
wpi::units::second_t timestamp;
/** A list of the targets used to compute this pose */
wpi::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
wpi::util::SmallVector<PhotonTrackedTarget, 10> targetsUsed;
/** The strategy actually used to produce this pose */
PoseStrategy strategy;
EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_,
EstimatedRobotPose(wpi::math::Pose3d pose_, wpi::units::second_t time_,
std::span<const PhotonTrackedTarget> targets,
PoseStrategy strategy_)
: estimatedPose(pose_),
@@ -92,7 +95,7 @@ class PhotonPoseEstimator {
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
frc::Transform3d robotToCamera);
wpi::math::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
@@ -108,16 +111,18 @@ class PhotonPoseEstimator {
[[deprecated(
"Use individual estimation methods with the 2 argument constructor "
"instead.")]]
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
explicit PhotonPoseEstimator(wpi::apriltag::AprilTagFieldLayout aprilTags,
PoseStrategy strategy,
frc::Transform3d robotToCamera);
wpi::math::Transform3d robotToCamera);
/**
* Get the AprilTagFieldLayout being used by the PositionEstimator.
*
* @return the AprilTagFieldLayout
*/
frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; }
wpi::apriltag::AprilTagFieldLayout GetFieldLayout() const {
return aprilTags;
}
/**
* Get the Position Estimation Strategy being used by the Position Estimator.
@@ -161,7 +166,7 @@ class PhotonPoseEstimator {
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
frc::Pose3d GetReferencePose() const {
wpi::math::Pose3d GetReferencePose() const {
return referencePose;
}
@@ -173,7 +178,7 @@ class PhotonPoseEstimator {
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetReferencePose(frc::Pose3d referencePose) {
inline void SetReferencePose(wpi::math::Pose3d referencePose) {
if (this->referencePose != referencePose) {
InvalidatePoseCache();
}
@@ -184,7 +189,7 @@ class PhotonPoseEstimator {
* @return The current transform from the center of the robot to the camera
* mount position.
*/
inline frc::Transform3d GetRobotToCameraTransform() {
inline wpi::math::Transform3d GetRobotToCameraTransform() {
return m_robotToCamera;
}
@@ -194,7 +199,7 @@ class PhotonPoseEstimator {
* @param robotToCamera The current transform from the center of the robot to
* the camera mount position.
*/
inline void SetRobotToCameraTransform(frc::Transform3d robotToCamera) {
inline void SetRobotToCameraTransform(wpi::math::Transform3d robotToCamera) {
m_robotToCamera = robotToCamera;
}
@@ -206,8 +211,10 @@ class PhotonPoseEstimator {
* @deprecated Use individual estimation methods instead.
*/
[[deprecated("Use individual estimation methods instead.")]]
inline void SetLastPose(frc::Pose3d lastPose) {
inline void SetLastPose(wpi::math::Pose3d lastPose) {
this->lastPose = lastPose;
}
/**
@@ -218,8 +225,8 @@ class PhotonPoseEstimator {
* @param heading Field-relative heading at the given timestamp. Standard
* WPILIB field coordinates.
*/
inline void AddHeadingData(units::second_t timestamp,
frc::Rotation2d heading) {
inline void AddHeadingData(wpi::units::second_t timestamp,
wpi::math::Rotation2d heading) {
this->headingBuffer.AddSample(timestamp, heading);
}
@@ -231,8 +238,8 @@ class PhotonPoseEstimator {
* @param heading Field-relative heading at the given timestamp. Standard
* WPILIB coordinates.
*/
inline void AddHeadingData(units::second_t timestamp,
frc::Rotation3d heading) {
inline void AddHeadingData(wpi::units::second_t timestamp,
wpi::math::Rotation3d heading) {
AddHeadingData(timestamp, heading.ToRotation2d());
}
@@ -245,8 +252,8 @@ class PhotonPoseEstimator {
* @param heading Field-relative robot heading at given timestamp. Standard
* WPILIB field coordinates.
*/
inline void ResetHeadingData(units::second_t timestamp,
frc::Rotation2d heading) {
inline void ResetHeadingData(wpi::units::second_t timestamp,
wpi::math::Rotation2d heading) {
headingBuffer.Clear();
AddHeadingData(timestamp, heading);
}
@@ -260,8 +267,8 @@ class PhotonPoseEstimator {
* @param heading Field-relative robot heading at given timestamp. Standard
* WPILIB field coordinates.
*/
inline void ResetHeadingData(units::second_t timestamp,
frc::Rotation3d heading) {
inline void ResetHeadingData(wpi::units::second_t timestamp,
wpi::math::Rotation3d heading) {
ResetHeadingData(timestamp, heading.ToRotation2d());
}
@@ -322,7 +329,7 @@ class PhotonPoseEstimator {
* targets used to create the estimate, or std::nullopt if there's no targets.
*/
std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
PhotonPipelineResult cameraResult, wpi::math::Pose3d referencePose);
/**
* Return the estimated position of the robot by using all visible tags to
@@ -410,22 +417,22 @@ class PhotonPoseEstimator {
std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
photon::PhotonPipelineResult cameraResult,
photon::PhotonCamera::CameraMatrix cameraMatrix,
photon::PhotonCamera::DistortionMatrix distCoeffs, frc::Pose3d seedPose,
photon::PhotonCamera::DistortionMatrix distCoeffs, wpi::math::Pose3d seedPose,
bool headingFree, double headingScaleFactor);
private:
frc::AprilTagFieldLayout aprilTags;
wpi::apriltag::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
frc::Transform3d m_robotToCamera;
wpi::math::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
frc::Pose3d referencePose;
wpi::math::Pose3d lastPose;
wpi::math::Pose3d referencePose;
units::second_t poseCacheTimestamp;
wpi::units::second_t poseCacheTimestamp;
frc::TimeInterpolatableBuffer<frc::Rotation2d> headingBuffer;
wpi::math::TimeInterpolatableBuffer<wpi::math::Rotation2d> headingBuffer;
inline static int InstanceCount = 1;

94
photon-lib/src/main/native/include/photon/PhotonUtils.h
View File

@@ -24,13 +24,13 @@
#pragma once
#include <frc/geometry/Pose2d.h>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Transform2d.h>
#include <frc/geometry/Translation2d.h>
#include <units/angle.h>
#include <units/length.h>
#include <units/math.h>
#include <wpi/math/geometry/Pose2d.hpp>
#include <wpi/math/geometry/Rotation2d.hpp>
#include <wpi/math/geometry/Transform2d.hpp>
#include <wpi/math/geometry/Translation2d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
#include <wpi/units/math.hpp>
namespace photon {
class PhotonUtils {
@@ -50,12 +50,12 @@ class PhotonUtils {
* values up.
* @return The estimated distance to the target.
*/
static units::meter_t CalculateDistanceToTarget(units::meter_t cameraHeight,
units::meter_t targetHeight,
units::radian_t cameraPitch,
units::radian_t targetPitch) {
static wpi::units::meter_t CalculateDistanceToTarget(wpi::units::meter_t cameraHeight,
wpi::units::meter_t targetHeight,
wpi::units::radian_t cameraPitch,
wpi::units::radian_t targetPitch) {
return (targetHeight - cameraHeight) /
units::math::tan(cameraPitch + targetPitch);
wpi::units::math::tan(cameraPitch + targetPitch);
}
/**
@@ -66,8 +66,8 @@ class PhotonUtils {
*
* @return The target's camera-relative translation.
*/
static frc::Translation2d EstimateCameraToTargetTranslation(
units::meter_t targetDistance, const frc::Rotation2d& yaw) {
static wpi::math::Translation2d EstimateCameraToTargetTranslation(
wpi::units::meter_t targetDistance, const wpi::math::Rotation2d& yaw) {
return {targetDistance * yaw.Cos(), targetDistance * yaw.Sin()};
}
@@ -89,20 +89,22 @@ class PhotonUtils {
* CW-positive.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @param fieldToTarget A frc::Pose2d representing the target position in
* the field coordinate system.
* @param fieldToTarget A wpi::math::Pose2d representing the target
* position in the field coordinate system.
* @param cameraToRobot The position of the robot relative to the camera.
* If the camera was mounted 3 inches behind the
* "origin" (usually physical center) of the robot,
* this would be frc::Transform2d(3 inches, 0
* this would be wpi::math::Transform2d(3 inches, 0
* inches, 0 degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
units::meter_t cameraHeight, units::meter_t targetHeight,
units::radian_t cameraPitch, units::radian_t targetPitch,
const frc::Rotation2d& targetYaw, const frc::Rotation2d& gyroAngle,
const frc::Pose2d& fieldToTarget, const frc::Transform2d& cameraToRobot) {
static wpi::math::Pose2d EstimateFieldToRobot(
wpi::units::meter_t cameraHeight, wpi::units::meter_t targetHeight,
wpi::units::radian_t cameraPitch, wpi::units::radian_t targetPitch,
const wpi::math::Rotation2d& targetYaw,
const wpi::math::Rotation2d& gyroAngle,
const wpi::math::Pose2d& fieldToTarget,
const wpi::math::Transform2d& cameraToRobot) {
return EstimateFieldToRobot(
EstimateCameraToTarget(
EstimateCameraToTargetTranslation(
@@ -114,30 +116,32 @@ class PhotonUtils {
}
/**
* Estimates a {@link frc::Transform2d} that maps the camera position to the
* target position, using the robot's gyro. Note that the gyro angle provided
* *must* line up with the field coordinate system -- that is, it should read
* zero degrees when pointed towards the opposing alliance station, and
* increase as the robot rotates CCW.
* Estimates a {@link wpi::math::Transform2d} that maps the camera position to
* the target position, using the robot's gyro. Note that the gyro angle
* provided *must* line up with the field coordinate system -- that is, it
* should read zero degrees when pointed towards the opposing alliance
* station, and increase as the robot rotates CCW.
*
* @param cameraToTargetTranslation A Translation2d that encodes the x/y
* position of the target relative to the
* camera.
* @param fieldToTarget A frc::Pose2d representing the target
* position in the field coordinate system.
* @param fieldToTarget A wpi::math::Pose2d representing the
* target position in the field coordinate system.
* @param gyroAngle The current robot gyro angle, likely from
* odometry.
* @return A frc::Transform2d that takes us from the camera to the target.
* @return A wpi::math::Transform2d that takes us from the camera to the
* target.
*/
static frc::Transform2d EstimateCameraToTarget(
const frc::Translation2d& cameraToTargetTranslation,
const frc::Pose2d& fieldToTarget, const frc::Rotation2d& gyroAngle) {
static wpi::math::Transform2d EstimateCameraToTarget(
const wpi::math::Translation2d& cameraToTargetTranslation,
const wpi::math::Pose2d& fieldToTarget,
const wpi::math::Rotation2d& gyroAngle) {
// This pose maps our camera at the origin out to our target, in the robot
// reference frame
// The translation part of this frc::Transform2d is from the above step, and
// the rotation uses our robot's gyro.
return frc::Transform2d(cameraToTargetTranslation,
-gyroAngle - fieldToTarget.Rotation());
// The translation part of this wpi::math::Transform2d is from the above
// step, and the rotation uses our robot's gyro.
return wpi::math::Transform2d(cameraToTargetTranslation,
-gyroAngle - fieldToTarget.Rotation());
}
/**
@@ -150,12 +154,14 @@ class PhotonUtils {
* @param cameraToRobot The position of the robot relative to the camera. If
* the camera was mounted 3 inches behind the "origin"
* (usually physical center) of the robot, this would be
* frc::Transform2d(3 inches, 0 inches, 0 degrees).
* wpi::math::Transform2d(3 inches, 0 inches, 0
* degrees).
* @return The position of the robot in the field.
*/
static frc::Pose2d EstimateFieldToRobot(
const frc::Transform2d& cameraToTarget, const frc::Pose2d& fieldToTarget,
const frc::Transform2d& cameraToRobot) {
static wpi::math::Pose2d EstimateFieldToRobot(
const wpi::math::Transform2d& cameraToTarget,
const wpi::math::Pose2d& fieldToTarget,
const wpi::math::Transform2d& cameraToRobot) {
return EstimateFieldToCamera(cameraToTarget, fieldToTarget)
.TransformBy(cameraToRobot);
}
@@ -170,9 +176,9 @@ class PhotonUtils {
* @param fieldToTarget The position of the target in the field.
* @return The position of the camera in the field.
*/
static frc::Pose2d EstimateFieldToCamera(
const frc::Transform2d& cameraToTarget,
const frc::Pose2d& fieldToTarget) {
static wpi::math::Pose2d EstimateFieldToCamera(
const wpi::math::Transform2d& cameraToTarget,
const wpi::math::Pose2d& fieldToTarget) {
auto targetToCamera = cameraToTarget.Inverse();
return fieldToTarget.TransformBy(targetToCamera);
}

41
photon-lib/src/main/native/include/photon/simulation/PhotonCameraSim.h
View File

@@ -27,15 +27,16 @@
#include <limits>
#include <vector>
#include <cameraserver/CameraServer.h>
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/apriltag/AprilTagFields.h>
#include <photon/PhotonCamera.h>
#include <photon/networktables/NTTopicSet.h>
#include <photon/simulation/SimCameraProperties.h>
#include <photon/simulation/VisionTargetSim.h>
#include <units/math.h>
#include <wpi/timestamp.h>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/apriltag/AprilTagFields.hpp>
#include <wpi/cameraserver/CameraServer.hpp>
#include <wpi/system/Timer.hpp>
#include <wpi/units/math.hpp>
#include <wpi/util/timestamp.h>
namespace photon {
class PhotonCameraSim {
@@ -68,9 +69,9 @@ class PhotonCameraSim {
* positions.
*/
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
const frc::AprilTagFieldLayout& tagLayout =
frc::AprilTagFieldLayout::LoadField(
frc::AprilTagField::kDefaultField));
const wpi::apriltag::AprilTagFieldLayout& tagLayout =
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::kDefaultField));
/**
* Constructs a handle for simulating PhotonCamera values. Processing
@@ -87,7 +88,7 @@ class PhotonCameraSim {
* separate from this.
*/
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
double minTargetAreaPercent, units::meter_t maxSightRange);
double minTargetAreaPercent, wpi::units::meter_t maxSightRange);
/**
* Returns the camera being simulated.
@@ -120,8 +121,8 @@ class PhotonCameraSim {
*
* @return The distance
*/
inline units::meter_t GetMaxSightRange() { return maxSightRange; }
inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
inline wpi::units::meter_t GetMaxSightRange() { return maxSightRange; }
inline const wpi::cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
inline const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
/**
@@ -132,7 +133,7 @@ class PhotonCameraSim {
* @param target Vision target containing pose and shape
* @return If this vision target can be seen before image projection.
*/
bool CanSeeTargetPose(const frc::Pose3d& camPose,
bool CanSeeTargetPose(const wpi::math::Pose3d& camPose,
const VisionTargetSim& target);
/**
@@ -182,7 +183,7 @@ class PhotonCameraSim {
*
* @param rangeMeters The distance
*/
inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
inline void SetMaxSightRange(wpi::units::meter_t range) { maxSightRange = range; }
/**
* Sets whether the raw video stream simulation is enabled.
@@ -225,8 +226,8 @@ class PhotonCameraSim {
inline void EnabledProcessedStream(double enabled) {
videoSimProcEnabled = enabled;
}
PhotonPipelineResult Process(units::second_t latency,
const frc::Pose3d& cameraPose,
PhotonPipelineResult Process(wpi::units::second_t latency,
const wpi::math::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets);
void SubmitProcessedFrame(const PhotonPipelineResult& result);
@@ -241,20 +242,20 @@ class PhotonCameraSim {
NTTopicSet ts{};
int64_t heartbeatCounter{0};
uint64_t nextNTEntryTime{wpi::Now()};
uint64_t nextNTEntryTime{wpi::util::Now()};
units::meter_t maxSightRange{std::numeric_limits<double>::max()};
wpi::units::meter_t maxSightRange{std::numeric_limits<double>::max()};
static constexpr double kDefaultMinAreaPx{100};
double minTargetAreaPercent;
frc::AprilTagFieldLayout tagLayout;
wpi::apriltag::AprilTagFieldLayout tagLayout;
cs::CvSource videoSimRaw;
wpi::cs::CvSource videoSimRaw;
cv::Mat videoSimFrameRaw{};
bool videoSimRawEnabled{true};
bool videoSimWireframeEnabled{false};
double videoSimWireframeResolution{0.1};
cs::CvSource videoSimProcessed;
wpi::cs::CvSource videoSimProcessed;
cv::Mat videoSimFrameProcessed{};
bool videoSimProcEnabled{true};
};

99
photon-lib/src/main/native/include/photon/simulation/SimCameraProperties.h
View File

@@ -30,11 +30,11 @@
#include <vector>
#include <Eigen/Core>
#include <frc/geometry/Rotation2d.h>
#include <frc/geometry/Translation3d.h>
#include <photon/estimation/OpenCVHelp.h>
#include <units/frequency.h>
#include <units/time.h>
#include <wpi/math/geometry/Rotation2d.hpp>
#include <wpi/math/geometry/Translation3d.hpp>
#include <wpi/units/frequency.hpp>
#include <wpi/units/time.hpp>
namespace photon {
@@ -56,7 +56,9 @@ namespace photon {
class SimCameraProperties {
public:
/** Default constructor which is the same as PERFECT_90DEG */
SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); }
SimCameraProperties() {
SetCalibration(960, 720, wpi::math::Rotation2d{90_deg});
}
/**
* Reads camera properties from a PhotonVision config.json file.
@@ -69,7 +71,7 @@ class SimCameraProperties {
*/
SimCameraProperties(std::string path, int width, int height) {}
void SetCalibration(int width, int height, frc::Rotation2d fovDiag);
void SetCalibration(int width, int height, wpi::math::Rotation2d fovDiag);
void SetCalibration(int width, int height,
const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
const Eigen::Matrix<double, 8, 1>& newDistCoeffs);
@@ -85,8 +87,8 @@ class SimCameraProperties {
* @param fps The average frames per second the camera should process at.
* **Exposure time limits FPS if set!**
*/
void SetFPS(units::hertz_t fps) {
frameSpeed = units::math::max(1 / fps, exposureTime);
void SetFPS(wpi::units::hertz_t fps) {
frameSpeed = wpi::units::math::max(1 / fps, exposureTime);
}
/**
@@ -95,9 +97,9 @@ class SimCameraProperties {
* @param exposureTime The amount of time the "shutter" is open for one frame.
* Affects motion blur. **Frame speed(from FPS) is limited to this!**
*/
void SetExposureTime(units::second_t exposureTime) {
void SetExposureTime(wpi::units::second_t exposureTime) {
this->exposureTime = exposureTime;
frameSpeed = units::math::max(frameSpeed, this->exposureTime);
frameSpeed = wpi::units::math::max(frameSpeed, this->exposureTime);
}
/**
@@ -106,7 +108,7 @@ class SimCameraProperties {
* @param avgLatency The average latency (from image capture to data
* published) a frame should have
*/
void SetAvgLatency(units::second_t avgLatency) {
void SetAvgLatency(wpi::units::second_t avgLatency) {
this->avgLatency = avgLatency;
}
@@ -115,7 +117,7 @@ class SimCameraProperties {
*
* @param latencyStdDev The standard deviation of the latency
*/
void SetLatencyStdDev(units::second_t latencyStdDev) {
void SetLatencyStdDev(wpi::units::second_t latencyStdDev) {
this->latencyStdDev = latencyStdDev;
}
@@ -159,35 +161,35 @@ class SimCameraProperties {
*
* @return The FPS
*/
units::hertz_t GetFPS() const { return 1 / frameSpeed; }
wpi::units::hertz_t GetFPS() const { return 1 / frameSpeed; }
/**
* Gets the time per frame of the simulated camera.
*
* @return The time per frame
*/
units::second_t GetFrameSpeed() const { return frameSpeed; }
wpi::units::second_t GetFrameSpeed() const { return frameSpeed; }
/**
* Gets the exposure time of the simulated camera.
*
* @return The exposure time
*/
units::second_t GetExposureTime() const { return exposureTime; }
wpi::units::second_t GetExposureTime() const { return exposureTime; }
/**
* Gets the average latency of the simulated camera.
*
* @return The average latency
*/
units::second_t GetAverageLatency() const { return avgLatency; }
wpi::units::second_t GetAverageLatency() const { return avgLatency; }
/**
* Gets the time per frame of the simulated camera.
*
* @return The time per frame
*/
units::second_t GetLatencyStdDev() const { return latencyStdDev; }
wpi::units::second_t GetLatencyStdDev() const { return latencyStdDev; }
/**
* The percentage (0 - 100) of this camera's resolution the contour takes up
@@ -203,11 +205,11 @@ class SimCameraProperties {
/** The yaw from the principal point of this camera to the pixel x value.
* Positive values left. */
frc::Rotation2d GetPixelYaw(double pixelX) const {
wpi::math::Rotation2d GetPixelYaw(double pixelX) const {
double fx = camIntrinsics(0, 0);
double cx = camIntrinsics(0, 2);
double xOffset = cx - pixelX;
return frc::Rotation2d{fx, xOffset};
return wpi::math::Rotation2d{fx, xOffset};
}
/**
@@ -217,11 +219,11 @@ class SimCameraProperties {
* Note that this angle is naively computed and may be incorrect. See
* #getCorrectedPixelRot(const cv::Point2d).
*/
frc::Rotation2d GetPixelPitch(double pixelY) const {
wpi::math::Rotation2d GetPixelPitch(double pixelY) const {
double fy = camIntrinsics(1, 1);
double cy = camIntrinsics(1, 2);
double yOffset = cy - pixelY;
return frc::Rotation2d{fy, -yOffset};
return wpi::math::Rotation2d{fy, -yOffset};
}
/**
* Finds the yaw and pitch to the given image point. Yaw is positive left, and
@@ -230,9 +232,9 @@ class SimCameraProperties {
* Note that pitch is naively computed and may be incorrect. See
* #getCorrectedPixelRot(const cv::Point2d).
*/
frc::Rotation3d GetPixelRot(const cv::Point2d& point) const {
return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(),
GetPixelYaw(point.x).Radians()};
wpi::math::Rotation3d GetPixelRot(const cv::Point2d& point) const {
return wpi::math::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(),
GetPixelYaw(point.x).Radians()};
}
/**
@@ -260,7 +262,7 @@ class SimCameraProperties {
* @return Rotation3d with yaw and pitch of the line projected out of the
* camera from the given pixel (roll is zero).
*/
frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const {
wpi::math::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const {
double fx = camIntrinsics(0, 0);
double cx = camIntrinsics(0, 2);
double xOffset = cx - point.x;
@@ -269,26 +271,27 @@ class SimCameraProperties {
double cy = camIntrinsics(1, 2);
double yOffset = cy - point.y;
frc::Rotation2d yaw{fx, xOffset};
frc::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)), -yOffset};
return frc::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()};
wpi::math::Rotation2d yaw{fx, xOffset};
wpi::math::Rotation2d pitch{fy / std::cos(std::atan(xOffset / fx)),
-yOffset};
return wpi::math::Rotation3d{0_rad, pitch.Radians(), yaw.Radians()};
}
frc::Rotation2d GetHorizFOV() const {
frc::Rotation2d left = GetPixelYaw(0);
frc::Rotation2d right = GetPixelYaw(resWidth);
wpi::math::Rotation2d GetHorizFOV() const {
wpi::math::Rotation2d left = GetPixelYaw(0);
wpi::math::Rotation2d right = GetPixelYaw(resWidth);
return left - right;
}
frc::Rotation2d GetVertFOV() const {
frc::Rotation2d above = GetPixelPitch(0);
frc::Rotation2d below = GetPixelPitch(resHeight);
wpi::math::Rotation2d GetVertFOV() const {
wpi::math::Rotation2d above = GetPixelPitch(0);
wpi::math::Rotation2d below = GetPixelPitch(resHeight);
return below - above;
}
frc::Rotation2d GetDiagFOV() const {
return frc::Rotation2d{
units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
wpi::math::Rotation2d GetDiagFOV() const {
return wpi::math::Rotation2d{
wpi::units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
}
/**
@@ -303,7 +306,7 @@ class SimCameraProperties {
* inside the camera frustum, {0.5, 1} would be returned.
*
* @param camRt The change in basis from world coordinates to camera
* coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d).
* coordinates. See RotTrlTransform3d#makeRelativeTo(wpi::math::Pose3d).
* @param a The initial translation of the line
* @param b The final translation of the line
* @return A Pair of Doubles. The values may be empty:
@@ -316,8 +319,8 @@ class SimCameraProperties {
* visible in the camera frustum.
*/
std::pair<std::optional<double>, std::optional<double>> GetVisibleLine(
const RotTrlTransform3d& camRt, const frc::Translation3d& a,
const frc::Translation3d& b) const;
const RotTrlTransform3d& camRt, const wpi::math::Translation3d& a,
const wpi::math::Translation3d& b) const;
/**
* Returns these points after applying this camera's estimated noise.
@@ -349,8 +352,8 @@ class SimCameraProperties {
*
* @return The latency estimate
*/
units::second_t EstLatency() {
return units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
wpi::units::second_t EstLatency() {
return wpi::units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
0_s);
}
@@ -359,8 +362,8 @@ class SimCameraProperties {
*
* @return The estimated time until the next frame
*/
units::second_t EstSecUntilNextFrame() {
return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed);
wpi::units::second_t EstSecUntilNextFrame() {
return frameSpeed + wpi::units::math::max(0_s, EstLatency() - frameSpeed);
}
/**
@@ -532,10 +535,10 @@ class SimCameraProperties {
Eigen::Matrix<double, 8, 1> distCoeffs;
double avgErrorPx{0};
double errorStdDevPx{0};
units::second_t frameSpeed{0};
units::second_t exposureTime{0};
units::second_t avgLatency{0};
units::second_t latencyStdDev{0};
wpi::units::second_t frameSpeed{0};
wpi::units::second_t exposureTime{0};
wpi::units::second_t avgLatency{0};
wpi::units::second_t latencyStdDev{0};
std::vector<Eigen::Matrix<double, 3, 1>> viewplanes{};
};
} // namespace photon

127
photon-lib/src/main/native/include/photon/simulation/VideoSimUtil.h
View File

@@ -31,12 +31,12 @@
#include <utility>
#include <vector>
#include <cscore_cv.h>
#include <frc/apriltag/AprilTag.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/objdetect.hpp>
#include <units/length.h>
#include <wpi/apriltag/AprilTag.hpp>
#include <wpi/cs/cscore_cv.hpp>
#include <wpi/units/length.hpp>
#include "photon/simulation/SimCameraProperties.h"
@@ -53,12 +53,12 @@ namespace VideoSimUtil {
// required
static constexpr int kNumTags36h11 = 40;
static constexpr units::meter_t fieldLength{16.54175_m};
static constexpr units::meter_t fieldWidth{8.0137_m};
static constexpr wpi::units::meter_t fieldLength{16.54175_m};
static constexpr wpi::units::meter_t fieldWidth{8.0137_m};
static cv::Mat Get36h11TagImage(int id) {
wpi::RawFrame frame;
frc::AprilTag::Generate36h11AprilTagImage(&frame, id);
wpi::util::RawFrame frame;
wpi::apriltag::AprilTag::Generate36h11AprilTagImage(&frame, id);
cv::Mat markerImage{frame.height, frame.width, CV_8UC1, frame.data,
static_cast<size_t>(frame.stride)};
cv::Mat markerClone = markerImage.clone();
@@ -128,7 +128,7 @@ static const std::vector<cv::Point2f> kTag36h11MarkPts = Get36h11MarkerPts();
/** Updates the properties of this cs::CvSource video stream with the given
* camera properties. */
[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
[[maybe_unused]] static void UpdateVideoProp(wpi::cs::CvSource& video,
const SimCameraProperties& prop) {
video.SetResolution(prop.GetResWidth(), prop.GetResHeight());
video.SetFPS(prop.GetFPS().to<int>());
@@ -343,55 +343,56 @@ static void DrawPoly(const std::vector<cv::Point2f>& dstPoints, int thickness,
* The translations used to draw the field side walls and driver station walls.
* It is a vector of vectors because the translations are not all connected.
*/
static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
std::vector<std::vector<frc::Translation3d>> list;
static std::vector<std::vector<wpi::math::Translation3d>> GetFieldWallLines() {
std::vector<std::vector<wpi::math::Translation3d>> list;
const units::meter_t sideHt = 19.5_in;
const units::meter_t driveHt = 35_in;
const units::meter_t topHt = 78_in;
const wpi::units::meter_t sideHt = 19.5_in;
const wpi::units::meter_t driveHt = 35_in;
const wpi::units::meter_t topHt = 78_in;
// field floor
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, 0_m, 0_m},
frc::Translation3d{fieldLength, 0_m, 0_m},
frc::Translation3d{fieldLength, fieldWidth, 0_m},
frc::Translation3d{0_m, fieldWidth, 0_m},
frc::Translation3d{0_m, 0_m, 0_m}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, 0_m, 0_m},
wpi::math::Translation3d{fieldLength, 0_m, 0_m},
wpi::math::Translation3d{fieldLength, fieldWidth, 0_m},
wpi::math::Translation3d{0_m, fieldWidth, 0_m},
wpi::math::Translation3d{0_m, 0_m, 0_m}});
// right side wall
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Translation3d{0_m, 0_m, sideHt},
frc::Translation3d{fieldLength, 0_m, sideHt},
frc::Translation3d{fieldLength, 0_m, 0_m}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, 0_m, 0_m},
wpi::math::Translation3d{0_m, 0_m, sideHt},
wpi::math::Translation3d{fieldLength, 0_m, sideHt},
wpi::math::Translation3d{fieldLength, 0_m, 0_m}});
// red driverstation
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{fieldLength, 0_m, sideHt},
frc::Translation3d{fieldLength, 0_m, topHt},
frc::Translation3d{fieldLength, fieldWidth, topHt},
frc::Translation3d{fieldLength, fieldWidth, sideHt},
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{fieldLength, 0_m, sideHt},
wpi::math::Translation3d{fieldLength, 0_m, topHt},
wpi::math::Translation3d{fieldLength, fieldWidth, topHt},
wpi::math::Translation3d{fieldLength, fieldWidth, sideHt},
});
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{fieldLength, 0_m, driveHt},
frc::Translation3d{fieldLength, fieldWidth, driveHt}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{fieldLength, 0_m, driveHt},
wpi::math::Translation3d{fieldLength, fieldWidth, driveHt}});
// left side wall
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, fieldWidth, 0_m},
frc::Translation3d{0_m, fieldWidth, sideHt},
frc::Translation3d{fieldLength, fieldWidth, sideHt},
frc::Translation3d{fieldLength, fieldWidth, 0_m}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, fieldWidth, 0_m},
wpi::math::Translation3d{0_m, fieldWidth, sideHt},
wpi::math::Translation3d{fieldLength, fieldWidth, sideHt},
wpi::math::Translation3d{fieldLength, fieldWidth, 0_m}});
// blue driverstation
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, 0_m, sideHt},
frc::Translation3d{0_m, 0_m, topHt},
frc::Translation3d{0_m, fieldWidth, topHt},
frc::Translation3d{0_m, fieldWidth, sideHt},
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, 0_m, sideHt},
wpi::math::Translation3d{0_m, 0_m, topHt},
wpi::math::Translation3d{0_m, fieldWidth, topHt},
wpi::math::Translation3d{0_m, fieldWidth, sideHt},
});
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, 0_m, driveHt},
frc::Translation3d{0_m, fieldWidth, driveHt}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, 0_m, driveHt},
wpi::math::Translation3d{0_m, fieldWidth, driveHt}});
return list;
}
@@ -405,19 +406,19 @@ static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
* floor. E.g. 3 subdivisions would mean 2 lines along the length and 2 lines
* along the width creating a 3x3 "grid".
*/
static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
static std::vector<std::vector<wpi::math::Translation3d>> GetFieldFloorLines(
int subdivisions) {
std::vector<std::vector<frc::Translation3d>> list;
const units::meter_t subLength = fieldLength / subdivisions;
const units::meter_t subWidth = fieldWidth / subdivisions;
std::vector<std::vector<wpi::math::Translation3d>> list;
const wpi::units::meter_t subLength = fieldLength / subdivisions;
const wpi::units::meter_t subWidth = fieldWidth / subdivisions;
for (int i = 0; i < subdivisions; i++) {
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{0_m, subWidth * (i + 1), 0_m},
frc::Translation3d{fieldLength, subWidth * (i + 1), 0_m}});
list.emplace_back(std::vector<frc::Translation3d>{
frc::Translation3d{subLength * (i + 1), 0_m, 0_m},
frc::Translation3d{subLength * (i + 1), fieldWidth, 0_m}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{0_m, subWidth * (i + 1), 0_m},
wpi::math::Translation3d{fieldLength, subWidth * (i + 1), 0_m}});
list.emplace_back(std::vector<wpi::math::Translation3d>{
wpi::math::Translation3d{subLength * (i + 1), 0_m, 0_m},
wpi::math::Translation3d{subLength * (i + 1), fieldWidth, 0_m}});
}
return list;
}
@@ -441,19 +442,19 @@ static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
*/
static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
const std::vector<frc::Translation3d>& trls, double resolution,
const std::vector<wpi::math::Translation3d>& trls, double resolution,
bool isClosed, cv::Mat& destination) {
resolution = std::hypot(destination.size().height, destination.size().width) *
resolution;
std::vector<frc::Translation3d> pts{trls};
std::vector<wpi::math::Translation3d> pts{trls};
if (isClosed) {
pts.emplace_back(pts[0]);
}
std::vector<std::vector<cv::Point2f>> polyPointList{};
for (size_t i = 0; i < pts.size() - 1; i++) {
frc::Translation3d pta = pts[i];
frc::Translation3d ptb = pts[i + 1];
wpi::math::Translation3d pta = pts[i];
wpi::math::Translation3d ptb = pts[i + 1];
std::pair<std::optional<double>, std::optional<double>> inter =
prop.GetVisibleLine(camRt, pta, ptb);
@@ -463,8 +464,8 @@ static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
double inter1 = inter.first.value();
double inter2 = inter.second.value();
frc::Translation3d baseDelta = ptb - pta;
frc::Translation3d old_pta = pta;
wpi::math::Translation3d baseDelta = ptb - pta;
wpi::math::Translation3d old_pta = pta;
if (inter1 > 0) {
pta = old_pta + baseDelta * inter1;
}
@@ -480,8 +481,8 @@ static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
double pxDist = std::hypot(pxb.x - pxa.x, pxb.y - pxa.y);
int subdivisions = static_cast<int>(pxDist / resolution);
frc::Translation3d subDelta = baseDelta / (subdivisions + 1);
std::vector<frc::Translation3d> subPts{};
wpi::math::Translation3d subDelta = baseDelta / (subdivisions + 1);
std::vector<wpi::math::Translation3d> subPts{};
for (int j = 0; j < subdivisions; j++) {
subPts.emplace_back(pta + (subDelta * (j + 1)));
}
@@ -501,7 +502,7 @@ static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
* Draw a wireframe of the field to the given image.
*
* @param camRt The change in basis from world coordinates to camera
* coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d).
* coordinates. See RotTrlTransform3d#makeRelativeTo(wpi::math::Pose3d).
* @param prop The simulated camera's properties.
* @param resolution Resolution as a fraction(0 - 1) of the video frame's
* diagonal length in pixels. Line segments will be subdivided if they exceed

120
photon-lib/src/main/native/include/photon/simulation/VisionSystemSim.h
View File

@@ -29,11 +29,11 @@
#include <utility>
#include <vector>
#include <frc/Timer.h>
#include <frc/interpolation/TimeInterpolatableBuffer.h>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/FieldObject2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <wpi/math/interpolation/TimeInterpolatableBuffer.hpp>
#include <wpi/smartdashboard/Field2d.hpp>
#include <wpi/smartdashboard/FieldObject2d.hpp>
#include <wpi/smartdashboard/SmartDashboard.hpp>
#include <wpi/system/Timer.hpp>
#include "photon/simulation/PhotonCameraSim.h"
@@ -60,7 +60,7 @@ class VisionSystemSim {
*/
explicit VisionSystemSim(std::string visionSystemName) {
std::string tableName = "VisionSystemSim-" + visionSystemName;
frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
wpi::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
}
/** Get one of the simulated cameras. */
@@ -91,17 +91,18 @@ class VisionSystemSim {
* @param robotToCamera The transform from the robot pose to the camera pose
*/
void AddCamera(PhotonCameraSim* cameraSim,
const frc::Transform3d& robotToCamera) {
const wpi::math::Transform3d& robotToCamera) {
auto found =
camSimMap.find(std::string{cameraSim->GetCamera()->GetCameraName()});
if (found == camSimMap.end()) {
camSimMap[std::string{cameraSim->GetCamera()->GetCameraName()}] =
cameraSim;
camTrfMap.insert(std::make_pair(
std::move(cameraSim),
frc::TimeInterpolatableBuffer<frc::Pose3d>{bufferLength}));
camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
frc::Pose3d{} + robotToCamera);
camTrfMap.insert(
std::make_pair(std::move(cameraSim),
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d>{
bufferLength}));
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(),
wpi::math::Pose3d{} + robotToCamera);
}
}
@@ -135,8 +136,9 @@ class VisionSystemSim {
* of
* @return The transform of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
std::optional<wpi::math::Transform3d> GetRobotToCamera(
PhotonCameraSim* cameraSim) {
return GetRobotToCamera(cameraSim, wpi::Timer::GetFPGATimestamp());
}
/**
@@ -148,17 +150,17 @@ class VisionSystemSim {
* @param time Timestamp of when the transform should be observed
* @return The transform of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
units::second_t time) {
std::optional<wpi::math::Transform3d> GetRobotToCamera(
PhotonCameraSim* cameraSim, wpi::units::second_t time) {
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
frc::TimeInterpolatableBuffer<frc::Pose3d> trfBuffer =
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d> trfBuffer =
camTrfMap.at(cameraSim);
std::optional<frc::Pose3d> sample = trfBuffer.Sample(time);
std::optional<wpi::math::Pose3d> sample = trfBuffer.Sample(time);
if (!sample) {
return std::nullopt;
} else {
return std::make_optional(
frc::Transform3d{frc::Pose3d{}, sample.value_or(frc::Pose3d{})});
return std::make_optional(wpi::math::Transform3d{
wpi::math::Pose3d{}, sample.value_or(wpi::math::Pose3d{})});
}
} else {
return std::nullopt;
@@ -172,8 +174,8 @@ class VisionSystemSim {
* @param cameraSim The specific camera to get the field pose of
* @return The pose of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
std::optional<wpi::math::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
return GetCameraPose(cameraSim, wpi::Timer::GetFPGATimestamp());
}
/**
@@ -184,8 +186,8 @@ class VisionSystemSim {
* @param time Timestamp of when the pose should be observed
* @return The pose of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
units::second_t time) {
std::optional<wpi::math::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
wpi::units::second_t time) {
auto robotToCamera = GetRobotToCamera(cameraSim, time);
if (!robotToCamera) {
return std::nullopt;
@@ -203,10 +205,10 @@ class VisionSystemSim {
* @return If the cameraSim was valid and transform was adjusted
*/
bool AdjustCamera(PhotonCameraSim* cameraSim,
const frc::Transform3d& robotToCamera) {
const wpi::math::Transform3d& robotToCamera) {
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
camTrfMap.at(cameraSim).AddSample(frc::Timer::GetFPGATimestamp(),
frc::Pose3d{} + robotToCamera);
camTrfMap.at(cameraSim).AddSample(wpi::Timer::GetFPGATimestamp(),
wpi::math::Pose3d{} + robotToCamera);
return true;
} else {
return false;
@@ -228,11 +230,12 @@ class VisionSystemSim {
* @return If the cameraSim was valid and transforms were reset
*/
bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
units::second_t now = frc::Timer::GetFPGATimestamp();
wpi::units::second_t now = wpi::Timer::GetFPGATimestamp();
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
auto trfBuffer = camTrfMap.at(cameraSim);
frc::Transform3d lastTrf{frc::Pose3d{},
trfBuffer.Sample(now).value_or(frc::Pose3d{})};
wpi::math::Transform3d lastTrf{
wpi::math::Pose3d{},
trfBuffer.Sample(now).value_or(wpi::math::Pose3d{})};
trfBuffer.Clear();
AdjustCamera(cameraSim, lastTrf);
return true;
@@ -312,9 +315,9 @@ class VisionSystemSim {
*
* @param layout The field tag layout to get Apriltag poses and IDs from
*/
void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
void AddAprilTags(const wpi::apriltag::AprilTagFieldLayout& layout) {
std::vector<VisionTargetSim> targets;
for (const frc::AprilTag& tag : layout.GetTags()) {
for (const wpi::apriltag::AprilTag& tag : layout.GetTags()) {
targets.emplace_back(VisionTargetSim{layout.GetTagPose(tag.ID).value(),
photon::kAprilTag36h11, tag.ID});
}
@@ -365,8 +368,8 @@ class VisionSystemSim {
*
* @return The latest robot pose
*/
frc::Pose3d GetRobotPose() {
return GetRobotPose(frc::Timer::GetFPGATimestamp());
wpi::math::Pose3d GetRobotPose() {
return GetRobotPose(wpi::Timer::GetFPGATimestamp());
}
/**
@@ -375,8 +378,8 @@ class VisionSystemSim {
* @param timestamp Timestamp of the desired robot pose
* @return The robot pose
*/
frc::Pose3d GetRobotPose(units::second_t timestamp) {
return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
wpi::math::Pose3d GetRobotPose(wpi::units::second_t timestamp) {
return robotPoseBuffer.Sample(timestamp).value_or(wpi::math::Pose3d{});
}
/**
@@ -384,8 +387,8 @@ class VisionSystemSim {
*
* @param robotPose The robot pose
*/
void ResetRobotPose(const frc::Pose2d& robotPose) {
ResetRobotPose(frc::Pose3d{robotPose});
void ResetRobotPose(const wpi::math::Pose2d& robotPose) {
ResetRobotPose(wpi::math::Pose3d{robotPose});
}
/**
@@ -393,11 +396,11 @@ class VisionSystemSim {
*
* @param robotPose The robot pose
*/
void ResetRobotPose(const frc::Pose3d& robotPose) {
void ResetRobotPose(const wpi::math::Pose3d& robotPose) {
robotPoseBuffer.Clear();
robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
robotPoseBuffer.AddSample(wpi::Timer::GetFPGATimestamp(), robotPose);
}
frc::Field2d& GetDebugField() { return dbgField; }
wpi::Field2d& GetDebugField() { return dbgField; }
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is
@@ -405,7 +408,9 @@ class VisionSystemSim {
*
* @param robotPoseMeters The simulated robot pose in meters
*/
void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); }
void Update(const wpi::math::Pose2d& robotPose) {
Update(wpi::math::Pose3d{robotPose});
}
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is
@@ -413,16 +418,16 @@ class VisionSystemSim {
*
* @param robotPoseMeters The simulated robot pose in meters
*/
void Update(const frc::Pose3d& robotPose) {
void Update(const wpi::math::Pose3d& robotPose) {
for (auto& set : targetSets) {
std::vector<frc::Pose2d> posesToAdd{};
std::vector<wpi::math::Pose2d> posesToAdd{};
for (auto& target : set.second) {
posesToAdd.emplace_back(target.GetPose().ToPose2d());
}
dbgField.GetObject(set.first)->SetPoses(posesToAdd);
}
units::second_t now = frc::Timer::GetFPGATimestamp();
wpi::units::second_t now = wpi::Timer::GetFPGATimestamp();
robotPoseBuffer.AddSample(now, robotPose);
dbgField.SetRobotPose(robotPose.ToPose2d());
@@ -433,8 +438,8 @@ class VisionSystemSim {
}
}
std::vector<frc::Pose2d> visTgtPoses2d{};
std::vector<frc::Pose2d> cameraPoses2d{};
std::vector<wpi::math::Pose2d> visTgtPoses2d{};
std::vector<wpi::math::Pose2d> cameraPoses2d{};
bool processed{false};
for (const auto& entry : camSimMap) {
auto camSim = entry.second;
@@ -445,12 +450,12 @@ class VisionSystemSim {
processed = true;
}
uint64_t timestampNt = optTimestamp.value();
units::second_t latency = camSim->prop.EstLatency();
units::second_t timestampCapture =
units::microsecond_t{static_cast<double>(timestampNt)} - latency;
wpi::units::second_t latency = camSim->prop.EstLatency();
wpi::units::second_t timestampCapture =
wpi::units::microsecond_t{static_cast<double>(timestampNt)} - latency;
frc::Pose3d lateRobotPose = GetRobotPose(timestampCapture);
frc::Pose3d lateCameraPose =
wpi::math::Pose3d lateRobotPose = GetRobotPose(timestampCapture);
wpi::math::Pose3d lateCameraPose =
lateRobotPose + GetRobotToCamera(camSim, timestampCapture).value();
cameraPoses2d.push_back(lateCameraPose.ToPose2d());
@@ -474,13 +479,14 @@ class VisionSystemSim {
private:
std::unordered_map<std::string, PhotonCameraSim*> camSimMap{};
static constexpr units::second_t bufferLength{1.5_s};
static constexpr wpi::units::second_t bufferLength{1.5_s};
std::unordered_map<PhotonCameraSim*,
frc::TimeInterpolatableBuffer<frc::Pose3d>>
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d>>
camTrfMap;
frc::TimeInterpolatableBuffer<frc::Pose3d> robotPoseBuffer{bufferLength};
wpi::math::TimeInterpolatableBuffer<wpi::math::Pose3d> robotPoseBuffer{
bufferLength};
std::unordered_map<std::string, std::vector<VisionTargetSim>> targetSets{};
frc::Field2d dbgField{};
const frc::Transform3d kEmptyTrf{};
wpi::Field2d dbgField{};
const wpi::math::Transform3d kEmptyTrf{};
};
} // namespace photon

41
photon-lib/src/main/native/include/photon/simulation/VisionTargetSim.h
View File

@@ -26,7 +26,7 @@
#include <vector>
#include <frc/geometry/Pose3d.h>
#include <wpi/math/geometry/Pose3d.hpp>
#include "photon/estimation/TargetModel.h"
@@ -42,7 +42,7 @@ class VisionTargetSim {
* @param pose Pose3d of the tag in field-relative coordinates
* @param model TargetModel which describes the geometry of the target
*/
VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model)
VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model)
: fiducialId(-1),
objDetClassId(-1),
objDetConf(-1),
@@ -57,7 +57,8 @@ class VisionTargetSim {
* @param model TargetModel which describes the geometry of the target(tag)
* @param id The ID of this fiducial tag
*/
VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model, int id)
VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model,
int id)
: fiducialId(id),
objDetClassId(-1),
objDetConf(-1),
@@ -79,7 +80,7 @@ class VisionTargetSim {
* simulation will compute a confidence based on the area of the target in the
* camera's field of view
*/
VisionTargetSim(const frc::Pose3d& pose, const TargetModel& model,
VisionTargetSim(const wpi::math::Pose3d& pose, const TargetModel& model,
int objDetClassId, float objDetConf)
: fiducialId(-1),
objDetClassId(objDetClassId),
@@ -92,7 +93,7 @@ class VisionTargetSim {
*
* @param newPose The pose in field-relative coordinates
*/
void SetPose(const frc::Pose3d& newPose) { pose = newPose; }
void SetPose(const wpi::math::Pose3d& newPose) { pose = newPose; }
/**
* Sets the model describing this target's geometry.
@@ -106,7 +107,7 @@ class VisionTargetSim {
*
* @return The pose in field-relative coordinates
*/
frc::Pose3d GetPose() const { return pose; }
wpi::math::Pose3d GetPose() const { return pose; }
/**
* Returns the model describing this target's geometry.
@@ -142,7 +143,7 @@ class VisionTargetSim {
* This target's vertices offset from its field pose.
* @return A vector of Translation3d representing the vertices of the target
*/
std::vector<frc::Translation3d> GetFieldVertices() const {
std::vector<wpi::math::Translation3d> GetFieldVertices() const {
return model.GetFieldVertices(pose);
}
@@ -151,18 +152,18 @@ class VisionTargetSim {
}
bool operator==(const VisionTargetSim& other) const {
return units::math::abs(pose.Translation().X() -
other.GetPose().Translation().X()) < 1_in &&
units::math::abs(pose.Translation().Y() -
other.GetPose().Translation().Y()) < 1_in &&
units::math::abs(pose.Translation().Z() -
other.GetPose().Translation().Z()) < 1_in &&
units::math::abs(pose.Rotation().X() -
other.GetPose().Rotation().X()) < 1_deg &&
units::math::abs(pose.Rotation().Y() -
other.GetPose().Rotation().Y()) < 1_deg &&
units::math::abs(pose.Rotation().Z() -
other.GetPose().Rotation().Z()) < 1_deg &&
return wpi::units::math::abs(pose.Translation().X() -
other.GetPose().Translation().X()) < 1_in &&
wpi::units::math::abs(pose.Translation().Y() -
other.GetPose().Translation().Y()) < 1_in &&
wpi::units::math::abs(pose.Translation().Z() -
other.GetPose().Translation().Z()) < 1_in &&
wpi::units::math::abs(pose.Rotation().X() -
other.GetPose().Rotation().X()) < 1_deg &&
wpi::units::math::abs(pose.Rotation().Y() -
other.GetPose().Rotation().Y()) < 1_deg &&
wpi::units::math::abs(pose.Rotation().Z() -
other.GetPose().Rotation().Z()) < 1_deg &&
model.GetIsPlanar() == other.GetModel().GetIsPlanar();
}
@@ -170,7 +171,7 @@ class VisionTargetSim {
int fiducialId;
int objDetClassId;
float objDetConf;
frc::Pose3d pose;
wpi::math::Pose3d pose;
TargetModel model;
};
} // namespace photon

34
photon-lib/src/test/java/org/photonvision/LegacyPhotonPoseEstimatorTest.java
View File

@@ -32,23 +32,6 @@ import static org.junit.jupiter.api.Assertions.assertNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
@@ -70,6 +53,23 @@ import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Quaternion;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Units;
import org.wpilib.util.runtime.RuntimeLoader;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
class LegacyPhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;

14
photon-lib/src/test/java/org/photonvision/OpenCVTest.java
View File

@@ -26,13 +26,6 @@ package org.photonvision;
import static org.junit.jupiter.api.Assertions.*;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.io.IOException;
import java.util.List;
import org.junit.jupiter.api.BeforeAll;
@@ -45,6 +38,13 @@ import org.photonvision.estimation.TargetModel;
import org.photonvision.jni.CombinedRuntimeLoader;
import org.photonvision.simulation.SimCameraProperties;
import org.photonvision.simulation.VisionTargetSim;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.util.MathUtil;
import org.wpilib.networktables.NetworkTableInstance;
public class OpenCVTest {
private static final double kTrlDelta = 0.005;

20
photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View File

@@ -31,15 +31,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.photonvision.UnitTestUtils.waitForCondition;
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.NetworkTablesJNI;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.simulation.SimHooks;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.io.IOException;
import java.util.Arrays;
import java.util.List;
@@ -61,6 +52,15 @@ import org.photonvision.jni.TimeSyncClient;
import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.targeting.PhotonPipelineMetadata;
import org.photonvision.targeting.PhotonPipelineResult;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.networktables.NetworkTablesJNI;
import org.wpilib.simulation.SimHooks;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.system.DataLogManager;
import org.wpilib.system.Timer;
import org.wpilib.util.runtime.RuntimeLoader;
@TestMethodOrder(MethodOrderer.OrderAnnotation.class)
class PhotonCameraTest {
@@ -260,7 +260,7 @@ class PhotonCameraTest {
}
if (i == robotStart || i == robotRestart) {
robotNt.startServer("networktables_random.json", "", 5940);
robotNt.startServer("networktables_random.json", "", "", 5940);
}
Thread.sleep(100);

32
photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View File

@@ -29,21 +29,6 @@ import static org.junit.jupiter.api.Assertions.assertNotNull;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.Assertions.fail;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
@@ -63,6 +48,23 @@ import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.photonvision.targeting.PnpResult;
import org.photonvision.targeting.TargetCorner;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Quaternion;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.linalg.MatBuilder;
import org.wpilib.math.linalg.VecBuilder;
import org.wpilib.math.util.Nat;
import org.wpilib.math.util.Units;
import org.wpilib.util.runtime.RuntimeLoader;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.apriltag.AprilTagFields;
class PhotonPoseEstimatorTest {
static AprilTagFieldLayout aprilTags;

4
photon-lib/src/test/java/org/photonvision/PhotonUtilTest.java
View File

@@ -26,9 +26,9 @@ package org.photonvision;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.*;
import edu.wpi.first.math.util.Units;
import org.junit.jupiter.api.Test;
import org.wpilib.math.geometry.*;
import org.wpilib.math.util.Units;
class PhotonUtilTest {
@Test

30
photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
View File

@@ -30,21 +30,6 @@ import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.photonvision.UnitTestUtils.waitForSequenceNumber;
import edu.wpi.first.apriltag.AprilTag;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.cscore.OpenCvLoader;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.io.IOException;
import java.util.ArrayList;
import java.util.List;
@@ -64,6 +49,21 @@ import org.photonvision.simulation.PhotonCameraSim;
import org.photonvision.simulation.VisionSystemSim;
import org.photonvision.simulation.VisionTargetSim;
import org.photonvision.targeting.PhotonTrackedTarget;
import org.wpilib.hardware.hal.HAL;
import org.wpilib.math.geometry.Pose2d;
import org.wpilib.math.geometry.Pose3d;
import org.wpilib.math.geometry.Rotation2d;
import org.wpilib.math.geometry.Rotation3d;
import org.wpilib.math.geometry.Transform3d;
import org.wpilib.math.geometry.Translation2d;
import org.wpilib.math.geometry.Translation3d;
import org.wpilib.math.util.Units;
import org.wpilib.networktables.NetworkTableInstance;
import org.wpilib.smartdashboard.SmartDashboard;
import org.wpilib.util.runtime.RuntimeLoader;
import org.wpilib.vision.apriltag.AprilTag;
import org.wpilib.vision.apriltag.AprilTagFieldLayout;
import org.wpilib.vision.camera.OpenCvLoader;
class VisionSystemSimTest {
private static final double kRotDeltaDeg = 0.25;

272
photon-lib/src/test/native/cpp/LegacyPhotonPoseEstimatorTest.cpp
View File

@@ -48,13 +48,13 @@
#include "photon/targeting/PnpResult.h"
WPI_IGNORE_DEPRECATED
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 std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
wpi::math::Rotation3d())}};
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
@@ -69,24 +69,24 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -95,14 +95,14 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
@@ -112,15 +112,15 @@ TEST(LegacyPhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
}
TEST(LegacyPhotonPoseEstimatorTest, 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())},
std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
wpi::math::Rotation3d())},
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
std::vector<std::pair<photon::PhotonCamera, wpi::math::Transform3d>> cameras;
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
@@ -130,24 +130,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -164,7 +164,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.02);
@@ -179,24 +179,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -207,7 +207,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
photon::PhotonPoseEstimator estimator(aprilTags,
photon::CLOSEST_TO_REFERENCE_POSE, {});
estimator.SetReferencePose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -215,7 +215,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
@@ -230,24 +230,24 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -258,7 +258,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -266,29 +266,29 @@ TEST(LegacyPhotonPoseEstimatorTest, ClosestToLastPose) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
@@ -325,15 +325,15 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d(
-12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::PNP_DISTANCE_TRIG_SOLVE, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
wpi::math::Pose3d realPose =
wpi::math::Pose3d(7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
@@ -349,7 +349,7 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
@@ -359,12 +359,12 @@ TEST(LegacyPhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
frc::Transform3d straightOnTestTransform =
frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
wpi::math::Transform3d straightOnTestTransform =
wpi::math::Transform3d(0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m,
wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
@@ -395,24 +395,24 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -429,7 +429,7 @@ TEST(LegacyPhotonPoseEstimatorTest, AverageBestPoses) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
.01);
@@ -444,24 +444,24 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -520,8 +520,8 @@ TEST(LegacyPhotonPoseEstimatorTest, PoseCache) {
EXPECT_FALSE(estimatedPose);
// Setting ReferencePose should also clear the cache
estimator.SetReferencePose(frc::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), frc::Rotation3d()));
estimator.SetReferencePose(wpi::math::Pose3d(units::meter_t(1), units::meter_t(2),
units::meter_t(3), wpi::math::Rotation3d()));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
@@ -538,17 +538,17 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
@@ -557,14 +557,14 @@ TEST(LegacyPhotonPoseEstimatorTest, MultiTagOnRioFallback) {
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
frc::Transform3d{});
wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -589,8 +589,8 @@ TEST(LegacyPhotonPoseEstimatorTest, CopyResult) {
TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, frc::Transform3d());
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
@@ -611,10 +611,10 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
frc::Transform3d poseTransform(
frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
wpi::math::Transform3d poseTransform(
wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
wpi::math::Rotation3d(wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
@@ -635,15 +635,15 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
const units::radian_t camPitch = 30_deg;
const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
frc::Rotation3d(0_rad, -camPitch, 0_rad)};
const wpi::math::Transform3d kRobotToCam{wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m),
wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
wpi::apriltag::AprilTagFieldLayout::LoadField(wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, kRobotToCam);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
wpi::math::Rotation2d());
auto estimatedPose =
estimator.Update(cameraOne.testResult[0], cameraMat, distortion,
@@ -651,7 +651,7 @@ TEST(LegacyPhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
ASSERT_TRUE(estimatedPose.has_value());
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);

10
photon-lib/src/test/native/cpp/PhotonCameraTest.cpp
View File

@@ -26,14 +26,14 @@
#include <vector>
#include <fmt/ranges.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include <net/TimeSyncClient.h>
#include <net/TimeSyncServer.h>
#include <networktables/NetworkTableInstance.h>
#include <photon/PhotonCamera.h>
#include <photon/simulation/PhotonCameraSim.h>
#include <wpi/hal/HAL.h>
#include <wpi/nt/NetworkTableInstance.hpp>
#include <wpi/smartdashboard/SmartDashboard.hpp>
TEST(TimeSyncProtocolTest, Smoketest) {
using namespace wpi::tsp;
@@ -60,10 +60,10 @@ TEST(TimeSyncProtocolTest, Smoketest) {
}
TEST(PhotonCameraTest, Alerts) {
using frc::SmartDashboard;
using wpi::SmartDashboard;
// GIVEN a local-only NT instance
auto inst = nt::NetworkTableInstance::GetDefault();
auto inst = wpi::nt::NetworkTableInstance::GetDefault();
inst.StopClient();
inst.StopServer();
inst.StartLocal();

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

@@ -27,14 +27,14 @@
#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 <gtest/gtest.h>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/SmallVector.h>
#include <wpi/apriltag/AprilTagFieldLayout.hpp>
#include <wpi/math/geometry/Pose3d.hpp>
#include <wpi/math/geometry/Rotation3d.hpp>
#include <wpi/math/geometry/Transform3d.hpp>
#include <wpi/units/angle.hpp>
#include <wpi/units/length.hpp>
#include <wpi/util/SmallVector.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
@@ -46,13 +46,13 @@
#include "photon/targeting/PhotonTrackedTarget.h"
#include "photon/targeting/PnpResult.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 std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3),
wpi::units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(wpi::units::meter_t(5), wpi::units::meter_t(5),
wpi::units::meter_t(5), wpi::math::Rotation3d())}};
static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static wpi::apriltag::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
static std::vector<photon::TargetCorner> corners{
photon::TargetCorner{1., 2.}, photon::TargetCorner{3., 4.},
@@ -67,57 +67,57 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::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);
EXPECT_NEAR(
11, wpi::units::unit_cast<double>(estimatedPose.value().timestamp), .02);
EXPECT_NEAR(1, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, wpi::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())},
std::vector<wpi::apriltag::AprilTag> tags = {
{0, wpi::math::Pose3d(wpi::units::meter_t(3), wpi::units::meter_t(3),
wpi::units::meter_t(3), wpi::math::Rotation3d())},
{1, wpi::math::Pose3d(wpi::units::meter_t(5), wpi::units::meter_t(5),
wpi::units::meter_t(5), wpi::math::Rotation3d())},
};
auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
auto aprilTags = wpi::apriltag::AprilTagFieldLayout(tags, 54_ft, 27_ft);
std::vector<std::pair<photon::PhotonCamera, frc::Transform3d>> cameras;
std::vector<std::pair<photon::PhotonCamera, wpi::math::Transform3d>> cameras;
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
@@ -127,24 +127,24 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 4_m, 4_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(5_m, 5_m, 5_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
@@ -160,13 +160,13 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::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);
EXPECT_NEAR(
17, wpi::units::unit_cast<double>(estimatedPose.value().timestamp), .02);
EXPECT_NEAR(4, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(4, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(0, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
@@ -175,30 +175,30 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, {});
@@ -206,17 +206,17 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
wpi::math::Pose3d(1_m, 1_m, 1_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::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);
EXPECT_NEAR(
17, wpi::units::unit_cast<double>(estimatedPose.value().timestamp), .01);
EXPECT_NEAR(1, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(.9, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
@@ -225,70 +225,70 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.2_m, 2.2_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(17));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, {});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateClosestToReferencePose(
result,
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
result, wpi::math::Pose3d(1_m, 1_m, 1_m,
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)));
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
std::vector<photon::PhotonTrackedTarget> targetsThree{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.1_m, 1.9_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2.4_m, 2.4_m, 2.2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targetsThree,
std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(21));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(21));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.EstimateClosestToReferencePose(result, pose);
@@ -297,11 +297,12 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(21.0, units::unit_cast<double>(estimatedPose.value().timestamp),
EXPECT_NEAR(21.0,
wpi::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);
EXPECT_NEAR(.9, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(1.1, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(1, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
@@ -318,14 +319,14 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
&cameraOne, photon::SimCameraProperties::PERFECT_90DEG());
/* Compound Rolled + Pitched + Yaw */
frc::Transform3d compoundTestTransform = frc::Transform3d(
-12_in, -11_in, 3_m, frc::Rotation3d(37_deg, 6_deg, 60_deg));
wpi::math::Transform3d compoundTestTransform = wpi::math::Transform3d(
-12_in, -11_in, 3_m, wpi::math::Rotation3d(37_deg, 6_deg, 60_deg));
photon::PhotonPoseEstimator estimator(aprilTags, compoundTestTransform);
/* real pose of the robot base to test against */
frc::Pose3d realPose =
frc::Pose3d(7.3_m, 4.42_m, 0_m, frc::Rotation3d(0_rad, 0_rad, 2.197_rad));
wpi::math::Pose3d realPose = wpi::math::Pose3d(
7.3_m, 4.42_m, 0_m, wpi::math::Rotation3d(0_rad, 0_rad, 2.197_rad));
photon::PhotonPipelineResult result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
@@ -341,22 +342,22 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.X()),
wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Y()),
wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Z()),
wpi::units::unit_cast<double>(pose.Z()), .01);
/* Straight on */
frc::Transform3d straightOnTestTransform =
frc::Transform3d(0_m, 0_m, 3_m, frc::Rotation3d(0_rad, 0_rad, 0_rad));
wpi::math::Transform3d straightOnTestTransform = wpi::math::Transform3d(
0_m, 0_m, 3_m, wpi::math::Rotation3d(0_rad, 0_rad, 0_rad));
estimator.SetRobotToCameraTransform(straightOnTestTransform);
realPose = frc::Pose3d(4.81_m, 2.38_m, 0_m,
frc::Rotation3d(0_rad, 0_rad, 2.818_rad));
realPose = wpi::math::Pose3d(4.81_m, 2.38_m, 0_m,
wpi::math::Rotation3d(0_rad, 0_rad, 2.818_rad));
result = cameraOneSim.Process(
1_ms, realPose.TransformBy(estimator.GetRobotToCameraTransform()),
targets);
@@ -373,12 +374,12 @@ TEST(PhotonPoseEstimatorTest, PnpDistanceTrigSolve) {
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(units::unit_cast<double>(realPose.X()),
units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Y()),
units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(units::unit_cast<double>(realPose.Z()),
units::unit_cast<double>(pose.Z()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.X()),
wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Y()),
wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(wpi::units::unit_cast<double>(realPose.Z()),
wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
@@ -387,30 +388,30 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 2_m, 2_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 1_m, 1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(3_m, 3_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners},
photon::PhotonTrackedTarget{
9.0, -2.0, 19.0, 3.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(0_m, 0_m, 0_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(2_m, 1.9_m, 2.1_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, {});
@@ -420,13 +421,14 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
EXPECT_NEAR(15.0,
wpi::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);
EXPECT_NEAR(2.15, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(2.15, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2.15, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
@@ -435,25 +437,25 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{
3.0, -4.0, 9.0, 4.0, 0, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(1_m, 2_m, 3_m),
wpi::math::Rotation3d(1_rad, 2_rad, 3_rad)),
0.7, corners, detectedCorners},
photon::PhotonTrackedTarget{
3.0, -4.0, 9.1, 6.7, 1, -1, -1.f,
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)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
wpi::math::Transform3d(wpi::math::Translation3d(4_m, 2_m, 3_m),
wpi::math::Rotation3d(0_rad, 0_rad, 0_rad)),
0.3, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt}};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(11));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, frc::Transform3d{});
photon::PhotonPoseEstimator estimator(aprilTags, wpi::math::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
@@ -464,14 +466,14 @@ TEST(PhotonPoseEstimatorTest, MultiTagOnCoprocFallback) {
estimatedPose = estimator.EstimateLowestAmbiguityPose(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
// Make sure values match what we'd expect for the LOWEST_AMBIGUITY strategy
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);
EXPECT_NEAR(
11, wpi::units::unit_cast<double>(estimatedPose.value().timestamp), .02);
EXPECT_NEAR(1, wpi::units::unit_cast<double>(pose.X()), .01);
EXPECT_NEAR(3, wpi::units::unit_cast<double>(pose.Y()), .01);
EXPECT_NEAR(2, wpi::units::unit_cast<double>(pose.Z()), .01);
}
TEST(PhotonPoseEstimatorTest, CopyResult) {
@@ -479,7 +481,7 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
auto testResult = photon::PhotonPipelineResult{
photon::PhotonPipelineMetadata{0, 0, 2000, 1000}, targets, std::nullopt};
testResult.SetReceiveTimestamp(units::second_t(11));
testResult.SetReceiveTimestamp(wpi::units::second_t(11));
auto test2 = testResult;
@@ -487,6 +489,17 @@ TEST(PhotonPoseEstimatorTest, CopyResult) {
test2.GetTimestamp().to<double>(), 0.001);
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpEmptyCase) {
photon::PhotonPoseEstimator estimator(
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
photon::CONSTRAINED_SOLVEPNP, wpi::math::Transform3d());
photon::PhotonPipelineResult result;
auto estimate = estimator.Update(result);
EXPECT_FALSE(estimate.has_value());
}
TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::PhotonCamera cameraOne = photon::PhotonCamera("test");
auto distortion = Eigen::VectorXd::Zero(8);
@@ -501,12 +514,12 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
photon::TargetCorner{127.17118732489361, 313.81406314178633},
photon::TargetCorner{104.28543773760417, 309.6516557438994}};
frc::Transform3d poseTransform(
frc::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
frc::Rotation3d(frc::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695,
0.9130568172784148)));
wpi::math::Transform3d poseTransform(
wpi::math::Translation3d(3.1665557336121353_m, 4.430673446050584_m,
0.48678786477534686_m),
wpi::math::Rotation3d(
wpi::math::Quaternion(0.3132532247418243, 0.24722671090692333,
-0.08413452932300695, 0.9130568172784148)));
std::vector<photon::PhotonTrackedTarget> targets{
photon::PhotonTrackedTarget{0.0, 0.0, 0.0, 0.0, 8, 0, 0.0f, poseTransform,
@@ -522,21 +535,23 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
cameraOne.test = true;
cameraOne.testResult = {result};
cameraOne.testResult[0].SetReceiveTimestamp(units::second_t(15));
cameraOne.testResult[0].SetReceiveTimestamp(wpi::units::second_t(15));
const units::radian_t camPitch = 30_deg;
const frc::Transform3d kRobotToCam{frc::Translation3d(0.5_m, 0.0_m, 0.5_m),
frc::Rotation3d(0_rad, -camPitch, 0_rad)};
const wpi::units::radian_t camPitch = 30_deg;
const wpi::math::Transform3d kRobotToCam{
wpi::math::Translation3d(0.5_m, 0.0_m, 0.5_m),
wpi::math::Rotation3d(0_rad, -camPitch, 0_rad)};
photon::PhotonPoseEstimator estimator(
frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo),
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
kRobotToCam);
auto estimatedMultiTagPose =
estimator.EstimateCoprocMultiTagPose(cameraOne.testResult[0]);
estimator.AddHeadingData(cameraOne.testResult[0].GetTimestamp(),
frc::Rotation2d());
wpi::math::Rotation2d());
auto estimatedPose = estimator.EstimateConstrainedSolvepnpPose(
cameraOne.testResult[0], cameraMat, distortion,
@@ -544,11 +559,11 @@ TEST(PhotonPoseEstimatorTest, ConstrainedPnpOneTag) {
ASSERT_TRUE(estimatedPose.has_value());
frc::Pose3d pose = estimatedPose.value().estimatedPose;
wpi::math::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(3.58, units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_NEAR(3.58, wpi::units::unit_cast<double>(pose.X()), 0.01);
EXPECT_NEAR(4.13, wpi::units::unit_cast<double>(pose.Y()), 0.01);
EXPECT_NEAR(0.0, wpi::units::unit_cast<double>(pose.Z()), 0.01);
EXPECT_EQ(photon::CONSTRAINED_SOLVEPNP, estimatedPose.value().strategy);
}

380
photon-lib/src/test/native/cpp/VisionSystemSimTest.cpp
View File

@@ -28,7 +28,7 @@
#include <vector>
#include <gtest/gtest.h>
#include <wpi/deprecated.h>
#include <wpi/util/deprecated.hpp>
#include "photon/PhotonUtils.h"
#include "photon/estimation/VisionEstimation.h"
@@ -38,7 +38,7 @@ WPI_IGNORE_DEPRECATED
class VisionSystemSimTest : public ::testing::Test {
void SetUp() override {
nt::NetworkTableInstance::GetDefault().StartServer();
wpi::nt::NetworkTableInstance::GetDefault().StartServer();
photon::PhotonCamera::SetVersionCheckEnabled(false);
}
@@ -47,71 +47,73 @@ class VisionSystemSimTest : public ::testing::Test {
class VisionSystemSimTestWithParamsTest
: public VisionSystemSimTest,
public testing::WithParamInterface<units::degree_t> {};
public testing::WithParamInterface<wpi::units::degree_t> {};
class VisionSystemSimTestDistanceParamsTest
: public VisionSystemSimTest,
public testing::WithParamInterface<
std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
std::tuple<wpi::units::foot_t, wpi::units::degree_t, wpi::units::foot_t>> {};
TEST_F(VisionSystemSimTest, TestVisibilityCupidShuffle) {
frc::Pose3d targetPose{
frc::Translation3d{15.98_m, 0_m, 2_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{1.0_m, 1.0_m}, 3}});
// To the right, to the right
frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-70_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{-70_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
// To the right, to the right
robotPose =
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{-95_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{-95_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
// To the left, to the left
robotPose =
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{90_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{90_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
// To the left, to the left
robotPose =
frc::Pose2d{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{65_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{65_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
// Now kick, now kick
robotPose = frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{5_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
// Now kick, now kick
robotPose =
frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-5_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
wpi::math::Rotation2d{-5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
// Now walk it by yourself
robotPose =
frc::Pose2d{frc::Translation2d{2_m, 0_m}, frc::Rotation2d{-179_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{2_m, 0_m},
wpi::math::Rotation2d{-179_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
// Now walk it by yourself
visionSysSim.AdjustCamera(
&cameraSim,
frc::Transform3d{
frc::Translation3d{},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
&cameraSim, wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
}
@@ -140,121 +142,129 @@ TEST_F(VisionSystemSimTest, TestBunchaTargets) {
}
TEST_F(VisionSystemSimTest, TestNotVisibleVert1) {
frc::Pose3d targetPose{
frc::Translation3d{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{3.0_m, 3.0_m}, 3}});
frc::Pose2d robotPose{frc::Translation2d{5_m, 0_m}, frc::Rotation2d{5_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
visionSysSim.AdjustCamera(
&cameraSim,
frc::Transform3d{
frc::Translation3d{0_m, 0_m, 5000_m},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi}}});
&cameraSim, wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 5000_m},
wpi::math::Rotation3d{
0_deg, 0_deg, wpi::units::radian_t{std::numbers::pi}}});
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
}
TEST_F(VisionSystemSimTest, TestNotVisibleVert2) {
frc::Pose3d targetPose{
frc::Translation3d{15.98_m, 0_m, 2_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 2_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
frc::Transform3d robotToCamera{
frc::Translation3d{0_m, 0_m, 1_m},
frc::Rotation3d{0_rad, units::radian_t{-std::numbers::pi / 4}, 0_rad}};
wpi::math::Transform3d robotToCamera{
wpi::math::Translation3d{0_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, wpi::units::radian_t{-std::numbers::pi / 4},
0_rad}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, robotToCamera);
cameraSim.prop.SetCalibration(1234, 1234, frc::Rotation2d{80_deg});
cameraSim.prop.SetCalibration(1234, 1234, wpi::math::Rotation2d{80_deg});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 1736}});
frc::Pose2d robotPose{frc::Translation2d{13.98_m, 0_m},
frc::Rotation2d{5_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{13.98_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
}
TEST_F(VisionSystemSimTest, TestNotVisibleTargetSize) {
frc::Pose3d targetPose{
frc::Translation3d{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{0.1_m, 0.1_m}, 24}});
frc::Pose2d robotPose{frc::Translation2d{12_m, 0_m}, frc::Rotation2d{5_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{12_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
}
TEST_F(VisionSystemSimTest, TestNotVisibleTooFarLeds) {
frc::Pose3d targetPose{
frc::Translation3d{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{15.98_m, 0_m, 1_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(1.0);
cameraSim.SetMaxSightRange(10_m);
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{targetPose, photon::TargetModel{1_m, 1_m}, 25}});
frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{5_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{0_m, 0_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.Update(robotPose);
ASSERT_FALSE(camera.GetLatestResult().HasTargets());
}
TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
const frc::Pose3d targetPose{
const wpi::math::Pose3d targetPose{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
wpi::math::Rotation3d{0_deg, 0_deg,
wpi::units::radian_t{3 * std::numbers::pi / 4}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(0.0);
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
// If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+)
frc::Pose2d robotPose{frc::Translation2d{10_m, 0_m},
frc::Rotation2d{GetParam()}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{10_m, 0_m},
wpi::math::Rotation2d{GetParam()}};
visionSysSim.Update(robotPose);
const auto result = camera.GetLatestResult();
@@ -263,26 +273,28 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
}
TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
const frc::Pose3d targetPose{
const wpi::math::Pose3d targetPose{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{3 * std::numbers::pi / 4}}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
wpi::math::Rotation3d{0_deg, 0_deg,
wpi::units::radian_t{3 * std::numbers::pi / 4}}};
wpi::math::Pose2d robotPose{{10_m, 0_m},
wpi::math::Rotation2d{GetParam() * -1.0}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{120_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{120_deg});
cameraSim.SetMinTargetAreaPixels(0.0);
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPose, photon::TargetModel{0.5_m, 0.5_m}, 3}});
robotPose = frc::Pose2d{frc::Translation2d{10_m, 0_m},
frc::Rotation2d{-1 * GetParam()}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{10_m, 0_m},
wpi::math::Rotation2d{-1 * GetParam()}};
visionSysSim.AdjustCamera(
&cameraSim,
frc::Transform3d{
frc::Translation3d{},
frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}});
wpi::math::Transform3d{
wpi::math::Translation3d{},
wpi::math::Rotation3d{0_rad, wpi::units::degree_t{GetParam()}, 0_rad}});
visionSysSim.Update(robotPose);
const auto result = camera.GetLatestResult();
@@ -295,24 +307,27 @@ INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest,
-2_deg, 5_deg, 7_deg, 10.23_deg));
TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) {
units::foot_t distParam;
units::degree_t pitchParam;
units::foot_t heightParam;
wpi::units::foot_t distParam;
wpi::units::degree_t pitchParam;
wpi::units::foot_t heightParam;
std::tie(distParam, pitchParam, heightParam) = GetParam();
const frc::Pose3d targetPose{
const wpi::math::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::radian_t{std::numbers::pi * 0.98}}};
frc::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m}, frc::Rotation3d{}};
frc::Transform3d robotToCamera{frc::Translation3d{0_m, 0_m, heightParam},
frc::Rotation3d{0_deg, pitchParam, 0_deg}};
wpi::math::Rotation3d{0_deg, 0_deg,
wpi::units::radian_t{std::numbers::pi * 0.98}}};
wpi::math::Pose3d robotPose{{15.98_m - distParam, 0_m, 0_m},
wpi::math::Rotation3d{}};
wpi::math::Transform3d robotToCamera{
wpi::math::Translation3d{0_m, 0_m, heightParam},
wpi::math::Rotation3d{0_deg, pitchParam, 0_deg}};
photon::VisionSystemSim visionSysSim{
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
"wsyourdaygoingihopegoodhaveagreatrestofyourlife"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{160_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{160_deg});
cameraSim.SetMinTargetAreaPixels(0.0);
visionSysSim.AdjustCamera(&cameraSim, robotToCamera);
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
@@ -325,11 +340,11 @@ TEST_P(VisionSystemSimTestDistanceParamsTest, DistanceCalc) {
ASSERT_NEAR(0.0, target.GetYaw(), 0.5);
units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
wpi::units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
robotToCamera.Z(), targetPose.Z(), -pitchParam,
units::degree_t{target.GetPitch()});
wpi::units::degree_t{target.GetPitch()});
ASSERT_NEAR(dist.to<double>(),
distParam.convert<units::meters>().to<double>(), 0.25);
distParam.convert<wpi::units::meters>().to<double>(), 0.25);
}
INSTANTIATE_TEST_SUITE_P(
@@ -353,69 +368,70 @@ INSTANTIATE_TEST_SUITE_P(
std::make_tuple(19.52_ft, -15.98_deg, 1.1_ft)));
TEST_F(VisionSystemSimTest, TestMultipleTargets) {
frc::Pose3d targetPoseL{
frc::Translation3d{15.98_m, 2_m, 0_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
frc::Pose3d targetPoseC{
frc::Translation3d{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
frc::Pose3d targetPoseR{
frc::Translation3d{15.98_m, -2_m, 0_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPoseL{
wpi::math::Translation3d{15.98_m, 2_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPoseC{
wpi::math::Translation3d{15.98_m, 0_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPoseR{
wpi::math::Translation3d{15.98_m, -2_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
targetPoseL.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 1}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
targetPoseC.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 2}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
targetPoseR.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 3}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
targetPoseL.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 4}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
targetPoseC.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 5}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
targetPoseR.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 1_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 6}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
targetPoseL.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 7}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
targetPoseC.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0.5_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 8}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
targetPoseL.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 9}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
targetPoseR.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0.75_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 10}});
visionSysSim.AddVisionTargets({photon::VisionTargetSim{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
targetPoseL.TransformBy(wpi::math::Transform3d{
wpi::math::Translation3d{0_m, 0_m, 0.25_m}, wpi::math::Rotation3d{}}),
photon::kAprilTag16h5, 11}});
frc::Pose2d robotPose{frc::Translation2d{6_m, 0_m}, frc::Rotation2d{.25_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{6_m, 0_m},
wpi::math::Rotation2d{.25_deg}};
visionSysSim.Update(robotPose);
photon::PhotonPipelineResult res = camera.GetLatestResult();
ASSERT_TRUE(res.HasTargets());
@@ -427,27 +443,31 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
std::vector<frc::AprilTag> tagList;
tagList.emplace_back(frc::AprilTag{
0, frc::Pose3d{12_m, 3_m, 1_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(frc::AprilTag{
1, frc::Pose3d{12_m, 1_m, -1_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(frc::AprilTag{
2, frc::Pose3d{11_m, 0_m, 2_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
units::meter_t fieldLength{54};
units::meter_t fieldWidth{27};
frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{5_deg}};
std::vector<wpi::apriltag::AprilTag> tagList;
tagList.emplace_back(wpi::apriltag::AprilTag{
0,
wpi::math::Pose3d{12_m, 3_m, 1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
1,
wpi::math::Pose3d{12_m, 1_m, -1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
2,
wpi::math::Pose3d{11_m, 0_m, 2_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
wpi::units::meter_t fieldLength{54};
wpi::units::meter_t fieldWidth{27};
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m},
wpi::math::Rotation2d{5_deg}};
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag16h5, 0}});
visionSysSim.Update(robotPose);
@@ -464,11 +484,11 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
auto results = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets, layout, photon::kAprilTag16h5);
ASSERT_TRUE(results);
frc::Pose3d pose = frc::Pose3d{} + results->best;
wpi::math::Pose3d pose = wpi::math::Pose3d{} + results->best;
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
visionSysSim.AddVisionTargets(
@@ -486,42 +506,47 @@ TEST_F(VisionSystemSimTest, TestPoseEstimation) {
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets2, layout, photon::kAprilTag16h5);
ASSERT_TRUE(results2);
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
wpi::math::Pose3d pose2 = wpi::math::Pose3d{} + results2->best;
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{5}.convert<units::radians>().to<double>(),
ASSERT_NEAR(wpi::units::degree_t{5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}
TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
frc::Transform3d robotToCamera{frc::Translation3d{6_in, 6_in, 6_in},
frc::Rotation3d{0_deg, -30_deg, 25.5_deg}};
wpi::math::Transform3d robotToCamera{
wpi::math::Translation3d{6_in, 6_in, 6_in},
wpi::math::Rotation3d{0_deg, -30_deg, 25.5_deg}};
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"cameraRotated"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, robotToCamera);
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{90_deg});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{90_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
std::vector<frc::AprilTag> tagList;
tagList.emplace_back(frc::AprilTag{
0, frc::Pose3d{12_m, 3_m, 1_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(frc::AprilTag{
1, frc::Pose3d{12_m, 1_m, -1_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(frc::AprilTag{
2, frc::Pose3d{11_m, 0_m, 2_m,
frc::Rotation3d{0_rad, 0_rad,
units::radian_t{std::numbers::pi}}}});
units::meter_t fieldLength{54};
units::meter_t fieldWidth{27};
frc::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
frc::Pose2d robotPose{frc::Translation2d{5_m, 1_m}, frc::Rotation2d{-5_deg}};
std::vector<wpi::apriltag::AprilTag> tagList;
tagList.emplace_back(wpi::apriltag::AprilTag{
0,
wpi::math::Pose3d{12_m, 3_m, 1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
1,
wpi::math::Pose3d{12_m, 1_m, -1_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
tagList.emplace_back(wpi::apriltag::AprilTag{
2,
wpi::math::Pose3d{11_m, 0_m, 2_m,
wpi::math::Rotation3d{
0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}}});
wpi::units::meter_t fieldLength{54};
wpi::units::meter_t fieldWidth{27};
wpi::apriltag::AprilTagFieldLayout layout{tagList, fieldLength, fieldWidth};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{5_m, 1_m},
wpi::math::Rotation2d{-5_deg}};
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{tagList[0].pose, photon::kAprilTag36h11, 0}});
visionSysSim.Update(robotPose);
@@ -542,12 +567,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
auto results = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets, layout, photon::kAprilTag36h11);
ASSERT_TRUE(results);
frc::Pose3d pose = frc::Pose3d{} + results->best;
wpi::math::Pose3d pose = wpi::math::Pose3d{} + results->best;
pose = pose.TransformBy(robotToCamera.Inverse());
ASSERT_NEAR(5, pose.X().to<double>(), 0.01);
ASSERT_NEAR(1, pose.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose.Rotation().Z().to<double>(), 0.01);
visionSysSim.AddVisionTargets(
@@ -565,12 +590,12 @@ TEST_F(VisionSystemSimTest, TestPoseEstimationRotated) {
auto results2 = photon::VisionEstimation::EstimateCamPosePNP(
camEigen, distEigen, targets2, layout, photon::kAprilTag36h11);
ASSERT_TRUE(results2);
frc::Pose3d pose2 = frc::Pose3d{} + results2->best;
wpi::math::Pose3d pose2 = wpi::math::Pose3d{} + results2->best;
pose2 = pose2.TransformBy(robotToCamera.Inverse());
ASSERT_NEAR(robotPose.X().to<double>(), pose2.X().to<double>(), 0.01);
ASSERT_NEAR(robotPose.Y().to<double>(), pose2.Y().to<double>(), 0.01);
ASSERT_NEAR(0, pose2.Z().to<double>(), 0.01);
ASSERT_NEAR(units::degree_t{-5}.convert<units::radians>().to<double>(),
ASSERT_NEAR(wpi::units::degree_t{-5}.convert<wpi::units::radians>().to<double>(),
pose2.Rotation().Z().to<double>(), 0.01);
}
@@ -578,24 +603,25 @@ TEST_F(VisionSystemSimTest, TestTagAmbiguity) {
photon::VisionSystemSim visionSysSim{"Test"};
photon::PhotonCamera camera{"camera"};
photon::PhotonCameraSim cameraSim{&camera};
visionSysSim.AddCamera(&cameraSim, frc::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, frc::Rotation2d{80_deg});
visionSysSim.AddCamera(&cameraSim, wpi::math::Transform3d{});
cameraSim.prop.SetCalibration(640, 480, wpi::math::Rotation2d{80_deg});
cameraSim.SetMinTargetAreaPixels(20.0);
frc::Pose3d targetPose{
frc::Translation3d{2_m, 0_m, 0_m},
frc::Rotation3d{0_rad, 0_rad, units::radian_t{std::numbers::pi}}};
wpi::math::Pose3d targetPose{
wpi::math::Translation3d{2_m, 0_m, 0_m},
wpi::math::Rotation3d{0_rad, 0_rad, wpi::units::radian_t{std::numbers::pi}}};
visionSysSim.AddVisionTargets(
{photon::VisionTargetSim{targetPose, photon::kAprilTag36h11, 3}});
frc::Pose2d robotPose{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{0_deg}};
wpi::math::Pose2d robotPose{wpi::math::Translation2d{0_m, 0_m},
wpi::math::Rotation2d{0_deg}};
visionSysSim.Update(robotPose);
double ambiguity =
camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
ASSERT_TRUE(ambiguity > 0.5);
robotPose =
frc::Pose2d{frc::Translation2d{-2_m, -2_m}, frc::Rotation2d{30_deg}};
robotPose = wpi::math::Pose2d{wpi::math::Translation2d{-2_m, -2_m},
wpi::math::Rotation2d{30_deg}};
visionSysSim.Update(robotPose);
ambiguity = camera.GetLatestResult().GetBestTarget().GetPoseAmbiguity();
ASSERT_TRUE(0 < ambiguity && ambiguity < 0.2);

2
photon-lib/src/test/native/cpp/main.cpp
View File

@@ -22,7 +22,7 @@
* SOFTWARE.
*/
#include <hal/HAL.h>
#include <wpi/hal/HAL.h>
#include "gtest/gtest.h"