Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +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

60
chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java Normal file
View File

@@ -0,0 +1,60 @@
package com.chameleonvision.config;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonIgnoreType;
import com.fasterxml.jackson.annotation.JsonProperty;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
/**
* A class that holds a camera matrix and distortion coefficients for a given resolution
*/
public class CameraCalibrationConfig {
@JsonProperty("resolution") public final Size resolution;
@JsonProperty("cameraMatrix") public final JsonMat cameraMatrix;
@JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs;
@JsonCreator
public CameraCalibrationConfig(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraMatrix") JsonMat cameraMatrix,
@JsonProperty("distortionCoeffs") JsonMat distortionCoeffs) {
this.resolution = resolution;
this.cameraMatrix = cameraMatrix;
this.distortionCoeffs = distortionCoeffs;
}
public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs) {
this.resolution = resolution;
this.cameraMatrix = JsonMat.fromMat(cameraMatrix);
this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs);
}
@JsonIgnoreType
public static class UICameraCalibrationConfig {
public final int width;
public final int height;
public final double[] cameraMatrix;
public final double[] distortionCoeffs;
public UICameraCalibrationConfig(CameraCalibrationConfig config) {
width = (int) config.resolution.width;
height = (int) config.resolution.height;
cameraMatrix = config.cameraMatrix.data;
distortionCoeffs = config.distortionCoeffs.data;
}
}
@JsonIgnore
public Mat getCameraMatrixAsMat() {
return cameraMatrix.toMat();
}
@JsonIgnore
public MatOfDouble getDistortionCoeffsAsMat() {
return new MatOfDouble(distortionCoeffs.toMat());
}
}

68
chameleon-server/src/main/java/com/chameleonvision/config/CameraConfig.java
View File

