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

Use ReadQueue for PhotonCamera timestamps (#1316)

Browse Source 
This removes the extra GetLastChange call to keep everything properly
atomic.

Closes #1303
This commit is contained in:
Matt
2024-08-04 14:23:46 -04:00
committed by GitHub
parent 37e9d40762
commit 67463a020a
29 changed files with 1057 additions and 1614 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
photon-lib/build.gradle
View File

@@ -53,6 +53,8 @@ model {
nativeUtils.useRequiredLibrary(it, "wpilib_shared")
nativeUtils.useRequiredLibrary(it, "apriltag_shared")
nativeUtils.useRequiredLibrary(it, "opencv_shared")
nativeUtils.useRequiredLibrary(it, "cscore_shared")
nativeUtils.useRequiredLibrary(it, "cameraserver_shared")
}
}
testSuites {

34
photon-lib/py/photonlibpy/photonCamera.py
View File

@@ -1,4 +1,5 @@
from enum import Enum
from typing import List
import ntcore
from wpilib import RobotController, Timer
import wpilib
@@ -75,6 +76,39 @@ class PhotonCamera:
self._prevHeartbeat = 0
self._prevHeartbeatChangeTime = Timer.getFPGATimestamp()
def getAllUnreadResults(self) -> List[PhotonPipelineResult]:
"""
The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
Calling this function clears the internal FIFO queue, and multiple calls to
getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
make sure to call this frequently enough to avoid old results being discarded, too!
"""
self._versionCheck()
changes = self._rawBytesEntry.readQueue()
ret = []
for change in changes:
byteList = change.value
timestamp = change.time
if len(byteList) < 1:
pass
else:
newResult = PhotonPipelineResult()
pkt = Packet(byteList)
newResult.populateFromPacket(pkt)
# NT4 allows us to correct the timestamp based on when the message was sent
newResult.setTimestampSeconds(
timestamp / 1e6 - newResult.getLatencyMillis() / 1e3
)
ret.append(newResult)
return ret
def getLatestResult(self) -> PhotonPipelineResult:
self._versionCheck()

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

@@ -44,8 +44,8 @@ import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;
@@ -137,10 +137,12 @@ public class PhotonCamera implements AutoCloseable {
cameraTable
.getRawTopic("rawBytes")
.subscribe(
"rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true));
resultSubscriber =
new PacketSubscriber<>(
rawBytesEntry, PhotonPipelineResult.serde, new PhotonPipelineResult());
"rawBytes",
new byte[] {},
PubSubOption.periodic(0.01),
PubSubOption.sendAll(true),
PubSubOption.pollStorage(20));
resultSubscriber = new PacketSubscriber<>(rawBytesEntry, PhotonPipelineResult.serde);
driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish();
driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false);
inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0);
@@ -176,21 +178,52 @@ public class PhotonCamera implements AutoCloseable {
}
/**
* Returns the latest pipeline result.
*
* @return The latest pipeline result.
* The list of pipeline results sent by PhotonVision since the last call to getAllUnreadResults().
* Calling this function clears the internal FIFO queue, and multiple calls to
* getAllUnreadResults() will return different (potentially empty) result arrays. Be careful to
* call this exactly ONCE per loop of your robot code! FIFO depth is limited to 20 changes, so
* make sure to call this frequently enough to avoid old results being discarded, too!
*/
public List<PhotonPipelineResult> getAllUnreadResults() {
List<PhotonPipelineResult> ret = new ArrayList<>();
var changes = resultSubscriber.getAllChanges();
// 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.
for (var c : changes) {
var result = c.value;
result.setRecieveTimestampMicros(c.timestamp);
ret.add(result);
}
return ret;
}
/**
* Returns the latest pipeline result. This is simply the most recent result recieved via NT.
* Calling this multiple times will always return the most recent result.
*
* <p>Replaced by {@link #getAllUnreadResults()} over getLatestResult, as this function can miss
* results, or provide duplicate ones!
*/
@Deprecated(since = "2024", forRemoval = true)
public PhotonPipelineResult getLatestResult() {
verifyVersion();
var ret = resultSubscriber.get();
// Set the timestamp of the result.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
ret.setRecieveTimestampMicros(RobotController.getFPGATime());
if (ret.timestamp == 0) return new PhotonPipelineResult();
// Return result.
return ret;
var result = ret.value;
// Set the timestamp of the result. Since PacketSubscriber doesn't realize that the result
// contains a thing with time knowledge, set it here.
// getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds.
// TODO: NT4 time sync is Not To Be Trusted, we should do something else?
result.setRecieveTimestampMicros(ret.timestamp);
return result;
}
/**

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

@@ -90,7 +90,6 @@ public class PhotonPoseEstimator {
private TargetModel tagModel = TargetModel.kAprilTag36h11;
private PoseStrategy primaryStrategy;
private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY;
private final PhotonCamera camera;
private Transform3d robotToCamera;
private Pose3d lastPose;
@@ -107,31 +106,21 @@ public class PhotonPoseEstimator {
* Coordinate System</a>. Note that setting the origin of this layout object will affect the
* results from this class.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCamera
* @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie,
* robot ➔ camera) in the <a href=
* "https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#robot-coordinate-system">Robot
* Coordinate System</a>.
*/
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags,
PoseStrategy strategy,
PhotonCamera camera,
Transform3d robotToCamera) {
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this.fieldTags = fieldTags;
this.primaryStrategy = strategy;
this.camera = camera;
this.robotToCamera = robotToCamera;
HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount);
InstanceCount++;
}
public PhotonPoseEstimator(
AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) {
this(fieldTags, strategy, null, robotToCamera);
}
/** Invalidates the pose cache. */
private void invalidatePoseCache() {
poseCacheTimestampSeconds = -1;
@@ -288,31 +277,8 @@ public class PhotonPoseEstimator {
}
/**
* Poll data from the configured cameras and update the estimated position of the robot. Returns
* empty if:
*
* <ul>
* <li>New data has not been received since the last call to {@code update()}.
* <li>No targets were found from the camera
* <li>There is no camera set
* </ul>
*
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update() {
if (camera == null) {
DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
return Optional.empty();
}
PhotonPipelineResult cameraResult = camera.getLatestResult();
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
}
/**
* Updates the estimated position of the robot. Returns empty if:
* Updates the estimated position of the robot, assuming no camera calibration is required for the
* selected strategy. Returns empty if:
*
* <ul>
* <li>The timestamp of the provided pipeline result is the same as in the previous call to
@@ -320,13 +286,15 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
* provided
*
* @param cameraResult The latest pipeline result from the camera
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
public Optional<EstimatedRobotPose> update(PhotonPipelineResult cameraResult) {
if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty());
return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs());
return update(cameraResult, Optional.empty(), Optional.empty());
}
/**
@@ -338,10 +306,10 @@ public class PhotonPoseEstimator {
* <li>No targets were found in the pipeline results.
* </ul>
*
* @param cameraMatrix Camera calibration data that can be used in the case of no assigned
* PhotonCamera.
* @param distCoeffs Camera calibration data that can be used in the case of no assigned
* PhotonCamera
* @param cameraMatrix Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @param distCoeffs Camera calibration data for multi-tag-on-rio strategy - can be empty
* otherwise
* @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
* create the estimate.
*/
@@ -378,7 +346,7 @@ public class PhotonPoseEstimator {
Optional<Matrix<N3, N3>> cameraMatrix,
Optional<Matrix<N8, N1>> distCoeffs,
PoseStrategy strat) {
Optional<EstimatedRobotPose> estimatedPose;
Optional<EstimatedRobotPose> estimatedPose = Optional.empty();
switch (strat) {
case LOWEST_AMBIGUITY:
estimatedPose = lowestAmbiguityStrategy(cameraResult);
@@ -397,10 +365,20 @@ public class PhotonPoseEstimator {
estimatedPose = averageBestTargetsStrategy(cameraResult);
break;
case MULTI_TAG_PNP_ON_RIO:
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
if (cameraMatrix.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else if (distCoeffs.isEmpty()) {
DriverStation.reportWarning(
"Camera matrix is empty for multi-tag-on-rio",
Thread.currentThread().getStackTrace());
} else {
estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
}
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs);
estimatedPose = multiTagOnCoprocStrategy(cameraResult);
break;
default:
DriverStation.reportError(
@@ -415,10 +393,7 @@ public class PhotonPoseEstimator {
return estimatedPose;
}
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(
PhotonPipelineResult result,
Optional<Matrix<N3, N3>> cameraMatrixOpt,
Optional<Matrix<N8, N1>> distCoeffsOpt) {
private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
if (result.getMultiTagResult().estimatedPose.isPresent) {
var best_tf = result.getMultiTagResult().estimatedPose.best;
var best =
@@ -433,7 +408,8 @@ public class PhotonPoseEstimator {
result.getTargets(),
PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
} else {
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
// We can nver fall back on another multitag strategy
return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
}
}

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

@@ -81,7 +81,7 @@ public class PhotonCameraSim implements AutoCloseable {
private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
private final AprilTagFieldLayout tagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();
AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
// video stream simulation
private final CvSource videoSimRaw;

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

@@ -74,7 +74,9 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance,
rootTable(mainTable->GetSubTable(cameraName)),
rawBytesEntry(
rootTable->GetRawTopic("rawBytes")
.Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})),
.Subscribe(
"rawBytes", {},
{.pollStorage = 20, .periodic = 0.01, .sendAll = true})),
inputSaveImgEntry(
rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()),
inputSaveImgSubscriber(
@@ -110,15 +112,15 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
PhotonPipelineResult PhotonCamera::GetLatestResult() {
if (test) {
return testResult;
if (testResult.size())
return testResult.back();
else
return PhotonPipelineResult{};
}
// Prints warning if not connected
VerifyVersion();
// Clear the current packet.
packet.Clear();
// Create the new result;
PhotonPipelineResult result;
@@ -137,6 +139,40 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() {
return result;
}
std::vector<PhotonPipelineResult> PhotonCamera::GetAllUnreadResults() {
if (test) {
return testResult;
}
// Prints warning if not connected
VerifyVersion();
const auto changes = rawBytesEntry.ReadQueue();
// Create the new result list -- these will be updated in-place
std::vector<PhotonPipelineResult> ret(changes.size());
for (size_t i = 0; i < changes.size(); i++) {
const nt::Timestamped<std::vector<uint8_t>>& value = changes[i];
if (!value.value.size() || value.time == 0) {
continue;
}
// Fill the packet with latest data and populate result.
photon::Packet packet{value.value};
PhotonPipelineResult& result = ret[i];
packet >> result;
// 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.SetRecieveTimestamp(units::microsecond_t(value.time) -
result.GetLatency());
}
return ret;
}
void PhotonCamera::SetDriverMode(bool driverMode) {
driverModePublisher.Set(driverMode);
}

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

@@ -70,22 +70,6 @@ PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
frc::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
camera(nullptr),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
poseCacheTimestamp(-1_s) {
HAL_Report(HALUsageReporting::kResourceType_PhotonPoseEstimator,
InstanceCount);
InstanceCount++;
}
PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
PoseStrategy strat, PhotonCamera&& cam,
frc::Transform3d robotToCamera)
: aprilTags(tags),
strategy(strat),
camera(std::make_shared<PhotonCamera>(std::move(cam))),
m_robotToCamera(robotToCamera),
lastPose(frc::Pose3d()),
referencePose(frc::Pose3d()),
@@ -110,25 +94,6 @@ void PhotonPoseEstimator::SetMultiTagFallbackStrategy(PoseStrategy strategy) {
multiTagFallbackStrategy = strategy;
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
if (!camera) {
FRC_ReportError(frc::warn::Warning, "[PhotonPoseEstimator] Missing camera!",
"");
return std::nullopt;
}
auto result = camera->GetLatestResult();
return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result) {
// If camera is null, best we can do is pass null calibration data
if (!camera) {
return Update(result, std::nullopt, std::nullopt, this->strategy);
}
return Update(result, camera->GetCameraMatrix(), camera->GetDistCoeffs());
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
@@ -181,11 +146,16 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
ret = AverageBestTargetsStrategy(result);
break;
case MULTI_TAG_PNP_ON_COPROCESSOR:
ret =
MultiTagOnCoprocStrategy(result, cameraMatrixData, cameraDistCoeffs);
ret = MultiTagOnCoprocStrategy(result);
break;
case MULTI_TAG_PNP_ON_RIO:
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
if (cameraMatrixData && cameraDistCoeffs) {
ret = MultiTagOnRioStrategy(result, cameraMatrixData, cameraDistCoeffs);
} else {
FRC_ReportError(frc::warn::Warning,
"No camera calibration provided to multi-tag-on-rio!",
"");
}
break;
default:
FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
@@ -378,9 +348,7 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
}
std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
PhotonPipelineResult result) {
if (result.MultiTagResult().result.isPresent) {
const auto field2camera = result.MultiTagResult().result.best;
@@ -408,6 +376,10 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
}
if (!camMat || !distCoeffs) {
FRC_ReportError(frc::warn::Warning,
"No camera calibration data provided to "
"PhotonPoseEstimator::MultiTagOnRioStrategy!",
"");
return Update(result, std::nullopt, std::nullopt,
this->multiTagFallbackStrategy);
}

392
photon-lib/src/main/native/cpp/photon/simulation/PhotonCameraSim.cpp Normal file
View File

@@ -0,0 +1,392 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photon/simulation/PhotonCameraSim.h"
#include <algorithm>
#include <string>
#include <utility>
#include <vector>
namespace photon {
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera)
: PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {}
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
const SimCameraProperties& props)
: prop(props), cam(camera) {
SetMinTargetAreaPixels(kDefaultMinAreaPx);
videoSimRaw =
frc::CameraServer::PutVideo(std::string{camera->GetCameraName()} + "-raw",
prop.GetResWidth(), prop.GetResHeight());
videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
videoSimProcessed = frc::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
prop.GetResHeight());
ts.subTable = cam->GetCameraTable();
ts.UpdateEntries();
}
PhotonCameraSim::PhotonCameraSim(PhotonCamera* camera,
const SimCameraProperties& props,
double minTargetAreaPercent,
units::meter_t maxSightRange)
: PhotonCameraSim(camera, props) {
this->minTargetAreaPercent = minTargetAreaPercent;
this->maxSightRange = maxSightRange;
}
bool PhotonCameraSim::CanSeeTargetPose(const frc::Pose3d& camPose,
const VisionTargetSim& target) {
CameraTargetRelation rel{camPose, target.GetPose()};
return ((units::math::abs(rel.camToTargYaw.Degrees()) <
prop.GetHorizFOV().Degrees() / 2.0) &&
(units::math::abs(rel.camToTargPitch.Degrees()) <
prop.GetVertFOV().Degrees() / 2.0) &&
(!target.GetModel().GetIsPlanar() ||
units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
(rel.camToTarg.Translation().Norm() <= maxSightRange));
}
bool PhotonCameraSim::CanSeeCorner(const std::vector<cv::Point2f>& points) {
for (const auto& pt : points) {
if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
return false;
}
}
return true;
}
std::optional<uint64_t> PhotonCameraSim::ConsumeNextEntryTime() {
uint64_t now = wpi::Now();
uint64_t timestamp{};
int iter = 0;
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
uint64_t frameTime = prop.EstSecUntilNextFrame()
.convert<units::microseconds>()
.to<uint64_t>();
nextNTEntryTime += frameTime;
if (iter++ > 50) {
timestamp = now;
nextNTEntryTime = now + frameTime;
break;
}
}
if (timestamp != 0) {
return timestamp;
} else {
return std::nullopt;
}
}
PhotonPipelineResult PhotonCameraSim::Process(
units::second_t latency, const frc::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets) {
std::sort(targets.begin(), targets.end(),
[cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
units::meter_t dist1 =
t1.GetPose().Translation().Distance(cameraPose.Translation());
units::meter_t dist2 =
t2.GetPose().Translation().Distance(cameraPose.Translation());
return dist1 > dist2;
});
std::vector<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
visibleTgts{};
std::vector<PhotonTrackedTarget> detectableTgts{};
RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose);
VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
blankFrame.assignTo(videoSimFrameRaw);
for (const auto& tgt : targets) {
if (!CanSeeTargetPose(cameraPose, tgt)) {
continue;
}
std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
if (tgt.GetModel().GetIsSpherical()) {
TargetModel model = tgt.GetModel();
fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
tgt.GetPose().Translation(), cameraPose.Translation()));
}
std::vector<cv::Point2f> imagePoints = OpenCVHelp::ProjectPoints(
prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners);
if (tgt.GetModel().GetIsSpherical()) {
cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints);
int l = 0;
int t = 0;
int b = 0;
int r = 0;
for (int i = 0; i < 4; i++) {
if (imagePoints[i].x < imagePoints[l].x) {
l = i;
}
}
cv::Point2d lc = imagePoints[l];
std::array<double, 4> angles{};
t = (l + 1) % 4;
b = (l + 1) % 4;
for (int i = 0; i < 4; i++) {
if (i == l) {
continue;
}
cv::Point2d ic = imagePoints[i];
angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x);
if (angles[i] >= angles[t]) {
t = i;
}
if (angles[i] <= angles[b]) {
b = i;
}
}
for (int i = 0; i < 4; i++) {
if (i != t && i != l && i != b) {
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>()};
std::vector<cv::Point2f> points{};
rect.points(points);
// Can't find an easier way to convert from Point2f to Point2d
imagePoints.clear();
std::transform(points.begin(), points.end(),
std::back_inserter(imagePoints),
[](const cv::Point2f& p) { return (cv::Point2d)p; });
}
visibleTgts.emplace_back(std::make_pair(tgt, imagePoints));
std::vector<cv::Point2f> noisyTargetCorners =
prop.EstPixelNoise(imagePoints);
cv::RotatedRect minAreaRect =
OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
std::vector<cv::Point2f> minAreaRectPts;
minAreaRectPts.reserve(4);
minAreaRect.points(minAreaRectPts);
cv::Point2d centerPt = minAreaRect.center;
frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
if (!(CanSeeCorner(noisyTargetCorners) &&
areaPercent >= minTargetAreaPercent)) {
continue;
}
PNPResult pnpSim{};
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
prop.GetIntrinsics(), prop.GetDistCoeffs(),
tgt.GetModel().GetVertices(), noisyTargetCorners);
}
std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> 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>(), tgt.fiducialId,
tgt.objDetClassId, tgt.objDetConf, pnpSim.best, pnpSim.alt,
pnpSim.ambiguity, smallVec, cornersDouble);
}
if (videoSimRawEnabled) {
if (videoSimWireframeEnabled) {
VideoSimUtil::DrawFieldWireFrame(camRt, prop, videoSimWireframeResolution,
1.5, cv::Scalar{80}, 6, 1,
cv::Scalar{30}, videoSimFrameRaw);
}
for (const auto& pair : visibleTgts) {
VisionTargetSim tgt = pair.first;
std::vector<cv::Point2f> corners = pair.second;
if (tgt.fiducialId > 0) {
VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
videoSimFrameRaw);
} else if (!tgt.GetModel().GetIsSpherical()) {
std::vector<cv::Point2f> contour = corners;
if (!tgt.GetModel().GetIsPlanar()) {
contour = OpenCVHelp::GetConvexHull(contour);
}
VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true,
videoSimFrameRaw);
} else {
VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255},
videoSimFrameRaw);
}
}
videoSimRaw.PutFrame(videoSimFrameRaw);
} else {
videoSimRaw.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
if (videoSimProcEnabled) {
cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed, cv::COLOR_GRAY2BGR);
cv::drawMarker(
videoSimFrameProcessed,
cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0},
cv::Scalar{0, 255, 0}, cv::MARKER_CROSS,
static_cast<int>(
VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
cv::LINE_AA);
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
tgt.GetMinAreaRectCorners();
std::vector<std::pair<float, float>> cornersCopy{};
cornersCopy.reserve(4);
for (const auto& corner : smallVec) {
cornersCopy.emplace_back(
std::make_pair(static_cast<float>(corner.first),
static_cast<float>(corner.second)));
}
VideoSimUtil::DrawPoly(
OpenCVHelp::CornersToPoints(cornersCopy),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
}
}
videoSimProcessed.PutFrame(videoSimFrameProcessed);
} else {
videoSimProcessed.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
}
heartbeatCounter++;
return PhotonPipelineResult{heartbeatCounter, 0_s, latency, detectableTgts,
multiTagResults};
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void PhotonCameraSim::SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
Packet newPacket{};
newPacket << result;
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
std::array<double, 4> poseData{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter, recieveTimestamp);
ts.subTable->GetInstance().Flush();
}
} // namespace photon

