mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
[WIP] Simulation Overhaul (#742)
### What does this do? - Deprecates previous sim classes - Has a `CameraProperties` class for describing a camera's basic/calibration info, and performance values for simulation. Calibration values can be loaded from the `config.json` in the settings exported by photonvision. - `OpenCVHelp` provides convenience functions for using opencv methods with wpilib/photonvision classes, mainly to project 3d points to a camera's 2d image and perform solvePnP with the above camera calibration info. - `TargetModel`s describe the 3d shape of a target, both for projecting into the camera's 2d image and use in solvePnP. - `PhotonCameraSim` uses camera properties to simulate how 3d targets would appear in its view, and has simulated noise, latency, and FPS. For apriltags, the best/alternate camera-to-target transform is also estimated with solvePnP. - `VideoSimUtil` has helper functions for drawing apriltags to a simulated raw and processed MJPEG stream for each camera using the projected tag corners. - `VisionSystemSim` stores `VisionTargetSim`s and `PhotonCameraSim`s, and is periodically updated with the robot's simulated pose. When updating, camera sims are automatically processed and published with their visible targets from their respective poses with proper latency. ### What's still not working? - Mac Arm builds are broken - More examples - Update website/docs
This commit is contained in:
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
package org.photonvision.simulation;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import java.util.List;
|
||||
import org.photonvision.estimation.TargetModel;
|
||||
|
||||
/** Describes a vision target located somewhere on the field that your vision system can detect. */
|
||||
public class VisionTargetSim {
|
||||
private Pose3d pose;
|
||||
private TargetModel model;
|
||||
|
||||
public final int fiducialID;
|
||||
|
||||
/**
|
||||
* Describes a vision target located somewhere on the field that your vision system can detect.
|
||||
*
|
||||
* @param pose Pose3d of the tag in field-relative coordinates
|
||||
* @param model TargetModel which describes the shape of the target
|
||||
*/
|
||||
public VisionTargetSim(Pose3d pose, TargetModel model) {
|
||||
this.pose = pose;
|
||||
this.model = model;
|
||||
this.fiducialID = -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Describes a fiducial tag located somewhere on the field that your vision system can detect.
|
||||
*
|
||||
* @param pose Pose3d of the tag in field-relative coordinates
|
||||
* @param model TargetModel which describes the shape of the target(tag)
|
||||
* @param id The ID of this fiducial tag
|
||||
*/
|
||||
public VisionTargetSim(Pose3d pose, TargetModel model, int id) {
|
||||
this.pose = pose;
|
||||
this.model = model;
|
||||
this.fiducialID = id;
|
||||
}
|
||||
|
||||
public void setPose(Pose3d pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public void setModel(TargetModel model) {
|
||||
this.model = model;
|
||||
}
|
||||
|
||||
public Pose3d getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public TargetModel getModel() {
|
||||
return model;
|
||||
}
|
||||
|
||||
/** This target's vertices offset from its field pose. */
|
||||
public List<Translation3d> getFieldVertices() {
|
||||
return model.getFieldVertices(pose);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (this == obj) return true;
|
||||
if (obj instanceof VisionTargetSim) {
|
||||
var o = (VisionTargetSim) obj;
|
||||
return pose.equals(o.pose) && model.equals(o.model);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user