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

Add skew calculation support (#36)

Browse Source 
This is a breaking change to PhotonLib.
This commit is contained in:
Matt
2020-07-16 17:50:40 -07:00
committed by GitHub
parent 7bf5142fad
commit 061385f0b6
8 changed files with 107 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimplePipelineResult.java
View File

@@ -1,110 +0,0 @@
/*
* Copyright (C) 2020 Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.dataflow.structures;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
public class SimplePipelineResult extends BytePackable {
private double latencyMillis;
private boolean hasTargets;
public final List<SimpleTrackedTarget> targets = new ArrayList<>();
public SimplePipelineResult() {}
public SimplePipelineResult(
double latencyMillis, boolean hasTargets, List<SimpleTrackedTarget> targets) {
this.latencyMillis = latencyMillis;
this.hasTargets = hasTargets;
this.targets.addAll(targets);
}
public SimplePipelineResult(CVPipelineResult origResult) {
update(origResult);
}
public void update(CVPipelineResult origResult) {
latencyMillis = origResult.getLatencyMillis();
hasTargets = origResult.hasTargets();
targets.clear();
for (var origTarget : origResult.targets) {
targets.add(new SimpleTrackedTarget(origTarget));
}
}
public double getLatencyMillis() {
return latencyMillis;
}
public boolean hasTargets() {
return hasTargets;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimplePipelineResult that = (SimplePipelineResult) o;
boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
boolean hasTargetsMatch = that.hasTargets == hasTargets;
boolean targetsMatch = that.targets.equals(targets);
return latencyMatch && hasTargetsMatch && targetsMatch;
}
@Override
public int hashCode() {
return Objects.hash(latencyMillis, hasTargets, targets);
}
@Override
public byte[] toByteArray() {
bufferPosition = 0;
int bufferSize =
8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
var buff = new byte[bufferSize];
bufferData(latencyMillis, buff);
bufferData(hasTargets, buff);
bufferData((byte) targets.size(), buff);
for (var target : targets) {
bufferData(target.toByteArray(), buff);
}
return buff;
}
@Override
public void fromByteArray(byte[] src) {
bufferPosition = 0;
latencyMillis = unbufferDouble(src);
hasTargets = unbufferBoolean(src);
byte targetCount = unbufferByte(src);
targets.clear();
for (int i = 0; i < targetCount; i++) {
var target = new SimpleTrackedTarget();
target.fromByteArray(unbufferBytes(src, 48));
bufferPosition += 48;
targets.add(target);
}
}
}

97
photon-server/src/main/java/org/photonvision/common/dataflow/structures/SimpleTrackedTarget.java
View File

@@ -1,97 +0,0 @@
/*
* Copyright (C) 2020 Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
package org.photonvision.common.dataflow.structures;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
import org.photonvision.vision.target.TrackedTarget;
public class SimpleTrackedTarget extends BytePackable {
public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
private double yaw;
private double pitch;
private double area;
private Pose2d robotRelativePose = new Pose2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
robotRelativePose = pose;
}
public SimpleTrackedTarget(TrackedTarget origTarget) {
yaw = origTarget.getYaw();
pitch = origTarget.getPitch();
area = origTarget.getArea();
robotRelativePose = origTarget.getRobotRelativePose();
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
SimpleTrackedTarget that = (SimpleTrackedTarget) o;
return Double.compare(that.yaw, yaw) == 0
&& Double.compare(that.pitch, pitch) == 0
&& Double.compare(that.area, area) == 0
&& Objects.equals(robotRelativePose, that.robotRelativePose);
}
@Override
public int hashCode() {
return Objects.hash(yaw, pitch, area, robotRelativePose);
}
@Override
public byte[] toByteArray() {
resetBufferPosition();
byte[] data = new byte[PACK_SIZE_BYTES];
bufferData(yaw, data);
bufferData(pitch, data);
bufferData(area, data);
bufferData(robotRelativePose.getTranslation().getX(), data);
bufferData(robotRelativePose.getTranslation().getY(), data);
bufferData(robotRelativePose.getRotation().getDegrees(), data);
return data;
}
@Override
public void fromByteArray(byte[] src) {
resetBufferPosition();
if (src.length < PACK_SIZE_BYTES) {
// TODO: log error?
return;
}
yaw = unbufferDouble(src);
pitch = unbufferDouble(src);
area = unbufferDouble(src);
var poseX = unbufferDouble(src);
var poseY = unbufferDouble(src);
var poseR = unbufferDouble(src);
robotRelativePose = new Pose2d(poseX, poseY, Rotation2d.fromDegrees(poseR));
}
}

4
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimplePipelineResult.java
View File

@@ -68,8 +68,8 @@ public class SimplePipelineResult extends BytePackable {
@Override
public byte[] toByteArray() {
bufferPosition = 0;
int bufferSize =
8 + 1 + 1 + (targets.size() * 48); // latencyMillis + hasTargets + targetCount + targets
// latencyMillis + hasTargets + targetCount + targets
int bufferSize = 8 + 1 + 1 + (targets.size() * 8 * SimpleTrackedTarget.PACK_SIZE_BYTES);
var buff = new byte[bufferSize];
bufferData(latencyMillis, buff);

10
photon-server/src/main/java/org/photonvision/vision/pipeline/result/SimpleTrackedTarget.java
View File

@@ -23,24 +23,26 @@ import java.util.Objects;
import org.photonvision.vision.target.TrackedTarget;
public class SimpleTrackedTarget extends BytePackable {
public static final int PACK_SIZE_BYTES = Double.BYTES * 6;
public static final int PACK_SIZE_BYTES = Double.BYTES * 7;
private double yaw;
private double pitch;
private double area;
private double skew;
private Pose2d robotRelativePose = new Pose2d();
public SimpleTrackedTarget() {}
public SimpleTrackedTarget(double yaw, double pitch, double area, Pose2d pose) {
public SimpleTrackedTarget(double yaw, double pitch, double area, double skew, Pose2d pose) {
this.yaw = yaw;
this.pitch = pitch;
this.area = area;
this.skew = skew;
robotRelativePose = pose;
}
public SimpleTrackedTarget(TrackedTarget t) {
this(t.getYaw(), t.getPitch(), t.getArea(), t.getRobotRelativePose());
this(t.getYaw(), t.getPitch(), t.getArea(), t.getSkew(), t.getRobotRelativePose());
}
public double getYaw() {
@@ -83,6 +85,7 @@ public class SimpleTrackedTarget extends BytePackable {
bufferData(yaw, data);
bufferData(pitch, data);
bufferData(area, data);
bufferData(skew, data);
if (robotRelativePose != null) {
bufferData(robotRelativePose.getTranslation().getX(), data);
bufferData(robotRelativePose.getTranslation().getY(), data);
@@ -107,6 +110,7 @@ public class SimpleTrackedTarget extends BytePackable {
yaw = unbufferDouble(src);
pitch = unbufferDouble(src);
area = unbufferDouble(src);
skew = unbufferByte(src);
var poseX = unbufferDouble(src);
var poseY = unbufferDouble(src);

11
photon-server/src/main/java/org/photonvision/vision/target/TargetCalculations.java
View File

@@ -35,6 +35,17 @@ public class TargetCalculations {
FastMath.atan((offsetCenterY - targetCenterY) / verticalFocalLength));
}
@SuppressWarnings("DuplicatedCode")
public static double calculateSkew(boolean isLandscape, RotatedRect minAreaRect) {
// https://namkeenman.wordpress.com/2015/12/18/open-cv-determine-angle-of-rotatedrect-minarearect/
var angle = minAreaRect.angle;
if (isLandscape && minAreaRect.size.width < minAreaRect.size.height) angle += 90;
else if (!isLandscape && minAreaRect.size.height < minAreaRect.size.width) angle += 90;
return angle;
}
public static Point calculateTargetOffsetPoint(
boolean isLandscape, TargetOffsetPointEdge offsetRegion, RotatedRect minAreaRect) {
Point[] vertices = new Point[4];

8
photon-server/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View File

@@ -42,6 +42,7 @@ public class TrackedTarget implements Releasable {
private double m_pitch;
private double m_yaw;
private double m_area;
private double m_skew;
private Pose2d m_robotRelativePose = new Pose2d();
@@ -79,9 +80,8 @@ public class TrackedTarget implements Releasable {
return m_yaw;
}
private double getSkew() {
// TODO skew;
return 5940;
public double getSkew() {
return m_skew;
}
public double getArea() {
@@ -116,6 +116,8 @@ public class TrackedTarget implements Releasable {
TargetCalculations.calculateYaw(
m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength);
m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea;
m_skew = TargetCalculations.calculateSkew(params.isLandscape, getMinAreaRect());
}
@Override