223
photon-lib/src/main/native/cpp/photon/simulation/SimCameraProperties.cpp Normal file
View File

@@ -0,0 +1,223 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "photon/simulation/SimCameraProperties.h"
#include <algorithm>
#include <utility>
#include <vector>
using namespace photon;
void SimCameraProperties::SetCalibration(int width, int height,
frc::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,
"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}};
Eigen::Matrix<double, 8, 1> newDistCoeffs =
Eigen::Matrix<double, 8, 1>::Zero();
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);
Eigen::Matrix<double, 3, 3> newCamIntrinsics;
newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
SetCalibration(width, height, newCamIntrinsics, newDistCoeffs);
}
void SimCameraProperties::SetCalibration(
int width, int height, const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
resWidth = width;
resHeight = height;
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}},
};
viewplanes.clear();
for (size_t i = 0; i < p.size(); i++) {
viewplanes.emplace_back(Eigen::Matrix<double, 3, 1>{
p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
}
}
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);
if (relA.X() <= 0_m && relB.X() <= 0_m) {
return {std::nullopt, std::nullopt};
}
Eigen::Matrix<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
relA.Z().to<double>()};
Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
relB.Z().to<double>()};
Eigen::Matrix<double, 3, 1> abv = bv - av;
bool aVisible = true;
bool bVisible = true;
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
double aVisibility = av.dot(normal);
if (aVisibility < 0) {
aVisible = false;
}
double bVisibility = bv.dot(normal);
if (bVisibility < 0) {
bVisible = false;
}
if (aVisibility <= 0 && bVisibility <= 0) {
return {std::nullopt, std::nullopt};
}
}
if (aVisible && bVisible) {
return {0, 1};
}
std::array<double, 4> intersections{std::nan(""), std::nan(""), std::nan(""),
std::nan("")};
std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
{std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
Eigen::Matrix<double, 3, 1> a_projn{};
a_projn = (av.dot(normal) / normal.dot(normal)) * normal;
if (std::abs(abv.dot(normal)) < 1e-5) {
continue;
}
intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn));
Eigen::Matrix<double, 3, 1> apv{};
apv = intersections[i] * abv;
Eigen::Matrix<double, 3, 1> intersectpt{};
intersectpt = av + apv;
ipts[i] = intersectpt;
for (size_t j = 1; j < viewplanes.size(); j++) {
int oi = (i + j) % viewplanes.size();
Eigen::Matrix<double, 3, 1> onormal = viewplanes[oi];
if (intersectpt.dot(onormal) < 0) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
if (!ipts[i]) {
continue;
}
for (int j = i - 1; j >= 0; j--) {
std::optional<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
if (!oipt) {
continue;
}
Eigen::Matrix<double, 3, 1> diff{};
diff = oipt.value() - intersectpt;
if (diff.cwiseAbs().maxCoeff() < 1e-4) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
}
double inter1 = std::nan("");
double inter2 = std::nan("");
for (double inter : intersections) {
if (!std::isnan(inter)) {
if (std::isnan(inter1)) {
inter1 = inter;
} else {
inter2 = inter;
}
}
}
if (!std::isnan(inter2)) {
double max = std::max(inter1, inter2);
double min = std::min(inter1, inter2);
if (aVisible) {
min = 0;
}
if (bVisible) {
max = 1;
}
return {min, max};
} else if (!std::isnan(inter1)) {
if (aVisible) {
return {0, inter1};
}
if (bVisible) {
return {inter1, 1};
}
return {inter1, std::nullopt};
} else {
return {std::nullopt, std::nullopt};
}
}

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

