Skip to content

Commit

Permalink
Merge branch 'master' into popout
Browse files Browse the repository at this point in the history
  • Loading branch information
@srimanachanta
srimanachanta committed Nov 15, 2023
2 parents 6f12191 + 308fd80 commit 16b11e2
Show file tree
Hide file tree
Showing 31 changed files with 833 additions and 191 deletions.
14 changes: 7 additions & 7 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 @@ -24,21 +24,21 @@
import org.photonvision.common.logging.Logger;
import org.photonvision.estimation.TargetModel;
import org.photonvision.estimation.VisionEstimation;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.target.TrackedTarget;

/** Estimate the camera pose given multiple Apriltag observations */
public class MultiTargetPNPPipe
extends CVPipe<
List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
List<TrackedTarget>, MultiTargetPNPResult, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);

private boolean hasWarned = false;

@Override
protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
protected MultiTargetPNPResult process(List<TrackedTarget> targetList) {
if (params == null
|| params.cameraCoefficients == null
|| params.cameraCoefficients.getCameraIntrinsicsMat() == null
Expand All @@ -48,13 +48,13 @@ protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
"Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
hasWarned = true;
}
return new MultiTargetPNPResults();
return new MultiTargetPNPResult();
}

return calculateCameraInField(targetList);
}