@@ -9,30 +9,32 @@ import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
public class CameraConfig {
private static final Path camerasConfigFolderPath = Paths.get(ConfigManager.SettingsPath.toString(), "cameras");
private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras");
private final String cameraConfigName;
private final CameraJsonConfig preliminaryConfig;
private final Path configFolderPath;
private final Path configPath;
private final Path driverModePath;
private final Path calibrationPath;
final Path pipelineFolderPath;
public final PipelineConfig pipelineConfig;
CameraConfig(CameraJsonConfig config) {
preliminaryConfig = config;
cameraConfigName = preliminaryConfig.name.replace(' ', '_');
String cameraConfigName = preliminaryConfig.name.replace(' ', '_');
pipelineConfig = new PipelineConfig(this);
configFolderPath = Paths.get(camerasConfigFolderPath.toString(), cameraConfigName);
configPath = Paths.get(configFolderPath.toString(), "camera.json");
driverModePath = Paths.get(configFolderPath.toString(), "drivermode.json");
configFolderPath = Path.of(camerasConfigFolderPath.toString(), cameraConfigName);
configPath = Path.of(configFolderPath.toString(), "camera.json");
driverModePath = Path.of(configFolderPath.toString(), "drivermode.json");
calibrationPath = Path.of(configFolderPath.toString(), "calibration.json");
pipelineFolderPath = Paths.get(configFolderPath.toString(), "pipelines");
}
@@ -40,9 +42,10 @@ public class CameraConfig {
checkFolder();
checkConfig();
checkDriverMode();
checkCalibration();
pipelineConfig.check();
return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), this);
return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this);
}
private CameraJsonConfig loadConfig() {
@@ -57,15 +60,28 @@ public class CameraConfig {
private CVPipelineSettings loadDriverMode() {
CVPipelineSettings driverMode = new CVPipelineSettings();
driverMode.nickname = "DRIVERMODE";
try {
driverMode = JacksonHelper.deserializer(driverModePath, CVPipelineSettings.class);
} catch (IOException e) {
System.err.println("Failed to load camera drivermode: " + driverModePath.toString());
}
if (driverMode != null) {
driverMode.nickname = "DRIVERMODE";
driverMode.index = -1;
}
return driverMode;
}
private List<CameraCalibrationConfig> loadCalibration() {
List<CameraCalibrationConfig> calibrations = new ArrayList<>();
try {
calibrations = List.of(Objects.requireNonNull(JacksonHelper.deserializer(calibrationPath, CameraCalibrationConfig[].class)));
} catch (Exception e) {
System.err.println("Failed to load camera calibration: " + driverModePath.toString());
}
return calibrations;
}
void saveConfig(CameraJsonConfig config) {
try {
JacksonHelper.serializer(configPath, config);
@@ -88,8 +104,19 @@ public class CameraConfig {
}
}
public void saveCalibration(List<CameraCalibrationConfig> cal) {
CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]);
try {
JacksonHelper.serializer(calibrationPath, configs);
FileHelper.setFilePerms(calibrationPath);
} catch (IOException e) {
System.err.println("Failed to save camera calibration file: " + calibrationPath.toString());
}
}
void checkFolder() {
if (!getConfigFolderExists()) {
if (!configFolderExists()) {
try {
if (!(new File(configFolderPath.toUri()).mkdirs())) {
System.err.println("Failed to create camera config folder: " + configFolderPath.toString());
@@ -125,15 +152,30 @@ public class CameraConfig {
}
}
private boolean getConfigFolderExists() {
private void checkCalibration() {
if (!calibrationExists()) {
try {
List<CameraCalibrationConfig> calibrations = new ArrayList<>();
JacksonHelper.serializer(calibrationPath, calibrations.toArray());
} catch (IOException e) {
System.err.println("Failed to create camera calibration file: " + calibrationPath.toString());
}
}
}
private boolean configFolderExists() {
return Files.exists(configFolderPath);
}
private boolean configExists() {
return getConfigFolderExists() && Files.exists(configPath);
return configFolderExists() && Files.exists(configPath);
}
private boolean driverModeExists() {
return getConfigFolderExists() && Files.exists(driverModePath);
return configFolderExists() && Files.exists(driverModePath);
}
private boolean calibrationExists() {
return configFolderExists() && Files.exists(calibrationPath);
}
}

1
chameleon-server/src/main/java/com/chameleonvision/config/ConfigManager.java
View File

@@ -65,6 +65,7 @@ public class ConfigManager {
System.out.println("Settings folder: " + SettingsPath.toString());
checkSettingsFolder();
checkSettingsFile();
FileHelper.setAllPerms(SettingsPath);
}
private static void saveSettingsFile() {

8
chameleon-server/src/main/java/com/chameleonvision/config/FullCameraConfiguration.java
View File

@@ -7,13 +7,15 @@ import java.util.List;
public class FullCameraConfiguration {
public final CameraJsonConfig cameraConfig;
public final List<CVPipelineSettings> pipelines;
public final CVPipelineSettings drivermode;
public final CVPipelineSettings driverMode;
public final List<CameraCalibrationConfig> calibration;
public final CameraConfig fileConfig;
FullCameraConfiguration(CameraJsonConfig cameraConfig, List<CVPipelineSettings> pipelines, CVPipelineSettings drivermode, CameraConfig fileConfig) {
FullCameraConfiguration(CameraJsonConfig cameraConfig, List<CVPipelineSettings> pipelines, CVPipelineSettings driverMode, List<CameraCalibrationConfig> calibration, CameraConfig fileConfig) {
this.cameraConfig = cameraConfig;
this.pipelines = pipelines;
this.drivermode = drivermode;
this.driverMode = driverMode;
this.calibration = calibration;
this.fileConfig = fileConfig;
}
}

78
chameleon-server/src/main/java/com/chameleonvision/config/JsonMat.java Normal file
View File

@@ -0,0 +1,78 @@
package com.chameleonvision.config;
import com.fasterxml.jackson.annotation.JsonProperty;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import java.lang.reflect.Array;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
public class JsonMat {
public final int rows;
public final int cols;
public final int type;
public final double[] data;
public JsonMat(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
}
public JsonMat(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@JsonProperty("data") double[] data) {
this.rows = rows;
this.cols = cols;
this.type = type;
this.data = data;
}
public Mat toMat() {
return toMat(this);
}
private static boolean isCameraMatrixMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3;
}
private static boolean isDistortionCoeffsMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1;
}
private static boolean isCalibrationMat(Mat mat) {
return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
}
public static double[] getDataFromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
double[] data = new double[(int)(mat.total()*mat.elemSize())];
mat.get(0, 0, data);
int dataLen = -1;
if (isCameraMatrixMat(mat)) dataLen = 9;
if (isDistortionCoeffsMat(mat)) dataLen = 5;
// truncate Mat data to correct number data points.
return Arrays.copyOfRange(data, 0, dataLen);
}
public static JsonMat fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
}
public static Mat toMat(JsonMat jsonMat) {
if (jsonMat.type != CvType.CV_64FC1) return null;
Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type);
retMat.put(0, 0, jsonMat.data);
return retMat;
}
}

46
chameleon-server/src/main/java/com/chameleonvision/config/PipelineConfig.java
View File

@@ -3,8 +3,7 @@ package com.chameleonvision.config;
import com.chameleonvision.util.FileHelper;
import com.chameleonvision.util.JacksonHelper;
import com.chameleonvision.vision.pipeline.*;
import com.chameleonvision.vision.pipeline.impl.CVPipeline2dSettings;
import com.chameleonvision.vision.pipeline.impl.CVPipeline3dSettings;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import java.io.File;
import java.io.IOException;
@@ -16,9 +15,6 @@ import java.util.List;
public class PipelineConfig {
private static final String CVPipeline2DPrefix = "CV2D";
private static final String CVPipeline3DPrefix = "CV3D";
private final CameraConfig cameraConfig;
/**
@@ -57,14 +53,13 @@ public class PipelineConfig {
checkFolder();
// Check if there's at least one pipe
if (!folderHasPipelines()) {
save(new CVPipeline2dSettings());
save(new StandardCVPipelineSettings());
}
}
private Path getPipelinePath(CVPipelineSettings setting) {
String pipelineName = setting.nickname.replace(' ', '_');
String prefix = ((setting instanceof CVPipeline2dSettings) ? CVPipeline2DPrefix : CVPipeline3DPrefix) + "-";
String fullFileName = prefix + pipelineName + ".json";
String fullFileName = pipelineName + ".json";
return Path.of(cameraConfig.pipelineFolderPath.toString(), fullFileName);
}
@@ -76,22 +71,11 @@ public class PipelineConfig {
var path = getPipelinePath(settings);
if (settings instanceof CVPipeline3dSettings) {
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
} else if (settings instanceof CVPipeline2dSettings) {
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
} else {
throw new RuntimeException("saving non-2d and non-3d pipelines not implemented~");
try {
JacksonHelper.serializer(path, settings);
FileHelper.setFilePerms(path);
} catch (IOException e) {
e.printStackTrace();
}
}
@@ -134,24 +118,12 @@ public class PipelineConfig {
System.err.println("no pipes to load! loading default");
} else {
for(File pipelineFile : pipelineFiles) {
var name = pipelineFile.getName();
if(name.startsWith(CVPipeline3DPrefix)) {
// try to load 3d pipe
try {
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), CVPipeline3dSettings.class);
deserializedList.add(pipe);
} catch (IOException e) {
System.err.println("couldn't load cvpipeline3d");
}
} else if(name.startsWith(CVPipeline2DPrefix)) {
// try to load 2d pipe
try {
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), CVPipeline2dSettings.class);
var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class);
deserializedList.add(pipe);
} catch (IOException e) {
System.err.println("couldn't load cvpipeline2d");
}
}
}
}

2
chameleon-server/src/main/java/com/chameleonvision/network/NetworkManager.java
View File

@@ -20,7 +20,7 @@ public class NetworkManager {
return;
}
Platform platform = Platform.getCurrentPlatform();
Platform platform = Platform.CurrentPlatform;
if (platform.isLinux()) {
networking = new LinuxNetworking();

18
chameleon-server/src/main/java/com/chameleonvision/util/FileHelper.java
View File

@@ -17,10 +17,28 @@ public class FileHelper {
public static void setFilePerms(Path path) throws IOException {
if (!Platform.CurrentPlatform.isWindows()) {
File thisFile = path.toFile();
Set<PosixFilePermission> perms = Files.readAttributes(path, PosixFileAttributes.class).permissions();
if (!perms.equals(allReadWriteExecutePerms)) {
System.out.printf("setting perms on %s\n", path.toString());
Files.setPosixFilePermissions(path, perms);
if (thisFile.isDirectory()) {
for (File subfile : thisFile.listFiles()) {
setFilePerms(subfile.toPath());
}
}
}
}
}
public static void setAllPerms(Path path) {
String command = String.format("chmod 777 -R %s", path.toString());
try {
Process p = Runtime.getRuntime().exec(command);
p.waitFor();
} catch (Exception e) {
e.printStackTrace();
}
}
}

2
chameleon-server/src/main/java/com/chameleonvision/util/JacksonHelper.java
View File

@@ -4,9 +4,9 @@ import com.fasterxml.jackson.databind.ObjectMapper;
import com.fasterxml.jackson.databind.json.JsonMapper;
import com.fasterxml.jackson.databind.jsontype.BasicPolymorphicTypeValidator;
import com.fasterxml.jackson.databind.jsontype.PolymorphicTypeValidator;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
public class JacksonHelper {

11
chameleon-server/src/main/java/com/chameleonvision/vision/VisionManager.java
View File

@@ -6,7 +6,6 @@ import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.config.FullCameraConfiguration;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import edu.wpi.cscore.UsbCamera;
@@ -84,9 +83,9 @@ public class VisionManager {
CameraJsonConfig cameraJsonConfig = config.cameraConfig;
USBCameraCapture camera = new USBCameraCapture(cameraJsonConfig);
VisionProcess process = new VisionProcess(camera, cameraJsonConfig.name, config.pipelines);
process.pipelineManager.driverModePipeline.settings = config.drivermode;
USBCameraCapture camera = new USBCameraCapture(config);
VisionProcess process = new VisionProcess(camera, config);
process.pipelineManager.driverModePipeline.settings = config.driverMode;
visionProcesses.add(new VisionProcessManageable(i, cameraJsonConfig.name, process));
}
currentUIVisionProcess = getVisionProcessByIndex(0);
@@ -102,6 +101,10 @@ public class VisionManager {
return currentUIVisionProcess;
}
public static CameraConfig getCurrentCameraConfig() {
return getCameraConfig(currentUIVisionProcess);
}
public static CameraConfig getCameraConfig(VisionProcess process) {
String cameraName = process.getCamera().getProperties().name;
return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig;

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;
}

6
chameleon-server/src/main/java/com/chameleonvision/vision/camera/CameraCapture.java
View File

@@ -1,9 +1,12 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.image.CaptureProperties;
import com.chameleonvision.vision.image.ImageCapture;
import edu.wpi.cscore.VideoMode;
import java.util.List;
public interface CameraCapture extends ImageCapture {
CaptureProperties getProperties();
@@ -39,4 +42,7 @@ public interface CameraCapture extends ImageCapture {
* @param gain the new gain to set the camera to
*/
void setGain(int gain);
CameraCalibrationConfig getCurrentCalibrationData();
List<CameraCalibrationConfig> getAllCalibrationData();
}

43
chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCameraCapture.java
View File

@@ -1,7 +1,7 @@
package com.chameleonvision.vision.camera;
import com.chameleonvision.config.CameraJsonConfig;
import com.chameleonvision.vision.image.CaptureProperties;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.FullCameraConfiguration;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoException;
@@ -9,14 +9,23 @@ import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import java.util.ArrayList;
import java.util.List;
public class USBCameraCapture implements CameraCapture {
private final UsbCamera baseCamera;
private final CvSink cvSink;
private List<CameraCalibrationConfig> calibrationList;
private Mat imageBuffer = new Mat();
private USBCaptureProperties properties;
public USBCameraCapture(CameraJsonConfig config) {
public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) {
var config = fullCameraConfiguration.cameraConfig;
this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration;
calibrationList.addAll(fullCameraConfiguration.calibration);
baseCamera = new UsbCamera(config.name, config.path);
cvSink = CameraServer.getInstance().getVideo(baseCamera);
properties = new USBCaptureProperties(baseCamera, config);
@@ -25,6 +34,24 @@ public class USBCameraCapture implements CameraCapture {
setVideoMode(videoMode);
}
public CameraCalibrationConfig getCalibration(Size size) {
for(var calibration: calibrationList) {
if(calibration.resolution.equals(size)) return calibration;
}
return null;
}
public CameraCalibrationConfig getCalibration(VideoMode mode) {
return getCalibration(new Size(mode.width, mode.height));
}
public void addCalibrationData(CameraCalibrationConfig newConfig) {
calibrationList.add(newConfig);
}
public List<CameraCalibrationConfig> getConfig() {
return calibrationList;
}
@Override
public USBCaptureProperties getProperties() {
@@ -91,4 +118,14 @@ public class USBCameraCapture implements CameraCapture {
}
}
}
@Override
public CameraCalibrationConfig getCurrentCalibrationData() {
return getCalibration(getCurrentVideoMode());
}
@Override
public List<CameraCalibrationConfig> getAllCalibrationData() {
return calibrationList;
}
}

5
chameleon-server/src/main/java/com/chameleonvision/vision/camera/USBCaptureProperties.java
View File

@@ -5,6 +5,7 @@ import com.chameleonvision.util.Platform;
import com.chameleonvision.vision.image.CaptureProperties;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Arrays;
import java.util.List;
@@ -12,7 +13,7 @@ import java.util.function.Predicate;
import java.util.stream.Collectors;
import java.util.stream.Stream;
public class USBCaptureProperties extends com.chameleonvision.vision.image.CaptureProperties {
public class USBCaptureProperties extends CaptureProperties {
public static final double DEFAULT_FOV = 70;
private static final int DEFAULT_EXPOSURE = 50;
private static final int DEFAULT_BRIGHTNESS = 50;
@@ -106,4 +107,6 @@ public class USBCaptureProperties extends com.chameleonvision.vision.image.Captu
return getVideoModes().indexOf(getCurrentVideoMode());
}
}

10
chameleon-server/src/main/java/com/chameleonvision/vision/image/CaptureProperties.java
View File

@@ -2,11 +2,13 @@ package com.chameleonvision.vision.image;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.opencv.core.Mat;
public class CaptureProperties {
protected CaptureStaticProperties staticProperties;
private Rotation2d tilt = new Rotation2d();
protected CaptureProperties() {
}
@@ -18,4 +20,12 @@ public class CaptureProperties {
public CaptureStaticProperties getStaticProperties() {
return staticProperties;
}
public Rotation2d getTilt() {
return tilt;
}
public void setTilt(Rotation2d tilt) {
this.tilt = tilt;
}
}

12
chameleon-server/src/main/java/com/chameleonvision/vision/image/StaticImageCapture.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.image;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCaptureProperties;
import edu.wpi.cscore.VideoMode;
@@ -9,6 +10,7 @@ import org.opencv.imgcodecs.Imgcodecs;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.List;
public class StaticImageCapture implements CameraCapture {
@@ -73,4 +75,14 @@ public class StaticImageCapture implements CameraCapture {
public void setGain(int gain) {
// do nothing
}
@Override
public CameraCalibrationConfig getCurrentCalibrationData() {
return null;
}
@Override
public List<CameraCalibrationConfig> getAllCalibrationData() {
return null;
}
}

5
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/CVPipeline.java
View File

@@ -1,7 +1,6 @@
package com.chameleonvision.vision.pipeline;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.pipeline.impl.CVPipeline3d;
import org.opencv.core.Mat;
/**
@@ -30,8 +29,4 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
cameraCapture.setGain((int) settings.gain);
}
abstract public R runPipeline(Mat inputMat);
public boolean is3D() {
return (this instanceof CVPipeline3d);
}
}

2
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Pipe.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/Pipe.java
View File

@@ -1,4 +1,4 @@
package com.chameleonvision.vision.pipeline.pipes;
package com.chameleonvision.vision.pipeline;
import org.apache.commons.lang3.tuple.Pair;

45
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/PipelineManager.java
View File

@@ -6,6 +6,7 @@ import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.pipeline.impl.*;
import com.chameleonvision.web.SocketHandler;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.networktables.NetworkTableEntry;
import java.util.Comparator;
@@ -17,10 +18,12 @@ import java.util.List;
public class PipelineManager {
private static final int DRIVERMODE_INDEX = -1;
private static final int CAL_3D_INDEX = -2;
public final LinkedList<CVPipeline> pipelines = new LinkedList<>();
public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings());
public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings());
private final VisionProcess parentProcess;
private int lastPipelineIndex;
@@ -30,7 +33,7 @@ public class PipelineManager {
public PipelineManager(VisionProcess visionProcess, List<CVPipelineSettings> loadedPipelineSettings) {
parentProcess = visionProcess;
if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) {
pipelines.add(new CVPipeline2d("New Pipeline"));
pipelines.add(new StandardCVPipeline("New Pipeline"));
} else {
for (CVPipelineSettings setting : loadedPipelineSettings) {
addInternalPipeline(setting);
@@ -72,10 +75,8 @@ public class PipelineManager {
}
private void addInternalPipeline(CVPipelineSettings setting) {
if (setting instanceof CVPipeline3dSettings) {
pipelines.add(new CVPipeline3d((CVPipeline3dSettings) setting));
} else if (setting instanceof CVPipeline2dSettings) {
pipelines.add(new CVPipeline2d((CVPipeline2dSettings) setting));
if (setting instanceof StandardCVPipelineSettings) {
pipelines.add(new StandardCVPipeline((StandardCVPipelineSettings) setting));
} else {
System.out.println("Non 2D/3D pipelines not supported!");
}
@@ -83,11 +84,18 @@ public class PipelineManager {
}
public void setDriverMode(boolean driverMode) {
if (driverMode) {
setCurrentPipeline(DRIVERMODE_INDEX);
} else {
setCurrentPipeline(lastPipelineIndex);
}
if (driverMode) setCurrentPipeline(DRIVERMODE_INDEX);
else setCurrentPipeline(lastPipelineIndex);
}
public void setCalibrationMode(boolean calibrationMode) {
setCurrentPipeline((calibrationMode ? CAL_3D_INDEX : lastPipelineIndex));
}
public void enableCalibrationMode(VideoMode mode) {
parentProcess.setVideoMode(mode);
calib3dPipe.setVideoMode(mode);
setCalibrationMode(true);
}
public boolean getDriverMode() {
@@ -99,8 +107,10 @@ public class PipelineManager {
}
public CVPipeline getCurrentPipeline() {
if (currentPipelineIndex <= DRIVERMODE_INDEX) {
if (currentPipelineIndex == DRIVERMODE_INDEX) {
return driverModePipeline;
} else if (currentPipelineIndex <= CAL_3D_INDEX) {
return calib3dPipe;
} else {
return pipelines.get(currentPipelineIndex);
}
@@ -113,6 +123,10 @@ public class PipelineManager {
// if we're changing into driver mode, try to set the nt entry to true
parentProcess.setDriverModeEntry(true);
} else if (index == CAL_3D_INDEX) {
parentProcess.setDriverModeEntry(true);
newPipeline = calib3dPipe;
} else {
if (index < pipelines.size()&&index>=0) {
newPipeline = pipelines.get(index);
@@ -163,13 +177,8 @@ public class PipelineManager {
savePipelineConfig(pipeline.settings);
}
public void addNewPipeline(boolean is3D, String piplineName) {
CVPipeline newPipeline;
if (!is3D) {
newPipeline = new CVPipeline2d();
} else {
newPipeline = new CVPipeline3d();
}
public void addNewPipeline(String piplineName) {
StandardCVPipeline newPipeline = new StandardCVPipeline();
newPipeline.settings.nickname = piplineName;
newPipeline.settings.index = pipelines.size();
addPipeline(newPipeline);

37
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/CVPipeline3d.java
View File

@@ -1,37 +0,0 @@
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineResult;
import org.opencv.core.Mat;
import java.util.List;
import static com.chameleonvision.vision.pipeline.impl.CVPipeline3d.*;
public class CVPipeline3d extends CVPipeline<CVPipeline3dResult, CVPipeline3dSettings> {
public CVPipeline3d(CVPipeline3dSettings settings) {
super(settings);
}
public CVPipeline3d() {
super(new CVPipeline3dSettings());
}
@Override
public CVPipeline3dResult runPipeline(Mat inputMat) {
return null;
}
public static class CVPipeline3dResult extends CVPipelineResult<Target3d> {
public CVPipeline3dResult(List<Target3d> targets, Mat outputMat, long processTime) {
super(targets, outputMat, processTime);
}
}
public static class Target3d extends CVPipeline2d.Target2d {
// TODO: (2.1) Define 3d-specific target data
}
}

7
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/CVPipeline3dSettings.java
View File

@@ -1,7 +0,0 @@
package com.chameleonvision.vision.pipeline.impl;
public class CVPipeline3dSettings extends CVPipeline2dSettings {
// TODO: (2.1) define 3d-specific pipeline settings
// add 3d-specific property to ensure serializing/deserializing works
public boolean placeholder = false;
}

166
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java Normal file
View File

@@ -0,0 +1,166 @@
package com.chameleonvision.vision.pipeline.impl;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.List;
public class Calibrate3dPipeline extends CVPipeline<DriverVisionPipeline.DriverPipelineResult, StandardCVPipelineSettings> {
private int checkerboardSquaresHigh = 7;
private int checkerboardSquaresWide = 7;
private MatOfPoint3f objP_ORIG;
private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3);
private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide);
private Size imageSize;
double checkerboardSquareSize = 1; // inches!
private MatOfPoint2f calibrationOutput = new MatOfPoint2f();
private List<Mat> objpoints = new ArrayList<>();
private List<Mat> imgpoints = new ArrayList<>();
public static double checkerboardSquareSizeUnits = Units.inchesToMeters(1.0);
public static final int MIN_COUNT = 15;
private VideoMode calibrationMode;
private final Size windowSize = new Size(11, 11);
private final Size zeroZone = new Size(-1, -1);
private TermCriteria criteria = new TermCriteria(3, 30, 0.001); //(Imgproc.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
private int captureCount = 0;
private boolean wantsSnapshot = false;
public Calibrate3dPipeline(StandardCVPipelineSettings settings) {
super(settings);
objP_ORIG = new MatOfPoint3f();
objP = new MatOfPoint3f();
for(int i = 0; i < checkerboardSquaresHigh * checkerboardSquaresWide; i++) {
objP_ORIG.push_back(new MatOfPoint3f(new Point3(i / checkerboardSquaresWide, i % checkerboardSquaresHigh, 0.0f)));
}
setSquareSize(checkerboardSquareSizeUnits);
objpoints.forEach(Mat::release);
imgpoints.forEach(Mat::release);
objpoints.clear();
imgpoints.clear();
}
public void setSquareSize(double size) {
Core.multiply(objP_ORIG, new Scalar(new double[] { size, size, size }), objP);
}
public void takeSnapshot() {
wantsSnapshot = true;
}
public boolean hasEnoughSnapshots() {
return captureCount >= MIN_COUNT - 1;
}
@Override
public DriverVisionPipeline.DriverPipelineResult runPipeline(Mat inputMat) {
// look for checkerboard
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_BGR2GRAY);
var checkerboardFound = Calib3d.findChessboardCorners(inputMat, patternSize, calibrationOutput);
if(!checkerboardFound) {
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
}
// System.out.println("[SolvePNP] checkerboard found!!");
// cool we found a checkerboard
// do corner subpixel
Imgproc.cornerSubPix(inputMat, calibrationOutput, windowSize, zeroZone, criteria);
// convert back to BGR
Imgproc.cvtColor(inputMat, inputMat, Imgproc.COLOR_GRAY2BGR);
// draw the chessboard
Calib3d.drawChessboardCorners(inputMat, patternSize, calibrationOutput, true);
if(wantsSnapshot) {
this.imageSize = new Size(inputMat.width(), inputMat.height());
var mat = new MatOfPoint3f();
calibrationOutput.copyTo(mat);
this.objpoints.add(objP);
imgpoints.add(mat);
captureCount++;
wantsSnapshot = false;
}
imageSize = new Size(inputMat.width(), inputMat.height());
return new DriverVisionPipeline.DriverPipelineResult(null, inputMat, 0);
}
@Override
public void initPipeline(CameraCapture camera) {
super.initPipeline(camera);
objpoints.clear();
imgpoints.clear();
captureCount = 0;
}
public boolean tryCalibration() {
if (!hasEnoughSnapshots()) return false;
Mat cameraMatrix = new Mat();
Mat distortionCoeffs = new Mat();
List<Mat> rvecs = new ArrayList<>();
List<Mat> tvecs = new ArrayList<>();
try {
Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
} catch(Exception e) {
System.err.println("Camera calibration failed!");
initPipeline(cameraCapture);
return false;
}
VideoMode currentVidMode = cameraCapture.getCurrentVideoMode();
Size resolution = new Size(currentVidMode.width, currentVidMode.height);
CameraCalibrationConfig cal = new CameraCalibrationConfig(resolution, cameraMatrix, distortionCoeffs);
VisionManager.getCurrentUIVisionProcess().addCalibration(cal);
try {
System.out.printf("CALIBRATION SUCCESS! camMatrix: \n%s\ndistortionCoeffs:\n%s\n",
new ObjectMapper().writeValueAsString(cal.cameraMatrix), new ObjectMapper().writeValueAsString(cal.distortionCoeffs));
} catch (JsonProcessingException e) {
e.printStackTrace();
}
ConfigManager.saveGeneralSettings();
return true;
}
public void setVideoMode(VideoMode mode){
this.calibrationMode = mode;
}
public int getSnapshotCount() {
return captureCount + 1;
}
}

86
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/CVPipeline2d.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java
View File

@@ -7,15 +7,16 @@ import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineResult;
import com.chameleonvision.vision.pipeline.pipes.*;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import java.util.List;
import static com.chameleonvision.vision.pipeline.impl.CVPipeline2d.*;
import static com.chameleonvision.vision.pipeline.impl.StandardCVPipeline.*;
@SuppressWarnings("WeakerAccess")
public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSettings> {
public class StandardCVPipeline extends CVPipeline<StandardCVPipelineResult, StandardCVPipelineSettings> {
private Mat rawCameraMat = new Mat();
@@ -32,6 +33,8 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
private Draw2dContoursPipe.Draw2dContoursSettings draw2dContoursSettings;
private Draw2dContoursPipe draw2dContoursPipe;
private Draw2dCrosshairPipe draw2dCrosshairPipe;
private DrawSolvePNPPipe drawSolvePNPPipe;
private SolvePNPPipe solvePNPPipe;
private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings draw2dCrosshairPipeSettings;
private OutputMatPipe outputMatPipe;
@@ -39,15 +42,15 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
private CaptureStaticProperties camProps;
private Scalar hsvLower, hsvUpper;
public CVPipeline2d() {
super(new CVPipeline2dSettings());
public StandardCVPipeline() {
super(new StandardCVPipelineSettings());
}
public CVPipeline2d(String name) {
super(name, new CVPipeline2dSettings());
public StandardCVPipeline(String name) {
super(name, new StandardCVPipelineSettings());
}
public CVPipeline2d(CVPipeline2dSettings settings) {
public StandardCVPipeline(StandardCVPipelineSettings settings) {
super(settings);
}
@@ -71,6 +74,7 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
collect2dTargetsPipe = new Collect2dTargetsPipe(settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB, camProps);
draw2dContoursSettings = new Draw2dContoursPipe.Draw2dContoursSettings();
draw2dCrosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings();
// TODO: make settable from UI? config?
draw2dContoursSettings.showCentroid = false;
draw2dContoursSettings.boxOutlineSize = 2;
@@ -86,7 +90,7 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
private final MemoryManager memManager = new MemoryManager(120, 20000);
@Override
public CVPipeline2dResult runPipeline(Mat inputMat) {
public StandardCVPipelineResult runPipeline(Mat inputMat) {
long totalPipelineTimeNanos = 0;
long pipelineStartTimeNanos = System.nanoTime();
@@ -121,12 +125,21 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
draw2dCrosshairPipe.setConfig(draw2dCrosshairPipeSettings,settings.calibrationMode,settings.point,settings.dualTargetCalibrationM,settings.dualTargetCalibrationB);
outputMatPipe.setConfig(settings.isBinary);
if(settings.is3D) {
if(solvePNPPipe == null) solvePNPPipe = new SolvePNPPipe(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
if(drawSolvePNPPipe == null) drawSolvePNPPipe = new DrawSolvePNPPipe(cameraCapture.getCurrentCalibrationData());
solvePNPPipe.setConfig(settings, cameraCapture.getCurrentCalibrationData(), cameraCapture.getProperties().getTilt());
drawSolvePNPPipe.setConfig(cameraCapture.getCurrentCalibrationData());
}
long pipeInitTimeNanos = System.nanoTime() - pipelineStartTimeNanos;
// run pipes
Pair<Mat, Long> rotateFlipResult = rotateFlipPipe.run(inputMat);
totalPipelineTimeNanos += rotateFlipResult.getRight();
inputMat.copyTo(rawCameraMat);
// Pair<Mat, Long> blurResult = blurPipe.run(rotateFlipResult.getLeft());
@@ -147,27 +160,48 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
Pair<List<MatOfPoint>, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft());
totalPipelineTimeNanos += speckleRejectResult.getRight();
Pair<List<RotatedRect>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
Pair<List<TrackedTarget>, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft());
totalPipelineTimeNanos += groupContoursResult.getRight();
Pair<List<RotatedRect>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
Pair<List<TrackedTarget>, Long> sortContoursResult = sortContoursPipe.run(groupContoursResult.getLeft());
totalPipelineTimeNanos += sortContoursResult.getRight();
Pair<List<Target2d>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
Pair<List<TrackedTarget>, Long> collect2dTargetsResult = collect2dTargetsPipe.run(Pair.of(sortContoursResult.getLeft(), camProps));
totalPipelineTimeNanos += collect2dTargetsResult.getRight();
// takes pair of (Mat of original camera image (8UC3), Mat of HSV thresholded image(8UC1))
Pair<Mat, Long> outputMatResult = outputMatPipe.run(Pair.of(rawCameraMat, hsvResult.getLeft()));
totalPipelineTimeNanos += outputMatResult.getRight();
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalPipelineTimeNanos += draw2dContoursResult.getRight();
Pair<Mat, Long> result;
if(!settings.is3D) {
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
result = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
totalPipelineTimeNanos += result.getRight();
} else {
result = outputMatResult;
}
// takes pair of (Mat to draw on, List<RotatedRect> of sorted contours)
Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(draw2dContoursResult.getLeft(),collect2dTargetsResult.getLeft()));
Pair<Mat, Long> draw2dCrosshairResult = draw2dCrosshairPipe.run(Pair.of(result.getLeft(),collect2dTargetsResult.getLeft()));
totalPipelineTimeNanos += draw2dCrosshairResult.getRight();
Mat outputMat;
if(settings.is3D) {
// once we've sorted our targets, perform solvePNP. The number of "best targets" is limited by the above pipe
Pair<List<TrackedTarget>, Long> solvePNPResult = solvePNPPipe.run(collect2dTargetsResult.getLeft());
totalPipelineTimeNanos += solvePNPResult.getRight();
Pair<Mat, Long> draw3dContoursResult = drawSolvePNPPipe.run(Pair.of(outputMatResult.getLeft(), solvePNPResult.getLeft()));
totalPipelineTimeNanos += draw3dContoursResult.getRight();
outputMat = draw3dContoursResult.getLeft();
} else {
outputMat = draw2dCrosshairResult.getLeft();
}
if (Main.testMode) {
pipelineTimeString += String.format("PipeInit: %.2fms, ", pipeInitTimeNanos / 1000000.0);
pipelineTimeString += String.format("RotateFlip: %.2fms, ", rotateFlipResult.getRight() / 1000000.0);
@@ -181,7 +215,7 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
pipelineTimeString += String.format("SortContours: %.2fms, ", sortContoursResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Collect2dTargets: %.2fms, ", collect2dTargetsResult.getRight() / 1000000.0);
pipelineTimeString += String.format("OutputMat: %.2fms, ", outputMatResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dContours: %.2fms, ", draw2dContoursResult.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dContours: %.2fms, ", result.getRight() / 1000000.0);
pipelineTimeString += String.format("Draw2dCrosshair: %.2fms, ", draw2dCrosshairResult.getRight() / 1000000.0);
System.out.println(pipelineTimeString);
@@ -195,22 +229,30 @@ public class CVPipeline2d extends CVPipeline<CVPipeline2dResult, CVPipeline2dSet
memManager.run();
return new CVPipeline2dResult(collect2dTargetsResult.getLeft(), draw2dCrosshairResult.getLeft(), totalPipelineTimeNanos);
return new StandardCVPipelineResult(collect2dTargetsResult.getLeft(), outputMat, totalPipelineTimeNanos);
}
public static class CVPipeline2dResult extends CVPipelineResult<Target2d> {
public CVPipeline2dResult(List<Target2d> targets, Mat outputMat, long processTimeNanos) {
public static class StandardCVPipelineResult extends CVPipelineResult<TrackedTarget> {
public StandardCVPipelineResult(List<TrackedTarget> targets, Mat outputMat, long processTimeNanos) {
super(targets, outputMat, processTimeNanos);
}
}
public static class Target2d {
public static class TrackedTarget {
public double calibratedX = 0.0;
public double calibratedY = 0.0;
public double pitch = 0.0;
public double yaw = 0.0;
public double area = 0.0;
public RotatedRect rawPoint;
public RotatedRect minAreaRect;
public MatOfPoint2f contour;
// 3d stuff
public Pose2d cameraRelativePose = new Pose2d();
public Mat rVector = new Mat();
public Mat tVector = new Mat();
public MatOfPoint2f imageCornerPoints = new MatOfPoint2f();
public Pair<Rect, Rect> leftRightDualTargetPair = null;
}

13
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/CVPipeline2dSettings.java → chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java
View File

@@ -5,11 +5,17 @@ import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.annotation.JsonIgnore;
import edu.wpi.first.wpilibj.util.Units;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import java.util.Arrays;
import java.util.List;
public class CVPipeline2dSettings extends CVPipelineSettings {
public class StandardCVPipelineSettings extends CVPipelineSettings {
public List<Number> hue = Arrays.asList(50, 180);
public List<Number> saturation = Arrays.asList(50, 255);
public List<Number> value = Arrays.asList(50, 255);
@@ -28,4 +34,9 @@ public class CVPipeline2dSettings extends CVPipelineSettings {
public CalibrationMode calibrationMode = CalibrationMode.None;
public double dualTargetCalibrationM = 1;
public double dualTargetCalibrationB = 0;
// 3d stuff
public double targetWidth = 15.5, targetHeight = 6.0;
public boolean is3D = false;
}

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/BlurPipe.java
View File

@@ -1,10 +1,8 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.CvException;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
public class BlurPipe implements Pipe<Mat, Mat> {

24
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Collect2dTargetsPipe.java
View File

@@ -1,23 +1,23 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.enums.CalibrationMode;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.RotatedRect;
import java.util.ArrayList;
import java.util.List;
public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, CaptureStaticProperties>, List<CVPipeline2d.Target2d>> {
public class Collect2dTargetsPipe implements Pipe<Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties>, List<StandardCVPipeline.TrackedTarget>> {
private CaptureStaticProperties camProps;
private CalibrationMode calibrationMode;
private List<Number> calibrationPoint;
private double calibrationM, calibrationB;
private List<CVPipeline2d.Target2d> targets = new ArrayList<>();
private List<StandardCVPipeline.TrackedTarget> targets = new ArrayList<>();
public Collect2dTargetsPipe(CalibrationMode calibrationMode, List<Number> calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) {
setConfig(calibrationMode, calibrationPoint, calibrationM, calibrationB, camProps);
@@ -32,7 +32,7 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, Captur
}
@Override
public Pair<List<CVPipeline2d.Target2d>, Long> run(Pair<List<RotatedRect>, CaptureStaticProperties> inputPair) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(Pair<List<StandardCVPipeline.TrackedTarget>, CaptureStaticProperties> inputPair) {
long processStartNanos = System.nanoTime();
targets.clear();
@@ -40,9 +40,7 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, Captur
var imageArea = inputPair.getRight().imageArea;
if (input.size() > 0) {
for (RotatedRect r : input) {
CVPipeline2d.Target2d t = new CVPipeline2d.Target2d();
t.rawPoint = r;
for (var t : input) {
switch (this.calibrationMode) {
case Single:
if(this.calibrationPoint.isEmpty())
@@ -58,14 +56,14 @@ public class Collect2dTargetsPipe implements Pipe<Pair<List<RotatedRect>, Captur
t.calibratedY = camProps.centerY;
break;
case Dual:
t.calibratedX = (r.center.y - this.calibrationB) / this.calibrationM;
t.calibratedY = (r.center.x * this.calibrationM) + this.calibrationB;
t.calibratedX = (t.minAreaRect.center.y - this.calibrationB) / this.calibrationM;
t.calibratedY = (t.minAreaRect.center.x * this.calibrationM) + this.calibrationB;
break;
}
t.pitch = calculatePitch(r.center.y, t.calibratedY);
t.yaw = calculateYaw(r.center.x, t.calibratedX);
t.area = r.size.area() / imageArea;
t.pitch = calculatePitch(t.minAreaRect.center.y, t.calibratedY);
t.yaw = calculateYaw(t.minAreaRect.center.x, t.calibratedX);
t.area = t.minAreaRect.size.area() / imageArea;
targets.add(t);
}

16
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dContoursPipe.java
View File

@@ -2,6 +2,8 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Point;
import org.opencv.core.*;
@@ -11,7 +13,7 @@ import java.awt.*;
import java.util.ArrayList;
import java.util.List;
public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Mat> {
public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private final Draw2dContoursSettings settings;
private CaptureStaticProperties camProps;
@@ -37,7 +39,7 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<RotatedRect>> input) {
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> input) {
long processStartNanos = System.nanoTime();
if (settings.showCentroid || settings.showMaximumBox || settings.showRotatedBox) {
@@ -49,7 +51,8 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
if (i != 0 && !settings.showMultiple){
break;
}
RotatedRect r = input.getRight().get(i);
StandardCVPipeline.TrackedTarget target = input.getRight().get(i);
RotatedRect r = input.getRight().get(i).minAreaRect;
if (r == null) continue;
drawnContours.forEach(Mat::release);
@@ -73,6 +76,12 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
Imgproc.rectangle(input.getLeft(), new Point(box.x, box.y), new Point((box.x + box.width), (box.y + box.height)), Helpers.colorToScalar(settings.maximumBoxColor), settings.boxOutlineSize);
}
if(settings.showContor) {
MatOfPoint matOfPoint = new MatOfPoint();
matOfPoint.fromArray(target.contour.toArray());
Imgproc.drawContours(input.getLeft(), List.of(matOfPoint), 0, new Scalar(255, 255, 255));
}
// contour.release();
}
}
@@ -95,5 +104,6 @@ public class Draw2dContoursPipe implements Pipe<Pair<Mat, List<RotatedRect>>, Ma
public Color centroidColor = Color.GREEN;
public Color rotatedBoxColor = Color.BLUE;
public Color maximumBoxColor = Color.RED;
public boolean showContor = true;
}
}

9
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/Draw2dCrosshairPipe.java
View File

@@ -2,7 +2,8 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.enums.CalibrationMode;
import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Point;
@@ -11,7 +12,7 @@ import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.List;
public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<CVPipeline2d.Target2d>>, Mat> {
public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
//Settings
private Draw2dCrosshairPipeSettings crosshairSettings;
@@ -35,10 +36,10 @@ public class Draw2dCrosshairPipe implements Pipe<Pair<Mat, List<CVPipeline2d.Tar
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<CVPipeline2d.Target2d>> inputPair) {
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> inputPair) {
long processStartNanos = System.nanoTime();
Mat image = inputPair.getLeft();
List<CVPipeline2d.Target2d> targets = inputPair.getRight();
List<StandardCVPipeline.TrackedTarget> targets = inputPair.getRight();
double x = 0, y = 0, scale = image.cols() / 32.0;
drawCrosshair:

109
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java Normal file
View File

@@ -0,0 +1,109 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.util.Helpers;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
import java.awt.*;
import java.util.List;
public class DrawSolvePNPPipe implements Pipe<Pair<Mat, List<StandardCVPipeline.TrackedTarget>>, Mat> {
private MatOfPoint3f boxCornerMat = new MatOfPoint3f();
public Scalar color = Helpers.colorToScalar(Color.GREEN);
public DrawSolvePNPPipe(CameraCalibrationConfig settings) {
setConfig(settings);
setObjectBox(14.5, 6, 2);
}
public void setObjectBox(double targetWidth, double targetHeight, double targetDepth) {
// implementation from 5190 Green Hope Falcons
boxCornerMat.release();
boxCornerMat = new MatOfPoint3f(
new Point3(-targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, targetHeight/2d, 0),
new Point3(targetWidth/2d, -targetHeight/2d, 0),
new Point3(-targetWidth/2d, -targetHeight/2d, -targetDepth),
new Point3(-targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, targetHeight/2d, -targetDepth),
new Point3(targetWidth/2d, -targetHeight/2d, -targetDepth)
);
}
private Mat cameraMatrix = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
public void setConfig(CameraCalibrationConfig config) {
if(config == null) {
System.err.println("got passed a null config! Returning...");
return;
}
setConfig(config.getCameraMatrixAsMat(), config.getDistortionCoeffsAsMat());
}
public void setConfig(Mat cameraMatrix_, MatOfDouble distortionMatrix_) {
this.cameraMatrix = cameraMatrix_;
this.distortionCoefficients = distortionMatrix_;
}
@Override
public Pair<Mat, Long> run(Pair<Mat, List<StandardCVPipeline.TrackedTarget>> targets) {
long processStartNanos = System.nanoTime();
var image = targets.getLeft();
for(var it : targets.getRight()) {
MatOfPoint2f imagePoints = new MatOfPoint2f();
try {
Calib3d.projectPoints(boxCornerMat, it.rVector, it.tVector, this.cameraMatrix, this.distortionCoefficients, imagePoints, new Mat() , 0);
} catch (Exception e) {
e.printStackTrace();
}
var pts = imagePoints.toList();
// draw left and right targets if possible
if(it.leftRightDualTargetPair != null) {
var left = it.leftRightDualTargetPair.getLeft();
var right = it.leftRightDualTargetPair.getRight();
Imgproc.rectangle(image, left.tl(), left.br(), new Scalar(200, 200, 0), 4);
Imgproc.rectangle(image, right.tl(), right.br(), new Scalar(200, 200, 0), 2);
}
// draw corners
for(int i = 0; i < it.imageCornerPoints.rows(); i++) {
var point = new Point(it.imageCornerPoints.get(i, 0));
Imgproc.circle(image, point, 4, new Scalar(0, 255, 0), 5);
}
// sketch out floor
Imgproc.line(image, pts.get(0), pts.get(1), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(2), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(3), new Scalar(0, 255, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(0), new Scalar(0, 255, 0), 3);
// draw pillars
Imgproc.line(image, pts.get(0), pts.get(4), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(1), pts.get(5), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(2), pts.get(6), new Scalar(255, 0, 0), 3);
Imgproc.line(image, pts.get(3), pts.get(7), new Scalar(255, 0, 0), 3);
// draw top
Imgproc.line(image, pts.get(4), pts.get(5), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(5), pts.get(6), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(6), pts.get(7), new Scalar(0, 0, 255), 3);
Imgproc.line(image, pts.get(7), pts.get(4), new Scalar(0, 0, 255), 3);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(image, processTime);
}
}

4
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/ErodeDilatePipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Size;
@@ -11,9 +12,6 @@ public class ErodeDilatePipe implements Pipe<Mat, Mat> {
private boolean dilate;
private Mat kernel;
private Mat processBuffer = new Mat();
private Mat outputMat = new Mat();
public ErodeDilatePipe(boolean erode, boolean dilate, int kernelSize) {
this.erode = erode;
this.dilate = dilate;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FilterContoursPipe.java
View File

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/FindContoursPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;

24
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/GroupContoursPipe.java
View File

@@ -3,6 +3,8 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.util.MathHandler;
import com.chameleonvision.vision.enums.TargetGroup;
import com.chameleonvision.vision.enums.TargetIntersection;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
@@ -13,7 +15,7 @@ import java.util.Collections;
import java.util.Comparator;
import java.util.List;
public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRect>> {
public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<StandardCVPipeline.TrackedTarget>> {
private static final Comparator<MatOfPoint> sortByMomentsX =
Comparator.comparingDouble(GroupContoursPipe::calcMomentsX);
@@ -21,7 +23,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
private TargetGroup group;
private TargetIntersection intersection;
private List<RotatedRect> groupedContours = new ArrayList<>();
private List<StandardCVPipeline.TrackedTarget> groupedContours = new ArrayList<>();
private MatOfPoint2f intersectMatA = new MatOfPoint2f();
private MatOfPoint2f intersectMatB = new MatOfPoint2f();
@@ -36,7 +38,7 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
}
@Override
public Pair<List<RotatedRect>, Long> run(List<MatOfPoint> input) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<MatOfPoint> input) {
long processStartNanos = System.nanoTime();
groupedContours.clear();
@@ -55,7 +57,10 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
contour.fromArray(c.toArray());
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.contour = contour;
groupedContours.add(target);
}
});
break;
@@ -77,15 +82,20 @@ public class GroupContoursPipe implements Pipe<List<MatOfPoint>, List<RotatedRec
intersectMatA.release();
intersectMatB.release();
firstContour.release();
secondContour.release();
MatOfPoint2f contour = new MatOfPoint2f();
contour.fromList(finalContourList);
if (contour.cols() != 0 && contour.rows() != 0) {
RotatedRect rect = Imgproc.minAreaRect(contour);
groupedContours.add(rect);
var target = new StandardCVPipeline.TrackedTarget();
target.minAreaRect = rect;
target.contour = contour;
target.leftRightDualTargetPair =
Pair.of(Imgproc.boundingRect(firstContour),
Imgproc.boundingRect(secondContour));
groupedContours.add(target);
}
} catch (IndexOutOfBoundsException e) {
finalContourList.clear();

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/HsvPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.CvException;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/OutputMatPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.CvException;
import org.opencv.core.Mat;

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/RotateFlipPipe.java
View File

@@ -2,6 +2,7 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.enums.ImageFlipMode;
import com.chameleonvision.vision.enums.ImageRotationMode;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Core;
import org.opencv.core.Mat;

263
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java Normal file
View File

@@ -0,0 +1,263 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
public class SolvePNPPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
private Double tilt_angle;
private MatOfPoint3f objPointsMat = new MatOfPoint3f();
private Mat rVec = new Mat();
private Mat tVec = new Mat();
private Mat rodriguez = new Mat();
private Mat pzero_world = new Mat();
private Mat cameraMatrix = new Mat();
Mat rot_inv = new Mat();
Mat kMat = new Mat();
private MatOfDouble distortionCoefficients = new MatOfDouble();
private List<StandardCVPipeline.TrackedTarget> poseList = new ArrayList<>();
Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) {
super();
setCameraCoeffs(calibration);
setTarget(settings.targetWidth, settings.targetHeight);
this.tilt_angle = tilt.getRadians();
}
public void setTarget(double targetWidth, double targetHeight) {
// order is left top, left bottom, right bottom, right top
List<Point3> corners = List.of(
new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0),
new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0),
new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0),
new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0)
);
setObjectCorners(corners);
}
public void setObjectCorners(List<Point3> objectCorners) {
objPointsMat.release();
objPointsMat = new MatOfPoint3f();
objPointsMat.fromList(objectCorners);
}
public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) {
setCameraCoeffs(camConfig);
setTarget(settings.targetWidth, settings.targetHeight);
tilt_angle = tilt.getRadians();
}
private void setCameraCoeffs(CameraCalibrationConfig settings) {
if(settings == null) {
System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not! Please calibrate to use solvePNP.");
return;
}
if(cameraMatrix != settings.getCameraMatrixAsMat()) {
cameraMatrix.release();
settings.getCameraMatrixAsMat().copyTo(cameraMatrix);
}
if(distortionCoefficients != settings.getDistortionCoeffsAsMat()) {
distortionCoefficients.release();
settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients);
}
}
@Override
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> targets) {
long processStartNanos = System.nanoTime();
poseList.clear();
for(var target: targets) {
var corners = (target.leftRightDualTargetPair != null) ? findCorner2019(target) : findBoundingBoxCorners(target);
var pose = calculatePose(corners, target);
if(pose != null) poseList.add(pose);
}
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(poseList, processTime);
}
private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) {
if(target.leftRightDualTargetPair == null) return null;
var left = target.leftRightDualTargetPair.getLeft();
var right = target.leftRightDualTargetPair.getRight();
// flip if the "left" target is to the right
if(left.x > right.x) {
var temp = left;
left = right;
right = temp;
}
var points = new MatOfPoint2f();
points.fromArray(
new Point(left.x, left.y + left.height),
new Point(left.x, left.y),
new Point(right.x + right.width, right.y),
new Point(right.x + right.width, right.y + right.height)
);
return points;
}
private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) {
// List<Pair<MatOfPoint2f, CVPipeline2d.Target2d>> list = new ArrayList<>();
// // find the corners based on the bounding box
// // order is left top, left bottom, right bottom, right top
// extract the corners
var points = new Point[4];
target.minAreaRect.points(points);
// find the tl/tr/bl/br corners
// first, min by left/right
var list_ = Arrays.asList(points);
list_.sort(leftRightComparator);
// of this, we now have left and right
// sort to get top and bottom
var left = new ArrayList<>(List.of(list_.get(0), list_.get(1)));
left.sort(verticalComparator);
var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
right.sort(verticalComparator);
// tl tr bl br
var tl = left.get(0);
var bl = left.get(1);
var tr = right.get(0);
var br = right.get(1);
var result = new MatOfPoint2f();
result.fromList(List.of(tl, bl, br, tr));
return result;
}
private Mat dstNorm = new Mat();
private Mat dstNormScaled = new Mat();
List<Point> tempCornerList = new ArrayList<>();
/**
* Find the corners in an image.
* @param targetImage the image to find corners in.
* @return the corners found in the image.
*/
private List<Point> findCornerHarris(Mat targetImage) {
// convert the image to greyscale
var gray = new Mat();
Imgproc.cvtColor(targetImage, gray, Imgproc.COLOR_BGR2GRAY);
Mat dst = Mat.zeros(targetImage.size(), CvType.CV_8U);
// constants
final int blockSize = 2;
final int apertureSize = 3;
final double k = 0.04;
final int threshold = 200;
/// Detecting corners
Imgproc.cornerHarris(gray, dst, blockSize, apertureSize, k);
/// Normalizing
Core.normalize(dst, dstNorm, 0, 255, Core.NORM_MINMAX);
Core.convertScaleAbs(dstNorm, dstNormScaled);
/// Drawing a circle around corners
float[] dstNormData = new float[(int) (dstNorm.total() * dstNorm.channels())];
dstNorm.get(0, 0, dstNormData);
tempCornerList.clear();
for (int i = 0; i < dstNorm.rows(); i++) {
for (int j = 0; j < dstNorm.cols(); j++) {
if ((int) dstNormData[i * dstNorm.cols() + j] > threshold) {
tempCornerList.add(new Point(j, i));
}
}
}
return tempCornerList;
}
private StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) {
if(objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) {
System.err.println("can't do solvePNP with invalid params!");
return null;
}
imageCornerPoints.copyTo(target.imageCornerPoints);
try {
Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec);
} catch (Exception e) {
e.printStackTrace();
return null;
}
// Algorithm from team 5190 Green Hope Falcons
// var tilt_angle = 0.0; // TODO add to settings
var x = tVec.get(0, 0)[0];
var z = FastMath.sin(tilt_angle) * tVec.get(1, 0)[0] + tVec.get(2, 0)[0] * FastMath.cos(tilt_angle);
// distance in the horizontal plane between camera and target
var distance = FastMath.sqrt(x * x + z * z);
// horizontal angle between center camera and target
@SuppressWarnings("SuspiciousNameCombination")
var angle1 = FastMath.atan2(x, z);
Calib3d.Rodrigues(rVec, rodriguez);
Core.transpose(rodriguez, rot_inv);
// This should be pzero_world = numpy.matmul(rot_inv, -tvec)
// pzero_world = rot_inv.mul(matScale(tVec, -1));
Core.gemm(rot_inv, matScale(tVec, -1), 1, kMat, 0, pzero_world);
var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]);
var targetAngle = -angle1; // radians
var targetRotation = -angle2; // radians
//noinspection UnnecessaryLocalVariable
var targetDistance = distance; // meters or whatever the calibration was in
var targetLocation = new Translation2d(targetDistance * FastMath.cos(targetAngle), targetDistance * FastMath.sin(targetAngle));
target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation));
target.rVector = rVec;
target.tVector = tVec;
System.out.println("Found target at pose: " + target.cameraRelativePose.toString());
return target;
}
/**
* Element-wise scale a matrix by a given factor
* @param src the source matrix
* @param factor by how much to scale each element
* @return the scaled matrix
*/
public Mat matScale(Mat src, double factor) {
Mat dst = new Mat(src.rows(),src.cols(),src.type());
Scalar s = new Scalar(factor);
Core.multiply(src, s, dst);
return dst;
}
}