@@ -75,13 +75,20 @@ class PhotonCamera {
PhotonCamera(PhotonCamera&&) = default;
virtual ~PhotonCamera() = default;
~PhotonCamera() = default;
/**
* Returns the latest pipeline result.
* @return The latest pipeline result.
* The list of pipeline results sent by PhotonVision since the last call to
* GetAllUnreadResults(). Calling this function clears the internal FIFO
* queue, and multiple calls to GetAllUnreadResults() will return different
* (potentially empty) result arrays. Be careful to call this exactly ONCE per
* loop of your robot code! FIFO depth is limited to 20 changes, so make sure
* to call this frequently enough to avoid old results being discarded, too!
*/
virtual PhotonPipelineResult GetLatestResult();
std::vector<PhotonPipelineResult> GetAllUnreadResults();
[[deprecated("Replace with GetAllUnreadResults")]]
PhotonPipelineResult GetLatestResult();
/**
* Toggles driver mode.
@@ -173,7 +180,7 @@ class PhotonCamera {
// For use in tests
bool test = false;
PhotonPipelineResult testResult;
std::vector<PhotonPipelineResult> testResult;
protected:
std::shared_ptr<nt::NetworkTable> mainTable;
@@ -201,8 +208,6 @@ class PhotonCamera {
std::string path;
std::string cameraName;
mutable Packet packet;
private:
units::second_t lastVersionCheckTime = 0_s;
static bool VERSION_CHECK_ENABLED;

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

@@ -29,6 +29,7 @@
#include <frc/apriltag/AprilTagFieldLayout.h>
#include <frc/geometry/Pose3d.h>
#include <frc/geometry/Transform3d.h>
#include <opencv2/core/mat.hpp>
#include "photon/PhotonCamera.h"
#include "photon/dataflow/structures/Packet.h"
@@ -37,10 +38,6 @@
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace cv {
class Mat;
} // namespace cv
namespace photon {
enum PoseStrategy {
LOWEST_AMBIGUITY = 0,
@@ -85,28 +82,6 @@ class PhotonPoseEstimator {
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
* @param camera PhotonCameras and
* @param robotToCamera Transform3d from the center of the robot to the camera
* mount positions (ie, robot ➔ camera).
*/
explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
PoseStrategy strategy, PhotonCamera&& camera,
frc::Transform3d robotToCamera);
/**
* Create a new PhotonPoseEstimator.
*
* <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
* <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
* at (1.0,2.0,3.0) </code> }
*
* @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with
* respect to the FIRST field.
* @param strategy The strategy it should use to determine the best pose.
@@ -198,32 +173,26 @@ class PhotonPoseEstimator {
inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
/**
* Update the pose estimator. Internally grabs a new PhotonPipelineResult from
* the camera and process it.
*/
std::optional<EstimatedRobotPose> Update();
/**
* Update the pose estimator.
*/
std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result);
/**
* Update the pose estimator.
* Update the pose estimator. If updating multiple times per loop, you should
* call this exactly once per new result, in order of increasing result
* timestamp.
*
* @param result The vision targeting result to process
* @param cameraIntrinsics The camera calibration pinhole coefficients matrix.
* Only required if doing multitag-on-rio, and may be nullopt otherwise.
* @param distCoeffsData The camera calibration distortion coefficients. Only
* required if doing multitag-on-rio, and may be nullopt otherwise.
*/
std::optional<EstimatedRobotPose> Update(
const PhotonPipelineResult& result,
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,
std::optional<PhotonCamera::DistortionMatrix> coeffsData);
inline std::shared_ptr<PhotonCamera> GetCamera() { return camera; }
std::optional<PhotonCamera::CameraMatrix> cameraMatrixData = std::nullopt,
std::optional<PhotonCamera::DistortionMatrix> coeffsData = std::nullopt);
private:
frc::AprilTagFieldLayout aprilTags;
PoseStrategy strategy;
PoseStrategy multiTagFallbackStrategy = LOWEST_AMBIGUITY;
std::shared_ptr<PhotonCamera> camera;
frc::Transform3d m_robotToCamera;
frc::Pose3d lastPose;
@@ -280,9 +249,7 @@ class PhotonPoseEstimator {
* @return the estimated position of the robot in the FCS
*/
std::optional<EstimatedRobotPose> MultiTagOnCoprocStrategy(
PhotonPipelineResult result,
std::optional<PhotonCamera::CameraMatrix> camMat,
std::optional<PhotonCamera::DistortionMatrix> distCoeffs);
PhotonPipelineResult result);
/**
* Return the pose calculation using all targets in view in the same PNP()

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

@@ -49,382 +49,50 @@
namespace photon {
class PhotonCameraSim {
public:
explicit PhotonCameraSim(PhotonCamera* camera)
: PhotonCameraSim(camera, photon::SimCameraProperties::PERFECT_90DEG()) {}
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props)
: prop(props), cam(camera) {
SetMinTargetAreaPixels(kDefaultMinAreaPx);
videoSimRaw = frc::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-raw", prop.GetResWidth(),
prop.GetResHeight());
videoSimRaw.SetPixelFormat(cs::VideoMode::PixelFormat::kGray);
videoSimProcessed = frc::CameraServer::PutVideo(
std::string{camera->GetCameraName()} + "-processed", prop.GetResWidth(),
prop.GetResHeight());
ts.subTable = cam->GetCameraTable();
ts.UpdateEntries();
}
explicit PhotonCameraSim(PhotonCamera* camera);
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props);
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
double minTargetAreaPercent, units::meter_t maxSightRange)
: PhotonCameraSim(camera, props) {
this->minTargetAreaPercent = minTargetAreaPercent;
this->maxSightRange = maxSightRange;
}
PhotonCamera* GetCamera() { return cam; }
double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
double GetMinTargetAreaPixels() {
double minTargetAreaPercent, units::meter_t maxSightRange);
inline PhotonCamera* GetCamera() { return cam; }
inline double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
inline double GetMinTargetAreaPixels() {
return minTargetAreaPercent / 100.0 * prop.GetResArea();
}
units::meter_t GetMaxSightRange() { return maxSightRange; }
const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
inline units::meter_t GetMaxSightRange() { return maxSightRange; }
inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
inline const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
bool CanSeeTargetPose(const frc::Pose3d& camPose,
const VisionTargetSim& target) {
CameraTargetRelation rel{camPose, target.GetPose()};
return ((units::math::abs(rel.camToTargYaw.Degrees()) <
prop.GetHorizFOV().Degrees() / 2.0) &&
(units::math::abs(rel.camToTargPitch.Degrees()) <
prop.GetVertFOV().Degrees() / 2.0) &&
(!target.GetModel().GetIsPlanar() ||
units::math::abs(rel.targToCamAngle.Degrees()) < 90_deg) &&
(rel.camToTarg.Translation().Norm() <= maxSightRange));
}
bool CanSeeCorner(const std::vector<cv::Point2f>& points) {
for (const auto& pt : points) {
if (std::clamp<float>(pt.x, 0, prop.GetResWidth()) != pt.x ||
std::clamp<float>(pt.y, 0, prop.GetResHeight()) != pt.y) {
return false;
}
}
return true;
}
std::optional<uint64_t> ConsumeNextEntryTime() {
uint64_t now = wpi::Now();
uint64_t timestamp{};
int iter = 0;
while (now >= nextNTEntryTime) {
timestamp = nextNTEntryTime;
uint64_t frameTime = prop.EstSecUntilNextFrame()
.convert<units::microseconds>()
.to<uint64_t>();
nextNTEntryTime += frameTime;
const VisionTargetSim& target);
bool CanSeeCorner(const std::vector<cv::Point2f>& points);
std::optional<uint64_t> ConsumeNextEntryTime();
if (iter++ > 50) {
timestamp = now;
nextNTEntryTime = now + frameTime;
break;
}
}
if (timestamp != 0) {
return timestamp;
} else {
return std::nullopt;
}
}
void SetMinTargetAreaPercent(double areaPercent) {
inline void SetMinTargetAreaPercent(double areaPercent) {
minTargetAreaPercent = areaPercent;
}
void SetMinTargetAreaPixels(double areaPx) {
inline void SetMinTargetAreaPixels(double areaPx) {
minTargetAreaPercent = areaPx / prop.GetResArea() * 100;
}
void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
void EnableDrawWireframe(bool enabled) { videoSimWireframeEnabled = enabled; }
void SetWireframeResolution(double resolution) {
inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
inline void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
inline void EnableDrawWireframe(bool enabled) {
videoSimWireframeEnabled = enabled;
}
inline void SetWireframeResolution(double resolution) {
videoSimWireframeResolution = resolution;
}
void EnabledProcessedStream(double enabled) { videoSimProcEnabled = enabled; }
inline void EnabledProcessedStream(double enabled) {
videoSimProcEnabled = enabled;
}
PhotonPipelineResult Process(units::second_t latency,
const frc::Pose3d& cameraPose,
std::vector<VisionTargetSim> targets) {
std::sort(
targets.begin(), targets.end(),
[cameraPose](const VisionTargetSim& t1, const VisionTargetSim& t2) {
units::meter_t dist1 =
t1.GetPose().Translation().Distance(cameraPose.Translation());
units::meter_t dist2 =
t2.GetPose().Translation().Distance(cameraPose.Translation());
return dist1 > dist2;
});
std::vector<VisionTargetSim> targets);
std::vector<std::pair<VisionTargetSim, std::vector<cv::Point2f>>>
visibleTgts{};
std::vector<PhotonTrackedTarget> detectableTgts{};
RotTrlTransform3d camRt = RotTrlTransform3d::MakeRelativeTo(cameraPose);
VideoSimUtil::UpdateVideoProp(videoSimRaw, prop);
VideoSimUtil::UpdateVideoProp(videoSimProcessed, prop);
cv::Size videoFrameSize{prop.GetResWidth(), prop.GetResHeight()};
cv::Mat blankFrame = cv::Mat::zeros(videoFrameSize, CV_8UC1);
blankFrame.assignTo(videoSimFrameRaw);
for (const auto& tgt : targets) {
if (!CanSeeTargetPose(cameraPose, tgt)) {
continue;
}
std::vector<frc::Translation3d> fieldCorners = tgt.GetFieldVertices();
if (tgt.GetModel().GetIsSpherical()) {
TargetModel model = tgt.GetModel();
fieldCorners = model.GetFieldVertices(TargetModel::GetOrientedPose(
tgt.GetPose().Translation(), cameraPose.Translation()));
}
std::vector<cv::Point2f> imagePoints = OpenCVHelp::ProjectPoints(
prop.GetIntrinsics(), prop.GetDistCoeffs(), camRt, fieldCorners);
if (tgt.GetModel().GetIsSpherical()) {
cv::Point2d center = OpenCVHelp::AvgPoint(imagePoints);
int l = 0;
int t = 0;
int b = 0;
int r = 0;
for (int i = 0; i < 4; i++) {
if (imagePoints[i].x < imagePoints[l].x) {
l = i;
}
}
cv::Point2d lc = imagePoints[l];
std::array<double, 4> angles{};
t = (l + 1) % 4;
b = (l + 1) % 4;
for (int i = 0; i < 4; i++) {
if (i == l) {
continue;
}
cv::Point2d ic = imagePoints[i];
angles[i] = std::atan2(lc.y - ic.y, ic.x - lc.x);
if (angles[i] >= angles[t]) {
t = i;
}
if (angles[i] <= angles[b]) {
b = i;
}
}
for (int i = 0; i < 4; i++) {
if (i != t && i != l && i != b) {
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>()};
std::vector<cv::Point2f> points{};
rect.points(points);
// Can't find an easier way to convert from Point2f to Point2d
imagePoints.clear();
std::transform(points.begin(), points.end(),
std::back_inserter(imagePoints),
[](const cv::Point2f& p) { return (cv::Point2d)p; });
}
visibleTgts.emplace_back(std::make_pair(tgt, imagePoints));
std::vector<cv::Point2f> noisyTargetCorners =
prop.EstPixelNoise(imagePoints);
cv::RotatedRect minAreaRect =
OpenCVHelp::GetMinAreaRect(noisyTargetCorners);
std::vector<cv::Point2f> minAreaRectPts;
minAreaRectPts.reserve(4);
minAreaRect.points(minAreaRectPts);
cv::Point2d centerPt = minAreaRect.center;
frc::Rotation3d centerRot = prop.GetPixelRot(centerPt);
double areaPercent = prop.GetContourAreaPercent(noisyTargetCorners);
if (!(CanSeeCorner(noisyTargetCorners) &&
areaPercent >= minTargetAreaPercent)) {
continue;
}
PNPResult pnpSim{};
if (tgt.fiducialId >= 0 && tgt.GetFieldVertices().size() == 4) {
pnpSim = OpenCVHelp::SolvePNP_Square(
prop.GetIntrinsics(), prop.GetDistCoeffs(),
tgt.GetModel().GetVertices(), noisyTargetCorners);
}
std::vector<std::pair<float, float>> tempCorners =
OpenCVHelp::PointsToCorners(minAreaRectPts);
wpi::SmallVector<std::pair<double, double>, 4> smallVec;
for (const auto& corner : tempCorners) {
smallVec.emplace_back(
std::make_pair(static_cast<double>(corner.first),
static_cast<double>(corner.second)));
}
std::vector<std::pair<float, float>> cornersFloat =
OpenCVHelp::PointsToCorners(noisyTargetCorners);
std::vector<std::pair<double, double>> cornersDouble{cornersFloat.begin(),
cornersFloat.end()};
detectableTgts.emplace_back(PhotonTrackedTarget{
-centerRot.Z().convert<units::degrees>().to<double>(),
-centerRot.Y().convert<units::degrees>().to<double>(), areaPercent,
centerRot.X().convert<units::degrees>().to<double>(), tgt.fiducialId,
-1, -1, pnpSim.best, pnpSim.alt, pnpSim.ambiguity, smallVec,
cornersDouble});
}
if (videoSimRawEnabled) {
if (videoSimWireframeEnabled) {
VideoSimUtil::DrawFieldWireFrame(
camRt, prop, videoSimWireframeResolution, 1.5, cv::Scalar{80}, 6, 1,
cv::Scalar{30}, videoSimFrameRaw);
}
for (const auto& pair : visibleTgts) {
VisionTargetSim tgt = pair.first;
std::vector<cv::Point2f> corners = pair.second;
if (tgt.fiducialId > 0) {
VideoSimUtil::Warp165h5TagImage(tgt.fiducialId, corners, true,
videoSimFrameRaw);
} else if (!tgt.GetModel().GetIsSpherical()) {
std::vector<cv::Point2f> contour = corners;
if (!tgt.GetModel().GetIsPlanar()) {
contour = OpenCVHelp::GetConvexHull(contour);
}
VideoSimUtil::DrawPoly(contour, -1, cv::Scalar{255}, true,
videoSimFrameRaw);
} else {
VideoSimUtil::DrawInscribedEllipse(corners, cv::Scalar{255},
videoSimFrameRaw);
}
}
videoSimRaw.PutFrame(videoSimFrameRaw);
} else {
videoSimRaw.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
if (videoSimProcEnabled) {
cv::cvtColor(videoSimFrameRaw, videoSimFrameProcessed,
cv::COLOR_GRAY2BGR);
cv::drawMarker(
videoSimFrameProcessed,
cv::Point2d{prop.GetResWidth() / 2.0, prop.GetResHeight() / 2.0},
cv::Scalar{0, 255, 0}, cv::MARKER_CROSS,
static_cast<int>(
VideoSimUtil::GetScaledThickness(15, videoSimFrameProcessed)),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::LINE_AA);
for (const auto& tgt : detectableTgts) {
auto detectedCornersDouble = tgt.GetDetectedCorners();
std::vector<std::pair<float, float>> detectedCornerFloat{
detectedCornersDouble.begin(), detectedCornersDouble.end()};
if (tgt.GetFiducialId() >= 0) {
VideoSimUtil::DrawTagDetection(
tgt.GetFiducialId(),
OpenCVHelp::CornersToPoints(detectedCornerFloat),
videoSimFrameProcessed);
} else {
cv::rectangle(videoSimFrameProcessed,
OpenCVHelp::GetBoundingRect(
OpenCVHelp::CornersToPoints(detectedCornerFloat)),
cv::Scalar{0, 0, 255},
static_cast<int>(VideoSimUtil::GetScaledThickness(
1, videoSimFrameProcessed)),
cv::LINE_AA);
wpi::SmallVector<std::pair<double, double>, 4> smallVec =
tgt.GetMinAreaRectCorners();
std::vector<std::pair<float, float>> cornersCopy{};
cornersCopy.reserve(4);
for (const auto& corner : smallVec) {
cornersCopy.emplace_back(
std::make_pair(static_cast<float>(corner.first),
static_cast<float>(corner.second)));
}
VideoSimUtil::DrawPoly(
OpenCVHelp::CornersToPoints(cornersCopy),
static_cast<int>(
VideoSimUtil::GetScaledThickness(1, videoSimFrameProcessed)),
cv::Scalar{255, 30, 30}, true, videoSimFrameProcessed);
}
}
videoSimProcessed.PutFrame(videoSimFrameProcessed);
} else {
videoSimProcessed.SetConnectionStrategy(
cs::VideoSource::ConnectionStrategy::kConnectionForceClose);
}
MultiTargetPNPResult multiTagResults{};
std::vector<frc::AprilTag> visibleLayoutTags =
VisionEstimation::GetVisibleLayoutTags(detectableTgts, tagLayout);
if (visibleLayoutTags.size() > 1) {
wpi::SmallVector<int16_t, 32> usedIds{};
std::transform(visibleLayoutTags.begin(), visibleLayoutTags.end(),
usedIds.begin(),
[](const frc::AprilTag& tag) { return tag.ID; });
std::sort(usedIds.begin(), usedIds.end());
PNPResult pnpResult = VisionEstimation::EstimateCamPosePNP(
prop.GetIntrinsics(), prop.GetDistCoeffs(), detectableTgts, tagLayout,
kAprilTag36h11);
multiTagResults = MultiTargetPNPResult{pnpResult, usedIds};
}
units::second_t now = frc::Timer::GetFPGATimestamp();
return PhotonPipelineResult{heartbeatCounter, now - latency, now,
detectableTgts, multiTagResults};
}
void SubmitProcessedFrame(const PhotonPipelineResult& result) {
SubmitProcessedFrame(result, wpi::Now());
}
void SubmitProcessedFrame(const PhotonPipelineResult& result);
void SubmitProcessedFrame(const PhotonPipelineResult& result,
uint64_t recieveTimestamp) {
ts.latencyMillisEntry.Set(
result.GetLatency().convert<units::milliseconds>().to<double>(),
recieveTimestamp);
uint64_t recieveTimestamp);
Packet newPacket{};
newPacket << result;
ts.rawBytesEntry.Set(newPacket.GetData(), recieveTimestamp);
bool hasTargets = result.HasTargets();
ts.hasTargetEntry.Set(hasTargets, recieveTimestamp);
if (!hasTargets) {
ts.targetPitchEntry.Set(0.0, recieveTimestamp);
ts.targetYawEntry.Set(0.0, recieveTimestamp);
ts.targetAreaEntry.Set(0.0, recieveTimestamp);
std::array<double, 3> poseData{0.0, 0.0, 0.0};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
ts.targetSkewEntry.Set(0.0, recieveTimestamp);
} else {
PhotonTrackedTarget bestTarget = result.GetBestTarget();
ts.targetPitchEntry.Set(bestTarget.GetPitch(), recieveTimestamp);
ts.targetYawEntry.Set(bestTarget.GetYaw(), recieveTimestamp);
ts.targetAreaEntry.Set(bestTarget.GetArea(), recieveTimestamp);
ts.targetSkewEntry.Set(bestTarget.GetSkew(), recieveTimestamp);
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
std::array<double, 4> poseData{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Rotation().ToRotation2d().Degrees().to<double>()};
ts.targetPoseEntry.Set(poseData, recieveTimestamp);
}
auto intrinsics = prop.GetIntrinsics();
std::vector<double> intrinsicsView{intrinsics.data(),
intrinsics.data() + intrinsics.size()};
ts.cameraIntrinsicsPublisher.Set(intrinsicsView, recieveTimestamp);
auto distortion = prop.GetDistCoeffs();
std::vector<double> distortionView{distortion.data(),
distortion.data() + distortion.size()};
ts.cameraDistortionPublisher.Set(distortionView, recieveTimestamp);
ts.heartbeatPublisher.Set(heartbeatCounter++, recieveTimestamp);
}
SimCameraProperties prop;
private:

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

@@ -45,77 +45,11 @@ class SimCameraProperties {
public:
SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); }
SimCameraProperties(std::string path, int width, int height) {}
void SetCalibration(int width, int height, frc::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,
"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}};
Eigen::Matrix<double, 8, 1> newDistCoeffs =
Eigen::Matrix<double, 8, 1>::Zero();
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);
Eigen::Matrix<double, 3, 3> newCamIntrinsics;
newCamIntrinsics << fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0;
SetCalibration(width, height, newCamIntrinsics, newDistCoeffs);
}
void SetCalibration(int width, int height, frc::Rotation2d fovDiag);
void SetCalibration(int width, int height,
const Eigen::Matrix<double, 3, 3>& newCamIntrinsics,
const Eigen::Matrix<double, 8, 1>& newDistCoeffs) {
resWidth = width;
resHeight = height;
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}},
};
viewplanes.clear();
for (size_t i = 0; i < p.size(); i++) {
viewplanes.emplace_back(Eigen::Matrix<double, 3, 1>{
p[i].X().to<double>(), p[i].Y().to<double>(), p[i].Z().to<double>()});
}
}
const Eigen::Matrix<double, 8, 1>& newDistCoeffs);
void SetCalibError(double newAvgErrorPx, double newErrorStdDevPx) {
avgErrorPx = newAvgErrorPx;
@@ -220,125 +154,7 @@ class SimCameraProperties {
std::pair<std::optional<double>, std::optional<double>> 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);
if (relA.X() <= 0_m && relB.X() <= 0_m) {
return {std::nullopt, std::nullopt};
}
Eigen::Matrix<double, 3, 1> av{relA.X().to<double>(), relA.Y().to<double>(),
relA.Z().to<double>()};
Eigen::Matrix<double, 3, 1> bv{relB.X().to<double>(), relB.Y().to<double>(),
relB.Z().to<double>()};
Eigen::Matrix<double, 3, 1> abv = bv - av;
bool aVisible = true;
bool bVisible = true;
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
double aVisibility = av.dot(normal);
if (aVisibility < 0) {
aVisible = false;
}
double bVisibility = bv.dot(normal);
if (bVisibility < 0) {
bVisible = false;
}
if (aVisibility <= 0 && bVisibility <= 0) {
return {std::nullopt, std::nullopt};
}
}
if (aVisible && bVisible) {
return {0, 1};
}
std::array<double, 4> intersections{std::nan(""), std::nan(""),
std::nan(""), std::nan("")};
std::vector<std::optional<Eigen::Matrix<double, 3, 1>>> ipts{
{std::nullopt, std::nullopt, std::nullopt, std::nullopt}};
for (size_t i = 0; i < viewplanes.size(); i++) {
Eigen::Matrix<double, 3, 1> normal = viewplanes[i];
Eigen::Matrix<double, 3, 1> a_projn{};
a_projn = (av.dot(normal) / normal.dot(normal)) * normal;
if (std::abs(abv.dot(normal)) < 1e-5) {
continue;
}
intersections[i] = a_projn.dot(a_projn) / -(abv.dot(a_projn));
Eigen::Matrix<double, 3, 1> apv{};
apv = intersections[i] * abv;
Eigen::Matrix<double, 3, 1> intersectpt{};
intersectpt = av + apv;
ipts[i] = intersectpt;
for (size_t j = 1; j < viewplanes.size(); j++) {
int oi = (i + j) % viewplanes.size();
Eigen::Matrix<double, 3, 1> onormal = viewplanes[oi];
if (intersectpt.dot(onormal) < 0) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
if (!ipts[i]) {
continue;
}
for (int j = i - 1; j >= 0; j--) {
std::optional<Eigen::Matrix<double, 3, 1>> oipt = ipts[j];
if (!oipt) {
continue;
}
Eigen::Matrix<double, 3, 1> diff{};
diff = oipt.value() - intersectpt;
if (diff.cwiseAbs().maxCoeff() < 1e-4) {
intersections[i] = std::nan("");
ipts[i] = std::nullopt;
break;
}
}
}
double inter1 = std::nan("");
double inter2 = std::nan("");
for (double inter : intersections) {
if (!std::isnan(inter)) {
if (std::isnan(inter1)) {
inter1 = inter;
} else {
inter2 = inter;
}
}
}
if (!std::isnan(inter2)) {
double max = std::max(inter1, inter2);
double min = std::min(inter1, inter2);
if (aVisible) {
min = 0;
}
if (bVisible) {
max = 1;
}
return {min, max};
} else if (!std::isnan(inter1)) {
if (aVisible) {
return {0, inter1};
}
if (bVisible) {
return {inter1, 1};
}
return {inter1, std::nullopt};
} else {
return {std::nullopt, std::nullopt};
}
}
const frc::Translation3d& b) const;
std::vector<cv::Point2f> EstPixelNoise(
const std::vector<cv::Point2f>& points) {

137
photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h
View File

@@ -1,137 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <algorithm>
#include <functional>
#include <memory>
#include <string>
#include <vector>
#include <networktables/NetworkTableInstance.h>
#include "photon/PhotonCamera.h"
#include "photon/PhotonTargetSortMode.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimPhotonCamera : public PhotonCamera {
public:
SimPhotonCamera(nt::NetworkTableInstance instance,
const std::string& cameraName)
: PhotonCamera(instance, cameraName) {
latencyMillisEntry = rootTable->GetEntry("latencyMillis");
hasTargetEntry = rootTable->GetEntry("hasTargetEntry");
targetPitchEntry = rootTable->GetEntry("targetPitchEntry");
targetYawEntry = rootTable->GetEntry("targetYawEntry");
targetAreaEntry = rootTable->GetEntry("targetAreaEntry");
targetSkewEntry = rootTable->GetEntry("targetSkewEntry");
targetPoseEntry = rootTable->GetEntry("targetPoseEntry");
rawBytesPublisher = rootTable->GetRawTopic("rawBytes").Publish("rawBytes");
versionEntry = instance.GetTable("photonvision")->GetEntry("version");
// versionEntry.SetString(PhotonVersion.versionString);
}
explicit SimPhotonCamera(const std::string& cameraName)
: SimPhotonCamera(nt::NetworkTableInstance::GetDefault(), cameraName) {}
virtual ~SimPhotonCamera() = default;
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latency Latency of the provided frame
* @param targetList List of targets detected
*/
void SubmitProcessedFrame(units::millisecond_t latency,
std::vector<PhotonTrackedTarget> targetList) {
SubmitProcessedFrame(latency, PhotonTargetSortMode::LeftMost(), targetList);
}
/**
* Simulate one processed frame of vision data, putting one result to NT.
*
* @param latency Latency of the provided frame
* @param sortMode Order in which to sort targets
* @param targetList List of targets detected
*/
void SubmitProcessedFrame(
units::millisecond_t latency,
std::function<bool(const PhotonTrackedTarget& target1,
const PhotonTrackedTarget& target2)>
sortMode,
std::vector<PhotonTrackedTarget> targetList) {
latencyMillisEntry.SetDouble(latency.to<double>());
std::sort(targetList.begin(), targetList.end(),
[&](auto lhs, auto rhs) { return sortMode(lhs, rhs); });
PhotonPipelineResult newResult{0, 0_s, latency, targetList};
Packet packet{};
packet << newResult;
rawBytesPublisher.Set(
std::span{packet.GetData().data(), packet.GetDataSize()});
bool hasTargets = newResult.HasTargets();
hasTargetEntry.SetBoolean(hasTargets);
if (!hasTargets) {
targetPitchEntry.SetDouble(0.0);
targetYawEntry.SetDouble(0.0);
targetAreaEntry.SetDouble(0.0);
targetPoseEntry.SetDoubleArray(
std::vector<double>{0.0, 0.0, 0.0, 0, 0, 0, 0});
targetSkewEntry.SetDouble(0.0);
} else {
PhotonTrackedTarget bestTarget = newResult.GetBestTarget();
targetPitchEntry.SetDouble(bestTarget.GetPitch());
targetYawEntry.SetDouble(bestTarget.GetYaw());
targetAreaEntry.SetDouble(bestTarget.GetArea());
targetSkewEntry.SetDouble(bestTarget.GetSkew());
frc::Transform3d transform = bestTarget.GetBestCameraToTarget();
targetPoseEntry.SetDoubleArray(std::vector<double>{
transform.X().to<double>(), transform.Y().to<double>(),
transform.Z().to<double>(), transform.Rotation().GetQuaternion().W(),
transform.Rotation().GetQuaternion().X(),
transform.Rotation().GetQuaternion().Y(),
transform.Rotation().GetQuaternion().Z()});
}
}
private:
nt::NetworkTableEntry latencyMillisEntry;
nt::NetworkTableEntry hasTargetEntry;
nt::NetworkTableEntry targetPitchEntry;
nt::NetworkTableEntry targetYawEntry;
nt::NetworkTableEntry targetAreaEntry;
nt::NetworkTableEntry targetSkewEntry;
nt::NetworkTableEntry targetPoseEntry;
nt::NetworkTableEntry versionEntry;
nt::RawPublisher rawBytesPublisher;
};
} // namespace photon