private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetList) {
private MultiTargetPNPResult calculateCameraInField(List<TrackedTarget> targetList) {
// Find tag IDs that exist in the tag layout
var tagIDsUsed = new ArrayList<Integer>();
for (var target : targetList) {
Expand All @@ -64,7 +64,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL

// Only run with multiple targets
if (tagIDsUsed.size() < 2) {
return new MultiTargetPNPResults();
return new MultiTargetPNPResult();
}

var estimatedPose =
Expand All @@ -75,7 +75,7 @@ private MultiTargetPNPResults calculateCameraInField(List<TrackedTarget> targetL
params.atfl,
params.targetModel);

return new MultiTargetPNPResults(estimatedPose, tagIDsUsed);
return new MultiTargetPNPResult(estimatedPose, tagIDsUsed);
}

public static class MultiTargetPNPPipeParams {
Expand Down
4 changes: 2 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
Expand Down Expand Up @@ -149,7 +149,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
}

// Do multi-tag pose estimation
MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
Expand Down
4 changes: 2 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.estimation.TargetModel;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.aruco.ArucoDetectionResult;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
Expand Down Expand Up @@ -170,7 +170,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
}

// Do multi-tag pose estimation
MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
MultiTargetPNPResult multiTagResult = new MultiTargetPNPResult();
if (settings.solvePNPEnabled && settings.doMultiTarget) {
var multiTagOutput = multiTagPNPPipe.run(targetList);
sumPipeNanosElapsed += multiTagOutput.nanosElapsed;
Expand Down
4 changes: 2 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.common.util.file.FileUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
Expand Down Expand Up @@ -147,7 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
sumPipeNanosElapsed,
fps, // Unused but here in case
Collections.emptyList(),
new MultiTargetPNPResults(),
new MultiTargetPNPResult(),
new Frame(
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
}
Expand Down
14 changes: 7 additions & 7 deletions photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
import java.util.Collections;
import java.util.List;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.targeting.MultiTargetPNPResults;
import org.photonvision.targeting.MultiTargetPNPResult;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.target.TrackedTarget;
Expand All @@ -31,23 +31,23 @@ public class CVPipelineResult implements Releasable {
public final double fps;
public final List<TrackedTarget> targets;
public final Frame inputAndOutputFrame;
public MultiTargetPNPResults multiTagResult;
public MultiTargetPNPResult multiTagResult;

public CVPipelineResult(
double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
this(processingNanos, fps, targets, new MultiTargetPNPResult(), inputFrame);
}

public CVPipelineResult(
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults,
MultiTargetPNPResult multiTagResult,
Frame inputFrame) {
this.processingNanos = processingNanos;
this.fps = fps;
this.targets = targets != null ? targets : Collections.emptyList();
this.multiTagResult = multiTagResults;
this.multiTagResult = multiTagResult;

this.inputAndOutputFrame = inputFrame;
}
Expand All @@ -56,8 +56,8 @@ public CVPipelineResult(
double processingNanos,
double fps,
List<TrackedTarget> targets,
MultiTargetPNPResults multiTagResults) {
this(processingNanos, fps, targets, multiTagResults, null);
MultiTargetPNPResult multiTagResult) {
this(processingNanos, fps, targets, multiTagResult, null);
}

public boolean hasTargets() {
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ dependencies {
implementation group: "com.fasterxml.jackson.core", name: "jackson-databind", version: wpi.versions.jacksonVersion.get()

implementation group: "org.ejml", name: "ejml-simple", version: wpi.versions.ejmlVersion.get()
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get();
implementation group: "us.hebi.quickbuf", name: "quickbuf-runtime", version: wpi.versions.quickbufVersion.get()

// Junit
testImplementation wpilibTools.deps.wpilibJava("cscore")
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/publish.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ publishing {

tasks.withType(PublishToMavenRepository) {
doFirst {
println("Publishing to " + repository.url);
println("Publishing to " + repository.url)
}
}

Expand Down
68 changes: 7 additions & 61 deletions photon-lib/src/main/java/org/photonvision/PhotonCamera.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,6 @@

package org.photonvision;

import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
Expand All @@ -43,7 +41,6 @@
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.PubSubOption;
import edu.wpi.first.networktables.RawSubscriber;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StringSubscriber;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
Expand Down Expand Up @@ -73,9 +70,8 @@ public class PhotonCamera implements AutoCloseable {
IntegerPublisher pipelineIndexRequest, ledModeRequest;
IntegerSubscriber pipelineIndexState, ledModeState;
IntegerSubscriber heartbeatEntry;
private DoubleArraySubscriber cameraIntrinsicsSubscriber;
private DoubleArraySubscriber cameraDistortionSubscriber;
private StringPublisher atflPublisher;
DoubleArraySubscriber cameraIntrinsicsSubscriber;
DoubleArraySubscriber cameraDistortionSubscriber;

@Override
public void close() {
Expand All @@ -99,7 +95,6 @@ public void close() {
pipelineIndexRequest.close();
cameraIntrinsicsSubscriber.close();
cameraDistortionSubscriber.close();
atflPublisher.close();
}

private final String path;
Expand All @@ -111,20 +106,14 @@ public void close() {

private long prevHeartbeatValue = -1;
private double prevHeartbeatChangeTime = 0;
private static final double HEARBEAT_DEBOUNCE_SEC = 0.5;
private static final double HEARTBEAT_DEBOUNCE_SEC = 0.5;

public static void setVersionCheckEnabled(boolean enabled) {
VERSION_CHECK_ENABLED = enabled;
}

Packet packet = new Packet(1);

private static AprilTagFieldLayout lastSetTagLayout =
AprilTagFields.kDefaultField.loadAprilTagLayoutField();

// Existing is enough to make this multisubscriber do its thing
private final MultiSubscriber m_topicNameSubscriber;

/**
* Constructs a PhotonCamera from a root table.
*
Expand Down Expand Up @@ -159,11 +148,8 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) {
ledModeState = photonvision_root_table.getIntegerTopic("ledModeState").subscribe(-1);
versionEntry = photonvision_root_table.getStringTopic("version").subscribe("");

atflPublisher = photonvision_root_table.getStringTopic("apriltag_field_layout").publish();
// Save the layout locally on Rio; on reboot, should be pushed out to NT clients
atflPublisher.getTopic().setPersistent(true);

m_topicNameSubscriber =
// Existing is enough to make this multisubscriber do its thing
MultiSubscriber m_topicNameSubscriber =
new MultiSubscriber(
instance, new String[] {"/photonvision/"}, PubSubOption.topicsOnly(true));
}
Expand Down Expand Up @@ -329,49 +315,9 @@ public boolean isConnected() {
prevHeartbeatValue = curHeartbeat;
}

return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
return (now - prevHeartbeatChangeTime) < HEARTBEAT_DEBOUNCE_SEC;
}

// TODO: Implement ATFL subscribing in backend
// /**
// * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all
// PhotonVision
// * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
// * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
// *
// * @return The last set layout
// */
// public AprilTagFieldLayout getAprilTagFieldLayout() {
// return lastSetTagLayout;
// }

// /**
// * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
// * later) connect to this robot. The topic is marked as persistent, so even if you only call
// this
// * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code
// reboots.
// * PhotonVision will also store a copy of this layout locally on the coprocessor, but
// subscribes
// * to this topic and the local copy will be updated when this function is called.
// *
// * @param layout The layout to use for *all* PhotonVision cameras
// * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
// * have updated their internal layouts.
// */
// public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
// try {
// var layout_json = new ObjectMapper().writeValueAsString(layout);
// atflPublisher.set(layout_json);
// lastSetTagLayout = layout;
// return true;
// } catch (JsonProcessingException e) {
// MathSharedStore.reportError("Error setting ATFL in " + this.getName(),
// e.getStackTrace());
// }
// return false;
// }

public Optional<Matrix<N3, N3>> getCameraMatrix() {
var cameraMatrix = cameraIntrinsicsSubscriber.get();
if (cameraMatrix != null && cameraMatrix.length == 9) {
Expand Down Expand Up @@ -428,7 +374,7 @@ else if (!isConnected()) {

// Check for version. Warn if the versions aren't aligned.
String versionString = versionEntry.get("");
if (!versionString.equals("") && !PhotonVersion.versionMatches(versionString)) {
if (!versionString.isEmpty() && !PhotonVersion.versionMatches(versionString)) {
// Error on a verified version mismatch
// But stay silent otherwise
DriverStation.reportWarning(
Expand Down
6 changes: 3 additions & 3 deletions photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -443,15 +443,15 @@ private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
}

var pnpResults =
var pnpResult =
VisionEstimation.estimateCamPosePNP(
cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
// try fallback strategy if solvePNP fails for some reason
if (!pnpResults.isPresent)
if (!pnpResult.isPresent)
return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
var best =
new Pose3d()
.plus(pnpResults.best) // field-to-camera
.plus(pnpResult.best) // field-to-camera
.plus(robotToCamera.inverse()); // field-to-robot

return Optional.of(
Expand Down
2 changes: 1 addition & 1 deletion photon-lib/src/main/java/org/photonvision/PhotonUtils.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,7 +201,7 @@ public static Rotation2d getYawToPose(Pose2d robotPose, Pose2d targetPose) {
*
* @param robotPose Pose of the robot
* @param targetPose Pose of the target
* @return
* @return distance to the pose
*/
public static double getDistanceToPose(Pose2d robotPose, Pose2d targetPose) {
return robotPose.getTranslation().getDistance(targetPose.getTranslation());
Expand Down
Loading

0 comments on commit 16b11e2

Please sign in to comment.