[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:
amquake
2023-06-18 15:54:12 -07:00
committed by GitHub
parent 4a94775639
commit f1cadc1e1e
56 changed files with 2471 additions and 199 deletions

View File

@@ -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;
}
}