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

Fix javadoc warnings (#2266)

Browse Source 
Persuant to #1093, I added as many docstrings as I could, at least for
things I knew about. Some of the classes I just suppressed the Javadoc
warnings in because they aren't particularly useful to document. This
gets us down to less than 100 Javadoc warnings in total. Docs for core
classes on the C++ side were also added for parity.
This commit is contained in:
Gold856
2026-01-11 14:25:49 -05:00
committed by GitHub
parent a5dc9ec0d6
commit e4749a3ea9
39 changed files with 1306 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -179,20 +179,29 @@ class PhotonCamera {
using DistortionMatrix = Eigen::Matrix<double, 8, 1>;
/**
* @brief Get the camera calibration matrix, in standard OpenCV form
* Get the camera calibration matrix, in standard OpenCV form
*
* @return std::optional<cv::Mat>
*/
std::optional<CameraMatrix> GetCameraMatrix();
/**
* @brief Get the camera calibration distortion coefficients, in OPENCV8 form.
* Higher order terms are set to zero.
* Returns the camera calibration's distortion coefficients, in OPENCV8 form.
* Higher-order terms are set to 0
*
* @return std::optional<cv::Mat>
* @return The distortion coefficients in a 8x1 matrix, if they are published
* by the camera. Empty otherwise.
*/
std::optional<DistortionMatrix> GetDistCoeffs();
/**
* Sets whether or not coprocessor version checks will occur. Setting this to
* true will silence all console warnings about coproccessor connection, so be
* careful when enabling this and ensure all your coprocessors are
* communicating to the robot properly and everything has matching versions.
*
* @param enabled Whether or not to enable coprocessor version checks
*/
static void SetVersionCheckEnabled(bool enabled);
std::shared_ptr<nt::NetworkTable> GetCameraTable() const { return rootTable; }

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

@@ -48,42 +48,188 @@
namespace photon {
class PhotonCameraSim {
public:
/**
* Constructs a handle for simulating PhotonCamera values. Processing
* simulated targets through this class will change the associated
* PhotonCamera's results.
*
* WARNING: This constructor's camera has a 90 deg FOV with no simulated lag!
*
* By default, the minimum target area is 100 pixels and there is no
* maximum sight range.
*
* @param camera The camera to be simulated
*/
explicit PhotonCameraSim(PhotonCamera* camera);
/**
* Constructs a handle for simulating PhotonCamera values. Processing
* simulated targets through this class will change the associated
* PhotonCamera's results.
*
* By default, the minimum target area is 100 pixels and there is no
* maximum sight range.
*
* @param camera The camera to be simulated
* @param prop Properties of this camera such as FOV and FPS
* @param tagLayout The AprilTagFieldLayout used to solve for tag
* positions.
*/
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
const frc::AprilTagFieldLayout& tagLayout =
frc::AprilTagFieldLayout::LoadField(
frc::AprilTagField::kDefaultField));
/**
* Constructs a handle for simulating PhotonCamera values. Processing
* simulated targets through this class will change the associated
* PhotonCamera's results.
*
* @param camera The camera to be simulated
* @param prop Properties of this camera such as FOV and FPS
* @param minTargetAreaPercent The minimum percentage (0 - 100) a detected
* target must take up of the camera's image to be processed. Match this with
* your contour filtering settings in the PhotonVision GUI.
* @param maxSightRange Maximum distance at which the target is
* illuminated to your camera. Note that minimum target area of the image is
* separate from this.
*/
PhotonCameraSim(PhotonCamera* camera, const SimCameraProperties& props,
double minTargetAreaPercent, units::meter_t maxSightRange);
/**
* Returns the camera being simulated.
*
* @return The camera
*/
inline PhotonCamera* GetCamera() { return cam; }
/**
* Returns the minimum percentage (0 - 100) a detected target must take up of
* the camera's image to be processed.
*
* @return The percentage
*/
inline double GetMinTargetAreaPercent() { return minTargetAreaPercent; }
/**
* Returns the minimum number of pixels a detected target must take up in the
* camera's image to be processed.
*
* @return The number of pixels
*/
inline double GetMinTargetAreaPixels() {
return minTargetAreaPercent / 100.0 * prop.GetResArea();
}
/**
* Returns the maximum distance at which the target is illuminated to your
* camera. Note that minimum target area of the image is separate from this.
*
* @return The distance
*/
inline units::meter_t GetMaxSightRange() { return maxSightRange; }
inline const cs::CvSource& GetVideoSimRaw() { return videoSimRaw; }
inline const cv::Mat& GetVideoSimFrameRaw() { return videoSimFrameRaw; }
/**
* Determines if this target's pose should be visible to the camera without
* considering its projected image points. Does not account for image area.
*
* @param camPose Camera's 3d pose
* @param target Vision target containing pose and shape
* @return If this vision target can be seen before image projection.
*/
bool CanSeeTargetPose(const frc::Pose3d& camPose,
const VisionTargetSim& target);
/**
* Determines if all target points are inside the camera's image.
*
* @param points The target's 2d image points
* @return True if all the target points are inside the camera's image, false
* otherwise.
*/
bool CanSeeCorner(const std::vector<cv::Point2f>& points);
/**
* Determine if this camera should process a new frame based on performance
* metrics and the time since the last update. This returns an Optional which
* is either empty if no update should occur or a Long of the timestamp in
* microseconds of when the frame which should be received by NT. If a
* timestamp is returned, the last frame update time becomes that timestamp.
*
* @return Optional long which is empty while blocked or the NT entry
* timestamp in microseconds if ready
*/
std::optional<uint64_t> ConsumeNextEntryTime();
/**
* Sets the minimum percentage (0 - 100) a detected target must take up of the
* camera's image to be processed.
*
* @param areaPercent The percentage
*/
inline void SetMinTargetAreaPercent(double areaPercent) {
minTargetAreaPercent = areaPercent;
}
/**
* Sets the minimum number of pixels a detected target must take up in the
* camera's image to be processed.
*
* @param areaPx The number of pixels
*/
inline void SetMinTargetAreaPixels(double areaPx) {
minTargetAreaPercent = areaPx / prop.GetResArea() * 100;
}
/**
* Sets the maximum distance at which the target is illuminated to your
* camera. Note that minimum target area of the image is separate from this.
*
* @param rangeMeters The distance
*/
inline void SetMaxSightRange(units::meter_t range) { maxSightRange = range; }
/**
* Sets whether the raw video stream simulation is enabled.
*
* Note: This may increase loop times.
*
* @param enabled Whether or not to enable the raw video stream
*/
inline void EnableRawStream(bool enabled) { videoSimRawEnabled = enabled; }
/**
* Sets whether a wireframe of the field is drawn to the raw video stream.
*
* Note: This will dramatically increase loop times.
*
* @param enabled Whether or not to enable the wireframe in the raw video
* stream
*/
inline void EnableDrawWireframe(bool enabled) {
videoSimWireframeEnabled = enabled;
}
/**
* Sets the resolution of the drawn wireframe if enabled. Drawn line segments
* will be subdivided into smaller segments based on a threshold set by the
* resolution.
*
* @param resolution Resolution as a fraction(0 - 1) of the video frame's
* diagonal length in pixels
*/
inline void SetWireframeResolution(double resolution) {
videoSimWireframeResolution = resolution;
}
/**
* Sets whether the processed video stream simulation is enabled.
*
* @param enabled Whether or not to enable the processed video stream
*/
inline void EnabledProcessedStream(double enabled) {
videoSimProcEnabled = enabled;
}

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

@@ -40,9 +40,36 @@
#include <units/time.h>
namespace photon {
/**
* Calibration and performance values for this camera.
*
* The resolution will affect the accuracy of projected(3d to 2d) target
* corners and similarly the severity of image noise on estimation(2d to 3d).
*
* The camera intrinsics and distortion coefficients describe the results of
* calibration, and how to map between 3d field points and 2d image points.
*
* The performance values (framerate/exposure time, latency) determine how
* often results should be updated and with how much latency in simulation. High
* exposure time causes motion blur which can inhibit target detection while
* moving. Note that latency estimation does not account for network latency and
* the latency reported will always be perfect.
*/
class SimCameraProperties {
public:
/** Default constructor which is the same as PERFECT_90DEG */
SimCameraProperties() { SetCalibration(960, 720, frc::Rotation2d{90_deg}); }
/**
* Reads camera properties from a PhotonVision config.json file.
* This is only the resolution, camera intrinsics, distortion coefficients,
* and average/std. dev. pixel error. Other camera properties must be set.
*
* @param path Path to the config.json file
* @param width The width of the desired resolution in the JSON
* @param height The height of the desired resolution in the JSON
*/
SimCameraProperties(std::string path, int width, int height) {}
void SetCalibration(int width, int height, frc::Rotation2d fovDiag);
@@ -55,27 +82,65 @@ class SimCameraProperties {
errorStdDevPx = newErrorStdDevPx;
}
/**
* Sets the simulated FPS for this camera.
*
* @param fps The average frames per second the camera should process at.
* **Exposure time limits FPS if set!**
*/
void SetFPS(units::hertz_t fps) {
frameSpeed = units::math::max(1 / fps, exposureTime);
}
void SetExposureTime(units::second_t newExposureTime) {
exposureTime = newExposureTime;
frameSpeed = units::math::max(frameSpeed, exposureTime);
/**
* Sets the simulated exposure time for this camera.
*
* @param exposureTime The amount of time the "shutter" is open for one frame.
* Affects motion blur. **Frame speed(from FPS) is limited to this!**
*/
void SetExposureTime(units::second_t exposureTime) {
this->exposureTime = exposureTime;
frameSpeed = units::math::max(frameSpeed, this->exposureTime);
}
void SetAvgLatency(units::second_t newAvgLatency) {
avgLatency = newAvgLatency;
/**
* Sets the simulated latency for this camera.
*
* @param avgLatency The average latency (from image capture to data
* published) a frame should have
*/
void SetAvgLatency(units::second_t avgLatency) {
this->avgLatency = avgLatency;
}
void SetLatencyStdDev(units::second_t newLatencyStdDev) {
latencyStdDev = newLatencyStdDev;
/**
* Sets the simulated latency variation for this camera.
*
* @param latencyStdDev The standard deviation of the latency
*/
void SetLatencyStdDev(units::second_t latencyStdDev) {
this->latencyStdDev = latencyStdDev;
}
/**
* Gets the width of the simulated camera image.
*
* @return The width in pixels
*/
int GetResWidth() const { return resWidth; }
/**
* Gets the height of the simulated camera image.
*
* @return The height in pixels
*/
int GetResHeight() const { return resHeight; }
/**
* Gets the area of the simulated camera image.
*
* @return The area in pixels
*/
int GetResArea() const { return resWidth * resHeight; }
double GetAspectRatio() const {
@@ -84,23 +149,63 @@ class SimCameraProperties {
Eigen::Matrix<double, 3, 3> GetIntrinsics() const { return camIntrinsics; }
/**
* Returns the camera calibration's distortion coefficients, in OPENCV8 form.
* Higher-order terms are set to 0
*
* @return The distortion coefficients in an 8x1 matrix
*/
Eigen::Matrix<double, 8, 1> GetDistCoeffs() const { return distCoeffs; }
/**
* Gets the FPS of the simulated camera.
*
* @return The FPS
*/
units::hertz_t GetFPS() const { return 1 / frameSpeed; }
/**
* Gets the time per frame of the simulated camera.
*
* @return The time per frame
*/
units::second_t GetFrameSpeed() const { return frameSpeed; }
/**
* Gets the exposure time of the simulated camera.
*
* @return The exposure time
*/
units::second_t GetExposureTime() const { return exposureTime; }
/**
* Gets the average latency of the simulated camera.
*
* @return The average latency
*/
units::second_t GetAverageLatency() const { return avgLatency; }
/**
* Gets the time per frame of the simulated camera.
*
* @return The time per frame
*/
units::second_t GetLatencyStdDev() const { return latencyStdDev; }
/**
* The percentage (0 - 100) of this camera's resolution the contour takes up
* in pixels of the image.
*
* @param points Points of the contour
* @return The percentage
*/
double GetContourAreaPercent(const std::vector<cv::Point2f>& points) {
return cv::contourArea(photon::OpenCVHelp::GetConvexHull(points)) /
GetResArea() * 100;
}
/** The yaw from the principal point of this camera to the pixel x value.
* Positive values left. */
frc::Rotation2d GetPixelYaw(double pixelX) const {
double fx = camIntrinsics(0, 0);
double cx = camIntrinsics(0, 2);
@@ -108,18 +213,56 @@ class SimCameraProperties {
return frc::Rotation2d{fx, xOffset};
}
/**
* The pitch from the principal point of this camera to the pixel y value.
* Pitch is positive down.
*
* Note that this angle is naively computed and may be incorrect. See
* #getCorrectedPixelRot(const cv::Point2d).
*/
frc::Rotation2d GetPixelPitch(double pixelY) const {
double fy = camIntrinsics(1, 1);
double cy = camIntrinsics(1, 2);
double yOffset = cy - pixelY;
return frc::Rotation2d{fy, -yOffset};
}
/**
* Finds the yaw and pitch to the given image point. Yaw is positive left, and
* pitch is positive down.
*
* Note that pitch is naively computed and may be incorrect. See
* #getCorrectedPixelRot(const cv::Point2d).
*/
frc::Rotation3d GetPixelRot(const cv::Point2d& point) const {
return frc::Rotation3d{0_rad, GetPixelPitch(point.y).Radians(),
GetPixelYaw(point.x).Radians()};
}
/**
* Gives the yaw and pitch of the line intersecting the camera lens and the
* given pixel coordinates on the sensor. Yaw is positive left, and pitch
* positive down.
*
* The pitch traditionally calculated from pixel offsets do not correctly
* account for non-zero values of yaw because of perspective distortion (not
* to be confused with lens distortion)-- for example, the pitch angle is
* naively calculated as:
*
* <pre>pitch = arctan(pixel y offset / focal length y)<pre>
*
* However, using focal length as a side of the associated right triangle is
* not correct when the pixel x value is not 0, because the distance from this
* pixel (projected on the x-axis) to the camera lens increases. Projecting a
* line back out of the camera with these naive angles will not intersect the
* 3d point that was originally projected into this 2d pixel. Instead, this
* length should be:
*
* <pre>focal length y ⟶ (focal length y / cos(arctan(pixel x offset / focal
* length x)))</pre>
*
* @return Rotation3d with yaw and pitch of the line projected out of the
* camera from the given pixel (roll is zero).
*/
frc::Rotation3d GetCorrectedPixelRot(const cv::Point2d& point) const {
double fx = camIntrinsics(0, 0);
double cx = camIntrinsics(0, 2);
@@ -151,10 +294,40 @@ class SimCameraProperties {
units::math::hypot(GetHorizFOV().Radians(), GetVertFOV().Radians())};
}
/**
* Determines where the line segment defined by the two given translations
* intersects the camera's frustum/field-of-vision, if at all.
*
* <p>The line is parametrized so any of its points <code>p = t * (b - a) +
* a</code>. This method returns these values of t, minimum first, defining
* the region of the line segment which is visible in the frustum. If both
* ends of the line segment are visible, this simply returns {0, 1}. If, for
* example, point b is visible while a is not, and half of the line segment is
* inside the camera frustum, {0.5, 1} would be returned.
*
* @param camRt The change in basis from world coordinates to camera
* coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d).
* @param a The initial translation of the line
* @param b The final translation of the line
* @return A Pair of Doubles. The values may be empty:
* - {double, double} : Two parametrized values(t), minimum first,
* representing which segment of the line is visible in the camera frustum.
* - {double, std::nullopt} : One value(t) representing a single
* intersection point. For example, the line only intersects the intersection
* of two adjacent viewplanes.
* - {std::nullopt, std::nullopt} : No values. The line segment is not
* visible in the camera frustum.
*/
std::pair<std::optional<double>, std::optional<double>> GetVisibleLine(
const RotTrlTransform3d& camRt, const frc::Translation3d& a,
const frc::Translation3d& b) const;
/**
* Returns these points after applying this camera's estimated noise.
*
* @param points The points to add noise to
* @return The points with noise
*/
std::vector<cv::Point2f> EstPixelNoise(
const std::vector<cv::Point2f>& points) {
if (avgErrorPx == 0 && errorStdDevPx == 0) {
@@ -174,17 +347,46 @@ class SimCameraProperties {
return noisyPts;
}
/**
* Returns an estimation of a frame's processing latency with noise added.
*
* @return The latency estimate
*/
units::second_t EstLatency() {
return units::math::max(avgLatency + gaussian(generator) * latencyStdDev,
0_s);
}
/**
* Estimates how long until the next frame should be processed.
*
* @return The estimated time until the next frame
*/
units::second_t EstSecUntilNextFrame() {
return frameSpeed + units::math::max(0_s, EstLatency() - frameSpeed);
}
/**
* Creates a set of camera properties where the camera has a 960x720
* resolution, 90 degree FOV, and is a "perfect" lagless camera.
*
* @return The properties for this theoretical camera
*/
static SimCameraProperties PERFECT_90DEG() { return SimCameraProperties{}; }
/**
* Creates a set of camera properties matching those of Microsoft Lifecam
* running on a Raspberry Pi 4 at 320x240 resolution.
*
* Note that this set of properties represents *a camera setup*, not *your
* camera setup*. Do not use these camera properties for any non-sim vision
* calculations, especially the calibration data. Always use your camera's
* calibration data to do vision calculations in non-sim environments. These
* properties exist as a sample that may be used to get representative data in
* sim.
*
* @return The properties for this camera setup
*/
static SimCameraProperties PI4_LIFECAM_320_240() {
SimCameraProperties prop{};
prop.SetCalibration(
@@ -202,6 +404,19 @@ class SimCameraProperties {
return prop;
}
/**
* Creates a set of camera properties matching those of Microsoft Lifecam
* running on a Raspberry Pi 4 at 640x480 resolution.
*
* <p>Note that this set of properties represents *a camera setup*, not *your
* camera setup*. Do not use these camera properties for any non-sim vision
* calculations, especially the calibration data. Always use your camera's
* calibration data to do vision calculations in non-sim environments. These
* properties exist as a sample that may be used to get representative data in
* sim.
*
* @return The properties for this camera setup
*/
static SimCameraProperties PI4_LIFECAM_640_480() {
SimCameraProperties prop{};
prop.SetCalibration(
@@ -219,6 +434,19 @@ class SimCameraProperties {
return prop;
}
/**
* Creates a set of camera properties matching those of a Limelight 2 running
* at 640x480 resolution.
*
* <p>Note that this set of properties represents *a camera setup*, not *your
* camera setup*. Do not use these camera properties for any non-sim vision
* calculations, especially the calibration data. Always use your camera's
* calibration data to do vision calculations in non-sim environments. These
* properties exist as a sample that may be used to get representative data in
* sim.
*
* @return The properties for this camera setup
*/
static SimCameraProperties LL2_640_480() {
SimCameraProperties prop{};
prop.SetCalibration(
@@ -236,6 +464,19 @@ class SimCameraProperties {
return prop;
}
/**
* Creates a set of camera properties matching those of a Limelight 2 running
* at 960x720 resolution.
*
* <p>Note that this set of properties represents *a camera setup*, not *your
* camera setup*. Do not use these camera properties for any non-sim vision
* calculations, especially the calibration data. Always use your camera's
* calibration data to do vision calculations in non-sim environments. These
* properties exist as a sample that may be used to get representative data in
* sim.
*
* @return The properties for this camera setup
*/
static SimCameraProperties LL2_960_720() {
SimCameraProperties prop{};
prop.SetCalibration(
@@ -253,6 +494,19 @@ class SimCameraProperties {
return prop;
}
/**
* Creates a set of camera properties matching those of a Limelight 2 running
* at 1280x720 resolution.
*
* <p>Note that this set of properties represents *a camera setup*, not *your
* camera setup*. Do not use these camera properties for any non-sim vision
* calculations, especially the calibration data. Always use your camera's
* calibration data to do vision calculations in non-sim environments. These
* properties exist as a sample that may be used to get representative data in
* sim.
*
* @return The properties for this camera setup
*/
static SimCameraProperties LL2_1280_720() {
SimCameraProperties prop{};
prop.SetCalibration(

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

@@ -73,6 +73,16 @@ static std::unordered_map<int, cv::Mat> LoadAprilTagImages() {
return retVal;
}
/**
* Gets the points representing the corners of this image. Because image pixels
* are accessed through a cv::Mat, the point (0,0) actually represents the
* center of the top-left pixel and not the actual top-left corner.
*
* <p>Order of corners returned is: [BL, BR, TR, TL]
*
* @param size Size of image
* @return The corners
*/
static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
std::vector<cv::Point2f> retVal{};
retVal.emplace_back(cv::Point2f{-0.5f, size.height - 0.5f});
@@ -82,6 +92,12 @@ static std::vector<cv::Point2f> GetImageCorners(const cv::Size& size) {
return retVal;
}
/**
* Gets the points representing the marker(black square) corners.
*
* @param scale The scale of the tag image (10*scale x 10*scale image)
* @return The points
*/
static std::vector<cv::Point2f> Get36h11MarkerPts(int scale) {
cv::Rect2f roi36h11{cv::Point2f{1, 1}, cv::Point2f{8, 8}};
roi36h11.x *= scale;
@@ -96,6 +112,11 @@ static std::vector<cv::Point2f> Get36h11MarkerPts(int scale) {
return pts;
}
/**
* Gets the points representing the marker(black square) corners.
*
* @return The points
*/
static std::vector<cv::Point2f> Get36h11MarkerPts() {
return Get36h11MarkerPts(1);
}
@@ -104,12 +125,26 @@ static const std::unordered_map<int, cv::Mat> kTag36h11Images =
LoadAprilTagImages();
static const std::vector<cv::Point2f> kTag36h11MarkPts = Get36h11MarkerPts();
/** Updates the properties of this cs::CvSource video stream with the given
* camera properties. */
[[maybe_unused]] static void UpdateVideoProp(cs::CvSource& video,
const SimCameraProperties& prop) {
video.SetResolution(prop.GetResWidth(), prop.GetResHeight());
video.SetFPS(prop.GetFPS().to<int>());
}
/**
* Warps the image of a specific 36h11 AprilTag onto the destination image at
* the given points.
*
* @param tagId The id of the specific tag to warp onto the destination image
* @param dstPoints Points(4) in destination image where the tag marker(black
* square) corners should be warped onto.
* @param antialiasing If antialiasing should be performed by automatically
* supersampling/interpolating the warped image. This should be used if
* better stream quality is desired or target detection is being done on the
* stream, but can hurt performance.
* @param destination The destination image to place the warped tag image onto.
*/
[[maybe_unused]] static void Warp165h5TagImage(
int tagId, const std::vector<cv::Point2f>& dstPoints, bool antialiasing,
cv::Mat& destination) {
@@ -232,14 +267,32 @@ static const std::vector<cv::Point2f> kTag36h11MarkPts = Get36h11MarkerPts();
cv::copyTo(tempRoi, destination(boundingRect), tempMask);
}
/**
* Given a line thickness in a 640x480 image, try to scale to the given
* destination image resolution.
*
* @param thickness480p A hypothetical line thickness in a 640x480 image
* @param destination The destination image to scale to
* @return Scaled thickness which cannot be less than 1
*/
static double GetScaledThickness(double thickness480p,
const cv::Mat& destinationMat) {
double scaleX = destinationMat.size().width / 640.0;
double scaleY = destinationMat.size().height / 480.0;
const cv::Mat& destination) {
double scaleX = destination.size().width / 640.0;
double scaleY = destination.size().height / 480.0;
double minScale = std::min(scaleX, scaleY);
return std::max(thickness480p * minScale, 1.0);
}
/**
* Draw a filled ellipse in the destination image.
*
* @param dstPoints The points in the destination image representing the
* rectangle in which the ellipse is inscribed.
* @param color The color of the ellipse. This is a scalar with BGR values
* (0-255)
* @param destination The destination image to draw onto. The image should be in
* the BGR color space.
*/
[[maybe_unused]] static void DrawInscribedEllipse(
const std::vector<cv::Point2f>& dstPoints, const cv::Scalar& color,
cv::Mat& destination) {
@@ -261,6 +314,16 @@ static void DrawPoly(const std::vector<cv::Point2f>& dstPoints, int thickness,
}
}
/**
* Draws a contour around the given points and text of the id onto the
* destination image.
*
* @param id Fiducial ID number to draw
* @param dstPoints Points representing the four corners of the tag marker(black
* square) in the destination image.
* @param destination The destination image to draw onto. The image should be in
* the BGR color space.
*/
[[maybe_unused]] static void DrawTagDetection(
int id, const std::vector<cv::Point2f>& dstPoints, cv::Mat& destination) {
double thickness = GetScaledThickness(1, destination);
@@ -275,6 +338,10 @@ static void DrawPoly(const std::vector<cv::Point2f>& dstPoints, int thickness,
static_cast<int>(thickness), cv::LINE_AA);
}
/**
* The translations used to draw the field side walls and driver station walls.
* It is a vector of vectors because the translations are not all connected.
*/
static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
std::vector<std::vector<frc::Translation3d>> list;
@@ -328,6 +395,15 @@ static std::vector<std::vector<frc::Translation3d>> GetFieldWallLines() {
return list;
}
/**
* The translations used to draw the field floor subdivisions (not the floor
* outline). It is a vector of vectors because the translations are not all
* connected.
*
* @param subdivisions How many "subdivisions" along the width/length of the
* floor. E.g. 3 subdivisions would mean 2 lines along the length and 2 lines
* along the width creating a 3x3 "grid".
*/
static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
int subdivisions) {
std::vector<std::vector<frc::Translation3d>> list;
@@ -345,6 +421,23 @@ static std::vector<std::vector<frc::Translation3d>> GetFieldFloorLines(
return list;
}
/**
* Convert 3D lines represented by the given series of translations into a
* polygon(s) in the camera's image.
*
* @param camRt The change in basis from world coordinates to camera
* coordinates. See RotTrlTransform3d#makeRelativeTo(Pose3d).
* @param prop The simulated camera's properties.
* @param trls A sequential series of translations defining the polygon to be
* drawn.
* @param resolution Resolution as a fraction(0 - 1) of the video frame's
* diagonal length in pixels. Line segments will be subdivided if they exceed
* this resolution.
* @param isClosed If the final translation should also draw a line to the first
* translation.
* @param destination The destination image that is being drawn to.
* @return A list of polygons(which are an array of points)
*/
static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
const std::vector<frc::Translation3d>& trls, double resolution,
@@ -403,6 +496,25 @@ static std::vector<std::vector<cv::Point2f>> PolyFrom3dLines(
return polyPointList;
}
/**
* Draw a wireframe of the field to the given image.
*
* @param camRt The change in basis from world coordinates to camera
* coordinates. See RotTrlTransform3d#makeRelativeTo(frc::Pose3d).
* @param prop The simulated camera's properties.
* @param resolution Resolution as a fraction(0 - 1) of the video frame's
* diagonal length in pixels. Line segments will be subdivided if they exceed
* this resolution.
* @param wallThickness Thickness of the lines used for drawing the field walls
* in pixels. This is scaled by #getScaledThickness(double, cv::Mat).
* @param wallColor Color of the lines used for drawing the field walls.
* @param floorSubdivisions A NxN "grid" is created from the floor where this
* parameter is N, which defines the floor lines.
* @param floorThickness Thickness of the lines used for drawing the field floor
* grid in pixels. This is scaled by #getScaledThickness(double, cv::Mat).
* @param floorColor Color of the lines used for drawing the field floor grid.
* @param destination The destination image to draw to.
*/
[[maybe_unused]] static void DrawFieldWireFrame(
const RotTrlTransform3d& camRt, const SimCameraProperties& prop,
double resolution, double wallThickness, const cv::Scalar& wallColor,

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

@@ -38,12 +38,32 @@
#include "photon/simulation/PhotonCameraSim.h"
namespace photon {
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted on
* a mobile robot running PhotonVision, detecting targets placed on the field.
* VisionTargetSims added to this class will be detected by the PhotonCameraSim
* added to this class. This class should be updated periodically with the
* robot's current pose in order to publish the simulated camera target info.
*/
class VisionSystemSim {
public:
/**
* A simulated vision system involving a camera(s) and coprocessor(s) mounted
* on a mobile robot running PhotonVision, detecting targets placed on the
* field. VisionTargetSims added to this class will be detected by the
* PhotonCameraSims added to this class. This class should be updated
* periodically with the robot's current pose in order to publish the
* simulated camera target info.
*
* @param visionSystemName The specific identifier for this vision system in
* NetworkTables.
*/
explicit VisionSystemSim(std::string visionSystemName) {
std::string tableName = "VisionSystemSim-" + visionSystemName;
frc::SmartDashboard::PutData(tableName + "/Sim Field", &dbgField);
}
/** Get one of the simulated cameras. */
std::optional<PhotonCameraSim*> GetCameraSim(std::string name) {
auto it = camSimMap.find(name);
if (it != camSimMap.end()) {
@@ -52,6 +72,8 @@ class VisionSystemSim {
return std::nullopt;
}
}
/** Get all the simulated cameras. */
std::vector<PhotonCameraSim*> GetCameraSims() {
std::vector<PhotonCameraSim*> retVal;
for (auto const& cam : camSimMap) {
@@ -59,6 +81,15 @@ class VisionSystemSim {
}
return retVal;
}
/**
* Adds a simulated camera to this vision system with a specified
* robot-to-camera transformation. The vision targets registered with this
* vision system simulation will be observed by the simulated PhotonCamera.
*
* @param cameraSim The camera simulation
* @param robotToCamera The transform from the robot pose to the camera pose
*/
void AddCamera(PhotonCameraSim* cameraSim,
const frc::Transform3d& robotToCamera) {
auto found =
@@ -73,10 +104,19 @@ class VisionSystemSim {
frc::Pose3d{} + robotToCamera);
}
}
/** Remove all simulated cameras from this vision system. */
void ClearCameras() {
camSimMap.clear();
camTrfMap.clear();
}
/**
* Remove a simulated camera from this vision system.
*
* @param cameraSim The camera to remove
* @return If the camera was present and removed
*/
bool RemoveCamera(PhotonCameraSim* cameraSim) {
int numOfElementsRemoved =
camSimMap.erase(std::string{cameraSim->GetCamera()->GetCameraName()});
@@ -86,9 +126,28 @@ class VisionSystemSim {
return false;
}
}
/**
* Get a simulated camera's position relative to the robot. If the requested
* camera is invalid, an empty optional is returned.
*
* @param cameraSim The specific camera to get the robot-to-camera transform
* of
* @return The transform of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim) {
return GetRobotToCamera(cameraSim, frc::Timer::GetFPGATimestamp());
}
/**
* Get a simulated camera's position relative to the robot. If the requested
* camera is invalid, an empty optional is returned.
*
* @param cameraSim The specific camera to get the robot-to-camera transform
* of
* @param time Timestamp of when the transform should be observed
* @return The transform of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Transform3d> GetRobotToCamera(PhotonCameraSim* cameraSim,
units::second_t time) {
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
@@ -105,9 +164,26 @@ class VisionSystemSim {
return std::nullopt;
}
}
/**
* Get a simulated camera's position on the field. If the requested camera is
* invalid, an empty optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @return The pose of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim) {
return GetCameraPose(cameraSim, frc::Timer::GetFPGATimestamp());
}
/**
* Get a simulated camera's position on the field. If the requested camera is
* invalid, an empty optional is returned.
*
* @param cameraSim The specific camera to get the field pose of
* @param time Timestamp of when the pose should be observed
* @return The pose of this camera, or an empty optional if it is invalid
*/
std::optional<frc::Pose3d> GetCameraPose(PhotonCameraSim* cameraSim,
units::second_t time) {
auto robotToCamera = GetRobotToCamera(cameraSim, time);
@@ -117,6 +193,15 @@ class VisionSystemSim {
return std::make_optional(GetRobotPose(time) + robotToCamera.value());
}
}
/**
* Adjust a camera's position relative to the robot. Use this if your camera
* is on a gimbal or turret or some other mobile platform.
*
* @param cameraSim The simulated camera to change the relative position of
* @param robotToCamera New transform from the robot to the camera
* @return If the cameraSim was valid and transform was adjusted
*/
bool AdjustCamera(PhotonCameraSim* cameraSim,
const frc::Transform3d& robotToCamera) {
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
@@ -127,11 +212,21 @@ class VisionSystemSim {
return false;
}
}
/** Reset the previous transforms for all cameras to their current transform.
*/
void ResetCameraTransforms() {
for (const auto& pair : camTrfMap) {
ResetCameraTransforms(pair.first);
}
}
/**
* Reset the transform history for this camera to just the current transform.
*
* @param cameraSim The camera to reset
* @return If the cameraSim was valid and transforms were reset
*/
bool ResetCameraTransforms(PhotonCameraSim* cameraSim) {
units::second_t now = frc::Timer::GetFPGATimestamp();
if (camTrfMap.find(cameraSim) != camTrfMap.end()) {
@@ -145,6 +240,12 @@ class VisionSystemSim {
return false;
}
}
/**
* Returns all the vision targets on the field.
*
* @return The vision targets
*/
std::vector<VisionTargetSim> GetVisionTargets() {
std::vector<VisionTargetSim> all{};
for (const auto& entry : targetSets) {
@@ -154,12 +255,40 @@ class VisionSystemSim {
}
return all;
}
/**
* Returns all the vision targets of the specified type on the field.
*
* @param type The type of vision targets to return
* @return The vision targets
*/
std::vector<VisionTargetSim> GetVisionTargets(std::string type) {
return targetSets[type];
}
/**
* Adds targets on the field which your vision system is designed to detect.
* The PhotonCameras simulated from this system will report the location of
* the camera relative to the subset of these targets which are visible from
* the given camera position.
*
* By default these are added under the type "targets".
*
* @param targets Targets to add to the simulated field
*/
void AddVisionTargets(const std::vector<VisionTargetSim>& targets) {
AddVisionTargets("targets", targets);
}
/**
* Adds targets on the field which your vision system is designed to detect.
* The PhotonCameras simulated from this system will report the location of
* the camera relative to the subset of these targets which are visible from
* the given camera position.
*
* @param type Type of target (e.g. "cargo").
* @param targets Targets to add to the simulated field
*/
void AddVisionTargets(std::string type,
const std::vector<VisionTargetSim>& targets) {
if (!targetSets.contains(type)) {
@@ -169,6 +298,20 @@ class VisionSystemSim {
targetSets[type].emplace_back(tgt);
}
}
/**
* Adds targets on the field which your vision system is designed to detect.
* The PhotonCameras simulated from this system will report the location of
* the camera relative to the subset of these targets which are visible from
* the given camera position.
*
* The AprilTags from this layout will be added as vision targets under the
* type "apriltag". The poses added preserve the tag layout's current alliance
* origin. If the tag layout's alliance origin is changed, these added tags
* will have to be cleared and re-added.
*
* @param layout The field tag layout to get Apriltag poses and IDs from
*/
void AddAprilTags(const frc::AprilTagFieldLayout& layout) {
std::vector<VisionTargetSim> targets;
for (const frc::AprilTag& tag : layout.GetTags()) {
@@ -177,9 +320,28 @@ class VisionSystemSim {
}
AddVisionTargets("apriltag", targets);
}
/** Removes every VisionTargetSim from the simulated field. */
void ClearVisionTargets() { targetSets.clear(); }
/** Removes all simulated AprilTag targets from the simulated field. */
void ClearAprilTags() { RemoveVisionTargets("apriltag"); }
/**
* Removes every VisionTargetSim of the specified type from the simulated
* field.
*
* @param type Type of target (e.g. "cargo"). Same as the type passed into
* #addVisionTargets(String, VisionTargetSim...)
* @return The removed targets, or null if no targets of the specified type
* exist
*/
void RemoveVisionTargets(std::string type) { targetSets.erase(type); }
/**
* Removes the specified VisionTargetSims from the simulated field.
*
* @param targets The targets to remove
* @return The targets that were actually removed
*/
std::vector<VisionTargetSim> RemoveVisionTargets(
const std::vector<VisionTargetSim>& targets) {
std::vector<VisionTargetSim> removedList;
@@ -194,21 +356,60 @@ class VisionSystemSim {
}
return removedList;
}
/**
* Get the latest robot pose in meters saved by the vision system.
*
* @return The latest robot pose
*/
frc::Pose3d GetRobotPose() {
return GetRobotPose(frc::Timer::GetFPGATimestamp());
}
/**
* Get the robot pose in meters saved by the vision system at this timestamp.
*
* @param timestamp Timestamp of the desired robot pose
* @return The robot pose
*/
frc::Pose3d GetRobotPose(units::second_t timestamp) {
return robotPoseBuffer.Sample(timestamp).value_or(frc::Pose3d{});
}
/**
* Clears all previous robot poses and sets robotPose at current time.
*
* @param robotPose The robot pose
*/
void ResetRobotPose(const frc::Pose2d& robotPose) {
ResetRobotPose(frc::Pose3d{robotPose});
}
/**
* Clears all previous robot poses and sets robotPose at current time.
*
* @param robotPose The robot pose
*/
void ResetRobotPose(const frc::Pose3d& robotPose) {
robotPoseBuffer.Clear();
robotPoseBuffer.AddSample(frc::Timer::GetFPGATimestamp(), robotPose);
}
frc::Field2d& GetDebugField() { return dbgField; }
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is
* used to automatically determine if a new frame should be submitted.
*
* @param robotPoseMeters The simulated robot pose in meters
*/
void Update(const frc::Pose2d& robotPose) { Update(frc::Pose3d{robotPose}); }
/**
* Periodic update. Ensure this is called repeatedly-- camera performance is
* used to automatically determine if a new frame should be submitted.
*
* @param robotPoseMeters The simulated robot pose in meters
*/
void Update(const frc::Pose3d& robotPose) {
for (auto& set : targetSets) {
std::vector<frc::Pose2d> posesToAdd{};