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

[photon-lib] Cleanup simulation Rotation3d usage (#982)

Browse Source
This commit is contained in:
amquake
2023-10-25 18:19:48 -07:00
committed by GitHub
parent 63147786b9
commit 5b2be119e7
4 changed files with 29 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -31,7 +31,6 @@ import edu.wpi.first.cscore.VideoSource.ConnectionStrategy;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;
import java.util.ArrayList;
@@ -49,6 +48,7 @@ import org.photonvision.PhotonCamera;
import org.photonvision.PhotonTargetSortMode;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.common.networktables.NTTopicSet;
import org.photonvision.estimation.CameraTargetRelation;
import org.photonvision.estimation.OpenCVHelp;
import org.photonvision.estimation.RotTrlTransform3d;
import org.photonvision.estimation.TargetModel;
@@ -202,23 +202,16 @@ public class PhotonCameraSim implements AutoCloseable {
* @return If this vision target can be seen before image projection.
*/
public boolean canSeeTargetPose(Pose3d camPose, VisionTargetSim target) {
// var rel = new CameraTargetRelation(camPose, target.getPose());
// TODO: removal awaiting wpilib Rotation3d performance improvements
var relTarget = RotTrlTransform3d.makeRelativeTo(camPose).apply(target.getPose());
var camToTargYaw = new Rotation2d(relTarget.getX(), relTarget.getY());
var camToTargPitch =
new Rotation2d(Math.hypot(relTarget.getX(), relTarget.getY()), -relTarget.getZ());
var relCam = RotTrlTransform3d.makeRelativeTo(target.getPose()).apply(camPose);
var targToCamAngle = new Rotation2d(relCam.getX(), Math.hypot(relCam.getY(), relCam.getZ()));
var rel = new CameraTargetRelation(camPose, target.getPose());
return (
// target translation is outside of camera's FOV
(Math.abs(camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
(Math.abs(rel.camToTargYaw.getDegrees()) < prop.getHorizFOV().getDegrees() / 2)
&& (Math.abs(rel.camToTargPitch.getDegrees()) < prop.getVertFOV().getDegrees() / 2)
&& (!target.getModel().isPlanar
|| Math.abs(targToCamAngle.getDegrees())
|| Math.abs(rel.targToCamAngle.getDegrees())
< 90) // camera is behind planar target and it should be occluded
&& (relTarget.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
&& (rel.camToTarg.getTranslation().getNorm() <= maxSightRangeMeters)); // target is too far
}
/**