30
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SortContoursPipe.java
View File

@@ -2,32 +2,33 @@ package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.camera.CaptureStaticProperties;
import com.chameleonvision.vision.enums.SortMode;
import com.chameleonvision.vision.pipeline.Pipe;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.RotatedRect;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRect>> {
public class SortContoursPipe implements Pipe<List<StandardCVPipeline.TrackedTarget>, List<StandardCVPipeline.TrackedTarget>> {
private final Comparator<RotatedRect> SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance);
private final Comparator<StandardCVPipeline.TrackedTarget> SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance);
private static final Comparator<RotatedRect> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.size.area(), rect1.size.area());
private static final Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area());
private static final Comparator<StandardCVPipeline.TrackedTarget> SortBySmallestComparator = SortByLargestComparator.reversed();
private static final Comparator<RotatedRect> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.center.y, rect1.center.y);
private static final Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.center.y, rect1.minAreaRect.center.y);
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLowestComparator = SortByHighestComparator.reversed();
private static final Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
private static final Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
public static final Comparator<StandardCVPipeline.TrackedTarget> SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x);
private static final Comparator<StandardCVPipeline.TrackedTarget> SortByRightmostComparator = SortByLeftmostComparator.reversed();
private SortMode sort;
private CaptureStaticProperties camProps;
private int maxTargets;
private List<RotatedRect> sortedContours = new ArrayList<>();
private List<StandardCVPipeline.TrackedTarget> sortedContours = new ArrayList<>();
public SortContoursPipe(SortMode sort, CaptureStaticProperties camProps, int maxTargets) {
this.sort = sort;
@@ -42,13 +43,13 @@ public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRec
}
@Override
public Pair<List<RotatedRect>, Long> run(List<RotatedRect> input) {
public Pair<List<StandardCVPipeline.TrackedTarget>, Long> run(List<StandardCVPipeline.TrackedTarget> input) {
long processStartNanos = System.nanoTime();
sortedContours.clear();
if (input.size() > 0) {
sortedContours.addAll(input.subList(0, Math.min(input.size(), maxTargets - 1)));
sortedContours.addAll(input);
switch (sort) {
case Largest:
@@ -77,11 +78,12 @@ public class SortContoursPipe implements Pipe<List<RotatedRect>, List<RotatedRec
}
}
sortedContours = sortedContours.subList(0, Math.min(input.size(), maxTargets - 1));
long processTime = System.nanoTime() - processStartNanos;
return Pair.of(sortedContours, processTime);
}
private double calcSquareCenterDistance(RotatedRect rect) {
return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.center.x, 2) + FastMath.pow(camProps.centerY - rect.center.y, 2));
private double calcSquareCenterDistance(StandardCVPipeline.TrackedTarget rect) {
return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.minAreaRect.center.x, 2) + FastMath.pow(camProps.centerY - rect.minAreaRect.center.y, 2));
}
}

