diff --git a/photon-client/package-lock.json b/photon-client/package-lock.json index 727546fc6b..aba1b2aa4b 100644 --- a/photon-client/package-lock.json +++ b/photon-client/package-lock.json @@ -12,10 +12,15 @@ "@mdi/font": "^7.2.96", "@msgpack/msgpack": "^3.0.0-beta2", "axios": "^1.4.0", + "chart.js": "2.9.3", + "chartjs-plugin-annotation": "^0.5.7", + "core-js": "^3.30.2", "jspdf": "^2.5.1", "pinia": "^2.1.4", "three": "^0.154.0", "vue": "^2.7.14", + "vue-axios": "^3.5.2", + "vue-chartjs": "^3.5.1", "vue-router": "^3.6.5", "vuetify": "^2.6.15" }, @@ -698,6 +703,14 @@ "integrity": "sha512-lB9lMjuqjtuJrx7/kOkqQBtllspPIN+96OvTCeJ2j5FEzinoAXTdAMFnDAQT1KVPRlnYfBrqxtqP66vDM40xxQ==", "dev": true }, + "node_modules/@types/chart.js": { + "version": "2.9.41", + "resolved": "https://registry.npmjs.org/@types/chart.js/-/chart.js-2.9.41.tgz", + "integrity": "sha512-3dvkDvueckY83UyUXtJMalYoH6faOLkWQoaTlJgB4Djde3oORmNP0Jw85HtzTuXyliUHcdp704s0mZFQKio/KQ==", + "dependencies": { + "moment": "^2.10.2" + } + }, "node_modules/@types/eslint": { "version": "8.40.2", "resolved": "https://registry.npmjs.org/@types/eslint/-/eslint-8.40.2.tgz", @@ -1601,6 +1614,53 @@ "url": "https://github.com/chalk/chalk?sponsor=1" } }, + "node_modules/chart.js": { + "version": "2.9.3", + "resolved": "https://registry.npmjs.org/chart.js/-/chart.js-2.9.3.tgz", + "integrity": "sha512-+2jlOobSk52c1VU6fzkh3UwqHMdSlgH1xFv9FKMqHiNCpXsGPQa/+81AFa+i3jZ253Mq9aAycPwDjnn1XbRNNw==", + "dependencies": { + "chartjs-color": "^2.1.0", + "moment": "^2.10.2" + } + }, + "node_modules/chartjs-color": { + "version": "2.4.1", + "resolved": "https://registry.npmjs.org/chartjs-color/-/chartjs-color-2.4.1.tgz", + "integrity": "sha512-haqOg1+Yebys/Ts/9bLo/BqUcONQOdr/hoEr2LLTRl6C5LXctUdHxsCYfvQVg5JIxITrfCNUDr4ntqmQk9+/0w==", + "dependencies": { + "chartjs-color-string": "^0.6.0", + "color-convert": "^1.9.3" + } + }, + "node_modules/chartjs-color-string": { + "version": "0.6.0", + "resolved": "https://registry.npmjs.org/chartjs-color-string/-/chartjs-color-string-0.6.0.tgz", + "integrity": "sha512-TIB5OKn1hPJvO7JcteW4WY/63v6KwEdt6udfnDE9iCAZgy+V4SrbSxoIbTw/xkUIapjEI4ExGtD0+6D3KyFd7A==", + "dependencies": { + "color-name": "^1.0.0" + } + }, + "node_modules/chartjs-color/node_modules/color-convert": { + "version": "1.9.3", + "resolved": "https://registry.npmjs.org/color-convert/-/color-convert-1.9.3.tgz", + "integrity": "sha512-QfAUtd+vFdAtFQcC8CCyYt1fYWxSqAiK2cSD6zDB8N3cpsEBAvRxp9zOGg6G/SHHJYAT88/az/IuDGALsNVbGg==", + "dependencies": { + "color-name": "1.1.3" + } + }, + "node_modules/chartjs-color/node_modules/color-name": { + "version": "1.1.3", + "resolved": "https://registry.npmjs.org/color-name/-/color-name-1.1.3.tgz", + "integrity": "sha512-72fSenhMw2HZMTVHeCA9KCmpEIbzWiQsjN+BHcBbS9vr1mtt+vJjPdksIBNUmKAW8TFUDPJK5SUU3QhE9NEXDw==" + }, + "node_modules/chartjs-plugin-annotation": { + "version": "0.5.7", + "resolved": "https://registry.npmjs.org/chartjs-plugin-annotation/-/chartjs-plugin-annotation-0.5.7.tgz", + "integrity": "sha512-tKN5KLc69unyZGTvSdhVQEyAOhVNnSkFCCgefZhO4UaqFfABZGFU/d9p6WM2KB0eXFs/rR3Jayh7dvyASC7K0A==", + "dependencies": { + "chart.js": "^2.4.0" + } + }, "node_modules/chokidar": { "version": "3.5.3", "resolved": "https://registry.npmjs.org/chokidar/-/chokidar-3.5.3.tgz", @@ -1665,8 +1725,7 @@ "node_modules/color-name": { "version": "1.1.4", "resolved": "https://registry.npmjs.org/color-name/-/color-name-1.1.4.tgz", - "integrity": "sha512-dOy+3AuW3a2wNbZHIuMZpTcgjGuLU/uBL/ubcZF9OXbDo8ff4O8yVp5Bf0efS8uEoYo5q4Fx7dY9OgQGXgAsQA==", - "dev": true + "integrity": "sha512-dOy+3AuW3a2wNbZHIuMZpTcgjGuLU/uBL/ubcZF9OXbDo8ff4O8yVp5Bf0efS8uEoYo5q4Fx7dY9OgQGXgAsQA==" }, "node_modules/combined-stream": { "version": "1.0.8", @@ -1696,7 +1755,6 @@ "resolved": "https://registry.npmjs.org/core-js/-/core-js-3.31.1.tgz", "integrity": "sha512-2sKLtfq1eFST7l7v62zaqXacPc7uG8ZAya8ogijLhTtaKNcpzpB4TMoTw2Si+8GYKRwFPMMtUT0263QFWFfqyQ==", "hasInstallScript": true, - "optional": true, "funding": { "type": "opencollective", "url": "https://opencollective.com/core-js" @@ -3483,6 +3541,14 @@ "node": "*" } }, + "node_modules/moment": { + "version": "2.29.4", + "resolved": "https://registry.npmjs.org/moment/-/moment-2.29.4.tgz", + "integrity": "sha512-5LC9SOxjSc2HF6vO2CyuTDNivEdoz2IvyJJGj6X8DJ0eFyfszE0QiEd+iXmBvUP3WHxSjFH/vIsA0EN00cgr8w==", + "engines": { + "node": "*" + } + }, "node_modules/ms": { "version": "2.1.2", "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.2.tgz", @@ -5276,6 +5342,30 @@ "csstype": "^3.1.0" } }, + "node_modules/vue-axios": { + "version": "3.5.2", + "resolved": "https://registry.npmjs.org/vue-axios/-/vue-axios-3.5.2.tgz", + "integrity": "sha512-GP+dct7UlAWkl1qoP3ppw0z6jcSua5/IrMpjB5O8bh089iIiJ+hdxPYH2NPEpajlYgkW5EVMP95ttXWdas1O0g==", + "peerDependencies": { + "axios": "*", + "vue": "^3.0.0 || ^2.0.0" + } + }, + "node_modules/vue-chartjs": { + "version": "3.5.1", + "resolved": "https://registry.npmjs.org/vue-chartjs/-/vue-chartjs-3.5.1.tgz", + "integrity": "sha512-foocQbJ7FtveICxb4EV5QuVpo6d8CmZFmAopBppDIGKY+esJV8IJgwmEW0RexQhxqXaL/E1xNURsgFFYyKzS/g==", + "dependencies": { + "@types/chart.js": "^2.7.55" + }, + "engines": { + "node": ">=6.9.0", + "npm": ">= 3.0.0" + }, + "peerDependencies": { + "chart.js": ">= 2.5" + } + }, "node_modules/vue-eslint-parser": { "version": "9.3.1", "resolved": "https://registry.npmjs.org/vue-eslint-parser/-/vue-eslint-parser-9.3.1.tgz", @@ -5888,6 +5978,14 @@ "integrity": "sha512-lB9lMjuqjtuJrx7/kOkqQBtllspPIN+96OvTCeJ2j5FEzinoAXTdAMFnDAQT1KVPRlnYfBrqxtqP66vDM40xxQ==", "dev": true }, + "@types/chart.js": { + "version": "2.9.41", + "resolved": "https://registry.npmjs.org/@types/chart.js/-/chart.js-2.9.41.tgz", + "integrity": "sha512-3dvkDvueckY83UyUXtJMalYoH6faOLkWQoaTlJgB4Djde3oORmNP0Jw85HtzTuXyliUHcdp704s0mZFQKio/KQ==", + "requires": { + "moment": "^2.10.2" + } + }, "@types/eslint": { "version": "8.40.2", "resolved": "https://registry.npmjs.org/@types/eslint/-/eslint-8.40.2.tgz", @@ -6552,6 +6650,55 @@ "supports-color": "^7.1.0" } }, + "chart.js": { + "version": "2.9.3", + "resolved": "https://registry.npmjs.org/chart.js/-/chart.js-2.9.3.tgz", + "integrity": "sha512-+2jlOobSk52c1VU6fzkh3UwqHMdSlgH1xFv9FKMqHiNCpXsGPQa/+81AFa+i3jZ253Mq9aAycPwDjnn1XbRNNw==", + "requires": { + "chartjs-color": "^2.1.0", + "moment": "^2.10.2" + } + }, + "chartjs-color": { + "version": "2.4.1", + "resolved": "https://registry.npmjs.org/chartjs-color/-/chartjs-color-2.4.1.tgz", + "integrity": "sha512-haqOg1+Yebys/Ts/9bLo/BqUcONQOdr/hoEr2LLTRl6C5LXctUdHxsCYfvQVg5JIxITrfCNUDr4ntqmQk9+/0w==", + "requires": { + "chartjs-color-string": "^0.6.0", + "color-convert": "^1.9.3" + }, + "dependencies": { + "color-convert": { + "version": "1.9.3", + "resolved": "https://registry.npmjs.org/color-convert/-/color-convert-1.9.3.tgz", + "integrity": "sha512-QfAUtd+vFdAtFQcC8CCyYt1fYWxSqAiK2cSD6zDB8N3cpsEBAvRxp9zOGg6G/SHHJYAT88/az/IuDGALsNVbGg==", + "requires": { + "color-name": "1.1.3" + } + }, + "color-name": { + "version": "1.1.3", + "resolved": "https://registry.npmjs.org/color-name/-/color-name-1.1.3.tgz", + "integrity": "sha512-72fSenhMw2HZMTVHeCA9KCmpEIbzWiQsjN+BHcBbS9vr1mtt+vJjPdksIBNUmKAW8TFUDPJK5SUU3QhE9NEXDw==" + } + } + }, + "chartjs-color-string": { + "version": "0.6.0", + "resolved": "https://registry.npmjs.org/chartjs-color-string/-/chartjs-color-string-0.6.0.tgz", + "integrity": "sha512-TIB5OKn1hPJvO7JcteW4WY/63v6KwEdt6udfnDE9iCAZgy+V4SrbSxoIbTw/xkUIapjEI4ExGtD0+6D3KyFd7A==", + "requires": { + "color-name": "^1.0.0" + } + }, + "chartjs-plugin-annotation": { + "version": "0.5.7", + "resolved": "https://registry.npmjs.org/chartjs-plugin-annotation/-/chartjs-plugin-annotation-0.5.7.tgz", + "integrity": "sha512-tKN5KLc69unyZGTvSdhVQEyAOhVNnSkFCCgefZhO4UaqFfABZGFU/d9p6WM2KB0eXFs/rR3Jayh7dvyASC7K0A==", + "requires": { + "chart.js": "^2.4.0" + } + }, "chokidar": { "version": "3.5.3", "resolved": "https://registry.npmjs.org/chokidar/-/chokidar-3.5.3.tgz", @@ -6598,8 +6745,7 @@ "color-name": { "version": "1.1.4", "resolved": "https://registry.npmjs.org/color-name/-/color-name-1.1.4.tgz", - "integrity": "sha512-dOy+3AuW3a2wNbZHIuMZpTcgjGuLU/uBL/ubcZF9OXbDo8ff4O8yVp5Bf0efS8uEoYo5q4Fx7dY9OgQGXgAsQA==", - "dev": true + "integrity": "sha512-dOy+3AuW3a2wNbZHIuMZpTcgjGuLU/uBL/ubcZF9OXbDo8ff4O8yVp5Bf0efS8uEoYo5q4Fx7dY9OgQGXgAsQA==" }, "combined-stream": { "version": "1.0.8", @@ -6624,8 +6770,7 @@ "core-js": { "version": "3.31.1", "resolved": "https://registry.npmjs.org/core-js/-/core-js-3.31.1.tgz", - "integrity": "sha512-2sKLtfq1eFST7l7v62zaqXacPc7uG8ZAya8ogijLhTtaKNcpzpB4TMoTw2Si+8GYKRwFPMMtUT0263QFWFfqyQ==", - "optional": true + "integrity": "sha512-2sKLtfq1eFST7l7v62zaqXacPc7uG8ZAya8ogijLhTtaKNcpzpB4TMoTw2Si+8GYKRwFPMMtUT0263QFWFfqyQ==" }, "cross-spawn": { "version": "7.0.3", @@ -7911,6 +8056,11 @@ "brace-expansion": "^1.1.7" } }, + "moment": { + "version": "2.29.4", + "resolved": "https://registry.npmjs.org/moment/-/moment-2.29.4.tgz", + "integrity": "sha512-5LC9SOxjSc2HF6vO2CyuTDNivEdoz2IvyJJGj6X8DJ0eFyfszE0QiEd+iXmBvUP3WHxSjFH/vIsA0EN00cgr8w==" + }, "ms": { "version": "2.1.2", "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.2.tgz", @@ -9097,6 +9247,20 @@ "csstype": "^3.1.0" } }, + "vue-axios": { + "version": "3.5.2", + "resolved": "https://registry.npmjs.org/vue-axios/-/vue-axios-3.5.2.tgz", + "integrity": "sha512-GP+dct7UlAWkl1qoP3ppw0z6jcSua5/IrMpjB5O8bh089iIiJ+hdxPYH2NPEpajlYgkW5EVMP95ttXWdas1O0g==", + "requires": {} + }, + "vue-chartjs": { + "version": "3.5.1", + "resolved": "https://registry.npmjs.org/vue-chartjs/-/vue-chartjs-3.5.1.tgz", + "integrity": "sha512-foocQbJ7FtveICxb4EV5QuVpo6d8CmZFmAopBppDIGKY+esJV8IJgwmEW0RexQhxqXaL/E1xNURsgFFYyKzS/g==", + "requires": { + "@types/chart.js": "^2.7.55" + } + }, "vue-eslint-parser": { "version": "9.3.1", "resolved": "https://registry.npmjs.org/vue-eslint-parser/-/vue-eslint-parser-9.3.1.tgz", diff --git a/photon-client/package.json b/photon-client/package.json index 63a0e080bd..a50beace73 100644 --- a/photon-client/package.json +++ b/photon-client/package.json @@ -17,26 +17,31 @@ "@mdi/font": "^7.2.96", "@msgpack/msgpack": "^3.0.0-beta2", "axios": "^1.4.0", + "chart.js": "2.9.3", + "chartjs-plugin-annotation": "^0.5.7", + "core-js": "^3.30.2", "jspdf": "^2.5.1", "pinia": "^2.1.4", "three": "^0.154.0", "vue": "^2.7.14", + "vue-axios": "^3.5.2", + "vue-chartjs": "^3.5.1", "vue-router": "^3.6.5", "vuetify": "^2.6.15" }, "devDependencies": { "@rushstack/eslint-patch": "^1.3.2", - "@vue/eslint-config-prettier": "^8.0.0", - "@vue/eslint-config-typescript": "^11.0.3", - "prettier": "^3.0.0", "@types/node": "^16.11.45", "@types/three": "^0.154.0", "@vitejs/plugin-vue2": "^2.2.0", + "@vue/eslint-config-prettier": "^8.0.0", + "@vue/eslint-config-typescript": "^11.0.3", "@vue/tsconfig": "^0.1.3", "deepmerge": "^4.3.1", "eslint": "^8.45.0", "eslint-plugin-vue": "^9.0.0", "npm-run-all": "^4.1.5", + "prettier": "^3.0.0", "sass": "~1.32", "sass-loader": "^13.3.2", "terser": "^5.14.2", diff --git a/photon-client/src/components/cameras/CameraCalibrationCard.vue b/photon-client/src/components/cameras/CameraCalibrationCard.vue index 0bbfbe3df9..ca71208133 100644 --- a/photon-client/src/components/cameras/CameraCalibrationCard.vue +++ b/photon-client/src/components/cameras/CameraCalibrationCard.vue @@ -1,7 +1,12 @@ diff --git a/photon-client/src/components/common/LineChart.vue b/photon-client/src/components/common/LineChart.vue new file mode 100644 index 0000000000..8a9ae06413 --- /dev/null +++ b/photon-client/src/components/common/LineChart.vue @@ -0,0 +1,102 @@ + diff --git a/photon-client/src/stores/settings/CameraSettingsStore.ts b/photon-client/src/stores/settings/CameraSettingsStore.ts index 861b192ec3..2e263bbc07 100644 --- a/photon-client/src/stores/settings/CameraSettingsStore.ts +++ b/photon-client/src/stores/settings/CameraSettingsStore.ts @@ -1,7 +1,6 @@ import { defineStore } from "pinia"; import type { CalibrationBoardTypes, - CameraCalibrationResult, CameraSettings, ConfigurableCameraSettings, RobotOffsetType, @@ -89,16 +88,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", { standardDeviation: v.standardDeviation, mean: v.mean })), - completeCalibrations: d.calibrations.map((calib) => ({ - resolution: { - height: calib.height, - width: calib.width - }, - distCoeffs: calib.distCoeffs, - standardDeviation: calib.standardDeviation, - perViewErrors: calib.perViewErrors, - intrinsics: calib.intrinsics - })), + completeCalibrations: d.calibrations, pipelineNicknames: d.pipelineNicknames, currentPipelineIndex: d.currentPipelineIndex, pipelineSettings: d.currentPipelineSettings diff --git a/photon-client/src/types/SettingTypes.ts b/photon-client/src/types/SettingTypes.ts index 7119b8cd90..5cdb8f51b0 100644 --- a/photon-client/src/types/SettingTypes.ts +++ b/photon-client/src/types/SettingTypes.ts @@ -76,16 +76,55 @@ export interface VideoFormat { diagonalFOV?: number; horizontalFOV?: number; verticalFOV?: number; - standardDeviation?: number; mean?: number; } +export enum CvType { + CV_8U = 0, + CV_8S = 1, + CV_16U = 2, + CV_16S = 3, + CV_32S = 4, + CV_32F = 5, + CV_64F = 6, + CV_16F = 7 +} + +export interface JsonMat { + rows: number; + cols: number; + type: CvType; + data: number[]; +} + +export interface CvPoint3 { + x: number; + y: number; + z: number; +} +export interface CvPoint { + x: number; + y: number; +} + +export interface Pose3d { + translation: { x: number; y: number; z: number }; + rotation: { quaternion: { W: number; X: number; Y: number; Z: number } }; +} + +export interface BoardObservation { + locationInObjectSpace: CvPoint3[]; + locationInImageSpace: CvPoint[]; + reprojectionErrors: CvPoint[]; + optimisedCameraToObject: Pose3d; +} + export interface CameraCalibrationResult { resolution: Resolution; - distCoeffs: number[]; - standardDeviation: number; - perViewErrors: number[]; - intrinsics: number[]; + cameraIntrinsics: JsonMat; + // TODO rename to be Right + cameraExtrinsics: JsonMat; + observations: BoardObservation[]; } export interface ConfigurableCameraSettings { @@ -128,19 +167,54 @@ export const PlaceholderCameraSettings: CameraSettings = { resolution: { width: 1920, height: 1080 }, fps: 60, pixelFormat: "RGB" - }, - { - resolution: { width: 1280, height: 720 }, - fps: 60, - pixelFormat: "RGB" - }, + } + // { + // resolution: { width: 1280, height: 720 }, + // fps: 60, + // pixelFormat: "RGB" + // }, + // { + // resolution: { width: 640, height: 480 }, + // fps: 30, + // pixelFormat: "RGB" + // } + ], + completeCalibrations: [ { - resolution: { width: 640, height: 480 }, - fps: 30, - pixelFormat: "RGB" + resolution: { width: 1920, height: 1080 }, + cameraIntrinsics: { + rows: 1, + cols: 1, + type: 1, + data: [1, 2, 3, 4, 5, 6, 7, 8, 9] + }, + cameraExtrinsics: { + rows: 1, + cols: 1, + type: 1, + data: [10, 11, 12, 13] + }, + observations: [ + { + locationInImageSpace: [ + { x: 100, y: 100 }, + { x: 210, y: 100 }, + { x: 320, y: 101 }, + ], + locationInObjectSpace: [{ x: 0, y: 0, z: 0 }], + optimisedCameraToObject: { + translation: { x: 1, y: 2, z: 3 }, + rotation: { quaternion: { W: 1, X: 0, Y: 0, Z: 0 } } + }, + reprojectionErrors: [ + { x: 1, y: 1 }, + { x: 2, y: 1 }, + { x: 3, y: 1 }, + ] + } + ] } ], - completeCalibrations: [], pipelineNicknames: ["Placeholder Pipeline"], lastPipelineIndex: 0, currentPipelineIndex: 0, diff --git a/photon-client/src/types/WebsocketDataTypes.ts b/photon-client/src/types/WebsocketDataTypes.ts index 0114e19e79..b62d2e788e 100644 --- a/photon-client/src/types/WebsocketDataTypes.ts +++ b/photon-client/src/types/WebsocketDataTypes.ts @@ -1,4 +1,11 @@ -import type { GeneralSettings, LightingSettings, LogLevel, MetricData, NetworkSettings } from "@/types/SettingTypes"; +import type { + CameraCalibrationResult, + GeneralSettings, + LightingSettings, + LogLevel, + MetricData, + NetworkSettings +} from "@/types/SettingTypes"; import type { ActivePipelineSettings } from "@/types/PipelineTypes"; import type { AprilTagFieldLayout, PipelineResult } from "@/types/PhotonTrackingTypes"; @@ -46,7 +53,7 @@ export type WebsocketVideoFormat = Record< >; export interface WebsocketCameraSettingsUpdate { - calibrations: WebsocketCompleteCalib[]; + calibrations: CameraCalibrationResult[]; currentPipelineIndex: number; currentPipelineSettings: ActivePipelineSettings; fov: number; diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java index fac672636c..60032451ea 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/PhotonConfiguration.java @@ -172,7 +172,7 @@ public static class UICameraConfiguration { public HashMap> videoFormatList; public int outputStreamPort; public int inputStreamPort; - public List> calibrations; + public List calibrations; public boolean isFovConfigurable = true; } } diff --git a/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java b/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java index 1ce4c55f18..1d6fbaa3b6 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java +++ b/photon-core/src/main/java/org/photonvision/common/util/ColorHelper.java @@ -24,4 +24,8 @@ public class ColorHelper { public static Scalar colorToScalar(Color color) { return new Scalar(color.getBlue(), color.getGreen(), color.getRed()); } + + public static Scalar colorToScalar(Color color, double alpha) { + return new Scalar(color.getBlue(), color.getGreen(), color.getRed(), alpha); + } } diff --git a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java index 2b6bb31ae3..85e5121957 100644 --- a/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java +++ b/photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java @@ -23,10 +23,12 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.WPIUtilJNI; import java.util.Arrays; import java.util.List; +import org.opencv.core.Core; import org.opencv.core.Mat; public class MathUtils { @@ -198,4 +200,15 @@ public static void rotationToOpencvRvec(Rotation3d rotation, Mat rvecOutput) { var axis = rotation.getAxis().times(angle); rvecOutput.put(0, 0, axis.getData()); } + + public static Pose3d opencvRTtoPose3d(Mat rVec, Mat tVec) { + Translation3d translation = + new Translation3d(tVec.get(0, 0)[0], tVec.get(1, 0)[0], tVec.get(2, 0)[0]); + Rotation3d rotation = + new Rotation3d( + VecBuilder.fill(rVec.get(0, 0)[0], rVec.get(1, 0)[0], rVec.get(2, 0)[0]), + Core.norm(rVec)); + + return new Pose3d(translation, rotation); + } } diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java index 4b4ae5a8cc..41d77a6eaa 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/CameraCalibrationCoefficients.java @@ -20,14 +20,45 @@ import com.fasterxml.jackson.annotation.JsonAlias; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnore; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.JsonNode; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.List; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; +import org.opencv.core.Point; +import org.opencv.core.Point3; import org.opencv.core.Size; import org.photonvision.vision.opencv.Releasable; +@JsonIgnoreProperties(ignoreUnknown = true) public class CameraCalibrationCoefficients implements Releasable { + public static final class BoardObservation { + @JsonProperty("locationInObjectSpace") + public List locationInObjectSpace; // Expected feature 3d location in the camera frame + + @JsonProperty("locationInImageSpace") + public List locationInImageSpace; // Observed location in pixel space + + @JsonProperty("reprojectionErrors") + public List reprojectionErrors; // (measured location in pixels) - (expected from FK) + + public Pose3d optimisedCameraToObject; // Solver optimized board poses + + @JsonCreator + public BoardObservation( + @JsonProperty("locationInObjectSpace") List locationInObjectSpace, + @JsonProperty("locationInImageSpace") List locationInImageSpace, + @JsonProperty("reprojectionErrors") List reprojectionErrors, + @JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject) { + this.locationInObjectSpace = locationInObjectSpace; + this.locationInImageSpace = locationInImageSpace; + this.reprojectionErrors = reprojectionErrors; + this.optimisedCameraToObject = optimisedCameraToObject; + } + } + @JsonProperty("resolution") public final Size resolution; @@ -38,14 +69,10 @@ public class CameraCalibrationCoefficients implements Releasable { @JsonAlias({"cameraExtrinsics", "distCoeffs"}) public final JsonMat distCoeffs; - @JsonProperty("perViewErrors") - public final double[] perViewErrors; - - @JsonProperty("standardDeviation") - public final double standardDeviation; + @JsonProperty("observations") + public final List observations; @JsonIgnore private final double[] intrinsicsArr = new double[9]; - @JsonIgnore private final double[] extrinsicsArr = new double[5]; @JsonCreator @@ -53,13 +80,16 @@ public CameraCalibrationCoefficients( @JsonProperty("resolution") Size resolution, @JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics, @JsonProperty("cameraExtrinsics") JsonMat distCoeffs, - @JsonProperty("perViewErrors") double[] perViewErrors, - @JsonProperty("standardDeviation") double standardDeviation) { + @JsonProperty("observations") List observations) { this.resolution = resolution; this.cameraIntrinsics = cameraIntrinsics; this.distCoeffs = distCoeffs; - this.perViewErrors = perViewErrors; - this.standardDeviation = standardDeviation; + + // Legacy migration just to make sure that observations is at worst empty and never null + if (observations == null) { + observations = List.of(); + } + this.observations = observations; // do this once so gets are quick getCameraIntrinsicsMat().get(0, 0, intrinsicsArr); @@ -87,13 +117,8 @@ public double[] getExtrinsicsArr() { } @JsonIgnore - public double[] getPerViewErrors() { - return perViewErrors; - } - - @JsonIgnore - public double getStandardDeviation() { - return standardDeviation; + public List getPerViewErrors() { + return observations; } @Override @@ -138,6 +163,6 @@ public static CameraCalibrationCoefficients parseFromCalibdbJson(JsonNode json) var height = json.get("img_size").get(1).doubleValue(); return new CameraCalibrationCoefficients( - new Size(width, height), cam_jsonmat, distortion_jsonmat, new double[] {error}, 0); + new Size(width, height), cam_jsonmat, distortion_jsonmat, List.of()); } } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index a2f2911431..d0406e09f4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -22,6 +22,7 @@ import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoException; import edu.wpi.first.cscore.VideoMode; +import edu.wpi.first.cscore.VideoProperty.Kind; import java.util.*; import java.util.stream.Collectors; import org.photonvision.common.configuration.CameraConfiguration; @@ -29,6 +30,7 @@ import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.util.TestUtils; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.frame.FrameProvider; import org.photonvision.vision.frame.provider.FileFrameProvider; import org.photonvision.vision.frame.provider.USBFrameProvider; @@ -170,6 +172,7 @@ public void setAutoExposure(boolean cameraAutoExposure) { if (canSetWhiteBalance) { camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset } + camera.setExposureAuto(); // auto exposure enabled } else { // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise // nice-for-humans @@ -210,6 +213,12 @@ public void setExposure(double exposure) { camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); + } else if (camera.getProperty("exposure_time_absolute").getKind() != Kind.kNone) { + // Seems like the name changed at some point in v4l? set it instead + var prop = camera.getProperty("exposure_time_absolute"); + var exposure_manual_val = + MathUtils.map(Math.round(exposure), 0, 100, prop.getMin(), prop.getMax()); + prop.set((int) exposure_manual_val); } else { scaledExposure = (int) Math.round(exposure); logger.debug("Setting camera exposure to " + scaledExposure); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java index bfca09d6fd..dcbda9b4ff 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/Calibrate3dPipe.java @@ -24,12 +24,15 @@ import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Triple; import org.opencv.calib3d.Calib3d; +import org.opencv.core.*; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; +import org.photonvision.common.util.math.MathUtils; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients.BoardObservation; import org.photonvision.vision.calibration.JsonMat; import org.photonvision.vision.pipe.CVPipe; @@ -79,16 +82,18 @@ protected CameraCalibrationCoefficients process(List> in) && it.getMiddle() != null && it.getRight() != null) .collect(Collectors.toList()); + + List objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList()); + List imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList()); + if (objPoints.size() != imgPts.size()) { + logger.error("objpts.size != imgpts.size"); + return null; + } + try { // FindBoardCorners pipe outputs all the image points, object points, and frames to calculate // imageSize from, other parameters are output Mats - var objPoints = in.stream().map(Triple::getMiddle).collect(Collectors.toList()); - var imgPts = in.stream().map(Triple::getRight).collect(Collectors.toList()); - if (objPoints.size() != imgPts.size()) { - logger.error("objpts.size != imgpts.size"); - return null; - } calibrationAccuracy = Calib3d.calibrateCameraExtended( objPoints, @@ -106,15 +111,52 @@ protected CameraCalibrationCoefficients process(List> in) e.printStackTrace(); return null; } + JsonMat cameraMatrixMat = JsonMat.fromMat(cameraMatrix); JsonMat distortionCoefficientsMat = JsonMat.fromMat(distortionCoefficients); - // Create a new CameraCalibrationCoefficients object to pass onto SolvePnP - double[] perViewErrorsArray = - new double[(int) perViewErrors.total() * perViewErrors.channels()]; - perViewErrors.get(0, 0, perViewErrorsArray); + + // For each observation, calc reprojection error + Mat jac_temp = new Mat(); + List observations = new ArrayList<>(); + for (int i = 0; i < objPoints.size(); i++) { + MatOfPoint3f i_objPtsNative = new MatOfPoint3f(); + objPoints.get(i).copyTo(i_objPtsNative); + var i_objPts = i_objPtsNative.toList(); + var i_imgPts = ((MatOfPoint2f) imgPts.get(i)).toList(); + + var img_pts_reprojected = new MatOfPoint2f(); + try { + Calib3d.projectPoints( + i_objPtsNative, + rvecs.get(i), + tvecs.get(i), + cameraMatrix, + distortionCoefficients, + img_pts_reprojected, + jac_temp, + 0.0); + } catch (Exception e) { + e.printStackTrace(); + continue; + } + var img_pts_reprojected_list = img_pts_reprojected.toList(); + + var reprojectionError = new ArrayList(); + for (int j = 0; j < img_pts_reprojected_list.size(); j++) { + // error = (measured - expected) + var measured = img_pts_reprojected_list.get(j); + var expected = i_imgPts.get(j); + var error = new Point(measured.x - expected.x, measured.y - expected.y); + reprojectionError.add(error); + } + + var camToBoard = MathUtils.opencvRTtoPose3d(rvecs.get(i), tvecs.get(i)); + + observations.add(new BoardObservation(i_objPts, i_imgPts, reprojectionError, camToBoard)); + } + jac_temp.release(); // Standard deviation of results - double stdDev = calculateSD(perViewErrorsArray); try { // Print calibration successful logger.info( @@ -126,32 +168,12 @@ protected CameraCalibrationCoefficients process(List> in) + new ObjectMapper().writeValueAsString(cameraMatrixMat) + "\ndistortionCoeffs:\n" + new ObjectMapper().writeValueAsString(distortionCoefficientsMat) - + "\nWith Standard Deviation Of\n" - + stdDev + "\n"); } catch (JsonProcessingException e) { logger.error("Failed to parse calibration data to json!", e); } return new CameraCalibrationCoefficients( - params.resolution, cameraMatrixMat, distortionCoefficientsMat, perViewErrorsArray, stdDev); - } - - // Calculate standard deviation of the RMS error of the snapshots - private static double calculateSD(double[] numArray) { - double sum = 0.0, standardDeviation = 0.0; - int length = numArray.length; - - for (double num : numArray) { - sum += num; - } - - double mean = sum / length; - - for (double num : numArray) { - standardDeviation += Math.pow(num - mean, 2); - } - - return Math.sqrt(standardDeviation / length); + params.resolution, cameraMatrixMat, distortionCoefficientsMat, observations); } public static class CalibratePipeParams { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java new file mode 100644 index 0000000000..a1207030a9 --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/DrawCalibrationPipe.java @@ -0,0 +1,62 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipe.impl; + +import java.awt.Color; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; +import org.photonvision.common.util.ColorHelper; +import org.photonvision.vision.frame.FrameDivisor; +import org.photonvision.vision.pipe.MutatingPipe; +import org.photonvision.vision.target.TrackedTarget; + +public class DrawCalibrationPipe + extends MutatingPipe< + Pair>, DrawCalibrationPipe.DrawCalibrationPipeParams> { + @Override + protected Void process(Pair> in) { + var image = in.getLeft(); + + for (var target : in.getRight()) { + for (var c : target.getTargetCorners()) { + c = + new Point( + c.x / params.divisor.value.doubleValue(), c.y / params.divisor.value.doubleValue()); + var r = 4; + var r2 = r / Math.sqrt(2); + var color = ColorHelper.colorToScalar(Color.RED, 0.4); + Imgproc.circle(image, c, r, color, 1); + Imgproc.line(image, new Point(c.x - r2, c.y - r2), new Point(c.x + r2, c.y + r2), color); + Imgproc.line(image, new Point(c.x + r2, c.y - r2), new Point(c.x - r2, c.y + r2), color); + } + } + + return null; + } + + public static class DrawCalibrationPipeParams { + private final FrameDivisor divisor; + + public DrawCalibrationPipeParams(FrameDivisor divisor) { + this.divisor = divisor; + } + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java index 678566c707..2c576f562d 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/FindBoardCornersPipe.java @@ -113,9 +113,6 @@ public void createObjectPoints() { */ @Override protected Triple process(Pair in) { - // Create the object points - createObjectPoints(); - return findBoardCorners(in); } @@ -228,10 +225,12 @@ private Triple findBoardCorners(Pair in) { boolean boardFound = false; if (params.type == UICalibrationData.BoardType.CHESSBOARD) { - // This is for chessboards - // Reduce the image size to be much more manageable - Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame)); + if (params.divisor != FrameDivisor.NONE) { + Imgproc.resize(inFrame, smallerInFrame, getFindCornersImgSize(inFrame)); + } else { + smallerInFrame = inFrame; + } // Run the chessboard corner finder on the smaller image boardFound = @@ -244,14 +243,13 @@ private Triple findBoardCorners(Pair in) { } } else if (params.type == UICalibrationData.BoardType.DOTBOARD) { - // For dot boards boardFound = Calib3d.findCirclesGrid( inFrame, patternSize, boardCorners, Calib3d.CALIB_CB_ASYMMETRIC_GRID); } if (!boardFound) { - // If we can't find a chessboard/dot board, just return + // If we can't find a chessboard/dot board, give up return null; } @@ -264,31 +262,25 @@ private Triple findBoardCorners(Pair in) { // Get the size of the inFrame this.imageSize = new Size(inFrame.width(), inFrame.height()); - // Do sub corner pix for drawing chessboard - Imgproc.cornerSubPix( - inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria); + // Do sub corner pix for drawing chessboard when using OpenCV + if (params.divisor != FrameDivisor.NONE) { + Imgproc.cornerSubPix( + inFrame, outBoardCorners, getWindowSize(outBoardCorners), zeroZone, criteria); + } - // convert back to BGR - // Imgproc.cvtColor(inFrame, inFrame, Imgproc.COLOR_GRAY2BGR); // draw the chessboard, doesn't have to be different for a dot board since it just re projects // the corners we found Calib3d.drawChessboardCorners(outFrame, patternSize, outBoardCorners, true); - // // Add the 3D points and the points of the corners found - // if (addToSnapList) { - // this.listOfObjectPoints.add(objectPoints); - // this.listOfImagePoints.add(boardCorners); - // } - return Triple.of(inFrame.size(), objPts, outBoardCorners); } public static class FindCornersPipeParams { - private final int boardHeight; - private final int boardWidth; - private final UICalibrationData.BoardType type; - private final double gridSize; - private final FrameDivisor divisor; + final int boardHeight; + final int boardWidth; + final UICalibrationData.BoardType type; + final double gridSize; + final FrameDivisor divisor; public FindCornersPipeParams( int boardHeight, diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java index e1a3bc8aae..3d546bd5bf 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java @@ -20,11 +20,13 @@ import edu.wpi.first.math.util.Units; import java.nio.file.Path; import java.util.ArrayList; -import java.util.Collections; import java.util.List; +import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.lang3.tuple.Triple; import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint2f; +import org.opencv.core.Point; import org.opencv.core.Size; import org.opencv.imgcodecs.Imgcodecs; import org.photonvision.common.configuration.ConfigManager; @@ -34,8 +36,8 @@ import org.photonvision.common.logging.Logger; import org.photonvision.common.util.SerializationUtils; import org.photonvision.common.util.file.FileUtils; -import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; +import org.photonvision.vision.calibration.CameraCalibrationCoefficients.BoardObservation; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; import org.photonvision.vision.opencv.CVMat; @@ -44,13 +46,14 @@ import org.photonvision.vision.pipe.impl.Calibrate3dPipe; import org.photonvision.vision.pipe.impl.FindBoardCornersPipe; import org.photonvision.vision.pipeline.result.CVPipelineResult; +import org.photonvision.vision.pipeline.result.CalibrationPipelineResult; public class Calibrate3dPipeline extends CVPipeline { // For logging private static final Logger logger = new Logger(Calibrate3dPipeline.class, LogGroup.General); - // Only 2 pipes needed, one for finding the board corners and one for actually calibrating + // Find board corners decides internally between opencv and mrgingham private final FindBoardCornersPipe findBoardCornersPipe = new FindBoardCornersPipe(); private final Calibrate3dPipe calibrate3dPipe = new Calibrate3dPipe(); private final CalculateFPSPipe calculateFPSPipe = new CalculateFPSPipe(); @@ -69,19 +72,20 @@ public class Calibrate3dPipeline private boolean calibrating = false; // Path to save images - private final Path imageDir = ConfigManager.getInstance().getCalibDir(); + private final Path imageDir; private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE; - public Calibrate3dPipeline() { - this(12); + public Calibrate3dPipeline(Path imgDir) { + this(imgDir, 12); } - public Calibrate3dPipeline(int minSnapshots) { + public Calibrate3dPipeline(Path imgDir, int minSnapshots) { super(PROCESSING_TYPE); this.settings = new Calibration3dPipelineSettings(); this.foundCornersList = new ArrayList<>(); this.minSnapshots = minSnapshots; + this.imageDir = imgDir; } @Override @@ -119,7 +123,8 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se // Check if the frame has chessboard corners var outputColorCVMat = new CVMat(); inputColorMat.copyTo(outputColorCVMat.getMat()); - var findBoardResult = + + Triple findBoardResult = findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output; var fpsResult = calculateFPSPipe.run(null); @@ -131,8 +136,9 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se if (findBoardResult != null) { foundCornersList.add(findBoardResult); + imageDir.toFile().mkdirs(); Imgcodecs.imwrite( - Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(), + Path.of(imageDir.toString(), Integer.toString(settings.cameraVideoModeIndex), "img" + foundCornersList.size() + ".jpg").toString(), inputColorMat); // update the UI @@ -143,13 +149,20 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se frame.release(); // Return the drawn chessboard if corners are found, if not, then return the input image. - return new CVPipelineResult( + return new CalibrationPipelineResult( sumPipeNanosElapsed, fps, // Unused but here in case - Collections.emptyList(), - new MultiTargetPNPResult(), + // Collections.emptyList(), + // new MultiTargetPNPResult(), new Frame( - new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties)); + new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties), + getCornersList()); + } + + List> getCornersList() { + return foundCornersList.stream() + .map(it -> ((MatOfPoint2f) it.getRight()).toList()) + .collect(Collectors.toList()); } public void deleteSavedImages() { @@ -188,8 +201,8 @@ public void takeSnapshot() { takeSnapshot = true; } - public double[] perViewErrors() { - return calibrationOutput.output.perViewErrors; + public List perViewErrors() { + return calibrationOutput.output.observations; } public void finishCalibration() { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java index 238333ee6a..bf5b73a4ea 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java @@ -38,6 +38,7 @@ public class OutputStreamPipeline { private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe(); private final Draw2dAprilTagsPipe draw2dAprilTagsPipe = new Draw2dAprilTagsPipe(); private final Draw3dAprilTagsPipe draw3dAprilTagsPipe = new Draw3dAprilTagsPipe(); + private final DrawCalibrationPipe drawCalibrationPipe = new DrawCalibrationPipe(); private final Draw2dArucoPipe draw2dArucoPipe = new Draw2dArucoPipe(); private final Draw3dArucoPipe draw3dArucoPipe = new Draw3dArucoPipe(); @@ -113,6 +114,9 @@ protected void setPipeParams( resizeImagePipe.setParams( new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor)); + + drawCalibrationPipe.setParams( + new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor)); } public CVPipelineResult process( @@ -149,8 +153,9 @@ public CVPipelineResult process( sumPipeNanosElapsed += pipeProfileNanos[3] = draw2dCrosshairResultOnInput.nanosElapsed; if (!(settings instanceof AprilTagPipelineSettings) - && !(settings instanceof ArucoPipelineSettings)) { - // If we're processing anything other than Apriltags... + && !(settings instanceof ArucoPipelineSettings) + && !(settings instanceof Calibration3dPipelineSettings)) { + // If we're processing anything other than Apriltags.. var draw2dCrosshairResultOnOutput = draw2dCrosshairPipe.run(Pair.of(outMat, targetsToDraw)); sumPipeNanosElapsed += pipeProfileNanos[4] = draw2dCrosshairResultOnOutput.nanosElapsed; @@ -172,7 +177,14 @@ public CVPipelineResult process( pipeProfileNanos[7] = 0; pipeProfileNanos[8] = 0; } + } else if (settings instanceof Calibration3dPipelineSettings) { + pipeProfileNanos[5] = 0; + pipeProfileNanos[6] = 0; + + var drawOnInputResult = drawCalibrationPipe.run(Pair.of(outMat, targetsToDraw)); + sumPipeNanosElapsed += pipeProfileNanos[7] = drawOnInputResult.nanosElapsed; + pipeProfileNanos[8] = 0; } else if (settings instanceof AprilTagPipelineSettings) { // If we are doing apriltags... if (settings.solvePNPEnabled) { diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java new file mode 100644 index 0000000000..7ad8ff83cf --- /dev/null +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/result/CalibrationPipelineResult.java @@ -0,0 +1,35 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +package org.photonvision.vision.pipeline.result; + +import java.util.List; +import java.util.stream.Collectors; +import org.opencv.core.Point; +import org.photonvision.vision.frame.Frame; +import org.photonvision.vision.target.TrackedTarget; + +public class CalibrationPipelineResult extends CVPipelineResult { + private static List cornersToTarget(List> corners) { + return corners.stream().map(TrackedTarget::new).collect(Collectors.toList()); + } + + public CalibrationPipelineResult( + double latencyNanos, double fps, Frame outputFrame, List> corners) { + super(latencyNanos, fps, cornersToTarget(corners), outputFrame); + } +} diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java index e4f6df3a97..79058f7f0f 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/PipelineManager.java @@ -17,6 +17,7 @@ package org.photonvision.vision.processes; +import java.nio.file.Path; import java.util.ArrayList; import java.util.Arrays; import java.util.Comparator; @@ -36,8 +37,10 @@ public class PipelineManager { public static final int DRIVERMODE_INDEX = -1; public static final int CAL_3D_INDEX = -2; + private final int streamIndex; + protected final List userPipelineSettings; - protected final Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(); + protected final Calibrate3dPipeline calibration3dPipeline; protected final DriverModePipeline driverModePipeline = new DriverModePipeline(); /** Index of the currently active pipeline. Defaults to 0. */ @@ -60,17 +63,22 @@ public class PipelineManager { * @param userPipelines Pipelines to add to the manager. */ public PipelineManager( - DriverModePipelineSettings driverSettings, List userPipelines) { + DriverModePipelineSettings driverSettings, List userPipelines, int streamIndex) { this.userPipelineSettings = new ArrayList<>(userPipelines); // This is to respect the default res idx for vendor cameras + this.streamIndex = streamIndex; + this.driverModePipeline.setSettings(driverSettings); + var calibSavePath = Path.of(ConfigManager.getInstance().getCalibDir().toString(), Integer.toString(streamIndex)); + this.calibration3dPipeline = new Calibrate3dPipeline(calibSavePath); + if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective); } public PipelineManager(CameraConfiguration config) { - this(config.driveModeSettings, config.pipelineSettings); + this(config.driveModeSettings, config.pipelineSettings, config.streamIndex); } /** diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java index 483f6a7c0c..982cd64864 100644 --- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java +++ b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java @@ -522,18 +522,17 @@ public PhotonConfiguration.UICameraConfiguration toUICameraConfig() { ret.outputStreamPort = this.outputStreamPort; ret.inputStreamPort = this.inputStreamPort; - var calList = new ArrayList>(); + var calList = new ArrayList(); for (var c : visionSource.getSettables().getConfiguration().calibrations) { - var internalMap = new HashMap(); + // var internalMap = new HashMap(); - internalMap.put("perViewErrors", c.perViewErrors); - internalMap.put("standardDeviation", c.standardDeviation); - internalMap.put("width", c.resolution.width); - internalMap.put("height", c.resolution.height); - internalMap.put("intrinsics", c.cameraIntrinsics.data); - internalMap.put("distCoeffs", c.distCoeffs.data); + // internalMap.put("observations", c.observations); + // internalMap.put("width", c.resolution.width); + // internalMap.put("height", c.resolution.height); + // internalMap.put("intrinsics", c.cameraIntrinsics.data); + // internalMap.put("distCoeffs", c.distCoeffs.data); - calList.add(internalMap); + calList.add(c); } ret.calibrations = calList; diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java index 2a28d05e56..c10927f0da 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TrackedTarget.java @@ -146,6 +146,15 @@ public TrackedTarget( m_skew = 0; } + public TrackedTarget(List corners) { + m_mainContour = new Contour(new MatOfPoint()); + m_mainContour.mat.fromList(List.of(new Point(0, 0), new Point(0, 1), new Point(1, 0))); + ; + this.setTargetCorners(corners); + m_targetOffsetPoint = new Point(); + m_robotOffsetPoint = new Point(); + } + public TrackedTarget( ArucoDetectionResult result, AprilTagPoseEstimate tagPose, diff --git a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java index 72547829f1..027b7cb9f5 100644 --- a/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/pipeline/Calibrate3dPipeTest.java @@ -78,9 +78,8 @@ public void perViewErrorsTest() { calibrate3dPipe.setParams(new Calibrate3dPipe.CalibratePipeParams(new Size(640, 480))); var calibrate3dPipeOutput = calibrate3dPipe.run(foundCornersList); - assertTrue(calibrate3dPipeOutput.output.perViewErrors.length > 0); - System.out.println( - "Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors)); + assertTrue(calibrate3dPipeOutput.output.observations.size() > 0); + // System.out.println("Per View Errors: " + Arrays.toString(calibrate3dPipeOutput.output.perViewErrors)); for (var f : frames) { f.release(); @@ -139,13 +138,12 @@ public void calibrationPipelineTest() { calibration3dPipeline.finishCalibration(); assertNotNull(cal); - assertNotNull(cal.perViewErrors); - System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); + assertNotNull(cal.observations); + // System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString()); System.out.println("Dist Coeffs: " + cal.distCoeffs.toString()); - System.out.println("Standard Deviation: " + cal.standardDeviation); - System.out.println( - "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); + // System.out.println("Standard Deviation: " + cal.standardDeviation); + // System.out.println("Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); // Confirm we didn't get leaky on our mat usage // assertTrue(CVMat.getMatCount() == startMatCount); // TODO Figure out why this doesn't work in @@ -298,7 +296,7 @@ public void calibrateSquaresCommon( // Confirm we have indeed gotten valid calibration objects assertNotNull(cal); - assertNotNull(cal.perViewErrors); + assertNotNull(cal.observations); // Confirm the calibrated center pixel is fairly close to of the "expected" location at the // center of the sensor. @@ -310,12 +308,11 @@ public void calibrateSquaresCommon( assertTrue(centerXErrPct < 10.0); assertTrue(centerYErrPct < 10.0); - System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); - System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics); + // System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors)); + System.out.println("Camera Intrinsics: " + cal.cameraIntrinsics.toString()); System.out.println("Dist Coeffs: " + cal.distCoeffs.toString()); - System.out.println("Standard Deviation: " + cal.standardDeviation); - System.out.println( - "Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); + // System.out.println("Standard Deviation: " + cal.standardDeviation); + // System.out.println("Mean: " + Arrays.stream(calibration3dPipeline.perViewErrors()).average().toString()); // Confirm we didn't get leaky on our mat usage // assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this doesn't diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java index 7b90ce027d..fce4f3e740 100644 --- a/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/processes/PipelineManagerTest.java @@ -29,7 +29,7 @@ public class PipelineManagerTest { @Test public void testUniqueName() { TestUtils.loadLibraries(); - PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of()); + PipelineManager manager = new PipelineManager(new DriverModePipelineSettings(), List.of(), 0); manager.addPipeline(PipelineType.Reflective, "Another"); // We now have ["New Pipeline", "Another"] diff --git a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java index d277f2904f..91b84c0351 100644 --- a/photon-server/src/main/java/org/photonvision/server/RequestHandler.java +++ b/photon-server/src/main/java/org/photonvision/server/RequestHandler.java @@ -563,6 +563,46 @@ public static void onImageSnapshotsRequest(Context ctx) { ctx.json(snapshots); } + public static void onCameraCalibImagesRequest(Context ctx) { + try { + var data = kObjectMapper.readTree(ctx.body()); + + int cameraIndex = data.get("cameraIndex").asInt(); + int videoModeIndex = data.get("videoModeIndex").asInt(); + + var calibImages = Path.of(ConfigManager.getInstance().getCalibDir().toString(), Integer.toString(cameraIndex), Integer.toString(videoModeIndex)).toFile().listFiles(); + var images = new ArrayList<>(); + + try { + if (calibImages != null) { + for (File calibImg : calibImages) { + var bufferedImage = ImageIO.read(calibImg); + var buffer = new ByteArrayOutputStream(); + ImageIO.write(bufferedImage, "jpg", buffer); + byte[] imgData = buffer.toByteArray(); + + images.add(imgData); + } + } + + } catch (IOException e) { + ctx.status(500); + ctx.result("Unable to read saved images"); + } + + ctx.json(images); + + } catch (JsonProcessingException e) { + ctx.status(400); + ctx.result("The provided camera data was malformed"); + logger.error("The provided camera data was malformed", e); + } catch (Exception e) { + ctx.status(500); + ctx.result("An error occurred while getting calib data"); + logger.error("An error occurred while getting calib data", e); + } + } + /** * Create a temporary file using the UploadedFile from Javalin. * diff --git a/photon-server/src/main/java/org/photonvision/server/Server.java b/photon-server/src/main/java/org/photonvision/server/Server.java index c77d03fe7e..365c8d0e85 100644 --- a/photon-server/src/main/java/org/photonvision/server/Server.java +++ b/photon-server/src/main/java/org/photonvision/server/Server.java @@ -90,6 +90,7 @@ public static void start(int port) { app.post("/api/settings/general", RequestHandler::onGeneralSettingsRequest); app.post("/api/settings/camera", RequestHandler::onCameraSettingsRequest); app.post("/api/settings/camera/setNickname", RequestHandler::onCameraNicknameChangeRequest); + app.get("/api/settings/camera/getCalibImages", RequestHandler::onCameraCalibImagesRequest); // Utilities app.post("/api/utils/offlineUpdate", RequestHandler::onOfflineUpdateRequest);