228
photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h
View File

@@ -1,228 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <string>
#include <vector>
#include <frc/smartdashboard/Field2d.h>
#include <frc/smartdashboard/SmartDashboard.h>
#include <units/angle.h>
#include <units/area.h>
#include "SimPhotonCamera.h"
#include "SimVisionTarget.h"
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimVisionSystem {
public:
SimPhotonCamera cam;
units::radian_t camHorizFOV{0};
units::radian_t camVertFOV{0};
units::meter_t maxLEDRange{0};
int cameraResWidth{0};
int cameraResHeight{0};
double minTargetArea{0.0};
frc::Transform3d cameraToRobot;
frc::Field2d dbgField;
frc::FieldObject2d* dbgRobot;
frc::FieldObject2d* dbgCamera;
std::vector<SimVisionTarget> targetList;
/**
* Create a simulated vision system involving a camera and coprocessor mounted
* on a mobile robot running PhotonVision, detecting one or more targets
* scattered around the field. This assumes a fairly simple and
* distortion-less pinhole camera model.
*
* @param camName Name of the PhotonVision camera to create. Align it with the
* settings you use in the PhotonVision GUI.
* @param camDiagFOV Diagonal Field of View of the camera used. Align it with
* the manufacturer specifications, and/or whatever is configured in the
* PhotonVision Setting page.
* @param cameraToRobot Transform to move from the camera's mount position to
* the robot's position
* @param maxLEDRange Maximum distance at which your camera can illuminate the
* target and make it visible. Set to 9000 or more if your vision system does
* not rely on LED's.
* @param cameraResWidth Width of your camera's image sensor in pixels
* @param cameraResHeight Height of your camera's image sensor in pixels
* @param minTargetArea Minimum area that that the target should be before
* it's recognized as a target by the camera. Match this with your contour
* filtering settings in the PhotonVision GUI.
*/
SimVisionSystem(std::string camName, units::degree_t camDiagFOV,
frc::Transform3d cameraToRobot, units::meter_t maxLEDRange,
int cameraResWidth, int cameraResHeight, double minTargetArea)
: cam(camName),
camHorizFOV((camDiagFOV * cameraResWidth) /
std::hypot(cameraResWidth, cameraResHeight)),
camVertFOV((camDiagFOV * cameraResHeight) /
std::hypot(cameraResWidth, cameraResHeight)),
maxLEDRange(maxLEDRange),
cameraResWidth(cameraResWidth),
cameraResHeight(cameraResHeight),
minTargetArea(minTargetArea),
cameraToRobot(cameraToRobot),
dbgField(),
dbgRobot(dbgField.GetRobotObject()),
dbgCamera(dbgField.GetObject(camName + " Camera")) {
frc::SmartDashboard::PutData(camName + " Sim Field", &dbgField);
}
/**
* Add a target on the field which your vision system is designed to detect.
* The PhotonCamera from this system will report the location of the robot
* relative to the subset of these targets which are visible from the given
* robot position.
*
* @param target Target to add to the simulated field
*/
void AddSimVisionTarget(SimVisionTarget target) {
targetList.push_back(target);
dbgField.GetObject("Target " + std::to_string(target.targetId))
->SetPose(target.targetPose.ToPose2d());
}
/**
* Clears all sim vision targets.
* This is useful for switching alliances and needing to repopulate the sim
* targets. NOTE: Old targets will still show on the Field2d unless
* overwritten by new targets with the same ID
*/
void ClearVisionTargets() { targetList.clear(); }
/**
* Adjust the camera position relative to the robot. Use this if your camera
* is on a gimbal or turret or some other mobile platform.
*
* @param newCameraToRobot New Transform from the robot to the camera
*/
void MoveCamera(frc::Transform3d newCameraToRobot) {
cameraToRobot = newCameraToRobot;
}
/**
* Periodic update. Call this once per frame of image data you wish to process
* and send to NetworkTables
*
* @param robotPose current pose of the robot on the field. Will be used to
* calculate which targets are actually in view, where they are at relative to
* the robot, and relevant PhotonVision parameters.
*/
void ProcessFrame(frc::Pose2d robotPose) {
ProcessFrame(frc::Pose3d{
robotPose.X(), robotPose.Y(), 0.0_m,
frc::Rotation3d{0_rad, 0_rad, robotPose.Rotation().Radians()}});
}
/**
* Periodic update. Call this once per frame of image data you wish to process
* and send to NetworkTables
*
* @param robotPose current pose of the robot in space. Will be used to
* calculate which targets are actually in view, where they are at relative to
* the robot, and relevant PhotonVision parameters.
*/
void ProcessFrame(frc::Pose3d robotPose) {
frc::Pose3d cameraPose = robotPose.TransformBy(cameraToRobot.Inverse());
dbgRobot->SetPose(robotPose.ToPose2d());
dbgCamera->SetPose(cameraPose.ToPose2d());
std::vector<PhotonTrackedTarget> visibleTargetList{};
for (const auto& target : targetList) {
frc::Transform3d camToTargetTransform{cameraPose, target.targetPose};
frc::Translation3d camToTargetTranslation{
camToTargetTransform.Translation()};
frc::Translation3d altTranslation{camToTargetTranslation.X(),
-1.0 * camToTargetTranslation.Y(),
camToTargetTranslation.Z()};
frc::Rotation3d altRotation{camToTargetTransform.Rotation() * -1.0};
frc::Transform3d camToTargetAltTransform{altTranslation, altRotation};
units::meter_t dist{camToTargetTranslation.Norm()};
double areaPixels{target.targetArea / GetM2PerPx(dist)};
units::radian_t yaw{units::math::atan2(camToTargetTranslation.Y(),
camToTargetTranslation.X())};
units::meter_t cameraHeightOffGround{cameraPose.Z()};
units::meter_t targetHeightAboveGround(target.targetPose.Z());
units::radian_t camPitch{cameraPose.Rotation().Y()};
frc::Transform2d transformAlongGround{cameraPose.ToPose2d(),
target.targetPose.ToPose2d()};
units::meter_t distanceAlongGround{
transformAlongGround.Translation().Norm()};
units::radian_t pitch =
units::math::atan2(targetHeightAboveGround - cameraHeightOffGround,
distanceAlongGround) -
camPitch;
if (CamCamSeeTarget(dist, yaw, pitch, areaPixels)) {
visibleTargetList.push_back(
PhotonTrackedTarget{yaw.convert<units::degree>().to<double>(),
pitch.convert<units::degree>().to<double>(),
areaPixels,
0.0,
target.targetId,
-1,
-1,
camToTargetTransform,
// TODO sim alternate pose
camToTargetTransform,
// TODO ambiguity
0.0,
{{0, 0}, {0, 0}, {0, 0}, {0, 0}},
{{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
}
cam.SubmitProcessedFrame(0_s, visibleTargetList);
}
}
units::square_meter_t GetM2PerPx(units::meter_t dist) {
units::meter_t widthMPerPx =
2 * dist * units::math::tan(camHorizFOV / 2) / cameraResWidth;
units::meter_t heightMPerPx =
2 * dist * units::math::tan(camVertFOV / 2) / cameraResHeight;
return widthMPerPx * heightMPerPx;
}
bool CamCamSeeTarget(units::meter_t dist, units::radian_t yaw,
units::radian_t pitch, double area) {
bool inRange = dist < maxLEDRange;
bool inHorizAngle = units::math::abs(yaw) < camHorizFOV / 2;
bool inVertAngle = units::math::abs(pitch) < camVertFOV / 2;
bool targetBigEnough = area > minTargetArea;
return (inRange && inHorizAngle && inVertAngle && targetBigEnough);
}
};
} // namespace photon

65
photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h
View File

@@ -1,65 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#pragma once
#include <frc/geometry/Pose3d.h>
#include <units/area.h>
#include "photon/dataflow/structures/Packet.h"
#include "photon/targeting/MultiTargetPNPResult.h"
#include "photon/targeting/PNPResult.h"
#include "photon/targeting/PhotonPipelineResult.h"
#include "photon/targeting/PhotonTrackedTarget.h"
namespace photon {
class SimVisionTarget {
public:
SimVisionTarget() = default;
/**
* Describes a vision target located somewhere on the field that your
* SimVisionSystem can detect.
*
* @param targetPose Pose3d of the target in field-relative coordinates
* @param targetWidth Width of the outer bounding box of the target.
* @param targetHeight Pair Height of the outer bounding box of the
* target.
* @param targetId Id of the target
*/
SimVisionTarget(frc::Pose3d targetPose, units::meter_t targetWidth,
units::meter_t targetHeight, int targetId)
: targetPose(targetPose),
targetWidth(targetWidth),
targetHeight(targetHeight),
targetArea(targetHeight * targetWidth),
targetId(targetId) {}
frc::Pose3d targetPose;
units::meter_t targetWidth;
units::meter_t targetHeight;
units::square_meter_t targetArea;
int targetId;
};
} // namespace photon

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

@@ -46,6 +46,9 @@ class VisionTargetSim {
}
int fiducialId;
int objDetClassId = -1;
float objDetConf = -1;
bool operator<(const VisionTargetSim& right) const {
return pose.Translation().Norm() < right.pose.Translation().Norm();
}

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

@@ -133,10 +133,9 @@ class PhotonPoseEstimatorTest {
cameraOne.result.setRecieveTimestampMicros((long) (11 * 1e6));
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d());
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(11, estimatedPose.get().timestampSeconds);
@@ -224,10 +223,9 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
cameraOne,
new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(4, estimatedPose.get().timestampSeconds);
@@ -314,11 +312,10 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(17, estimatedPose.get().timestampSeconds);
@@ -405,12 +402,11 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.CLOSEST_TO_LAST_POSE,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
cameraOne.result =
@@ -484,7 +480,7 @@ class PhotonPoseEstimatorTest {
new TargetCorner(7, 8)))));
cameraOne.result.setRecieveTimestampMicros((long) (7 * 1e6));
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
pose = estimatedPose.get().estimatedPose;
assertEquals(7, estimatedPose.get().timestampSeconds);
@@ -529,25 +525,24 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
// Empty result, expect empty result
cameraOne.result = new PhotonPipelineResult();
cameraOne.result.setRecieveTimestampMicros((long) (1 * 1e6));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Set actual result
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertTrue(estimatedPose.isPresent());
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
// And again -- pose cache should mean this is empty
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());
// Expect the old timestamp to still be here
assertEquals(20, estimator.poseCacheTimestampSeconds);
@@ -557,7 +552,7 @@ class PhotonPoseEstimatorTest {
assertEquals(-1, estimator.poseCacheTimestampSeconds);
// Update should cache the current timestamp (20) again
cameraOne.result = result;
estimatedPose = estimator.update();
estimatedPose = estimator.update(cameraOne.result);
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
assertEquals(20, estimator.poseCacheTimestampSeconds);
}
@@ -640,10 +635,9 @@ class PhotonPoseEstimatorTest {
new PhotonPoseEstimator(
aprilTags,
PoseStrategy.AVERAGE_BEST_TARGETS,
cameraOne,
new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
Optional<EstimatedRobotPose> estimatedPose = estimator.update();
Optional<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
Pose3d pose = estimatedPose.get().estimatedPose;
assertEquals(20, estimatedPose.get().timestampSeconds, .01);
@@ -659,6 +653,11 @@ class PhotonPoseEstimatorTest {
PhotonPipelineResult result;
@Override
public List<PhotonPipelineResult> getAllUnreadResults() {
return List.of(result);
}
@Override
public PhotonPipelineResult getLatestResult() {
return result;

10
photon-lib/src/test/java/org/photonvision/estimation/ApriltagWorkbenchTest.java
View File

@@ -61,7 +61,6 @@ public class ApriltagWorkbenchTest {
new PhotonPoseEstimator(
tagLayout,
PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
cam,
robotToCamera);
var field = new Field2d();
@@ -70,10 +69,11 @@ public class ApriltagWorkbenchTest {
while (!Thread.interrupted()) {
Thread.sleep(500);
var ret = pe.update();
System.out.println(ret);
field.setRobotPose(ret.get().estimatedPose.toPose2d());
for (var change : cam.getAllUnreadResults()) {
var ret = pe.update(change);
System.out.println(ret);
field.setRobotPose(ret.get().estimatedPose.toPose2d());
}
}
}
}

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

@@ -83,12 +83,16 @@ TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(11));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(11));
photon::PhotonPoseEstimator estimator(aprilTags, photon::LOWEST_AMBIGUITY,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
frc::Transform3d{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -138,13 +142,17 @@ TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(17_s);
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(17_s);
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
{{0_m, 0_m, 4_m}, {}});
auto estimatedPose = estimator.Update();
aprilTags, photon::CLOSEST_TO_CAMERA_HEIGHT, {{0_m, 0_m, 4_m}, {}});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -181,14 +189,19 @@ TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(
aprilTags, photon::CLOSEST_TO_REFERENCE_POSE, std::move(cameraOne), {});
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)));
auto estimatedPose = estimator.Update();
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -225,14 +238,19 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_s, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(17));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(17));
photon::PhotonPoseEstimator estimator(aprilTags, photon::CLOSEST_TO_LAST_POSE,
std::move(cameraOne), {});
{});
estimator.SetLastPose(
frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
auto estimatedPose = estimator.Update();
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
ASSERT_TRUE(estimatedPose);
frc::Pose3d pose = estimatedPose.value().estimatedPose;
@@ -259,10 +277,14 @@ TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
frc::Rotation3d(0_rad, 0_rad, 0_rad)),
0.4, corners, detectedCorners}};
estimator.GetCamera()->testResult = {0, 0_s, 2_ms, targetsThree};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(21));
cameraOne.testResult = {{0, 0_s, 2_ms, targetsThree}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(21));
// std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
estimatedPose = estimator.Update();
ASSERT_TRUE(estimatedPose);
pose = estimatedPose.value().estimatedPose;
@@ -300,12 +322,17 @@ TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
0.4, corners, detectedCorners}};
cameraOne.test = true;
cameraOne.testResult = {0, 0_ms, 2_ms, targets};
cameraOne.testResult.SetRecieveTimestamp(units::second_t(15));
cameraOne.testResult = {{0, 0_s, 2_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
auto estimatedPose = estimator.Update();
{});
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
frc::Pose3d pose = estimatedPose.value().estimatedPose;
EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
@@ -344,23 +371,35 @@ TEST(PhotonPoseEstimatorTest, PoseCache) {
cameraOne.test = true;
photon::PhotonPoseEstimator estimator(aprilTags, photon::AVERAGE_BEST_TARGETS,
std::move(cameraOne), {});
{});
// empty input, expect empty out
estimator.GetCamera()->testResult = {0, 0_s, 2_ms, {}};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(1));
auto estimatedPose = estimator.Update();
cameraOne.testResult = {{0, 0_s, 2_ms, {}}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(1));
std::optional<photon::EstimatedRobotPose> estimatedPose;
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
// Set result, and update -- expect present and timestamp to be 15
estimator.GetCamera()->testResult = {0, 0_s, 3_ms, targets};
estimator.GetCamera()->testResult.SetRecieveTimestamp(units::second_t(15));
estimatedPose = estimator.Update();
cameraOne.testResult = {{0, 0_s, 3_ms, targets}};
cameraOne.testResult[0].SetRecieveTimestamp(units::second_t(15));
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_TRUE(estimatedPose);
EXPECT_NEAR((15_s - 3_ms).to<double>(),
estimatedPose.value().timestamp.to<double>(), 1e-6);
// And again -- now pose cache should be empty
estimatedPose = estimator.Update();
for (const auto& result : cameraOne.GetAllUnreadResults()) {
estimatedPose = estimator.Update(result);
}
EXPECT_FALSE(estimatedPose);
}

