Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add solvePNP, 3d tab on the UI, and some other misc bug fixes (#35)

Browse Source 
* Rebase solvePNP on master

* added 3D tab minimap and csv reader

* More solvePNP

* Create draw pipe for pnp data

* SolvePNP piping work

* Move sorting into solvepnppipe

* Create calibration pipeline

* Update CalibrateSolvePNPPipeline.java

* add camera tilt angle

* Add calibration slider and snapshot button to 3D view

* Mirror updates in the socket handler

* add 3d calibration mode to the pipeline manager

* created calibration functions in ui and backend

* Start plumbing calibration

* Add snapshot and other handling to the RequestHandler

* added select resolution before starting calibration

* Rename solvePNPPipe to bounding box solve pnp pipe

* Update BoundingBoxSolvePNPPipe.java

* Add Mat serializer and CameraCalibrationConfig

* Begun calibration saving, fixed UI/Backend snapshot count mismatch

* Add (unplumbed) option to set checkerboard size

This will allow users to change the units their calibration is in

* Create chessboard.png

* Fix calibration NPE

* changed string serialization to a json send

* bug fixed cancellation button

* Fix spelling of snapshot in 3d.vue

* Plumb resolution change

* Set resolution during config, start on config serialization

* Update .gitignore

* Config fixes

* Start transition away from cvpipeline3d

* fix NPE on uncalibrated cameras

* clear list on fail

* Fix video mode index error

* ignore getters in camera calibration config

* Create json constructor for jsonmat

* get solvePNP mostly returning sane values

* Fix solvePNP bug and add unit test

* FIx calibration mat truncation

* added capture amount model upload and minimap data

* Standardize on meters in calibration and bounding box

* fix json out of bounds and handle null calibration more gracefully

* don't put text on calibrate image, go back to inches

* convert distance to meters

this means calibration will need to be in inches

* Actually save raw contor

* Update GroupContoursPipe.java

* Add all calibration return to camera capture

* hard code 2019 target

* bugfixed draw2d added fail calib popup, merge end and cancel

added the res index to the calib start

* Clarify error message and draw more fancy rectangles

* Cleanup memory in solvepnp

* re did minimap component

* fix npe if left/right is null

* remove references to 2d

* try-catch running the current pipeline

* Add method to find corners using the harris corner detector

* Possibly fix left/right missmatch

* Fix 3D Tab error

* FIx file permissions, mat serializer adjustments

* fixed mini map for field coordinates

* mini map changes fov

* Update SolvePNPPipe.java

* get rid of target corners

* some memory leak fixes

* fixed mini map location

* added position under minimap

* changed player fov look

* put all targets in the web send

* re did target send to ui added target tables, bugfix calibration

* fixed y position

* Add tilt angle to capture properties

* maybe fix y axis in minimap

* Add square size to onCalibrationEnding

* Possibly add square size to UI

* fix NPE with pitch

* Fix bug with sending multiple targets

* Only instantiate 3d stuff if we are in 3d mode

* Fix array list exceptions

* Fix bug in sort contors

list was truncated too early

* added download chess, tilt setting and ordinal tilt,

* added square size connection

* removed unused code

* Update pom version to 2.1-RELEASE

* Send camera calibrations to UI

* Stream pose list to a LIst

* Only stream necessary parts of the aux list entry

* Make broadcastMessage synchronized to prevent ConcurrentModificationExceptions

* added fps counter changed squaresize steps bug fixes in tables

* bugfix camera settings cam wont change

Authored-by: oriagranat9 <oriagranat9@gmail.com>
This commit is contained in:
Matt
2019-12-31 04:53:20 -08:00
committed by oriagranat9
parent d8c027dae7
commit 1decd2c3d7
70 changed files with 3029 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

146
chameleon-server/src/main/java/com/chameleonvision/vision/VisionProcess.java
View File

@@ -1,20 +1,24 @@
package com.chameleonvision.vision;
import com.chameleonvision.Debug;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.CameraConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.config.FullCameraConfiguration;
import com.chameleonvision.util.LoopingRunnable;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.camera.CameraStreamer;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.*;
import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
import com.chameleonvision.vision.pipeline.impl.CVPipeline3d;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.DriverVisionPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import com.chameleonvision.web.SocketHandler;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.*;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpiutil.CircularBuffer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
@@ -24,6 +28,7 @@ import java.util.HashMap;
import java.util.List;
import java.util.concurrent.BlockingQueue;
import java.util.concurrent.LinkedBlockingDeque;
import java.util.stream.Collectors;
public class VisionProcess {
@@ -31,6 +36,7 @@ public class VisionProcess {
private final USBCameraCapture cameraCapture;
private final CameraStreamerRunnable streamRunnable;
private final VisionProcessRunnable visionRunnable;
private final CameraConfig fileConfig;
public final CameraStreamer cameraStreamer;
public PipelineManager pipelineManager;
@@ -51,17 +57,20 @@ public class VisionProcess {
private NetworkTableEntry ntAreaEntry;
private NetworkTableEntry ntLatencyEntry;
private NetworkTableEntry ntValidEntry;
private NetworkTableEntry ntPoseEntry;
private ObjectMapper objectMapper = new ObjectMapper();
private long lastUIUpdateMs = 0;
VisionProcess(USBCameraCapture cameraCapture, String name, List<CVPipelineSettings> loadedPipelineSettings) {
VisionProcess(USBCameraCapture cameraCapture, FullCameraConfiguration config) {
this.cameraCapture = cameraCapture;
pipelineManager = new PipelineManager(this, loadedPipelineSettings);
fileConfig = config.fileConfig;
pipelineManager = new PipelineManager(this, config.pipelines);
// Thread to put frames on the dashboard
this.cameraStreamer = new CameraStreamer(cameraCapture, name,pipelineManager.getCurrentPipeline().settings.streamDivisor);
this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor);
this.streamRunnable = new CameraStreamerRunnable(30, cameraStreamer);
// Thread to process vision data
@@ -114,6 +123,7 @@ public class VisionProcess {
ntLatencyEntry = newTable.getEntry("latency");
ntValidEntry = newTable.getEntry("is_valid");
ntAuxListEntry = newTable.getEntry("aux_targets");
ntPoseEntry = newTable.getEntry("poseList");
ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate);
ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate);
ntDriverModeEntry.setBoolean(false);
@@ -132,6 +142,7 @@ public class VisionProcess {
/**
* Method called by the nt entry listener to update the next pipeline.
*
* @param notification the notification
*/
private void setPipeline(EntryNotification notification) {
@@ -143,7 +154,7 @@ public class VisionProcess {
// if it's null, we haven't even started the program yet, so just return
// otherwise, set it.
if(ntDriverModeEntry != null) {
if (ntDriverModeEntry != null) {
ntDriverModeEntry.setBoolean(isDriverMode);
}
}
@@ -151,41 +162,55 @@ public class VisionProcess {
private void updateUI(CVPipelineResult data) {
// 30 "FPS" update rate
long currentMillis = System.currentTimeMillis();
if (currentMillis - lastUIUpdateMs > 1000/30) {
if (currentMillis - lastUIUpdateMs > 1000 / 30) {
lastUIUpdateMs = currentMillis;
if(cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) {
HashMap<String, Object> WebSend = new HashMap<>();
HashMap<String, Object> point = new HashMap<>();
HashMap<String, Object> calculated = new HashMap<>();
HashMap<String, Object> pointMap = new HashMap<>();
ArrayList<Object> webTargets = new ArrayList<Object>();
List<Double> center = new ArrayList<>();
if (data.hasTarget) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
CVPipeline2d.Target2d bestTarget = result.targets.get(0);
center.add(bestTarget.rawPoint.center.x);
center.add(bestTarget.rawPoint.center.y);
calculated.put("pitch", bestTarget.pitch);
calculated.put("yaw", bestTarget.yaw);
calculated.put("area", bestTarget.area);
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff in UI
} else {
center.add(null);
center.add(null);
calculated.put("pitch", null);
calculated.put("yaw", null);
calculated.put("area", null);
if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
for (var target : result.targets) {
pointMap = new HashMap<>();
pointMap.put("pitch", target.pitch);
pointMap.put("yaw", target.yaw);
pointMap.put("area", target.area);
pointMap.put("pose", target.cameraRelativePose);
webTargets.add(pointMap);
}
} else {
pointMap.put("pitch", bestTarget.pitch);
pointMap.put("yaw", bestTarget.yaw);
pointMap.put("area", bestTarget.area);
pointMap.put("pose", bestTarget.cameraRelativePose);
webTargets.add(pointMap);
}
center.add(bestTarget.minAreaRect.center.x);
center.add(bestTarget.minAreaRect.center.y);
}
} else {
pointMap.put("pitch", null);
pointMap.put("yaw", null);
pointMap.put("area", null);
pointMap.put("pose", new Pose2d());
webTargets.add(pointMap);
center.add(null);
center.add(null);
calculated.put("pitch", null);
calculated.put("yaw", null);
calculated.put("area", null);
}
point.put("fps", visionRunnable.fps);
point.put("calculated", calculated);
point.put("targets", webTargets);
point.put("rawPoint", center);
WebSend.put("point", point);
SocketHandler.broadcastMessage(WebSend);
@@ -195,29 +220,32 @@ public class VisionProcess {
private void updateNetworkTableData(CVPipelineResult data) {
ntValidEntry.setBoolean(data.hasTarget);
if(data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
if(data instanceof CVPipeline2d.CVPipeline2dResult) {
if (data.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
//noinspection unchecked
List<CVPipeline2d.Target2d> targets = (List<CVPipeline2d.Target2d>) data.targets;
List<StandardCVPipeline.TrackedTarget> targets = (List<StandardCVPipeline.TrackedTarget>) data.targets;
ntLatencyEntry.setDouble(MathHandler.roundTo(data.processTime * 1e-6, 3));
ntPitchEntry.setDouble(targets.get(0).pitch);
ntYawEntry.setDouble(targets.get(0).yaw);
ntAreaEntry.setDouble(targets.get(0).area);
try {
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets));
ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream()
.map(it -> List.of(it.pitch, it.yaw, it.area, it.cameraRelativePose))
.collect(Collectors.toList())));
// TODO: (2.1) 3d stuff...
ntPoseEntry.setString(objectMapper.writeValueAsString(targets.stream().map(target -> target.cameraRelativePose).collect(Collectors.toList())));
} catch (JsonProcessingException e) {
e.printStackTrace();
}
} else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
// TODO: (2.1) 3d stuff...
} else {
ntPitchEntry.setDouble(0.0);
ntYawEntry.setDouble(0.0);
ntAreaEntry.setDouble(0.0);
ntLatencyEntry.setDouble(0.0);
ntAuxListEntry.setString("");
}
} else {
ntPitchEntry.setDouble(0.0);
ntYawEntry.setDouble(0.0);
ntAreaEntry.setDouble(0.0);
ntLatencyEntry.setDouble(0.0);
ntAuxListEntry.setString("");
}
tableInstance.flush();
@@ -244,6 +272,27 @@ public class VisionProcess {
return pipelineManager.driverModePipeline.settings;
}
public void addCalibration(CameraCalibrationConfig cal) {
cameraCapture.addCalibrationData(cal);
System.out.println("saving to file");
fileConfig.saveCalibration(cameraCapture.getConfig());
}
public void setIs3d(Boolean value) {
var settings = pipelineManager.getCurrentPipeline().settings;
if (settings instanceof StandardCVPipelineSettings) {
((StandardCVPipelineSettings) settings).is3D = value;
}
}
public boolean getIs3d() {
var settings = pipelineManager.getCurrentPipeline().settings;
if (settings instanceof StandardCVPipelineSettings) {
return ((StandardCVPipelineSettings) settings).is3D;
}
return false;
}
/**
* VisionProcessRunnable will process images as quickly as possible
*/
@@ -255,14 +304,21 @@ public class VisionProcess {
@Override
public void run() {
var lastUpdateTimeNanos = System.nanoTime();
while(!Thread.interrupted()) {
while (!Thread.interrupted()) {
// blocking call, will block until camera has a new frame.
Pair<Mat, Long> camData = cameraCapture.getFrame();
Mat camFrame = camData.getLeft();
if (camFrame.cols() > 0 && camFrame.rows() > 0) {
CVPipelineResult result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
CVPipelineResult result = null;
try {
result = pipelineManager.getCurrentPipeline().runPipeline(camFrame);
} catch (Exception e) {
System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!");
e.printStackTrace();
}
camFrame.release();
if (result != null) {
@@ -289,7 +345,7 @@ public class VisionProcess {
double getAverageFPS() {
var temp = 0.0;
for(int i = 0; i < 7; i++) {
for (int i = 0; i < 7; i++) {
temp += fpsAveragingBuffer.get(i);
}
temp /= 7.0;
@@ -305,7 +361,7 @@ public class VisionProcess {
private CameraStreamerRunnable(int cameraFPS, CameraStreamer streamer) {
// add 2 FPS to allow for a bit of overhead
super(1000L/(cameraFPS + 2));
super(1000L / (cameraFPS + 2));
this.streamer = streamer;
}