1
chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SpeckleRejectPipe.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.vision.pipeline.pipes;
import com.chameleonvision.vision.pipeline.Pipe;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.MatOfPoint;
import org.opencv.imgproc.Imgproc;

108
chameleon-server/src/main/java/com/chameleonvision/web/RequestHandler.java
View File

@@ -4,12 +4,20 @@ import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.network.NetworkIPMode;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
import com.chameleonvision.vision.camera.CameraCapture;
import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.chameleonvision.vision.pipeline.PipelineManager;
import com.chameleonvision.vision.pipeline.impl.Calibrate3dPipeline;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import io.javalin.http.Context;
import io.javalin.http.Handler;
import java.io.IOException;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class RequestHandler {
@@ -35,6 +43,43 @@ public class RequestHandler {
}
}
public static void onDuplicatePipeline(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
try {
Map newPipelineData = objectMapper.readValue(ctx.body(), Map.class);
int newCam = -1;
try {
newCam = (Integer) newPipelineData.get("camera");
} catch (Exception e) {
// ignored
}
var pipeline = (CVPipelineSettings) newPipelineData.get("pipeline");
if (newCam == -1) {
if (VisionManager.getCurrentCameraPipelineNicknames().contains(pipeline.nickname)) {
ctx.status(400); // BAD REQUEST
} else {
VisionManager.getCurrentUIVisionProcess().pipelineManager.addPipeline(pipeline);
ctx.status(200);
}
} else {
var cam = VisionManager.getVisionProcessByIndex(newCam);
if (cam != null && cam.pipelineManager.pipelines.stream().anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))) {
ctx.status(400); // BAD REQUEST
} else {
cam.pipelineManager.addPipeline(pipeline);
ctx.status(200);
}
}
} catch (JsonProcessingException e) {
ctx.status(500);
}
}
public static void onCameraSettings(Context ctx) {
ObjectMapper objectMapper = new ObjectMapper();
try {
@@ -43,13 +88,19 @@ public class RequestHandler {
VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
USBCameraCapture currentCamera = currentVisionProcess.getCamera();
double newFOV;
double newFOV, tilt;
try {
newFOV = (Double) camSettings.get("fov");
} catch (Exception ignored) {
newFOV = (Integer) camSettings.get("fov");
}
try {
tilt = (Double) camSettings.get("tilt");
} catch (Exception ignored) {
tilt = (Integer) camSettings.get("tilt");
}
currentCamera.getProperties().setFOV(newFOV);
currentCamera.getProperties().setTilt(Rotation2d.fromDegrees(tilt));
VisionManager.saveCurrentCameraSettings();
SocketHandler.sendFullSettings();
ctx.status(200);
@@ -58,4 +109,57 @@ public class RequestHandler {
ctx.status(500);
}
}
public static void onCalibrationStart(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
ObjectMapper objectMapper = new ObjectMapper();
var data = objectMapper.readValue(ctx.body(), Map.class);
int resolutionIndex = (Integer) data.get("resolution");
double squareSize;
try {
squareSize = (Double) data.get("squareSize");
} catch (Exception e) {
squareSize = (Integer) data.get("squareSize");
}
// convert from mm to meters
pipeManager.calib3dPipe.setSquareSize(squareSize / 1000d);
VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex;
VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true);
VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex);
}
public static void onSnapshot(Context ctx) {
Calibrate3dPipeline calPipe = VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe;
calPipe.takeSnapshot();
HashMap<String, Object> toSend = new HashMap<>();
toSend.put("snapshotCount", calPipe.getSnapshotCount());
toSend.put("hasEnough", calPipe.hasEnoughSnapshots());
ctx.json(toSend);
ctx.status(200);
}
public static void onCalibrationEnding(Context ctx) throws JsonProcessingException {
PipelineManager pipeManager = VisionManager.getCurrentUIVisionProcess().pipelineManager;
System.out.println("Finishing Cal");
if (pipeManager.calib3dPipe.hasEnoughSnapshots()) {
if (pipeManager.calib3dPipe.tryCalibration()) {
ctx.status(200);
} else {
System.err.println("CALFAIL");
ctx.status(500);
}
}
pipeManager.setCalibrationMode(false);
ctx.status(200);
}
public static void onPnpModel(Context ctx) throws JsonProcessingException {
System.out.println(ctx.body());
ObjectMapper objectMapper = new ObjectMapper();
List points = objectMapper.readValue(ctx.body(), List.class);
System.out.println(points);
}
}