360
photon-lib/src/test/native/cpp/SimVisionSystemTest.cpp
View File

@@ -1,360 +0,0 @@
/*
* MIT License
*
* Copyright (c) PhotonVision
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include <tuple>
#include "gtest/gtest.h"
#include "photon/PhotonUtils.h"
#include "photon/simulation/SimVisionSystem.h"
class SimVisionSystemTest : public ::testing::Test {
void SetUp() override {
nt::NetworkTableInstance::GetDefault().StartServer();
photon::PhotonCamera::SetVersionCheckEnabled(false);
}
void TearDown() override {}
};
class SimVisionSystemTestWithParamsTest
: public SimVisionSystemTest,
public testing::WithParamInterface<units::degree_t> {};
class SimVisionSystemTestDistanceParamsTest
: public SimVisionSystemTest,
public testing::WithParamInterface<
std::tuple<units::foot_t, units::degree_t, units::foot_t>> {};
TEST_F(SimVisionSystemTest, TestEmpty) {
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 320, 240, 0};
SUCCEED();
}
TEST_F(SimVisionSystemTest, TestVisibilityCupidShuffle) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 2_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3});
// To the right, to the right
frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{-70_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// To the right, to the right
robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{-95_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// To the left, to the left
robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{90_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// To the left, to the left
robotPose = frc::Pose2d{{5_m, 0_m}, frc::Rotation2d{65_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// now kick, now kick
robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
// now kick, now kick
robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
// now walk it by yourself
robotPose = frc::Pose2d{{2_m, 0_m}, frc::Rotation2d{-179_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// now walk it by yourself
sys.MoveCamera(frc::Transform3d{
frc::Translation3d{},
frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}});
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
}
TEST_F(SimVisionSystemTest, TestNotVisibleVertOne) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 3_m, 3});
frc::Pose2d robotPose{{5_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
sys.MoveCamera(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 5000_m},
frc::Rotation3d{0_rad, 0_rad, units::constants::detail::PI_VAL * 1_rad}});
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
}
// TEST_F(SimVisionSystemTest, TestNotVisibleVertTwo) {
// const frc::Pose3d targetPose{
// {15.98_m, 0_m, 2_m},
// frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL *
// 1_rad}};
// frc::Transform3d robotToCamera{
// frc::Translation3d{0_m, 0_m, 1_m},
// frc::Rotation3d{0_deg, (units::constants::detail::PI_VAL / 4) * 1_rad,
// 0_deg}};
// photon::SimVisionSystem sys{
// "Test", 80.0_deg, robotToCamera.Inverse(), 99999_m, 1234, 1234, 0};
// sys.AddSimVisionTarget(
// photon::SimVisionTarget{targetPose, 3_m, 0.5_m, 1736});
// frc::Pose2d robotPose{{14.98_m, 0_m}, frc::Rotation2d{5_deg}};
// sys.ProcessFrame(robotPose);
// ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
// robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m},
// frc::Rotation2d{5_deg}}; sys.ProcessFrame(robotPose);
// ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
// }
TEST_F(SimVisionSystemTest, TestNotVisibleTargetSize) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{
"Test", 80.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
sys.AddSimVisionTarget(
photon::SimVisionTarget{targetPose, 0.1_m, 0.025_m, 24});
frc::Pose2d robotPose{{12_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
}
TEST_F(SimVisionSystemTest, TestNotVisibleTooFarForLEDs) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{"Test", 80.0_deg, frc::Transform3d{}, 10_m, 640,
480, 1.0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 1_m, 0.25_m, 78});
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_TRUE(sys.cam.GetLatestResult().HasTargets());
robotPose = frc::Pose2d{frc::Translation2d{0_m, 0_m}, frc::Rotation2d{5_deg}};
sys.ProcessFrame(robotPose);
ASSERT_FALSE(sys.cam.GetLatestResult().HasTargets());
}
TEST_P(SimVisionSystemTestWithParamsTest, YawAngles) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg,
(3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{GetParam() * -1.0}};
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.ProcessFrame(robotPose);
auto results = sys.cam.GetLatestResult();
ASSERT_TRUE(results.HasTargets());
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(GetParam().to<double>(), target.GetYaw(), 0.0001);
}
TEST_P(SimVisionSystemTestWithParamsTest, PitchAngles) {
const frc::Pose3d targetPose{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg,
(3 * units::constants::detail::PI_VAL / 4) * 1_rad}};
frc::Pose2d robotPose{{10_m, 0_m}, frc::Rotation2d{0_deg}};
photon::SimVisionSystem sys{
"Test", 120.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 23});
sys.MoveCamera(frc::Transform3d{frc::Translation3d{},
frc::Rotation3d{0_deg, GetParam(), 0_deg}});
sys.ProcessFrame(robotPose);
auto results = sys.cam.GetLatestResult();
ASSERT_TRUE(results.HasTargets());
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(GetParam().to<double>(), target.GetPitch(), 0.0001);
}
INSTANTIATE_TEST_SUITE_P(AnglesTests, SimVisionSystemTestWithParamsTest,
testing::Values(-10_deg, -5_deg, -0_deg, -1_deg,
-2_deg, 5_deg, 7_deg, 10.23_deg,
20.21_deg, -19.999_deg));
TEST_P(SimVisionSystemTestDistanceParamsTest, DistanceCalc) {
units::foot_t distParam;
units::degree_t pitchParam;
units::foot_t heightParam;
std::tie(distParam, pitchParam, heightParam) = GetParam();
const frc::Pose3d targetPose{
{15.98_m, 0_m, 1_m},
frc::Rotation3d{0_deg, 0_deg,
(units::constants::detail::PI_VAL * 0.98) * 1_rad}};
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}};
photon::SimVisionSystem sys{
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysoho"
"wsyourdaygoingihopegoodhaveagreatrestofyourlife",
160.0_deg,
robotToCamera.Inverse(),
99999_m,
640,
480,
0};
sys.AddSimVisionTarget(photon::SimVisionTarget{targetPose, 0.5_m, 0.5_m, 0});
sys.ProcessFrame(robotPose);
auto results = sys.cam.GetLatestResult();
ASSERT_TRUE(results.HasTargets());
photon::PhotonTrackedTarget target = results.GetBestTarget();
ASSERT_NEAR(target.GetYaw(), 0.0, 0.0001);
units::meter_t dist = photon::PhotonUtils::CalculateDistanceToTarget(
robotToCamera.Z(), targetPose.Z(), pitchParam,
units::degree_t{target.GetPitch()});
ASSERT_NEAR(dist.to<double>(),
distParam.convert<units::meters>().to<double>(), 0.001);
}
INSTANTIATE_TEST_SUITE_P(
DistanceParamsTests, SimVisionSystemTestDistanceParamsTest,
testing::Values(std::make_tuple(5_ft, 15.98_deg, 0_ft),
std::make_tuple(6_ft, 15.98_deg, 1_ft),
std::make_tuple(10_ft, 15.98_deg, 0_ft),
std::make_tuple(15_ft, 15.98_deg, 2_ft),
std::make_tuple(19.95_ft, 15.98_deg, 0_ft),
std::make_tuple(20_ft, 15.98_deg, 0_ft),
std::make_tuple(5_ft, 42_deg, 1_ft),
std::make_tuple(6_ft, 42_deg, 0_ft),
std::make_tuple(10_ft, 42_deg, 2_ft),
std::make_tuple(15_ft, 42_deg, 0.5_ft),
std::make_tuple(19.42_ft, 15.98_deg, 0_ft),
std::make_tuple(20_ft, 42_deg, 0_ft),
std::make_tuple(5_ft, 55_deg, 2_ft),
std::make_tuple(6_ft, 55_deg, 0_ft),
std::make_tuple(10_ft, 54_deg, 2.2_ft),
std::make_tuple(15_ft, 53_deg, 0_ft),
std::make_tuple(19.52_ft, 15.98_deg, 1.1_ft)));
TEST_F(SimVisionSystemTest, TestMultipleTargets) {
const frc::Pose3d targetPoseL{
{15.98_m, 2_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
const frc::Pose3d targetPoseC{
{15.98_m, 0_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
const frc::Pose3d targetPoseR{
{15.98_m, -2_m, 0_m},
frc::Rotation3d{0_deg, 0_deg, units::constants::detail::PI_VAL * 1_rad}};
photon::SimVisionSystem sys{
"Test", 160.0_deg, frc::Transform3d{}, 99999_m, 640, 480, 20.0};
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 1});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 2});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 3});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 4});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 5});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 1_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 6});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 7});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseC.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.5_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 8});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 9});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseR.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.75_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 10});
sys.AddSimVisionTarget(photon::SimVisionTarget{
targetPoseL.TransformBy(frc::Transform3d{
frc::Translation3d{0_m, 0_m, 0.25_m}, frc::Rotation3d{}}),
0.25_m, 0.25_m, 11});
frc::Pose2d robotPose{{6_m, 0_m}, frc::Rotation2d{.25_deg}};
sys.ProcessFrame(robotPose);
auto results = sys.cam.GetLatestResult();
ASSERT_TRUE(results.HasTargets());
auto targetList = results.GetTargets();
ASSERT_EQ(targetList.size(), size_t(11));
}

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

@@ -27,10 +27,15 @@
#include <tuple>
#include <vector>
#include <wpi/deprecated.h>
#include "gtest/gtest.h"
#include "photon/PhotonUtils.h"
#include "photon/simulation/VisionSystemSim.h"
// Ignore GetLatestResult warnings
WPI_IGNORE_DEPRECATED
class VisionSystemSimTest : public ::testing::Test {
void SetUp() override {
nt::NetworkTableInstance::GetDefault().StartServer();
@@ -229,9 +234,10 @@ TEST_P(VisionSystemSimTestWithParamsTest, YawAngles) {
robotPose =
frc::Pose2d{frc::Translation2d{10_m, 0_m}, frc::Rotation2d{GetParam()}};
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
ASSERT_NEAR(GetParam().to<double>(),
camera.GetLatestResult().GetBestTarget().GetYaw(), 0.25);
const auto result = camera.GetLatestResult();
ASSERT_TRUE(result.HasTargets());
ASSERT_NEAR(GetParam().to<double>(), result.GetBestTarget().GetYaw(), 0.25);
}
TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
@@ -257,9 +263,9 @@ TEST_P(VisionSystemSimTestWithParamsTest, PitchAngles) {
frc::Rotation3d{0_rad, units::degree_t{GetParam()}, 0_rad}});
visionSysSim.Update(robotPose);
ASSERT_TRUE(camera.GetLatestResult().HasTargets());
ASSERT_NEAR(GetParam().to<double>(),
camera.GetLatestResult().GetBestTarget().GetPitch(), 0.25);
const auto result = camera.GetLatestResult();
ASSERT_TRUE(result.HasTargets());
ASSERT_NEAR(GetParam().to<double>(), result.GetBestTarget().GetPitch(), 0.25);
}
INSTANTIATE_TEST_SUITE_P(AnglesTests, VisionSystemSimTestWithParamsTest,

65
photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java
View File

@@ -18,33 +18,84 @@
package org.photonvision.common.networktables;
import edu.wpi.first.networktables.RawSubscriber;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.dataflow.structures.PacketSerde;
public class PacketSubscriber<T> implements AutoCloseable {
public static class PacketResult<U> {
public final U value;
public final long timestamp;
public PacketResult(U value, long timestamp) {
this.value = value;
this.timestamp = timestamp;
}
public PacketResult() {
this(null, 0);
}
}
public final RawSubscriber subscriber;
private final PacketSerde<T> serde;
private final T defaultValue;
private final Packet packet = new Packet(1);
public PacketSubscriber(RawSubscriber subscriber, PacketSerde<T> serde, T defaultValue) {
/**
* Create a PacketSubscriber
*
* @param subscriber NT subscriber. Set pollStorage to 1 to make get() faster
* @param serde How we convert raw to actual things
*/
public PacketSubscriber(RawSubscriber subscriber, PacketSerde<T> serde) {
this.subscriber = subscriber;
this.serde = serde;
this.defaultValue = defaultValue;
}
public T get() {
/** Parse one chunk of timestamped data into T */
private PacketResult<T> parse(byte[] data, long timestamp) {
packet.clear();
packet.setData(data);
if (packet.getSize() < 1) {
return new PacketResult<T>();
}
packet.setData(subscriber.get(new byte[] {}));
if (packet.getSize() < 1) return defaultValue;
return new PacketResult<>(serde.unpack(packet), timestamp);
}
return serde.unpack(packet);
/**
* Get the latest value sent over NT. If the value has never been set, returns the provided
* default
*/
public PacketResult<T> get() {
// Get /all/ changes since last call to readQueue
var data = subscriber.getAtomic();
// Topic has never been published to?
if (data.timestamp == 0) {
return new PacketResult<>();
}
return parse(data.value, data.timestamp);
}
@Override
public void close() {
subscriber.close();
}
public List<PacketResult<T>> getAllChanges() {
List<PacketResult<T>> ret = new ArrayList<>();
// Get /all/ changes since last call to readQueue
var changes = subscriber.readQueue();
for (var change : changes) {
ret.add(parse(change.value, change.timestamp));
}
return ret;
}
}

