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>

.gitignore

@@ -119,3 +119,4 @@ New client/chameleon-client/*
 
 *.prefs
 *.jfr
+.DS_Store

chameleon-client/package-lock.json

@@ -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",

chameleon-client/package.json

@@ -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",

chameleon-client/src/assets/robotIcon.svg

@@ -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>

chameleon-client/src/components/3D/MiniMap.vue

@@ -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>

chameleon-client/src/components/cv-number-input.vue

@@ -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 {}
         },

chameleon-client/src/store.js

@@ -37,7 +37,8 @@ export default new Vuex.Store({
             isBinary: 0,
             calibrationMode: 0,
             videoModeIndex:0,
-            streamDivisor:0
+            streamDivisor:0,
+            is3D:false
         },
         cameraSettings: {},
         resolutionList: [],

chameleon-client/src/views/Camera.vue

@@ -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"
+                        <v-row align="center">
-                             crossorigin="Anonymous"/>
+                            <img id="CameraStream" style="display: block;margin: auto; width: 70%;height: 70%;"
-                        <span v-else>No Cameras Are connected</span>
+                                 v-if="cameraList.length > 0"
-                        <h5 id="Point">{{point}}</h5>
+                                 :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;
+                    return this.$store.state.point.targets;
-                    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 {
+            fps: {
-                        return undefined;
+                get() {
-                    }
+                    return this.$store.state.point.fps;
                 }
             },
             currentCameraIndex: {
@@ -384,17 +419,16 @@
         text-align: center;
     }
 
-    .videoClass img {
+
-        max-height: 70vh;
+    .tableClass {
-        max-width: 70%;
+        padding-top: 5px;
         width: 70%;
-        object-fit: cover;
+        text-align: center;
-        vertical-align: middle;
     }
 
-    #Point {
+    th {
-        padding-top: 5px;
+        width: 80px;
         text-align: center;
-        color: #f4f4f4;
     }
+
 </style>

chameleon-client/src/views/CameraViewes/3D.vue

@@ -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>

chameleon-client/src/views/CameraViewes/OutputTab.vue

@@ -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']"

chameleon-client/src/views/SettingsViewes/Cameras.vue

@@ -1,8 +1,54 @@
 <template>
     <div>
-        <CVselect name="Camera" :list="cameraList" v-model="currentCameraIndex"/>
+        <div>
-        <CVnumberinput name="Diagonal FOV" v-model="cameraSettings.fov"/>
+            <CVselect name="Camera" :list="cameraList" v-model="currentCameraIndex" @input="handleInput('currentCamera',currentCameraIndex)"/>
-        <v-btn style="margin-top:10px" small color="#4baf62" @click="sendCameraSettings">Save Camera Settings</v-btn>
+            <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;

chameleon-server/chameleon-vision.iml

@@ -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>

chameleon-server/pom.xml

@@ -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-->

chameleon-server/src/main/java/com/chameleonvision/config/CameraCalibrationConfig.java

@@ -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());
+    }
+}

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

@@ -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);
+        configFolderPath = Path.of(camerasConfigFolderPath.toString(), cameraConfigName);
-        configPath = Paths.get(configFolderPath.toString(), "camera.json");
+        configPath = Path.of(configFolderPath.toString(), "camera.json");
-        driverModePath = Paths.get(configFolderPath.toString(), "drivermode.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);
     }
 }

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

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

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

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

chameleon-server/src/main/java/com/chameleonvision/config/JsonMat.java

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

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

@@ -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.StandardCVPipelineSettings;
-import com.chameleonvision.vision.pipeline.impl.CVPipeline3dSettings;
 
 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 = pipelineName + ".json";
-        String fullFileName = prefix + 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 {
-            try {
+            JacksonHelper.serializer(path, settings);
-                JacksonHelper.serializer(path, settings);
+            FileHelper.setFilePerms(path);
-                FileHelper.setFilePerms(path);
+        } catch (IOException e) {
-            } catch (IOException e) {
+            e.printStackTrace();
-                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~");
         }
     }
 
@@ -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);
+                        var pipe = JacksonHelper.deserializer(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.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);
                         deserializedList.add(pipe);
                     } catch (IOException e) {
                         System.err.println("couldn't load cvpipeline2d");
                     }
-                }
             }
         }
 

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

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

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

@@ -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();
+        }
+    }
 }

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

@@ -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 {

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

@@ -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);
+            USBCameraCapture camera = new USBCameraCapture(config);
-            VisionProcess process = new VisionProcess(camera, cameraJsonConfig.name, config.pipelines);
+            VisionProcess process = new VisionProcess(camera, config);
-            process.pipelineManager.driverModePipeline.settings = config.drivermode;
+            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;

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

@@ -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.StandardCVPipeline;
-import com.chameleonvision.vision.pipeline.impl.CVPipeline3d;
 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) {
+                    if (data instanceof StandardCVPipeline.StandardCVPipelineResult) {
-                        CVPipeline2d.CVPipeline2dResult result = (CVPipeline2d.CVPipeline2dResult) data;
+                        StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data;
-                        CVPipeline2d.Target2d bestTarget = result.targets.get(0);
+                        StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0);
-                        center.add(bestTarget.rawPoint.center.x);
+                        if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) {
-                        center.add(bestTarget.rawPoint.center.y);
+                            for (var target : result.targets) {
-                        calculated.put("pitch", bestTarget.pitch);
+                                pointMap = new HashMap<>();
-                        calculated.put("yaw", bestTarget.yaw);
+                                pointMap.put("pitch", target.pitch);
-                        calculated.put("area", bestTarget.area);
+                                pointMap.put("yaw", target.yaw);
-                    } else if (data instanceof CVPipeline3d.CVPipeline3dResult) {
+                                pointMap.put("area", target.area);
-                        // TODO: (2.1) 3d stuff in UI
+                                pointMap.put("pose", target.cameraRelativePose);
-                    } else {
+                                webTargets.add(pointMap);
-                        center.add(null);
+                            }
-                        center.add(null);
+                        } else {
-                        calculated.put("pitch", null);
+                            pointMap.put("pitch", bestTarget.pitch);
-                        calculated.put("yaw", null);
+                            pointMap.put("yaw", bestTarget.yaw);
-                        calculated.put("area", null);
+                            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.hasTarget && !(data instanceof DriverVisionPipeline.DriverPipelineResult)) {
-            if(data instanceof CVPipeline2d.CVPipeline2dResult) {
+            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) {
+            } else {
-                // TODO: (2.1) 3d stuff...
+                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;
         }
 

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

@@ -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();
 }

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

@@ -1,7 +1,7 @@
 package com.chameleonvision.vision.camera;
 
-import com.chameleonvision.config.CameraJsonConfig;
+import com.chameleonvision.config.CameraCalibrationConfig;
-import com.chameleonvision.vision.image.CaptureProperties;
+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;
+    }
 }

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

@@ -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());
     }
 
+
+
 }

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

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

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

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

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

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

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/Pipe.java

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

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

@@ -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) {
+        if (setting instanceof StandardCVPipelineSettings) {
-            pipelines.add(new CVPipeline3d((CVPipeline3dSettings) setting));
+            pipelines.add(new StandardCVPipeline((StandardCVPipelineSettings) setting));
-        } else if (setting instanceof CVPipeline2dSettings) {
-            pipelines.add(new CVPipeline2d((CVPipeline2dSettings) 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) {
+        if (driverMode) setCurrentPipeline(DRIVERMODE_INDEX);
-            setCurrentPipeline(DRIVERMODE_INDEX);
+        else setCurrentPipeline(lastPipelineIndex);
-        } 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) {
+    public void addNewPipeline(String piplineName) {
-        CVPipeline newPipeline;
+        StandardCVPipeline newPipeline = new StandardCVPipeline();
-        if (!is3D) {
-            newPipeline = new CVPipeline2d();
-        } else {
-            newPipeline = new CVPipeline3d();
-        }
         newPipeline.settings.nickname = piplineName;
         newPipeline.settings.index = pipelines.size();
         addPipeline(newPipeline);

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

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

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

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

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/Calibrate3dPipeline.java

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

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipeline.java

@@ -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() {
+    public StandardCVPipeline() {
-        super(new CVPipeline2dSettings());
+        super(new StandardCVPipelineSettings());
     }
 
-    public CVPipeline2d(String name) {
+    public StandardCVPipeline(String name) {
-        super(name, new CVPipeline2dSettings());
+        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,6 +125,15 @@ 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
@@ -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> result;
-        Pair<Mat, Long> draw2dContoursResult = draw2dContoursPipe.run(Pair.of(outputMatResult.getLeft(), sortContoursResult.getLeft()));
+
-        totalPipelineTimeNanos += draw2dContoursResult.getRight();
+        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 static class StandardCVPipelineResult extends CVPipelineResult<TrackedTarget> {
-        public CVPipeline2dResult(List<Target2d> targets, Mat outputMat, long processTimeNanos) {
+        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;
     }
 
 

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/impl/StandardCVPipelineSettings.java

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

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

@@ -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> {
 

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

@@ -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) {
+            for (var t : input) {
-                CVPipeline2d.Target2d t = new CVPipeline2d.Target2d();
-                t.rawPoint = r;
                 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.calibratedX = (t.minAreaRect.center.y - this.calibrationB) / this.calibrationM;
-                        t.calibratedY = (r.center.x * this.calibrationM) + this.calibrationB;
+                        t.calibratedY = (t.minAreaRect.center.x * this.calibrationM) + this.calibrationB;
                         break;
                 }
 
-                t.pitch = calculatePitch(r.center.y, t.calibratedY);
+                t.pitch = calculatePitch(t.minAreaRect.center.y, t.calibratedY);
-                t.yaw = calculateYaw(r.center.x, t.calibratedX);
+                t.yaw = calculateYaw(t.minAreaRect.center.x, t.calibratedX);
-                t.area = r.size.area() / imageArea;
+                t.area = t.minAreaRect.size.area() / imageArea;
 
                 targets.add(t);
             }

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

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

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

@@ -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:

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/DrawSolvePNPPipe.java

@@ -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);
+    }
+}

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

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

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

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

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

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

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

@@ -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();

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

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

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

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

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

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

chameleon-server/src/main/java/com/chameleonvision/vision/pipeline/pipes/SolvePNPPipe.java

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

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

@@ -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<StandardCVPipeline.TrackedTarget> SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area());
-    private static final Comparator<RotatedRect> SortBySmallestComparator = SortByLargestComparator.reversed();
+    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<StandardCVPipeline.TrackedTarget> SortByHighestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.center.y, rect1.minAreaRect.center.y);
-    private static final Comparator<RotatedRect> SortByLowestComparator = SortByHighestComparator.reversed();
+    private static final Comparator<StandardCVPipeline.TrackedTarget> SortByLowestComparator = SortByHighestComparator.reversed();
 
-    private static final Comparator<RotatedRect> SortByLeftmostComparator = Comparator.comparingDouble(rect -> rect.center.x);
+    public static final Comparator<StandardCVPipeline.TrackedTarget> SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x);
-    private static final Comparator<RotatedRect> SortByRightmostComparator = SortByLeftmostComparator.reversed();
+    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) {
+    private double calcSquareCenterDistance(StandardCVPipeline.TrackedTarget rect) {
-        return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.center.x, 2) + FastMath.pow(camProps.centerY - rect.center.y, 2));
+        return FastMath.sqrt(FastMath.pow(camProps.centerX - rect.minAreaRect.center.x, 2) + FastMath.pow(camProps.centerY - rect.minAreaRect.center.y, 2));
     }
 }

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

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

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

@@ -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);
+    }
 }

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

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

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

@@ -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(pipeName);
-                        currentProcess.pipelineManager.addNewPipeline(is3d, 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)
+                                if (currentPipeline instanceof StandardCVPipeline)
-                                    ((CVPipeline2d) currentPipeline).settings.point = Arrays.asList(prop.mode.width / 2, prop.mode.height / 2);//Reset Crosshair in single point calib
+                                    ((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)
+                                if (currentPipeline instanceof StandardCVPipeline)
-                                    ((CVPipeline2d) currentPipeline).settings.point = new ArrayList<>();//This will reset the calibration
+                                    ((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)
+        synchronized (broadcastLock) {
-            for (WsContext user : users) {
+            if (users != null) {
-                if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
+                var userList = users;
-                    continue;
+                for (WsContext user : userList) {
-                }
+                    if (userToSkip != null && user.getSessionId().equals(userToSkip.getSessionId())) {
-                try {
+                        continue;
-                    ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
+                    }
-                    user.send(b);
+                    try {
-                } catch (JsonProcessingException e) {
+                        ByteBuffer b = ByteBuffer.wrap(objectMapper.writeValueAsBytes(obj));
-                    e.printStackTrace();
+                        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;
     }
 

chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java

@@ -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);
+        }
+    }
+}

chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java

@@ -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);
+        }
+    }
+}

chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

@@ -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);
+    }
+}

chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java

@@ -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);
+        }
+    }
+}

chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java

@@ -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);
+    }
+}

chameleon-server/src/main/java/edu/wpi/first/wpilibj/util/Units.java

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

chameleon-server/src/test/java/com/chameleonvision/config/StaticCaptureTest.java

@@ -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);
 

chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java

@@ -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();
+    }
+
+}