4
chameleon-server/src/main/java/com/chameleonvision/web/Server.java
View File

@@ -30,6 +30,10 @@ public class Server {
});
app.post("/api/settings/general", RequestHandler::onGeneralSettings);
app.post("/api/settings/camera", RequestHandler::onCameraSettings);
app.post("/api/settings/startCalibration", RequestHandler::onCalibrationStart);
app.post("/api/settings/snapshot", RequestHandler::onSnapshot);
app.post("/api/settings/endCalibration", RequestHandler::onCalibrationEnding);
app.post("/api/vision/pnpModel", RequestHandler::onPnpModel);
app.start(port);
}
}

59
chameleon-server/src/main/java/com/chameleonvision/web/SocketHandler.java
View File

@@ -1,5 +1,6 @@
package com.chameleonvision.web;
import com.chameleonvision.config.CameraCalibrationConfig;
import com.chameleonvision.config.ConfigManager;
import com.chameleonvision.vision.VisionManager;
import com.chameleonvision.vision.VisionProcess;
@@ -9,7 +10,7 @@ import com.chameleonvision.vision.camera.USBCameraCapture;
import com.chameleonvision.vision.enums.ImageRotationMode;
import com.chameleonvision.vision.enums.StreamDivisor;
import com.chameleonvision.vision.pipeline.CVPipeline;
import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import com.chameleonvision.vision.pipeline.CVPipelineSettings;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.core.type.TypeReference;
@@ -26,6 +27,7 @@ import java.lang.reflect.Field;
import java.nio.ByteBuffer;
import java.util.*;
import java.util.List;
import java.util.stream.Collectors;
public class SocketHandler {
@@ -33,6 +35,8 @@ public class SocketHandler {
private static List<WsContext> users;
private static ObjectMapper objectMapper;
private static final Object broadcastLock = new Object();
SocketHandler() {
users = new ArrayList<>();
objectMapper = new ObjectMapper(new MessagePackFactory());
@@ -107,8 +111,7 @@ public class SocketHandler {
// HashMap<String, Object> data = (HashMap<String, Object>) entry.getValue();
String pipeName = (String) entry.getValue();
// TODO: add to UI selection for new 2d/3d
boolean is3d = false;
currentProcess.pipelineManager.addNewPipeline(is3d, pipeName);
currentProcess.pipelineManager.addNewPipeline(pipeName);
sendFullSettings();
VisionManager.saveCurrentCameraPipelines();
break;
@@ -134,11 +137,22 @@ public class SocketHandler {
sendFullSettings();
break;
}
case "is3D": {
VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue());
break;
}
case "currentPipeline": {
currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue());
sendFullSettings();
break;
}
case "isPNPCalibration": {
currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue());
break;
}
case "takeCalibrationSnapshot": {
currentProcess.pipelineManager.calib3dPipe.takeSnapshot();
}
default: {
switch (entry.getKey()) {//Pre field value set
@@ -156,8 +170,8 @@ public class SocketHandler {
}
prop = currentCamera.getProperties().getStaticProperties();
currentProcess.cameraStreamer.recalculateDivision();
if (currentPipeline instanceof CVPipeline2d)
((CVPipeline2d) currentPipeline).settings.point = Arrays.asList(prop.mode.width / 2, prop.mode.height / 2);//Reset Crosshair in single point calib
if (currentPipeline instanceof StandardCVPipeline)
((StandardCVPipeline) currentPipeline).settings.point = Arrays.asList(prop.mode.width / 2, prop.mode.height / 2);//Reset Crosshair in single point calib
break;
}
@@ -181,8 +195,8 @@ public class SocketHandler {
break;
}
case "videoModeIndex": {
if (currentPipeline instanceof CVPipeline2d)
((CVPipeline2d) currentPipeline).settings.point = new ArrayList<>();//This will reset the calibration
if (currentPipeline instanceof StandardCVPipeline)
((StandardCVPipeline) currentPipeline).settings.point = new ArrayList<>();//This will reset the calibration
currentCamera.setVideoMode((Integer) entry.getValue());
currentProcess.cameraStreamer.recalculateDivision();
break;
@@ -217,18 +231,22 @@ public class SocketHandler {
}
private static void broadcastMessage(Object obj, WsContext userToSkip) {
if (users != null)
for (WsContext user : users) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
try {
ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
user.send(b);
} catch (JsonProcessingException e) {
e.printStackTrace();
synchronized (broadcastLock) {
if (users != null) {
var userList = users;
for (WsContext user : userList) {
if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
continue;
}
try {
ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
user.send(b);
} catch (JsonProcessingException e) {
e.printStackTrace();
}
}
}
}
}
public static void broadcastMessage(Object obj) {
@@ -267,9 +285,16 @@ public class SocketHandler {
HashMap<String, Object> tmp = new HashMap<>();
VisionProcess currentVisionProcess = VisionManager.getCurrentUIVisionProcess();
USBCameraCapture currentCamera = VisionManager.getCurrentUIVisionProcess().getCamera();
tmp.put("fov", currentCamera.getProperties().getFOV());
tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal());
tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex());
tmp.put("tilt", currentVisionProcess.getCamera().getProperties().getTilt().getDegrees());
List<CameraCalibrationConfig.UICameraCalibrationConfig> calibrations = currentCamera.getAllCalibrationData().stream()
.map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList());
tmp.put("calibration", calibrations);
return tmp;
}