2
photon-targeting/src/main/native/include/photon/estimation/OpenCVHelp.h
View File

@@ -51,6 +51,7 @@ static std::vector<cv::Point2f> GetConvexHull(
return convexPoints;
}
[[maybe_unused]]
static cv::RotatedRect GetMinAreaRect(const std::vector<cv::Point2f>& points) {
return cv::minAreaRect(points);
}
@@ -120,6 +121,7 @@ static std::vector<cv::Point3f> RotationToRVec(
return cv::boundingRect(points);
}
[[maybe_unused]]
static std::vector<cv::Point2f> ProjectPoints(
const Eigen::Matrix<double, 3, 3>& cameraMatrix,
const Eigen::Matrix<double, 8, 1>& distCoeffs,

1
photon-targeting/src/main/native/include/photon/estimation/VisionEstimation.h
View File

@@ -32,6 +32,7 @@
namespace photon {
namespace VisionEstimation {
[[maybe_unused]]
static std::vector<frc::AprilTag> GetVisibleLayoutTags(
const std::vector<PhotonTrackedTarget>& visTags,
const frc::AprilTagFieldLayout& layout) {

12
photonlib-cpp-examples/apriltagExample/src/main/include/PhotonCameraWrapper.h
View File

@@ -33,15 +33,21 @@
#include <frc/apriltag/AprilTagFields.h>
class PhotonCameraWrapper {
private:
photon::PhotonCamera camera{"WPI2023"};
public:
photon::PhotonPoseEstimator m_poseEstimator{
frc::LoadAprilTagLayoutField(frc::AprilTagField::k2023ChargedUp),
photon::MULTI_TAG_PNP_ON_RIO, std::move(photon::PhotonCamera{"WPI2023"}),
frc::Transform3d{}};
photon::MULTI_TAG_PNP_ON_RIO, frc::Transform3d{}};
inline std::optional<photon::EstimatedRobotPose> Update(
frc::Pose2d estimatedPose) {
m_poseEstimator.SetReferencePose(frc::Pose3d(estimatedPose));
return m_poseEstimator.Update();
std::optional<photon::EstimatedRobotPose> ret = std::nullopt;
for (const auto& change : camera.GetAllUnreadResults())
ret = m_poseEstimator.Update(change);
return ret;
}
};

46
photonlib-cpp-examples/swervedriveposeestsim/src/main/include/Vision.h
View File

@@ -58,37 +58,37 @@ class Vision {
cameraProp->SetAvgLatency(50_ms);
cameraProp->SetLatencyStdDev(15_ms);
cameraSim = std::make_shared<photon::PhotonCameraSim>(camera.get(),
*cameraProp.get());
cameraSim =
std::make_shared<photon::PhotonCameraSim>(&camera, *cameraProp.get());
visionSim->AddCamera(cameraSim.get(), constants::Vision::kRobotToCam);
cameraSim->EnableDrawWireframe(true);
}
}
photon::PhotonPipelineResult GetLatestResult() {
return camera->GetLatestResult();
}
photon::PhotonPipelineResult GetLatestResult() { return m_latestResult; }
std::optional<photon::EstimatedRobotPose> GetEstimatedGlobalPose() {
auto visionEst = photonEstimator.Update();
units::second_t latestTimestamp = camera->GetLatestResult().GetTimestamp();
bool newResult =
units::math::abs(latestTimestamp - lastEstTimestamp) > 1e-5_s;
if (frc::RobotBase::IsSimulation()) {
if (visionEst.has_value()) {
GetSimDebugField()
.GetObject("VisionEstimation")
->SetPose(visionEst.value().estimatedPose.ToPose2d());
} else {
if (newResult) {
std::optional<photon::EstimatedRobotPose> visionEst;
// Run each new pipeline result through our pose estimator
for (const auto& result : camera.GetAllUnreadResults()) {
// cache result and update pose estimator
auto visionEst = photonEstimator.Update(result);
m_latestResult = result;
// In sim only, add our vision estimate to the sim debug field
if (frc::RobotBase::IsSimulation()) {
if (visionEst.has_value()) {
GetSimDebugField()
.GetObject("VisionEstimation")
->SetPose(visionEst.value().estimatedPose.ToPose2d());
} else {
GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
}
}
}
if (newResult) {
lastEstTimestamp = latestTimestamp;
}
return visionEst;
}
@@ -141,10 +141,12 @@ class Vision {
photon::PhotonPoseEstimator photonEstimator{
constants::Vision::kTagLayout,
photon::PoseStrategy::MULTI_TAG_PNP_ON_COPROCESSOR,
photon::PhotonCamera{"photonvision"}, constants::Vision::kRobotToCam};
std::shared_ptr<photon::PhotonCamera> camera{photonEstimator.GetCamera()};
constants::Vision::kRobotToCam};
photon::PhotonCamera camera{"photonvision"};
std::unique_ptr<photon::VisionSystemSim> visionSim;
std::unique_ptr<photon::SimCameraProperties> cameraProp;
std::shared_ptr<photon::PhotonCameraSim> cameraSim;
units::second_t lastEstTimestamp{0_s};
// The most recent result, cached for calculating std devs
photon::PhotonPipelineResult m_latestResult;
};

31
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/Vision.java
View File

@@ -46,7 +46,6 @@ import org.photonvision.targeting.PhotonPipelineResult;
public class Vision {
private final PhotonCamera camera;
private final PhotonPoseEstimator photonEstimator;
private double lastEstTimestamp = 0;
// Simulation
private PhotonCameraSim cameraSim;
@@ -56,8 +55,7 @@ public class Vision {
camera = new PhotonCamera(kCameraName);
photonEstimator =
new PhotonPoseEstimator(
kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, kRobotToCam);
new PhotonPoseEstimator(kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, kRobotToCam);
photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
// ----- Simulation
@@ -95,20 +93,21 @@ public class Vision {
* used for estimation.
*/
public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
var visionEst = photonEstimator.update();
double latestTimestamp = camera.getLatestResult().getTimestampSeconds();
boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5;
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(
est ->
getSimDebugField()
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
if (newResult) getSimDebugField().getObject("VisionEstimation").setPoses();
});
Optional<EstimatedRobotPose> visionEst = Optional.empty();
for (var change : camera.getAllUnreadResults()) {
visionEst = photonEstimator.update(change);
if (Robot.isSimulation()) {
visionEst.ifPresentOrElse(
est ->
getSimDebugField()
.getObject("VisionEstimation")
.setPose(est.estimatedPose.toPose2d()),
() -> {
getSimDebugField().getObject("VisionEstimation").setPoses();
});
}
}
if (newResult) lastEstTimestamp = latestTimestamp;
return visionEst;
}

9
photonlib-java-examples/swervedriveposeestsim/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java
View File

@@ -24,6 +24,7 @@
package frc.robot.subsystems.drivetrain;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
@@ -114,18 +115,18 @@ public class SwerveDriveSim {
SwerveDriveKinematics kinematics) {
this(
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -driveFF.kv / driveFF.ka),
VecBuilder.fill(0.0, 1.0 / driveFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
driveFF.ks,
driveMotor,
driveGearing,
driveWheelRadius,
new LinearSystem<N2, N1, N2>(
Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka),
MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -steerFF.kv / steerFF.ka),
VecBuilder.fill(0.0, 1.0 / steerFF.ka),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0),
VecBuilder.fill(0.0, 0.0)),
steerFF.ks,
steerMotor,