Skip to content

Commit

Permalink
fully working everything
Browse files Browse the repository at this point in the history
  • Loading branch information
@srimanachanta
srimanachanta committed Nov 6, 2023
1 parent 381e756 commit 3fbab3a
Show file tree
Hide file tree
Showing 11 changed files with 347 additions and 130 deletions.
10 changes: 5 additions & 5 deletions ...on-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,11 +128,11 @@ public void updateCameraNickname(String newCameraNickname) {

@Override
public void accept(CVPipelineResult result) {
var res = new PhotonPipelineResult(
result.getLatencyMillis(),
TrackedTarget.simpleFromTrackedTargets(result.targets),
result.multiTagResult
);
var res =
new PhotonPipelineResult(
result.getLatencyMillis(),
TrackedTarget.simpleFromTrackedTargets(result.targets),
result.multiTagResult);

ts.rawBytesEntry.set(res);

Expand Down
4 changes: 2 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,8 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
VisionEstimation.estimateCamPosePNP(
params.cameraCoefficients.cameraIntrinsics.getAsWpilibMat(),
params.cameraCoefficients.distCoeffs.getAsWpilibMat(),
List.of(TrackedTarget.simpleFromTrackedTargets(targetList)),
params.atfl);
TrackedTarget.simpleFromTrackedTargets(targetList),
params.atfl,
params.targetModel);

return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
Expand Down
18 changes: 10 additions & 8 deletions photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,11 @@ public TrackedTarget(
tvec.put(
0,
0,
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
);
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);

// Opencv expects a 3d vector with norm = angle and direction = axis
Expand Down Expand Up @@ -203,10 +204,11 @@ public TrackedTarget(
tvec.put(
0,
0,
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
);
new double[] {
bestPose.getTranslation().getX(),
bestPose.getTranslation().getY(),
bestPose.getTranslation().getZ()
});
setCameraRelativeTvec(tvec);

var rvec = new Mat(3, 1, CvType.CV_64FC1);
Expand Down
1 change: 0 additions & 1 deletion photon-targeting/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@ plugins {
id 'edu.wpi.first.WpilibTools' version '1.3.0'
}


apply from: "${rootDir}/shared/common.gradle"

dependencies {
Expand Down
24 changes: 9 additions & 15 deletions photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,8 +30,6 @@
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.Optional;

import org.ejml.simple.SimpleMatrix;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
Expand Down Expand Up @@ -172,11 +170,8 @@ public static Point[] cornersToPoints(TargetCorner... corners) {

public static List<TargetCorner> pointsToCorners(Point... points) {
var corners = new ArrayList<TargetCorner>(points.length);
for (Point point : points) {
corners.add(new TargetCorner(
point.x,
point.y
));
for (int i = 0; i < points.length; i++) {
corners.add(new TargetCorner(points[i].x, points[i].y));
}
return corners;
}
Expand Down Expand Up @@ -406,7 +401,7 @@ public static Point[] getConvexHull(Point[] points) {
* @return The resulting transformation that maps the camera pose to the target pose and the
* ambiguity if an alternate solution is available.
*/
public static Optional<PNPResults> solvePNP_SQUARE(
public static PNPResults solvePNP_SQUARE(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> modelTrls,
Expand Down Expand Up @@ -471,16 +466,15 @@ public static Optional<PNPResults> solvePNP_SQUARE(
// check if solvePnP failed with NaN results and retrying failed
if (Double.isNaN(errors[0])) throw new Exception("SolvePNP_SQUARE NaN result");


if (alt != null)
return Optional.of(new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]));
else return Optional.of(new PNPResults(best, errors[0]));
return new PNPResults(best, alt, errors[0] / errors[1], errors[0], errors[1]);
else return new PNPResults(best, errors[0]);
}
// solvePnP failed
catch (Exception e) {
System.err.println("SolvePNP_SQUARE failed!");
e.printStackTrace();
return Optional.empty();
return new PNPResults();
} finally {
// release our Mats from native memory
objectMat.release();
Expand Down Expand Up @@ -515,7 +509,7 @@ public static Optional<PNPResults> solvePNP_SQUARE(
* model points are supplied relative to the origin, this transformation brings the camera to
* the origin.
*/
public static Optional<PNPResults> solvePNP_SQPNP(
public static PNPResults solvePNP_SQPNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<Translation3d> objectTrls,
Expand Down Expand Up @@ -564,11 +558,11 @@ public static Optional<PNPResults> solvePNP_SQPNP(
// check if solvePnP failed with NaN results
if (Double.isNaN(error[0])) throw new Exception("SolvePNP_SQPNP NaN result");

return Optional.of(new PNPResults(best, error[0]));
return new PNPResults(best, error[0]);
} catch (Exception e) {
System.err.println("SolvePNP_SQPNP failed!");
e.printStackTrace();
return Optional.empty();
return new PNPResults();
}
}
}
31 changes: 14 additions & 17 deletions photon-targeting/src/main/java/org/photonvision/estimation/VisionEstimation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import java.util.Optional;
import java.util.stream.Collectors;
import org.opencv.core.Point;
import org.photonvision.targeting.PNPResults;
Expand Down Expand Up @@ -67,17 +66,17 @@ public static List<AprilTag> getVisibleLayoutTags(
* @return The transformation that maps the field origin to the camera pose. Ensure the {@link
* PNPResults} are present before utilizing them.
*/
public static Optional<PNPResults> estimateCamPosePNP(
public static PNPResults estimateCamPosePNP(
Matrix<N3, N3> cameraMatrix,
Matrix<N5, N1> distCoeffs,
List<PhotonTrackedTarget> visTags,
AprilTagFieldLayout tagLayout,
TargetModel tagModel) {
if (tagLayout == null
|| visTags == null
|| tagLayout.getTags().isEmpty()
|| visTags.isEmpty()) {
return Optional.empty();
|| tagLayout.getTags().size() == 0
|| visTags.size() == 0) {
return new PNPResults();
}

var corners = new ArrayList<TargetCorner>();
Expand All @@ -93,43 +92,41 @@ public static Optional<PNPResults> estimateCamPosePNP(
corners.addAll(tgt.getDetectedCorners());
});
}
if (knownTags.isEmpty() || corners.isEmpty() || corners.size() % 4 != 0) {
return Optional.empty();
if (knownTags.size() == 0 || corners.size() == 0 || corners.size() % 4 != 0) {
return new PNPResults();
}
Point[] points = OpenCVHelp.cornersToPoints(corners);

// single-tag pnp
if (knownTags.size() == 1) {
var camToTagOpt =
var camToTag =
OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, tagModel.vertices, points);
if (camToTagOpt.isEmpty()) return Optional.empty();
var camToTag = camToTagOpt.get();
if (!camToTag.isPresent) return new PNPResults();
var bestPose = knownTags.get(0).pose.transformBy(camToTag.best.inverse());
var altPose = new Pose3d();
if (camToTag.ambiguity != 0)
altPose = knownTags.get(0).pose.transformBy(camToTag.alt.inverse());

var o = new Pose3d();
return Optional.of(new PNPResults(
return new PNPResults(
new Transform3d(o, bestPose),
new Transform3d(o, altPose),
camToTag.ambiguity,
camToTag.bestReprojErr,
camToTag.altReprojErr));
camToTag.altReprojErr);
}
// multi-tag pnp
else {
var objectTrls = new ArrayList<Translation3d>();
for (var tag : knownTags) objectTrls.addAll(tagModel.getFieldVertices(tag.pose));
var camToOriginOpt = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (camToOriginOpt.isEmpty()) return Optional.empty();
var camToOrigin = camToOriginOpt.get();
return Optional.of(new PNPResults(
var camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points);
if (!camToOrigin.isPresent) return new PNPResults();
return new PNPResults(
camToOrigin.best.inverse(),
camToOrigin.alt.inverse(),
camToOrigin.ambiguity,
camToOrigin.bestReprojErr,
camToOrigin.altReprojErr));
camToOrigin.altReprojErr);
}
}
}
48 changes: 38 additions & 10 deletions photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,21 +18,51 @@
package org.photonvision.targeting;

import edu.wpi.first.util.protobuf.Protobuf;
import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults;
import us.hebi.quickbuf.Descriptors.Descriptor;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults;
import us.hebi.quickbuf.Descriptors.Descriptor;

public class MultiTargetPNPResults {
public final PNPResults estimatedPose;
public final List<Integer> fiducialIDsUsed;
// Seeing 32 apriltags at once seems like a sane limit
private static final int MAX_IDS = 32;
// pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);

public PNPResults estimatedPose = new PNPResults();
public List<Integer> fiducialIDsUsed = List.of();

public MultiTargetPNPResults() {}

public MultiTargetPNPResults(PNPResults results, List<Integer> ids) {
estimatedPose = results;
fiducialIDsUsed = ids;
}

public static MultiTargetPNPResults createFromPacket(Packet packet) {
var results = PNPResults.createFromPacket(packet);
var ids = new ArrayList<Integer>(MAX_IDS);
for (int i = 0; i < MAX_IDS; i++) {
int targetId = (int) packet.decodeShort();
if (targetId > -1) ids.add(targetId);
}
return new MultiTargetPNPResults(results, ids);
}

public void populatePacket(Packet packet) {
estimatedPose.populatePacket(packet);
for (int i = 0; i < MAX_IDS; i++) {
if (i < fiducialIDsUsed.size()) {
packet.encode((short) fiducialIDsUsed.get(i).byteValue());
} else {
packet.encode((short) -1);
}
}
}

@Override
public int hashCode() {
final int prime = 31;
Expand Down Expand Up @@ -66,7 +96,8 @@ public String toString() {
+ "]";
}

public static final class AProto implements Protobuf<MultiTargetPNPResults, ProtobufMultiTargetPNPResults> {
public static final class AProto
implements Protobuf<MultiTargetPNPResults, ProtobufMultiTargetPNPResults> {
@Override
public Class<MultiTargetPNPResults> getTypeClass() {
return MultiTargetPNPResults.class;
Expand All @@ -87,10 +118,7 @@ public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) {
return new MultiTargetPNPResults(
PNPResults.proto.unpack(msg.getEstimatedPose()),
// TODO better way of doing this
Arrays.stream(msg.getFiducialIdsUsed().array())
.boxed()
.collect(Collectors.toList())
);
Arrays.stream(msg.getFiducialIdsUsed().array()).boxed().collect(Collectors.toList()));
}

@Override
Expand All @@ -99,7 +127,7 @@ public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value)

// TODO better way of doing this
int[] ids = new int[value.fiducialIDsUsed.size()];
for(int i = 0; i < ids.length; i++) {
for (int i = 0; i < ids.length; i++) {
ids[i] = value.fiducialIDsUsed.get(i);
}
msg.addAllFiducialIdsUsed(ids);
Expand Down
Loading

0 comments on commit 3fbab3a

Please sign in to comment.