mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-21 01:01:41 +00:00
Add solvePNP, 3d tab on the UI, and some other misc bug fixes (#35)
* 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
1
.gitignore
vendored
1
.gitignore
vendored
@@ -119,3 +119,4 @@ New client/chameleon-client/*
|
||||
|
||||
*.prefs
|
||||
*.jfr
|
||||
.DS_Store
|
||||
|
||||
6
chameleon-client/package-lock.json
generated
6
chameleon-client/package-lock.json
generated
@@ -7952,6 +7952,12 @@
|
||||
"integrity": "sha512-0DTvPVU3ed8+HNXOu5Bs+o//Mbdj9VNQMUOe9oKCwh8l0GNwpTDMKCWbRjgtD291AWnkAgkqA/LOnQS8AmS1tw==",
|
||||
"dev": true
|
||||
},
|
||||
"papaparse": {
|
||||
"version": "5.1.0",
|
||||
"resolved": "https://registry.npmjs.org/papaparse/-/papaparse-5.1.0.tgz",
|
||||
"integrity": "sha512-3jEYMiCc8qN7V5ffi2BTS2mRauKxCu5AIED6DxbjnHhIm7OY7fzKYkndfPlHWaaKUDCTml5XTU6V+hiuxGlZuw==",
|
||||
"dev": true
|
||||
},
|
||||
"parallel-transform": {
|
||||
"version": "1.2.0",
|
||||
"resolved": "https://registry.npmjs.org/parallel-transform/-/parallel-transform-1.2.0.tgz",
|
||||
|
||||
@@ -27,6 +27,7 @@
|
||||
"babel-eslint": "^10.0.1",
|
||||
"eslint": "^5.16.0",
|
||||
"eslint-plugin-vue": "^5.0.0",
|
||||
"papaparse": "^5.1.0",
|
||||
"sass": "^1.17.4",
|
||||
"sass-loader": "^7.1.0",
|
||||
"vue-cli-plugin-vuetify": "^0.6.3",
|
||||
|
||||
BIN
chameleon-client/src/assets/chessboard.png
Normal file
BIN
chameleon-client/src/assets/chessboard.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 61 KiB |
140
chameleon-client/src/assets/robotIcon.svg
Normal file
140
chameleon-client/src/assets/robotIcon.svg
Normal file
@@ -0,0 +1,140 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<!-- Created with Inkscape (http://www.inkscape.org/) -->
|
||||
|
||||
<svg
|
||||
xmlns:dc="http://purl.org/dc/elements/1.1/"
|
||||
xmlns:cc="http://creativecommons.org/ns#"
|
||||
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
|
||||
xmlns:svg="http://www.w3.org/2000/svg"
|
||||
xmlns="http://www.w3.org/2000/svg"
|
||||
xmlns:xlink="http://www.w3.org/1999/xlink"
|
||||
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
|
||||
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
|
||||
version="1.1"
|
||||
id="svg3713"
|
||||
width="219"
|
||||
height="230"
|
||||
viewBox="0 0 219 230"
|
||||
sodipodi:docname="robotIcon.svg"
|
||||
inkscape:version="0.92.4 (5da689c313, 2019-01-14)">
|
||||
<metadata
|
||||
id="metadata3719">
|
||||
<rdf:RDF>
|
||||
<cc:Work
|
||||
rdf:about="">
|
||||
<dc:format>image/svg+xml</dc:format>
|
||||
<dc:type
|
||||
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
|
||||
<dc:title />
|
||||
</cc:Work>
|
||||
</rdf:RDF>
|
||||
</metadata>
|
||||
<defs
|
||||
id="defs3717">
|
||||
<linearGradient
|
||||
id="linearGradient937"
|
||||
inkscape:collect="always">
|
||||
<stop
|
||||
id="stop933"
|
||||
offset="0"
|
||||
style="stop-color:#228a42;stop-opacity:1" />
|
||||
<stop
|
||||
id="stop935"
|
||||
offset="1"
|
||||
style="stop-color:#5cb34a;stop-opacity:1" />
|
||||
</linearGradient>
|
||||
<linearGradient
|
||||
id="linearGradient931"
|
||||
inkscape:collect="always">
|
||||
<stop
|
||||
id="stop927"
|
||||
offset="0"
|
||||
style="stop-color:#228a42;stop-opacity:1" />
|
||||
<stop
|
||||
id="stop929"
|
||||
offset="1"
|
||||
style="stop-color:#5cb34a;stop-opacity:1" />
|
||||
</linearGradient>
|
||||
<linearGradient
|
||||
inkscape:collect="always"
|
||||
id="linearGradient925">
|
||||
<stop
|
||||
style="stop-color:#228a42;stop-opacity:1"
|
||||
offset="0"
|
||||
id="stop921" />
|
||||
<stop
|
||||
style="stop-color:#5cb34a;stop-opacity:1"
|
||||
offset="1"
|
||||
id="stop923" />
|
||||
</linearGradient>
|
||||
<linearGradient
|
||||
inkscape:collect="always"
|
||||
xlink:href="#linearGradient925"
|
||||
id="linearGradient4820"
|
||||
x1="108.25"
|
||||
y1="186.25"
|
||||
x2="109.5"
|
||||
y2="21"
|
||||
gradientUnits="userSpaceOnUse" />
|
||||
<linearGradient
|
||||
inkscape:collect="always"
|
||||
xlink:href="#linearGradient937"
|
||||
id="linearGradient4822"
|
||||
x1="108.25"
|
||||
y1="186.25"
|
||||
x2="109.5"
|
||||
y2="21"
|
||||
gradientUnits="userSpaceOnUse" />
|
||||
<linearGradient
|
||||
inkscape:collect="always"
|
||||
xlink:href="#linearGradient931"
|
||||
id="linearGradient4824"
|
||||
x1="108.25"
|
||||
y1="186.25"
|
||||
x2="109.5"
|
||||
y2="21"
|
||||
gradientUnits="userSpaceOnUse" />
|
||||
</defs>
|
||||
<sodipodi:namedview
|
||||
pagecolor="#ffffff"
|
||||
bordercolor="#666666"
|
||||
borderopacity="1"
|
||||
objecttolerance="10"
|
||||
gridtolerance="10"
|
||||
guidetolerance="10"
|
||||
inkscape:pageopacity="0"
|
||||
inkscape:pageshadow="2"
|
||||
inkscape:window-width="1920"
|
||||
inkscape:window-height="1017"
|
||||
id="namedview3715"
|
||||
showgrid="false"
|
||||
inkscape:zoom="4"
|
||||
inkscape:cx="110.62282"
|
||||
inkscape:cy="130.60249"
|
||||
inkscape:window-x="-8"
|
||||
inkscape:window-y="-8"
|
||||
inkscape:window-maximized="1"
|
||||
inkscape:current-layer="svg3713"
|
||||
showguides="false"
|
||||
inkscape:snap-page="false"
|
||||
inkscape:snap-others="true"
|
||||
inkscape:snap-object-midpoints="true" />
|
||||
<path
|
||||
style="opacity:1;fill:url(#linearGradient4824);fill-opacity:1;stroke:#000000;stroke-width:0.49889764;stroke-linecap:butt;stroke-linejoin:round;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
|
||||
d="m 167,156.45722 -41,-2 -14,-17 V 55.457215 Z"
|
||||
id="path3725-6"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
<path
|
||||
style="fill:url(#linearGradient4822);stroke:#020000;stroke-width:0.49889764;stroke-linecap:butt;stroke-linejoin:round;stroke-opacity:1;fill-opacity:1;opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
|
||||
d="m 76.866,158.246 13,-0.496 19.567,11.61661 19.567,-11.39151 13,0.2709 -32.5675,19.39302 z"
|
||||
id="path3742"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccccc" />
|
||||
<path
|
||||
style="fill:url(#linearGradient4820);stroke:#020000;stroke-width:0.49889764;stroke-linecap:butt;stroke-linejoin:round;stroke-opacity:1;fill-opacity:1;opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
|
||||
d="m 51.864904,156.45722 41,-2 13.999996,-17 V 55.457209 Z"
|
||||
id="path3725-6-5"
|
||||
inkscape:connector-curvature="0"
|
||||
sodipodi:nodetypes="ccccc" />
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 4.4 KiB |
165
chameleon-client/src/components/3D/MiniMap.vue
Normal file
165
chameleon-client/src/components/3D/MiniMap.vue
Normal file
@@ -0,0 +1,165 @@
|
||||
<template>
|
||||
<div>
|
||||
<v-row style="width: 400px;" align="center">
|
||||
<canvas id="canvasId" width="800" height="800"/>
|
||||
</v-row>
|
||||
<v-row style="width: 400px;" align="center" justify="middle">
|
||||
<v-simple-table
|
||||
style="text-align: center;background-color: transparent; display: block;margin: auto"
|
||||
dense dark>
|
||||
<template v-slot:default>
|
||||
<thead>
|
||||
<tr>
|
||||
<th class="text-center">Target</th>
|
||||
<th class="text-center">X</th>
|
||||
<th class="text-center">Y</th>
|
||||
<th class="text-center">Angle</th>
|
||||
</tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr v-for="(target, index) in targets" :key="index">
|
||||
<td>{{ index}}</td>
|
||||
<td>{{ target.pose.translation.x.toFixed(2) }}</td>
|
||||
<td>{{ target.pose.translation.y.toFixed(2) }}</td>
|
||||
<td>{{ target.pose.rotation.radians.toFixed(2) }}</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</template>
|
||||
</v-simple-table>
|
||||
</v-row>
|
||||
</div>
|
||||
|
||||
</template>
|
||||
|
||||
<script>
|
||||
export default {
|
||||
name: "MiniMap",
|
||||
props: {
|
||||
targets: Array,
|
||||
horizontalFOV: Number
|
||||
},
|
||||
data() {
|
||||
return {
|
||||
ctx: undefined,
|
||||
canvas: undefined,
|
||||
x: 0,
|
||||
y: 0,
|
||||
targetWidth: 40,
|
||||
targetHeight: 6
|
||||
}
|
||||
},
|
||||
watch: {
|
||||
targets: {
|
||||
deep: true,
|
||||
handler() {
|
||||
this.draw();
|
||||
}
|
||||
},
|
||||
horizontalFOV() {
|
||||
this.draw();
|
||||
}
|
||||
},
|
||||
methods: {
|
||||
draw() {
|
||||
this.clearBoard();
|
||||
this.drawPlayer();
|
||||
for (let index in this.targets) {
|
||||
this.drawTarget(index, this.targets[index].pose);
|
||||
}
|
||||
},
|
||||
drawTarget(index, target) {
|
||||
// first save the untranslated/unrotated context
|
||||
let x = 800 - (160 * target.translation.x); // getting meters as pixels
|
||||
let y = 400 - (160 * target.translation.y);
|
||||
this.ctx.save();
|
||||
this.ctx.beginPath();
|
||||
// move the rotation point to the center of the rect
|
||||
this.ctx.translate(y + this.targetWidth / 2, x + this.targetHeight / 2); // wpi lib makes x forward and back and y left to right
|
||||
// rotate the rect
|
||||
this.ctx.rotate(target.rotation.radians);
|
||||
|
||||
// draw the rect on the transformed context
|
||||
// Note: after transforming [0,0] is visually [x,y]
|
||||
// so the rect needs to be offset accordingly when drawn
|
||||
this.ctx.rect(-this.targetWidth / 2, -this.targetHeight / 2, this.targetWidth, this.targetHeight);
|
||||
|
||||
this.ctx.fillStyle = "#01a209";
|
||||
this.ctx.fill();
|
||||
|
||||
// restore the context to its untranslated/unrotated state
|
||||
this.ctx.restore();
|
||||
this.ctx.fillStyle = "whitesmoke";
|
||||
this.ctx.beginPath();
|
||||
this.ctx.arc(y + this.targetWidth / 2, x + this.targetHeight / 2, 3, 0, 2 * Math.PI, true);
|
||||
this.ctx.fill();
|
||||
this.ctx.fillText(index, y - 30, x - 5);
|
||||
|
||||
},
|
||||
drawPlayer() {
|
||||
this.ctx.beginPath();
|
||||
this.ctx.moveTo(400, 820);
|
||||
this.ctx.lineTo(400 + this.hLen, 650);
|
||||
this.ctx.lineTo(400 - this.hLen, 650);
|
||||
this.ctx.closePath();
|
||||
this.ctx.fillStyle = this.grad;
|
||||
this.ctx.fill();
|
||||
this.ctx.beginPath();
|
||||
this.ctx.moveTo(400, 820);
|
||||
this.ctx.lineTo(400 + this.hLen, 650);
|
||||
this.ctx.stroke();
|
||||
this.ctx.moveTo(400, 820);
|
||||
this.ctx.lineTo(400 - this.hLen, 650);
|
||||
this.ctx.stroke();
|
||||
|
||||
},
|
||||
clearBoard() {
|
||||
this.ctx.clearRect(0, 0, this.canvas.width, this.canvas.height); // clearing the canvas
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
hLen: {
|
||||
get() {
|
||||
return Math.tan(this.horizontalFOV / 2 * Math.PI / 180) * 150;
|
||||
}
|
||||
}
|
||||
},
|
||||
mounted: function () {
|
||||
const canvas = document.getElementById("canvasId"); // getting the canvas element
|
||||
const ctx = canvas.getContext("2d"); // getting the canvas context
|
||||
this.canvas = canvas; // setting the canvas as a vue variable
|
||||
this.ctx = ctx; // setting the canvas context as a vue variable
|
||||
this.grad = this.ctx.createLinearGradient(400, 800, 400, 600);
|
||||
this.grad.addColorStop(0, "rgb(119,119,119)");
|
||||
this.grad.addColorStop(0.05, "rgba(14,92,22,0.96)");
|
||||
this.grad.addColorStop(0.8, "rgba(43,43,43,0.48)");
|
||||
|
||||
// setting canvas context values for drawing
|
||||
|
||||
|
||||
this.ctx.font = "26px Arial";
|
||||
this.ctx.strokeStyle = "whitesmoke";
|
||||
this.ctx.lineWidth = 2;
|
||||
|
||||
this.$nextTick(function () {
|
||||
this.drawPlayer();
|
||||
|
||||
});
|
||||
}
|
||||
}
|
||||
</script>
|
||||
|
||||
<style scoped>
|
||||
#canvasId {
|
||||
width: 400px;
|
||||
height: 400px;
|
||||
background-color: #2b2b2b;
|
||||
border-radius: 5px;
|
||||
border: 2px solid grey;
|
||||
box-shadow: 0 0 5px 1px;
|
||||
}
|
||||
|
||||
th {
|
||||
width: 80px;
|
||||
text-align: center;
|
||||
}
|
||||
</style>
|
||||
@@ -6,7 +6,7 @@
|
||||
</v-col>
|
||||
<v-col>
|
||||
<v-text-field dark v-model="localValue" class="mt-0 pt-0" hide-details single-line type="number"
|
||||
style="width: 70px"/>
|
||||
style="width: 70px" :step="step"/>
|
||||
</v-col>
|
||||
</v-row>
|
||||
</div>
|
||||
@@ -15,7 +15,7 @@
|
||||
<script>
|
||||
export default {
|
||||
name: 'NumberInput',
|
||||
props: ['name', 'value'],
|
||||
props: ['name', 'value', 'step'],
|
||||
data() {
|
||||
return {}
|
||||
},
|
||||
|
||||
@@ -37,7 +37,8 @@ export default new Vuex.Store({
|
||||
isBinary: 0,
|
||||
calibrationMode: 0,
|
||||
videoModeIndex:0,
|
||||
streamDivisor:0
|
||||
streamDivisor:0,
|
||||
is3D:false
|
||||
},
|
||||
cameraSettings: {},
|
||||
resolutionList: [],
|
||||
|
||||
@@ -75,6 +75,7 @@
|
||||
<v-tab>Threshold</v-tab>
|
||||
<v-tab>Contours</v-tab>
|
||||
<v-tab>Output</v-tab>
|
||||
<v-tab>3D</v-tab>
|
||||
</v-tabs>
|
||||
<div v-else style="height: 48px"></div>
|
||||
<div style="padding-left:30px">
|
||||
@@ -96,10 +97,40 @@
|
||||
<div v-else style="height: 58px"></div>
|
||||
<!-- camera image stream -->
|
||||
<div class="videoClass">
|
||||
<img id="CameraStream" v-if="cameraList.length > 0" :src="streamAddress" @click="onImageClick"
|
||||
crossorigin="Anonymous"/>
|
||||
<span v-else>No Cameras Are connected</span>
|
||||
<h5 id="Point">{{point}}</h5>
|
||||
<v-row align="center">
|
||||
<img id="CameraStream" style="display: block;margin: auto; width: 70%;height: 70%;"
|
||||
v-if="cameraList.length > 0"
|
||||
:src="streamAddress" @click="onImageClick"
|
||||
crossorigin="Anonymous"/>
|
||||
<span v-else>No Cameras Are connected</span>
|
||||
</v-row>
|
||||
<v-row justify="end">
|
||||
<span style="margin-right: 45px">FPS:{{fps.toFixed(2)}}</span>
|
||||
</v-row>
|
||||
<v-row align="center">
|
||||
<v-simple-table
|
||||
style="text-align: center;background-color: transparent; display: block;margin: auto"
|
||||
dense dark>
|
||||
<template v-slot:default>
|
||||
<thead>
|
||||
<tr>
|
||||
<th class="text-center">Target</th>
|
||||
<th class="text-center">Pitch</th>
|
||||
<th class="text-center">Yaw</th>
|
||||
<th class="text-center">Area</th>
|
||||
</tr>
|
||||
</thead>
|
||||
<tbody>
|
||||
<tr v-for="(value, index) in targets" :key="index">
|
||||
<td>{{ index}}</td>
|
||||
<td>{{ parseFloat(value.pitch).toFixed(2) }}</td>
|
||||
<td>{{ parseFloat(value.yaw).toFixed(2) }}</td>
|
||||
<td>{{ parseFloat(value.area).toFixed(2) }}</td>
|
||||
</tr>
|
||||
</tbody>
|
||||
</template>
|
||||
</v-simple-table>
|
||||
</v-row>
|
||||
</div>
|
||||
</div>
|
||||
</v-col>
|
||||
@@ -154,6 +185,7 @@
|
||||
import ThresholdTab from './CameraViewes/ThresholdTab'
|
||||
import ContoursTab from './CameraViewes/ContoursTab'
|
||||
import OutputTab from './CameraViewes/OutputTab'
|
||||
import pnpTab from './CameraViewes/3D'
|
||||
import CVselect from '../components/cv-select'
|
||||
import CVicon from '../components/cv-icon'
|
||||
import CVinput from '../components/cv-input'
|
||||
@@ -165,6 +197,7 @@
|
||||
ThresholdTab,
|
||||
ContoursTab,
|
||||
OutputTab,
|
||||
pnpTab,
|
||||
CVselect,
|
||||
CVicon,
|
||||
CVinput
|
||||
@@ -319,19 +352,21 @@
|
||||
return "ContoursTab";
|
||||
case 3:
|
||||
return "OutputTab";
|
||||
case 4:
|
||||
return "pnpTab";
|
||||
}
|
||||
return "";
|
||||
}
|
||||
},
|
||||
point: {
|
||||
targets: {
|
||||
get: function () {
|
||||
let p = this.$store.state.point.calculated;
|
||||
let fps = this.$store.state.point.fps;
|
||||
if (p !== undefined) {
|
||||
return `Pitch: ${parseFloat(p['pitch']).toFixed(2)}, Yaw: ${parseFloat(p['yaw']).toFixed(2)}, Area: ${parseFloat(p['area']).toFixed(2)}, FPS: ${parseFloat(fps).toFixed(2)}`
|
||||
} else {
|
||||
return undefined;
|
||||
}
|
||||
|
||||
return this.$store.state.point.targets;
|
||||
}
|
||||
},
|
||||
fps: {
|
||||
get() {
|
||||
return this.$store.state.point.fps;
|
||||
}
|
||||
},
|
||||
currentCameraIndex: {
|
||||
@@ -384,17 +419,16 @@
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
.videoClass img {
|
||||
max-height: 70vh;
|
||||
max-width: 70%;
|
||||
|
||||
.tableClass {
|
||||
padding-top: 5px;
|
||||
width: 70%;
|
||||
object-fit: cover;
|
||||
vertical-align: middle;
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
#Point {
|
||||
padding-top: 5px;
|
||||
th {
|
||||
width: 80px;
|
||||
text-align: center;
|
||||
color: #f4f4f4;
|
||||
}
|
||||
|
||||
</style>
|
||||
86
chameleon-client/src/views/CameraViewes/3D.vue
Normal file
86
chameleon-client/src/views/CameraViewes/3D.vue
Normal file
@@ -0,0 +1,86 @@
|
||||
<template>
|
||||
<div>
|
||||
<v-row align="center" justify="start" dense>
|
||||
<v-col :cols="6">
|
||||
<CVswitch v-model="value.is3D" name="Enable 3D" @input="handleData('is3D')"/>
|
||||
</v-col>
|
||||
<v-col>
|
||||
<CVnumberInput name="Max Height:" v-model="value.targetHeight" @input="handleData('targetHeight')"/>
|
||||
<CVnumberInput name="Max Width:" v-model="value.targetWidth" @input="handleData('targetWidth')"/>
|
||||
</v-col>
|
||||
<!-- <v-col>-->
|
||||
<!-- <input type="file" ref="file" style="display: none" accept=".csv" @change="readFile">-->
|
||||
<!-- <v-btn @click="$refs.file.click()" small>-->
|
||||
<!-- upload model-->
|
||||
<!-- </v-btn>-->
|
||||
<!-- </v-col>-->
|
||||
|
||||
</v-row>
|
||||
<mini-map class="miniMapClass" :targets="targets" :horizontal-f-o-v="horizontalFOV"/>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
<script>
|
||||
import miniMap from '../../components/3D/MiniMap';
|
||||
import CVswitch from '../../components/cv-switch';
|
||||
import CVnumberInput from '../../components/cv-number-input';
|
||||
import Papa from 'papaparse';
|
||||
|
||||
export default {
|
||||
name: "solvePNP",
|
||||
props: ['value'],
|
||||
components: {
|
||||
CVswitch,
|
||||
CVnumberInput,
|
||||
miniMap
|
||||
},
|
||||
data() {
|
||||
return {
|
||||
is3D: false,
|
||||
isPNPCalibration: false,
|
||||
}
|
||||
},
|
||||
methods: {
|
||||
handleData(val) {
|
||||
this.handleInput(val, this.value[val]);
|
||||
this.$emit('update')
|
||||
},
|
||||
readFile(event) {
|
||||
let file = event.target.files[0];
|
||||
Papa.parse(file, {
|
||||
complete: this.onParse,
|
||||
skipEmptyLines: true
|
||||
});
|
||||
},
|
||||
onParse(result) {
|
||||
// console.log(result.data);
|
||||
this.axios.post("http://" + this.$address + "/api/vision/pnpModel", result.data);
|
||||
},
|
||||
},
|
||||
computed: {
|
||||
targets: {
|
||||
get() {
|
||||
return this.$store.state.point.targets;
|
||||
}
|
||||
|
||||
},
|
||||
horizontalFOV: {
|
||||
get() {
|
||||
let index = this.$store.state.cameraSettings.resolution;
|
||||
let FOV = this.$store.state.cameraSettings.fov;
|
||||
let resolution = this.$store.state.resolutionList[index];
|
||||
let diagonalView = FOV * (Math.PI / 180);
|
||||
let diagonalAspect = Math.hypot(resolution.width, resolution.height);
|
||||
return Math.atan(Math.tan(diagonalView / 2) * (resolution.width / diagonalAspect)) * 2 * (180 / Math.PI)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
</script>
|
||||
|
||||
<style scoped>
|
||||
.miniMapClass {
|
||||
width: 50% !important;
|
||||
height: 50% !important;
|
||||
}
|
||||
</style>
|
||||
@@ -3,7 +3,7 @@
|
||||
<CVselect name="SortMode" v-model="value.sortMode"
|
||||
:list="['Largest','Smallest','Highest','Lowest','Rightmost','Leftmost','Centermost']"
|
||||
@input="handleData('sortMode')"/>
|
||||
<CVswitch name="Output multiple" v-model="value.multiple" @input="handleData('multiple')"></CVswitch>
|
||||
<CVswitch name="Output multiple" v-model="value.multiple" @input="handleData('multiple')"/>
|
||||
<span>Calibrate:</span>
|
||||
<v-divider dark color="white"/>
|
||||
<CVselect name="Calibration Mode" v-model="value.calibrationMode" :list="['None','Single point','Dual point']"
|
||||
|
||||
@@ -1,8 +1,54 @@
|
||||
<template>
|
||||
<div>
|
||||
<CVselect name="Camera" :list="cameraList" v-model="currentCameraIndex"/>
|
||||
<CVnumberinput name="Diagonal FOV" v-model="cameraSettings.fov"/>
|
||||
<v-btn style="margin-top:10px" small color="#4baf62" @click="sendCameraSettings">Save Camera Settings</v-btn>
|
||||
<div>
|
||||
<CVselect name="Camera" :list="cameraList" v-model="currentCameraIndex" @input="handleInput('currentCamera',currentCameraIndex)"/>
|
||||
<CVnumberinput name="Diagonal FOV" v-model="cameraSettings.fov"/>
|
||||
<br>
|
||||
<CVnumberinput name="Camera pitch" v-model="cameraSettings.tilt" :step="0.01"/>
|
||||
<br>
|
||||
<v-btn style="margin-top:10px" small color="#4baf62" @click="sendCameraSettings">Save Camera Settings
|
||||
</v-btn>
|
||||
</div>
|
||||
<div style="margin-top: 15px">
|
||||
<span>3D Calibration</span>
|
||||
<v-divider color="white" style="margin-bottom: 10px"/>
|
||||
<v-row>
|
||||
<v-col>
|
||||
<CVselect name="Resolution" v-model="resolutionIndex" :list="resolutionList"/>
|
||||
</v-col>
|
||||
<v-col>
|
||||
<CVnumberinput name="Square Size (mm)" v-model="squareSize"/>
|
||||
</v-col>
|
||||
</v-row>
|
||||
<v-row>
|
||||
<v-col>
|
||||
<v-btn small :color="calibrationModeButton.color" @click="sendCalibrationMode"
|
||||
:disabled="checkResolution">
|
||||
{{calibrationModeButton.text}}
|
||||
</v-btn>
|
||||
</v-col>
|
||||
<v-col>
|
||||
<v-btn small :color="cancellationModeButton.color" @click="sendCalibrationFinish"
|
||||
:disabled="checkCancelation">
|
||||
{{cancellationModeButton.text}}
|
||||
</v-btn>
|
||||
</v-col>
|
||||
<v-col>
|
||||
<v-btn color="whitesmoke" small><a style="color: black; text-decoration: none"
|
||||
:href="require('../../assets/chessboard.png')"
|
||||
download="Calibration Board.png">Download Checkerboard</a>
|
||||
</v-btn>
|
||||
</v-col>
|
||||
</v-row>
|
||||
<v-row v-if="isCalibrating">
|
||||
<v-col>
|
||||
<span>Snapshot Amount: {{snapshotAmount}}</span>
|
||||
</v-col>
|
||||
</v-row>
|
||||
</div>
|
||||
<v-snackbar v-model="snack">
|
||||
<span>Calibration Failed</span>
|
||||
</v-snackbar>
|
||||
</div>
|
||||
</template>
|
||||
|
||||
@@ -17,7 +63,22 @@
|
||||
CVnumberinput
|
||||
},
|
||||
data() {
|
||||
return {}
|
||||
return {
|
||||
isCalibrating: false,
|
||||
resolutionIndex: undefined,
|
||||
calibrationModeButton: {
|
||||
text: "Start Calibration",
|
||||
color: "green"
|
||||
},
|
||||
cancellationModeButton: {
|
||||
text: "Cancel Calibration",
|
||||
color: "red"
|
||||
},
|
||||
squareSize: 2.54,
|
||||
snapshotAmount: 0,
|
||||
hasEnough: false,
|
||||
snack: false
|
||||
}
|
||||
},
|
||||
methods: {
|
||||
sendCameraSettings() {
|
||||
@@ -30,9 +91,69 @@
|
||||
}
|
||||
)
|
||||
},
|
||||
|
||||
sendCalibrationMode() {
|
||||
const self = this;
|
||||
let data = {};
|
||||
let connection_string = "/api/settings/";
|
||||
if (self.isCalibrating === true) {
|
||||
connection_string += "snapshot"
|
||||
} else {
|
||||
connection_string += "startCalibration";
|
||||
data['resolution'] = this.resolutionIndex;
|
||||
data['squareSize'] = this.squareSize;
|
||||
self.hasEnough = false;
|
||||
}
|
||||
this.axios.post("http://" + this.$address + connection_string, data).then(
|
||||
function (response) {
|
||||
if (response.status === 200) {
|
||||
if (self.isCalibrating) {
|
||||
self.snapshotAmount = response.data['snapshotCount'];
|
||||
self.hasEnough = response.data['hasEnough'];
|
||||
if (self.hasEnough === true) {
|
||||
self.cancellationModeButton.text = "Finish Calibration";
|
||||
self.cancellationModeButton.color = "green";
|
||||
}
|
||||
} else {
|
||||
self.calibrationModeButton.text = "Take Snapshot";
|
||||
self.isCalibrating = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
);
|
||||
},
|
||||
sendCalibrationFinish() {
|
||||
const self = this;
|
||||
let connection_string = "/api/settings/endCalibration";
|
||||
let data = {};
|
||||
data['squareSize'] = this.squareSize;
|
||||
self.axios.post("http://" + this.$address + connection_string, data).then(
|
||||
function (response) {
|
||||
if (response.status === 500) {
|
||||
self.snack = true;
|
||||
}
|
||||
self.isCalibrating = false;
|
||||
self.hasEnough = false;
|
||||
self.snapshotAmount = 0;
|
||||
self.calibrationModeButton.text = "Start Calibration";
|
||||
self.cancellationModeButton.text = "Cancel Calibration";
|
||||
self.cancellationModeButton.color = "red";
|
||||
}
|
||||
);
|
||||
}
|
||||
},
|
||||
computed: {
|
||||
checkResolution() {
|
||||
return this.resolutionIndex === undefined;
|
||||
},
|
||||
checkCancelation() {
|
||||
if (this.isCalibrating) {
|
||||
return false
|
||||
} else if (this.checkResolution) {
|
||||
return true;
|
||||
} else {
|
||||
return true
|
||||
}
|
||||
},
|
||||
currentCameraIndex: {
|
||||
get() {
|
||||
return this.$store.state.currentCameraIndex;
|
||||
@@ -49,6 +170,15 @@
|
||||
this.$store.commit('cameraList', value);
|
||||
}
|
||||
},
|
||||
resolutionList: {
|
||||
get() {
|
||||
let tmp_list = [];
|
||||
for (let i of this.$store.state.resolutionList) {
|
||||
tmp_list.push(`${i['width']} X ${i['height']} at ${i['fps']} FPS, ${i['pixelFormat']}`)
|
||||
}
|
||||
return tmp_list;
|
||||
}
|
||||
},
|
||||
cameraSettings: {
|
||||
get() {
|
||||
return this.$store.state.cameraSettings;
|
||||
|
||||
72
chameleon-server/chameleon-vision.iml
Normal file
72
chameleon-server/chameleon-vision.iml
Normal file
@@ -0,0 +1,72 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module org.jetbrains.idea.maven.project.MavenProjectsManager.isMavenModule="true" type="JAVA_MODULE" version="4">
|
||||
<component name="NewModuleRootManager" LANGUAGE_LEVEL="JDK_12">
|
||||
<output url="file://$MODULE_DIR$/target/classes" />
|
||||
<output-test url="file://$MODULE_DIR$/target/test-classes" />
|
||||
<content url="file://$MODULE_DIR$">
|
||||
<sourceFolder url="file://$MODULE_DIR$/src/main/java" isTestSource="false" />
|
||||
<sourceFolder url="file://$MODULE_DIR$/src/main/resources" type="java-resource" />
|
||||
<sourceFolder url="file://$MODULE_DIR$/src/test/java" isTestSource="true" />
|
||||
<excludeFolder url="file://$MODULE_DIR$/target" />
|
||||
</content>
|
||||
<orderEntry type="inheritedJdk" />
|
||||
<orderEntry type="sourceFolder" forTests="false" />
|
||||
<orderEntry type="library" name="Maven: io.javalin:javalin:3.4.1" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib-jdk8:1.3.31" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib:1.3.31" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib-common:1.3.31" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.jetbrains:annotations:13.0" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.jetbrains.kotlin:kotlin-stdlib-jdk7:1.3.31" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.slf4j:slf4j-api:1.7.26" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-server:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: javax.servlet:javax.servlet-api:3.1.0" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-http:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-util:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-io:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-webapp:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-xml:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-servlet:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-security:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty.websocket:websocket-server:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty.websocket:websocket-common:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty.websocket:websocket-api:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty.websocket:websocket-client:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty:jetty-client:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.eclipse.jetty.websocket:websocket-servlet:9.4.19.v20190610" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.json:json:20190722" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.slf4j:slf4j-nop:1.7.26" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.apache.commons:commons-math3:3.6.1" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.msgpack:msgpack-core:0.8.18" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.msgpack:jackson-dataformat-msgpack:0.8.18" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.apache.commons:commons-lang3:3.9" level="project" />
|
||||
<orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-core:2.10.1" level="project" />
|
||||
<orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-annotations:2.10.1" level="project" />
|
||||
<orderEntry type="library" name="Maven: com.fasterxml.jackson.core:jackson-databind:2.10.1" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.junit.jupiter:junit-jupiter-engine:5.5.2" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.apiguardian:apiguardian-api:1.1.0" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.junit.platform:junit-platform-engine:1.5.2" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.opentest4j:opentest4j:1.2.0" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.junit.platform:junit-platform-commons:1.5.2" level="project" />
|
||||
<orderEntry type="library" name="Maven: org.junit.jupiter:junit-jupiter-api:5.5.2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-java:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxaarch64bionic:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxraspbian:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:linuxx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:osxx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cscore:cscore-jni:windowsx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.cameraserver:cameraserver-java:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-java:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:osxx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:linuxraspbian:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:linuxx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:linuxaarch64bionic:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.ntcore:ntcore-jni:windowsx86-64:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.wpiutil:wpiutil-java:2020.1.1-beta-3-15-g7b9c6eb" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-java:3.4.7-2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:linuxaarch64bionic:3.4.7-2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:linuxraspbian:3.4.7-2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:linuxx86-64:3.4.7-2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:osxx86-64:3.4.7-2" level="project" />
|
||||
<orderEntry type="library" name="Maven: edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:windowsx86-64:3.4.7-2" level="project" />
|
||||
</component>
|
||||
</module>
|
||||
@@ -5,7 +5,7 @@
|
||||
<modelVersion>4.0.0</modelVersion>
|
||||
<groupId>org.chameleon-vision.main</groupId>
|
||||
<artifactId>chameleon-vision</artifactId>
|
||||
<version>2.0.1-BETA</version>
|
||||
<version>2.1-RELEASE</version>
|
||||
<build>
|
||||
<plugins>
|
||||
<!--setup for java jdk 12-->
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -65,6 +65,7 @@ public class ConfigManager {
|
||||
System.out.println("Settings folder: " + SettingsPath.toString());
|
||||
checkSettingsFolder();
|
||||
checkSettingsFile();
|
||||
FileHelper.setAllPerms(SettingsPath);
|
||||
}
|
||||
|
||||
private static void saveSettingsFile() {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -20,7 +20,7 @@ public class NetworkManager {
|
||||
return;
|
||||
}
|
||||
|
||||
Platform platform = Platform.getCurrentPlatform();
|
||||
Platform platform = Platform.CurrentPlatform;
|
||||
|
||||
if (platform.isLinux()) {
|
||||
networking = new LinuxNetworking();
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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,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());
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
package com.chameleonvision.vision.pipeline.pipes;
|
||||
package com.chameleonvision.vision.pipeline;
|
||||
|
||||
import org.apache.commons.lang3.tuple.Pair;
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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> {
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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,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;
|
||||
|
||||
@@ -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,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,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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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,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;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,271 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.util.Objects;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonGenerator;
|
||||
import com.fasterxml.jackson.core.JsonParser;
|
||||
import com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.DeserializationContext;
|
||||
import com.fasterxml.jackson.databind.JsonNode;
|
||||
import com.fasterxml.jackson.databind.SerializerProvider;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
|
||||
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
|
||||
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
|
||||
|
||||
/**
|
||||
* Represents a 2d pose containing translational and rotational elements.
|
||||
*/
|
||||
@JsonSerialize(using = Pose2d.PoseSerializer.class)
|
||||
@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
|
||||
public class Pose2d {
|
||||
private final Translation2d m_translation;
|
||||
private final Rotation2d m_rotation;
|
||||
|
||||
/**
|
||||
* Constructs a pose at the origin facing toward the positive X axis.
|
||||
* (Translation2d{0, 0} and Rotation{0})
|
||||
*/
|
||||
public Pose2d() {
|
||||
m_translation = new Translation2d();
|
||||
m_rotation = new Rotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a pose with the specified translation and rotation.
|
||||
*
|
||||
* @param translation The translational component of the pose.
|
||||
* @param rotation The rotational component of the pose.
|
||||
*/
|
||||
public Pose2d(Translation2d translation, Rotation2d rotation) {
|
||||
m_translation = translation;
|
||||
m_rotation = rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convenience constructors that takes in x and y values directly instead of
|
||||
* having to construct a Translation2d.
|
||||
*
|
||||
* @param x The x component of the translational component of the pose.
|
||||
* @param y The y component of the translational component of the pose.
|
||||
* @param rotation The rotational component of the pose.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Pose2d(double x, double y, Rotation2d rotation) {
|
||||
m_translation = new Translation2d(x, y);
|
||||
m_rotation = rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transforms the pose by the given transformation and returns the new
|
||||
* transformed pose.
|
||||
*
|
||||
* <p>The matrix multiplication is as follows
|
||||
* [x_new] [cos, -sin, 0][transform.x]
|
||||
* [y_new] += [sin, cos, 0][transform.y]
|
||||
* [t_new] [0, 0, 1][transform.t]
|
||||
*
|
||||
* @param other The transform to transform the pose by.
|
||||
* @return The transformed pose.
|
||||
*/
|
||||
public Pose2d plus(Transform2d other) {
|
||||
return transformBy(other);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Transform2d that maps the one pose to another.
|
||||
*
|
||||
* @param other The initial pose of the transformation.
|
||||
* @return The transform that maps the other pose to the current pose.
|
||||
*/
|
||||
public Transform2d minus(Pose2d other) {
|
||||
final var pose = this.relativeTo(other);
|
||||
return new Transform2d(pose.getTranslation(), pose.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the pose.
|
||||
*/
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return The rotational component of the pose.
|
||||
*/
|
||||
public Rotation2d getRotation() {
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transforms the pose by the given transformation and returns the new pose.
|
||||
* See + operator for the matrix multiplication performed.
|
||||
*
|
||||
* @param other The transform to transform the pose by.
|
||||
* @return The transformed pose.
|
||||
*/
|
||||
public Pose2d transformBy(Transform2d other) {
|
||||
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
|
||||
m_rotation.plus(other.getRotation()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the other pose relative to the current pose.
|
||||
*
|
||||
* <p>This function can often be used for trajectory tracking or pose
|
||||
* stabilization algorithms to get the error between the reference and the
|
||||
* current pose.
|
||||
*
|
||||
* @param other The pose that is the origin of the new coordinate frame that
|
||||
* the current pose will be converted into.
|
||||
* @return The current pose relative to the new origin pose.
|
||||
*/
|
||||
public Pose2d relativeTo(Pose2d other) {
|
||||
var transform = new Transform2d(other, this);
|
||||
return new Pose2d(transform.getTranslation(), transform.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Obtain a new Pose2d from a (constant curvature) velocity.
|
||||
*
|
||||
* <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
|
||||
* Controls Engineering in the FIRST Robotics Competition</a>
|
||||
* section on nonlinear pose estimation for derivation.
|
||||
*
|
||||
* <p>The twist is a change in pose in the robot's coordinate frame since the
|
||||
* previous pose update. When the user runs exp() on the previous known
|
||||
* field-relative pose with the argument being the twist, the user will
|
||||
* receive the new field-relative pose.
|
||||
*
|
||||
* <p>"Exp" represents the pose exponential, which is solving a differential
|
||||
* equation moving the pose forward in time.
|
||||
*
|
||||
* @param twist The change in pose in the robot's coordinate frame since the
|
||||
* previous pose update. For example, if a non-holonomic robot moves forward
|
||||
* 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
|
||||
* the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
|
||||
* @return The new pose of the robot.
|
||||
*/
|
||||
@SuppressWarnings("LocalVariableName")
|
||||
public Pose2d exp(Twist2d twist) {
|
||||
double dx = twist.dx;
|
||||
double dy = twist.dy;
|
||||
double dtheta = twist.dtheta;
|
||||
|
||||
double sinTheta = Math.sin(dtheta);
|
||||
double cosTheta = Math.cos(dtheta);
|
||||
|
||||
double s;
|
||||
double c;
|
||||
if (Math.abs(dtheta) < 1E-9) {
|
||||
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
|
||||
c = 0.5 * dtheta;
|
||||
} else {
|
||||
s = sinTheta / dtheta;
|
||||
c = (1 - cosTheta) / dtheta;
|
||||
}
|
||||
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
|
||||
new Rotation2d(cosTheta, sinTheta));
|
||||
|
||||
return this.plus(transform);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a Twist2d that maps this pose to the end pose. If c is the output
|
||||
* of a.Log(b), then a.Exp(c) would yield b.
|
||||
*
|
||||
* @param end The end pose for the transformation.
|
||||
* @return The twist that maps this to end.
|
||||
*/
|
||||
public Twist2d log(Pose2d end) {
|
||||
final var transform = end.relativeTo(this);
|
||||
final var dtheta = transform.getRotation().getRadians();
|
||||
final var halfDtheta = dtheta / 2.0;
|
||||
|
||||
final var cosMinusOne = transform.getRotation().getCos() - 1;
|
||||
|
||||
double halfThetaByTanOfHalfDtheta;
|
||||
if (Math.abs(cosMinusOne) < 1E-9) {
|
||||
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
|
||||
} else {
|
||||
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
|
||||
}
|
||||
|
||||
Translation2d translationPart = transform.getTranslation().rotateBy(
|
||||
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
|
||||
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
|
||||
|
||||
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Pose2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Pose2d) {
|
||||
return ((Pose2d) obj).m_translation.equals(m_translation)
|
||||
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_translation, m_rotation);
|
||||
}
|
||||
|
||||
static class PoseSerializer extends StdSerializer<Pose2d> {
|
||||
PoseSerializer() {
|
||||
super(Pose2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void serialize(
|
||||
Pose2d value, JsonGenerator jgen, SerializerProvider provider)
|
||||
throws IOException, JsonProcessingException {
|
||||
|
||||
jgen.writeStartObject();
|
||||
jgen.writeObjectField("translation", value.m_translation);
|
||||
jgen.writeObjectField("rotation", value.m_rotation);
|
||||
jgen.writeEndObject();
|
||||
}
|
||||
}
|
||||
|
||||
static class PoseDeserializer extends StdDeserializer<Pose2d> {
|
||||
PoseDeserializer() {
|
||||
super(Pose2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
|
||||
throws IOException, JsonProcessingException {
|
||||
JsonNode node = jp.getCodec().readTree(jp);
|
||||
|
||||
Translation2d translation =
|
||||
jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
|
||||
Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
|
||||
return new Pose2d(translation, rotation);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,251 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.util.Objects;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonGenerator;
|
||||
import com.fasterxml.jackson.core.JsonParser;
|
||||
import com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.DeserializationContext;
|
||||
import com.fasterxml.jackson.databind.JsonNode;
|
||||
import com.fasterxml.jackson.databind.SerializerProvider;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
|
||||
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
|
||||
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
|
||||
|
||||
/**
|
||||
* A rotation in a 2d coordinate frame represented a point on the unit circle
|
||||
* (cosine and sine).
|
||||
*/
|
||||
@JsonSerialize(using = Rotation2d.RotationSerializer.class)
|
||||
@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
|
||||
public class Rotation2d {
|
||||
private final double m_value;
|
||||
private final double m_cos;
|
||||
private final double m_sin;
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with a default angle of 0 degrees.
|
||||
*/
|
||||
public Rotation2d() {
|
||||
m_value = 0.0;
|
||||
m_cos = 1.0;
|
||||
m_sin = 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with the given radian value.
|
||||
* The x and y don't have to be normalized.
|
||||
*
|
||||
* @param value The value of the angle in radians.
|
||||
*/
|
||||
public Rotation2d(double value) {
|
||||
m_value = value;
|
||||
m_cos = Math.cos(value);
|
||||
m_sin = Math.sin(value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Rotation2d with the given x and y (cosine and sine)
|
||||
* components.
|
||||
*
|
||||
* @param x The x component or cosine of the rotation.
|
||||
* @param y The y component or sine of the rotation.
|
||||
*/
|
||||
@SuppressWarnings("ParameterName")
|
||||
public Rotation2d(double x, double y) {
|
||||
double magnitude = Math.hypot(x, y);
|
||||
if (magnitude > 1e-6) {
|
||||
m_sin = y / magnitude;
|
||||
m_cos = x / magnitude;
|
||||
} else {
|
||||
m_sin = 0.0;
|
||||
m_cos = 1.0;
|
||||
}
|
||||
m_value = Math.atan2(m_sin, m_cos);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs and returns a Rotation2d with the given degree value.
|
||||
*
|
||||
* @param degrees The value of the angle in degrees.
|
||||
* @return The rotation object with the desired angle value.
|
||||
*/
|
||||
public static Rotation2d fromDegrees(double degrees) {
|
||||
return new Rotation2d(Math.toRadians(degrees));
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds two rotations together, with the result being bounded between -pi and
|
||||
* pi.
|
||||
*
|
||||
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
|
||||
* Rotation2d{-pi/2}
|
||||
*
|
||||
* @param other The rotation to add.
|
||||
* @return The sum of the two rotations.
|
||||
*/
|
||||
public Rotation2d plus(Rotation2d other) {
|
||||
return rotateBy(other);
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtracts the new rotation from the current rotation and returns the new
|
||||
* rotation.
|
||||
*
|
||||
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
|
||||
* Rotation2d{-pi/2}
|
||||
*
|
||||
* @param other The rotation to subtract.
|
||||
* @return The difference between the two rotations.
|
||||
*/
|
||||
public Rotation2d minus(Rotation2d other) {
|
||||
return rotateBy(other.unaryMinus());
|
||||
}
|
||||
|
||||
/**
|
||||
* Takes the inverse of the current rotation. This is simply the negative of
|
||||
* the current angular value.
|
||||
*
|
||||
* @return The inverse of the current rotation.
|
||||
*/
|
||||
public Rotation2d unaryMinus() {
|
||||
return new Rotation2d(-m_value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies the current rotation by a scalar.
|
||||
*
|
||||
* @param scalar The scalar.
|
||||
* @return The new scaled Rotation2d.
|
||||
*/
|
||||
public Rotation2d times(double scalar) {
|
||||
return new Rotation2d(m_value * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds the new rotation to the current rotation using a rotation matrix.
|
||||
*
|
||||
* <p>The matrix multiplication is as follows:
|
||||
* [cos_new] [other.cos, -other.sin][cos]
|
||||
* [sin_new] = [other.sin, other.cos][sin]
|
||||
* value_new = atan2(cos_new, sin_new)
|
||||
*
|
||||
* @param other The rotation to rotate by.
|
||||
* @return The new rotated Rotation2d.
|
||||
*/
|
||||
public Rotation2d rotateBy(Rotation2d other) {
|
||||
return new Rotation2d(
|
||||
m_cos * other.m_cos - m_sin * other.m_sin,
|
||||
m_cos * other.m_sin + m_sin * other.m_cos
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
* Returns the radian value of the rotation.
|
||||
*
|
||||
* @return The radian value of the rotation.
|
||||
*/
|
||||
public double getRadians() {
|
||||
return m_value;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the degree value of the rotation.
|
||||
*
|
||||
* @return The degree value of the rotation.
|
||||
*/
|
||||
public double getDegrees() {
|
||||
return Math.toDegrees(m_value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the cosine of the rotation.
|
||||
*
|
||||
* @return The cosine of the rotation.
|
||||
*/
|
||||
public double getCos() {
|
||||
return m_cos;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the sine of the rotation.
|
||||
*
|
||||
* @return The sine of the rotation.
|
||||
*/
|
||||
public double getSin() {
|
||||
return m_sin;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the tangent of the rotation.
|
||||
*
|
||||
* @return The tangent of the rotation.
|
||||
*/
|
||||
public double getTan() {
|
||||
return m_sin / m_cos;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Rotation2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Rotation2d) {
|
||||
return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_value);
|
||||
}
|
||||
|
||||
static class RotationSerializer extends StdSerializer<Rotation2d> {
|
||||
RotationSerializer() {
|
||||
super(Rotation2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void serialize(
|
||||
Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
|
||||
throws IOException, JsonProcessingException {
|
||||
|
||||
jgen.writeStartObject();
|
||||
jgen.writeNumberField("radians", value.m_value);
|
||||
jgen.writeEndObject();
|
||||
}
|
||||
}
|
||||
|
||||
static class RotationDeserializer extends StdDeserializer<Rotation2d> {
|
||||
RotationDeserializer() {
|
||||
super(Rotation2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
|
||||
throws IOException, JsonProcessingException {
|
||||
JsonNode node = jp.getCodec().readTree(jp);
|
||||
double radians = node.get("radians").numberValue().doubleValue();
|
||||
|
||||
return new Rotation2d(radians);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* Represents a transformation for a Pose2d.
|
||||
*/
|
||||
public class Transform2d {
|
||||
private final Translation2d m_translation;
|
||||
private final Rotation2d m_rotation;
|
||||
|
||||
/**
|
||||
* Constructs the transform that maps the initial pose to the final pose.
|
||||
*
|
||||
* @param initial The initial pose for the transformation.
|
||||
* @param last The final pose for the transformation.
|
||||
*/
|
||||
public Transform2d(Pose2d initial, Pose2d last) {
|
||||
// We are rotating the difference between the translations
|
||||
// using a clockwise rotation matrix. This transforms the global
|
||||
// delta into a local delta (relative to the initial pose).
|
||||
m_translation = last.getTranslation().minus(initial.getTranslation())
|
||||
.rotateBy(initial.getRotation().unaryMinus());
|
||||
|
||||
m_rotation = last.getRotation().minus(initial.getRotation());
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a transform with the given translation and rotation components.
|
||||
*
|
||||
* @param translation Translational component of the transform.
|
||||
* @param rotation Rotational component of the transform.
|
||||
*/
|
||||
public Transform2d(Translation2d translation, Rotation2d rotation) {
|
||||
m_translation = translation;
|
||||
m_rotation = rotation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs the identity transform -- maps an initial pose to itself.
|
||||
*/
|
||||
public Transform2d() {
|
||||
m_translation = new Translation2d();
|
||||
m_rotation = new Rotation2d();
|
||||
}
|
||||
|
||||
/**
|
||||
* Scales the transform by the scalar.
|
||||
*
|
||||
* @param scalar The scalar.
|
||||
* @return The scaled Transform2d.
|
||||
*/
|
||||
public Transform2d times(double scalar) {
|
||||
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the translation component of the transformation.
|
||||
*
|
||||
* @return The translational component of the transform.
|
||||
*/
|
||||
public Translation2d getTranslation() {
|
||||
return m_translation;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotational component of the transformation.
|
||||
*
|
||||
* @return Reference to the rotational component of the transform.
|
||||
*/
|
||||
public Rotation2d getRotation() {
|
||||
return m_rotation;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Transform2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Transform2d) {
|
||||
return ((Transform2d) obj).m_translation.equals(m_translation)
|
||||
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_translation, m_rotation);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,243 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.util.Objects;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonGenerator;
|
||||
import com.fasterxml.jackson.core.JsonParser;
|
||||
import com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.DeserializationContext;
|
||||
import com.fasterxml.jackson.databind.JsonNode;
|
||||
import com.fasterxml.jackson.databind.SerializerProvider;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
|
||||
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
|
||||
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
|
||||
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
|
||||
|
||||
/**
|
||||
* Represents a translation in 2d space.
|
||||
* This object can be used to represent a point or a vector.
|
||||
*
|
||||
* <p>This assumes that you are using conventional mathematical axes.
|
||||
* When the robot is placed on the origin, facing toward the X direction,
|
||||
* moving forward increases the X, whereas moving to the left increases the Y.
|
||||
*/
|
||||
@JsonSerialize(using = Translation2d.TranslationSerializer.class)
|
||||
@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
|
||||
@SuppressWarnings({"ParameterName", "MemberName"})
|
||||
public class Translation2d {
|
||||
private final double m_x;
|
||||
private final double m_y;
|
||||
|
||||
/**
|
||||
* Constructs a Translation2d with X and Y components equal to zero.
|
||||
*/
|
||||
public Translation2d() {
|
||||
this(0.0, 0.0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Translation2d with the X and Y components equal to the
|
||||
* provided values.
|
||||
*
|
||||
* @param x The x component of the translation.
|
||||
* @param y The y component of the translation.
|
||||
*/
|
||||
public Translation2d(double x, double y) {
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the distance between two translations in 2d space.
|
||||
*
|
||||
* <p>This function uses the pythagorean theorem to calculate the distance.
|
||||
* distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
|
||||
*
|
||||
* @param other The translation to compute the distance to.
|
||||
* @return The distance between the two translations.
|
||||
*/
|
||||
public double getDistance(Translation2d other) {
|
||||
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the X component of the translation.
|
||||
*
|
||||
* @return The x component of the translation.
|
||||
*/
|
||||
public double getX() {
|
||||
return m_x;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the Y component of the translation.
|
||||
*
|
||||
* @return The y component of the translation.
|
||||
*/
|
||||
public double getY() {
|
||||
return m_y;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the norm, or distance from the origin to the translation.
|
||||
*
|
||||
* @return The norm of the translation.
|
||||
*/
|
||||
public double getNorm() {
|
||||
return Math.hypot(m_x, m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Applies a rotation to the translation in 2d space.
|
||||
*
|
||||
* <p>This multiplies the translation vector by a counterclockwise rotation
|
||||
* matrix of the given angle.
|
||||
* [x_new] [other.cos, -other.sin][x]
|
||||
* [y_new] = [other.sin, other.cos][y]
|
||||
*
|
||||
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
|
||||
* Translation2d of {0, 2}.
|
||||
*
|
||||
* @param other The rotation to rotate the translation by.
|
||||
* @return The new rotated translation.
|
||||
*/
|
||||
public Translation2d rotateBy(Rotation2d other) {
|
||||
return new Translation2d(
|
||||
m_x * other.getCos() - m_y * other.getSin(),
|
||||
m_x * other.getSin() + m_y * other.getCos()
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds two translations in 2d space and returns the sum. This is similar to
|
||||
* vector addition.
|
||||
*
|
||||
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
|
||||
* Translation2d{3.0, 8.0}
|
||||
*
|
||||
* @param other The translation to add.
|
||||
* @return The sum of the translations.
|
||||
*/
|
||||
public Translation2d plus(Translation2d other) {
|
||||
return new Translation2d(m_x + other.m_x, m_y + other.m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtracts the other translation from the other translation and returns the
|
||||
* difference.
|
||||
*
|
||||
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
|
||||
* Translation2d{4.0, 2.0}
|
||||
*
|
||||
* @param other The translation to subtract.
|
||||
* @return The difference between the two translations.
|
||||
*/
|
||||
public Translation2d minus(Translation2d other) {
|
||||
return new Translation2d(m_x - other.m_x, m_y - other.m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the inverse of the current translation. This is equivalent to
|
||||
* rotating by 180 degrees, flipping the point over both axes, or simply
|
||||
* negating both components of the translation.
|
||||
*
|
||||
* @return The inverse of the current translation.
|
||||
*/
|
||||
public Translation2d unaryMinus() {
|
||||
return new Translation2d(-m_x, -m_y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Multiplies the translation by a scalar and returns the new translation.
|
||||
*
|
||||
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
|
||||
*
|
||||
* @param scalar The scalar to multiply by.
|
||||
* @return The scaled translation.
|
||||
*/
|
||||
public Translation2d times(double scalar) {
|
||||
return new Translation2d(m_x * scalar, m_y * scalar);
|
||||
}
|
||||
|
||||
/**
|
||||
* Divides the translation by a scalar and returns the new translation.
|
||||
*
|
||||
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
|
||||
*
|
||||
* @param scalar The scalar to multiply by.
|
||||
* @return The reference to the new mutated object.
|
||||
*/
|
||||
public Translation2d div(double scalar) {
|
||||
return new Translation2d(m_x / scalar, m_y / scalar);
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
|
||||
}
|
||||
|
||||
public static Translation2d fromRotation2d(Rotation2d rotation) {
|
||||
return new Translation2d(rotation.getCos(), rotation.getSin());
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Translation2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Translation2d) {
|
||||
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
|
||||
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(m_x, m_y);
|
||||
}
|
||||
|
||||
static class TranslationSerializer extends StdSerializer<Translation2d> {
|
||||
TranslationSerializer() {
|
||||
super(Translation2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void serialize(
|
||||
Translation2d value, JsonGenerator jgen, SerializerProvider provider)
|
||||
throws IOException, JsonProcessingException {
|
||||
|
||||
jgen.writeStartObject();
|
||||
jgen.writeNumberField("x", value.m_x);
|
||||
jgen.writeNumberField("y", value.m_y);
|
||||
jgen.writeEndObject();
|
||||
}
|
||||
}
|
||||
|
||||
static class TranslationDeserializer extends StdDeserializer<Translation2d> {
|
||||
TranslationDeserializer() {
|
||||
super(Translation2d.class);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
|
||||
throws IOException, JsonProcessingException {
|
||||
JsonNode node = jp.getCodec().readTree(jp);
|
||||
double xval = node.get("x").numberValue().doubleValue();
|
||||
double yval = node.get("y").numberValue().doubleValue();
|
||||
|
||||
return new Translation2d(xval, yval);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.geometry;
|
||||
|
||||
import java.util.Objects;
|
||||
|
||||
/**
|
||||
* A change in distance along arc since the last pose update. We can use ideas
|
||||
* from differential calculus to create new Pose2ds from a Twist2d and vise
|
||||
* versa.
|
||||
*
|
||||
* <p>A Twist can be used to represent a difference between two poses.
|
||||
*/
|
||||
@SuppressWarnings("MemberName")
|
||||
public class Twist2d {
|
||||
/**
|
||||
* Linear "dx" component.
|
||||
*/
|
||||
public double dx;
|
||||
|
||||
/**
|
||||
* Linear "dy" component.
|
||||
*/
|
||||
public double dy;
|
||||
|
||||
/**
|
||||
* Angular "dtheta" component (radians).
|
||||
*/
|
||||
public double dtheta;
|
||||
|
||||
public Twist2d() {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a Twist2d with the given values.
|
||||
* @param dx Change in x direction relative to robot.
|
||||
* @param dy Change in y direction relative to robot.
|
||||
* @param dtheta Change in angle relative to robot.
|
||||
*/
|
||||
public Twist2d(double dx, double dy, double dtheta) {
|
||||
this.dx = dx;
|
||||
this.dy = dy;
|
||||
this.dtheta = dtheta;
|
||||
}
|
||||
|
||||
@Override
|
||||
public String toString() {
|
||||
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks equality between this Twist2d and another object.
|
||||
*
|
||||
* @param obj The other object.
|
||||
* @return Whether the two objects are equal or not.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object obj) {
|
||||
if (obj instanceof Twist2d) {
|
||||
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
|
||||
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
|
||||
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return Objects.hash(dx, dy, dtheta);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package edu.wpi.first.wpilibj.util;
|
||||
|
||||
/**
|
||||
* Utility class that converts between commonly used units in FRC.
|
||||
*/
|
||||
public final class Units {
|
||||
private static final double kInchesPerFoot = 12.0;
|
||||
private static final double kMetersPerInch = 0.0254;
|
||||
private static final double kSecondsPerMinute = 60;
|
||||
|
||||
/**
|
||||
* Utility class, so constructor is private.
|
||||
*/
|
||||
private Units() {
|
||||
throw new UnsupportedOperationException("This is a utility class!");
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given meters to feet.
|
||||
*
|
||||
* @param meters The meters to convert to feet.
|
||||
* @return Feet converted from meters.
|
||||
*/
|
||||
public static double metersToFeet(double meters) {
|
||||
return metersToInches(meters) / kInchesPerFoot;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given feet to meters.
|
||||
*
|
||||
* @param feet The feet to convert to meters.
|
||||
* @return Meters converted from feet.
|
||||
*/
|
||||
public static double feetToMeters(double feet) {
|
||||
return inchesToMeters(feet * kInchesPerFoot);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given meters to inches.
|
||||
*
|
||||
* @param meters The meters to convert to inches.
|
||||
* @return Inches converted from meters.
|
||||
*/
|
||||
public static double metersToInches(double meters) {
|
||||
return meters / kMetersPerInch;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given inches to meters.
|
||||
*
|
||||
* @param inches The inches to convert to meters.
|
||||
* @return Meters converted from inches.
|
||||
*/
|
||||
public static double inchesToMeters(double inches) {
|
||||
return inches * kMetersPerInch;
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given degrees to radians.
|
||||
*
|
||||
* @param degrees The degrees to convert to radians.
|
||||
* @return Radians converted from degrees.
|
||||
*/
|
||||
public static double degreesToRadians(double degrees) {
|
||||
return Math.toRadians(degrees);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts given radians to degrees.
|
||||
*
|
||||
* @param radians The radians to convert to degrees.
|
||||
* @return Degrees converted from radians.
|
||||
*/
|
||||
public static double radiansToDegrees(double radians) {
|
||||
return Math.toDegrees(radians);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts rotations per minute to radians per second.
|
||||
*
|
||||
* @param rpm The rotations per minute to convert to radians per second.
|
||||
* @return Radians per second converted from rotations per minute.
|
||||
*/
|
||||
public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
|
||||
return rpm * Math.PI / (kSecondsPerMinute / 2);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts radians per second to rotations per minute.
|
||||
*
|
||||
* @param radiansPerSecond The radians per second to convert to from rotations per minute.
|
||||
* @return Rotations per minute converted from radians per second.
|
||||
*/
|
||||
public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
|
||||
return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
|
||||
}
|
||||
}
|
||||
BIN
chameleon-server/src/main/resources/web/img/chessboard.png
Normal file
BIN
chameleon-server/src/main/resources/web/img/chessboard.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 61 KiB |
@@ -3,7 +3,7 @@ package com.chameleonvision.config;
|
||||
import com.chameleonvision.util.ProgramDirectoryUtilities;
|
||||
import com.chameleonvision.vision.camera.CameraStreamer;
|
||||
import com.chameleonvision.vision.image.StaticImageCapture;
|
||||
import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
|
||||
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
|
||||
import edu.wpi.cscore.CameraServerCvJNI;
|
||||
import edu.wpi.cscore.CameraServerJNI;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
@@ -58,7 +58,7 @@ class StaticCaptureTest {
|
||||
@Test
|
||||
void ImageProcessTest() throws InterruptedException {
|
||||
ImageLoadTest();
|
||||
CVPipeline2d testPipeline = new CVPipeline2d();
|
||||
StandardCVPipeline testPipeline = new StandardCVPipeline();
|
||||
String testImage1 = "CargoSideStraightDark36in";
|
||||
StaticImageCapture testCapture1 = loadedImages.get(testImage1);
|
||||
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 170 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 54 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 132 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 61 KiB |
@@ -0,0 +1,38 @@
|
||||
package com.chameleonvision.vision.pipeline;
|
||||
|
||||
import com.chameleonvision.vision.image.StaticImageCapture;
|
||||
import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
|
||||
import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
|
||||
import edu.wpi.cscore.CameraServerCvJNI;
|
||||
import edu.wpi.cscore.CameraServerJNI;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Path;
|
||||
|
||||
public class SolvePNPtest {
|
||||
|
||||
private static final Path root = Path.of("src", "test", "java", "com", "chameleonvision", "vision", "pipeline");
|
||||
|
||||
@Test public void test20in() {
|
||||
|
||||
try {
|
||||
forceLoad();
|
||||
} catch (IOException e) {
|
||||
return;
|
||||
}
|
||||
|
||||
// mock up pipeline
|
||||
var pipeline = new StandardCVPipeline();
|
||||
var capture = new StaticImageCapture(Path.of(root.toString(), "20in.png"));
|
||||
pipeline.initPipeline(capture);
|
||||
var settings = new StandardCVPipelineSettings();
|
||||
|
||||
}
|
||||
|
||||
private void forceLoad() throws IOException {
|
||||
CameraServerJNI.forceLoad();
|
||||
CameraServerCvJNI.forceLoad();
|
||||
}
|
||||
|
||||
}
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 1.7 MiB |
Reference in New Issue
Block a user