Save calibration data and show preliminary GUI (#1078)

* Serialize all calibration data

* Run lint

* typing nit

* fix code

* move these tables around some

* Add cool formatting

* add request to get snapshots by resolution and camera

* re-enable all resolutions

* add wip so i can change computers (SQUASH ME AND KILL ME AHHHH)

* Get everything working but viewing snapshots

* Update RequestHandler.java

* Update CameraCalibrationInfoCard.vue

* Update CameraCalibrationInfoCard.vue

* add observation viewer

* round

* fix illiegal import

* Swap to PNG and serialize insolution

* move import/export buttons TO THE TOP

* Update WebsocketDataTypes.ts

* Add snapshotname to observation

* Refactor to serialize snapshot image itself

* Run lint

* Use new base64 image data in info card

* Update SettingTypes.ts

* Create calibration json -> mrcal converter script

* Update calibrationUtils.py

* Fix calibrate NPEs in teest

* Run lint

* Always run cornersubpix

* Update CameraCalibrationInfoCard.vue

Update CameraCalibrationInfoCard.vue

* Update OpenCVHelp.java

* Update OpenCVHelp.java

* Replace test mode camera JSONs

* Run wpiformat

* Revert intrinsics but keep other data

* Remove misc comments

* Rename JsonMat->JsonImageMat and add calobject_warp

* Update Server.java

* Rename cameraExtrinsics to distCoeffs

* fix typing issues

* use util methods

* Formatting fixes

* fix styling

* move to devTools

* remove unneeded or unused imports

* Remove fixed-right css

If its really that big of a deal, we can add it back later, kind of a drag to fix rn.

* Create util method

* Remove extra legacy calibration things

---------

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>
This commit is contained in:
Matt
2024-01-03 14:32:04 -07:00
committed by GitHub
parent e685334baa
commit 7f09f9e4f5
43 changed files with 1247 additions and 396 deletions

2
.gitignore vendored
View File

@@ -162,3 +162,5 @@ photonlib-cpp-examples/*/networktables.json.bck
photonlib-java-examples/*/networktables.json.bck
*.sqlite
photon-server/src/main/resources/web/index.html
venv

View File

@@ -0,0 +1,134 @@
import base64
from dataclasses import dataclass
import json
import os
import cv2
import numpy as np
@dataclass
class Resolution:
width: int
height: int
@dataclass
class JsonMatOfDoubles:
rows: int
cols: int
type: int
data: list[float]
@dataclass
class JsonMat:
rows: int
cols: int
type: int
data: str # Base64-encoded PNG data
@dataclass
class Point2:
x: float
y: float
@dataclass
class Translation3d:
x: float
y: float
z: float
@dataclass
class Quaternion:
X: float
Y: float
Z: float
W: float
@dataclass
class Rotation3d:
quaternion: Quaternion
@dataclass
class Pose3d:
translation: Translation3d
rotation: Rotation3d
@dataclass
class Point3:
x: float
y: float
z: float
@dataclass
class Observation:
# Expected feature 3d location in the camera frame
locationInObjectSpace: list[Point3]
# Observed location in pixel space
locationInImageSpace: list[Point2]
# (measured location in pixels) - (expected from FK)
reprojectionErrors: list[Point2]
# Solver optimized board poses
optimisedCameraToObject: Pose3d
# If we should use this observation when re-calculating camera calibration
includeObservationInCalibration: bool
snapshotName: str
# The actual image the snapshot is from
snapshotData: JsonMat
@dataclass
class CameraCalibration:
resolution: Resolution
cameraIntrinsics: JsonMatOfDoubles
distCoeffs: JsonMatOfDoubles
observations: list[Observation]
def convert_photon_to_mrcal(photon_cal_json_path: str, output_folder: str):
"""
Unpack a Photon calibration JSON (eg, photon_calibration_Microsoft_LifeCam_HD-3000_800x600.json) into
the output_folder directory with images and corners.vnl file for use with mrcal.
"""
with open(photon_cal_json_path, "r") as cal_json:
# Convert to nested objects instead of nameddicts on json-loads
class Generic:
@classmethod
def from_dict(cls, dict):
obj = cls()
obj.__dict__.update(dict)
return obj
camera_cal_data: CameraCalibration = json.loads(
cal_json.read(), object_hook=Generic.from_dict
)
# Create output_folder if not exists
if not os.path.exists(output_folder):
os.makedirs(output_folder)
# Decode each image and save it as a png
for obs in camera_cal_data.observations:
image = obs.snapshotData.data
decoded_data = base64.b64decode(image)
np_data = np.frombuffer(decoded_data, np.uint8)
img = cv2.imdecode(np_data, cv2.IMREAD_UNCHANGED)
cv2.imwrite(f"{output_folder}/{obs.snapshotName}", img)
# And create a VNL file for use with mrcal
with open(f"{output_folder}/corners.vnl", "w+") as vnl_file:
vnl_file.write("# filename x y level\n")
for obs in camera_cal_data.observations:
for corner in obs.locationInImageSpace:
# Always level zero
vnl_file.write(f"{obs.snapshotName} {corner.x} {corner.y} 0\n")
vnl_file.flush()

View File

@@ -1,7 +1,7 @@
<script setup lang="ts">
import { computed, ref } from "vue";
import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
import { CalibrationBoardTypes, type Resolution, type VideoFormat } from "@/types/SettingTypes";
import { CalibrationBoardTypes, type VideoFormat } from "@/types/SettingTypes";
import JsPDF from "jspdf";
import { font as PromptRegular } from "@/assets/fonts/PromptRegular";
import MonoLogo from "@/assets/images/logoMono.png";
@@ -12,37 +12,40 @@ import PvSelect from "@/components/common/pv-select.vue";
import PvNumberInput from "@/components/common/pv-number-input.vue";
import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
import { getResolutionString, resolutionsAreEqual } from "@/lib/PhotonUtils";
import CameraCalibrationInfoCard from "@/components/cameras/CameraCalibrationInfoCard.vue";
const settingsValid = ref(true);
const getCalibrationCoeffs = (resolution: Resolution) => {
return useCameraSettingsStore().currentCameraSettings.completeCalibrations.find((cal) =>
resolutionsAreEqual(cal.resolution, resolution)
);
};
const getUniqueVideoResolutions = (): VideoFormat[] => {
const getUniqueVideoFormatsByResolution = (): VideoFormat[] => {
const uniqueResolutions: VideoFormat[] = [];
useCameraSettingsStore().currentCameraSettings.validVideoFormats.forEach((format, index) => {
if (!uniqueResolutions.some((v) => resolutionsAreEqual(v.resolution, format.resolution))) {
format.index = index;
const calib = getCalibrationCoeffs(format.resolution);
const calib = useCameraSettingsStore().getCalibrationCoeffs(format.resolution);
if (calib !== undefined) {
format.standardDeviation = calib.standardDeviation;
format.mean =
calib.perViewErrors === null
? Number.NaN
: calib.perViewErrors.reduce((a, b) => a + b) / calib.perViewErrors.length;
format.horizontalFOV = 2 * Math.atan2(format.resolution.width / 2, calib.intrinsics[0]) * (180 / Math.PI);
format.verticalFOV = 2 * Math.atan2(format.resolution.height / 2, calib.intrinsics[4]) * (180 / Math.PI);
// Is this the right formula for RMS error? who knows! not me!
const perViewSumSquareReprojectionError = calib.observations.flatMap((it) =>
it.reprojectionErrors.flatMap((it2) => [it2.x, it2.y])
);
// For each error, square it, sum the squares, and divide by total points N
format.mean = Math.sqrt(
perViewSumSquareReprojectionError.map((it) => Math.pow(it, 2)).reduce((a, b) => a + b, 0) /
perViewSumSquareReprojectionError.length
);
format.horizontalFOV =
2 * Math.atan2(format.resolution.width / 2, calib.cameraIntrinsics.data[0]) * (180 / Math.PI);
format.verticalFOV =
2 * Math.atan2(format.resolution.height / 2, calib.cameraIntrinsics.data[4]) * (180 / Math.PI);
format.diagonalFOV =
2 *
Math.atan2(
Math.sqrt(
format.resolution.width ** 2 +
(format.resolution.height / (calib.intrinsics[4] / calib.intrinsics[0])) ** 2
(format.resolution.height / (calib.cameraIntrinsics.data[4] / calib.cameraIntrinsics.data[0])) ** 2
) / 2,
calib.intrinsics[0]
calib.cameraIntrinsics.data[0]
) *
(180 / Math.PI);
}
@@ -55,7 +58,7 @@ const getUniqueVideoResolutions = (): VideoFormat[] => {
return uniqueResolutions;
};
const getUniqueVideoResolutionStrings = (): { name: string; value: number }[] =>
getUniqueVideoResolutions().map<{ name: string; value: number }>((f) => ({
getUniqueVideoFormatsByResolution().map<{ name: string; value: number }>((f) => ({
name: `${getResolutionString(f.resolution)}`,
// Index won't ever be undefined
value: f.index || 0
@@ -150,7 +153,7 @@ const importCalibrationFromCalibDB = ref();
const openCalibUploadPrompt = () => {
importCalibrationFromCalibDB.value.click();
};
const readImportedCalibration = () => {
const readImportedCalibrationFromCalibDB = () => {
const files = importCalibrationFromCalibDB.value.files;
if (files.length === 0) return;
@@ -214,6 +217,13 @@ const endCalibration = () => {
isCalibrating.value = false;
});
};
let showCalDialog = ref(false);
let selectedVideoFormat = ref<VideoFormat | undefined>(undefined);
const setSelectedVideoFormat = (format: VideoFormat) => {
selectedVideoFormat.value = format;
showCalDialog.value = true;
};
</script>
<template>
@@ -234,7 +244,12 @@ const endCalibration = () => {
</tr>
</thead>
<tbody>
<tr v-for="(value, index) in getUniqueVideoResolutions()" :key="index">
<tr
v-for="(value, index) in getUniqueVideoFormatsByResolution()"
:key="index"
title="Click to get calibration specific information"
@click="setSelectedVideoFormat(value)"
>
<td>{{ getResolutionString(value.resolution) }}</td>
<td>
{{ value.mean !== undefined ? (isNaN(value.mean) ? "NaN" : value.mean.toFixed(2) + "px") : "-" }}
@@ -429,7 +444,7 @@ const endCalibration = () => {
type="file"
accept=".json"
style="display: none"
@change="readImportedCalibration"
@change="readImportedCalibrationFromCalibDB"
/>
</v-col>
</v-row>
@@ -478,6 +493,9 @@ const endCalibration = () => {
</v-card-actions>
</v-card>
</v-dialog>
<v-dialog v-model="showCalDialog" width="80em">
<CameraCalibrationInfoCard v-if="selectedVideoFormat" :video-format="selectedVideoFormat" />
</v-dialog>
</div>
</template>
@@ -494,6 +512,7 @@ const endCalibration = () => {
tbody :hover td {
background-color: #005281 !important;
cursor: pointer;
}
::-webkit-scrollbar {

View File

@@ -0,0 +1,251 @@
<script setup lang="ts">
import type { BoardObservation, CameraCalibrationResult, VideoFormat } from "@/types/SettingTypes";
import { useCameraSettingsStore } from "@/stores/settings/CameraSettingsStore";
import { useStateStore } from "@/stores/StateStore";
import { ref } from "vue";
import loadingImage from "@/assets/images/loading.svg";
import { getResolutionString, parseJsonFile } from "@/lib/PhotonUtils";
const props = defineProps<{
videoFormat: VideoFormat;
}>();
const getMeanFromView = (o: BoardObservation) => {
// Is this the right formula for RMS error? who knows! not me!
const perViewSumSquareReprojectionError = o.reprojectionErrors.flatMap((it2) => [it2.x, it2.y]);
// For each error, square it, sum the squares, and divide by total points N
return Math.sqrt(
perViewSumSquareReprojectionError.map((it) => Math.pow(it, 2)).reduce((a, b) => a + b, 0) /
perViewSumSquareReprojectionError.length
);
};
// Import and export functions
const downloadCalibration = () => {
const calibData = useCameraSettingsStore().getCalibrationCoeffs(props.videoFormat.resolution);
if (calibData === undefined) {
useStateStore().showSnackbarMessage({
color: "error",
message:
"Calibration data isn't available for the requested resolution, please calibrate the requested resolution first"
});
return;
}
const camUniqueName = useCameraSettingsStore().currentCameraSettings.uniqueName;
const filename = `photon_calibration_${camUniqueName}_${calibData.resolution.width}x${calibData.resolution.height}.json`;
const fileData = JSON.stringify(calibData);
const element = document.createElement("a");
element.style.display = "none";
element.setAttribute("href", "data:text/plain;charset=utf-8," + encodeURIComponent(fileData));
element.setAttribute("download", filename);
document.body.appendChild(element);
element.click();
document.body.removeChild(element);
};
const importCalibrationFromPhotonJson = ref();
const openUploadPhotonCalibJsonPrompt = () => {
importCalibrationFromPhotonJson.value.click();
};
const importCalibration = async () => {
const files = importCalibrationFromPhotonJson.value.files;
if (files.length === 0) return;
const uploadedJson = files[0];
const data = await parseJsonFile<CameraCalibrationResult>(uploadedJson);
if (
data.resolution.height != props.videoFormat.resolution.height ||
data.resolution.width != props.videoFormat.resolution.width
) {
useStateStore().showSnackbarMessage({
color: "error",
message: `The resolution of the calibration export doesn't match the current resolution ${props.videoFormat.resolution.height}x${props.videoFormat.resolution.width}`
});
return;
}
useCameraSettingsStore()
.importCalibrationFromData({ calibration: data })
.then((response) => {
useStateStore().showSnackbarMessage({
color: "success",
message: response.data.text || response.data
});
})
.catch((error) => {
if (error.response) {
useStateStore().showSnackbarMessage({
color: "error",
message: error.response.data.text || error.response.data
});
} else if (error.request) {
useStateStore().showSnackbarMessage({
color: "error",
message: "Error while trying to process the request! The backend didn't respond."
});
} else {
useStateStore().showSnackbarMessage({
color: "error",
message: "An error occurred while trying to process the request."
});
}
});
};
interface ObservationDetails {
snapshotSrc: any;
mean: number;
index: number;
}
const getObservationDetails = (): ObservationDetails[] | undefined => {
return getCalibrationCoeffs()?.observations.map((o, i) => ({
index: i,
mean: parseFloat(getMeanFromView(o).toFixed(2)),
snapshotSrc: o.includeObservationInCalibration ? "data:image/png;base64," + o.snapshotData.data : loadingImage
}));
};
</script>
<template>
<v-card color="primary" class="pa-6" dark>
<v-row>
<v-col cols="12" md="5">
<v-card-title class="pl-0 ml-0"
><span class="text-no-wrap" style="white-space: pre !important">Calibration Details: </span
><span class="text-no-wrap"
>{{ useCameraSettingsStore().currentCameraName }}@{{ getResolutionString(videoFormat.resolution) }}</span
></v-card-title
>
</v-col>
<v-col>
<v-btn color="secondary" class="mt-4" style="width: 100%" @click="openUploadPhotonCalibJsonPrompt">
<v-icon left> mdi-import</v-icon>
<span>Import</span>
</v-btn>
<input
ref="importCalibrationFromPhotonJson"
type="file"
accept=".json"
style="display: none"
@change="importCalibration"
/>
</v-col>
<v-col>
<v-btn
color="secondary"
class="mt-4"
:disabled="getCalibrationCoeffs() === undefined"
style="width: 100%"
@click="downloadCalibration"
>
<v-icon left>mdi-export</v-icon>
<span>Export</span>
</v-btn>
</v-col>
</v-row>
<v-row v-if="getCalibrationCoeffs() !== undefined" class="pt-2">
<v-card-subtitle>Calibration Details</v-card-subtitle>
<v-simple-table dense style="width: 100%" class="pl-2 pr-2">
<template #default>
<thead>
<tr>
<th class="text-left">Name</th>
<th class="text-left">Value</th>
</tr>
</thead>
<tbody>
<tr>
<td>Fx</td>
<td>{{ getCalibrationCoeffs()?.cameraIntrinsics.data[0].toFixed(2) || 0.0 }} mm</td>
</tr>
<tr>
<td>Fy</td>
<td>{{ getCalibrationCoeffs()?.cameraIntrinsics.data[4].toFixed(2) || 0.0 }} mm</td>
</tr>
<tr>
<td>Cx</td>
<td>{{ getCalibrationCoeffs()?.cameraIntrinsics.data[2].toFixed(2) || 0.0 }} px</td>
</tr>
<tr>
<td>Cy</td>
<td>{{ getCalibrationCoeffs()?.cameraIntrinsics.data[5].toFixed(2) || 0.0 }} px</td>
</tr>
<tr>
<td>Distortion</td>
<td>{{ getCalibrationCoeffs()?.distCoeffs.data.map((it) => parseFloat(it.toFixed(3))) || [] }}</td>
</tr>
<tr>
<td>Mean Err</td>
<td>
{{
videoFormat.mean !== undefined
? isNaN(videoFormat.mean)
? "NaN"
: videoFormat.mean.toFixed(2) + "px"
: "-"
}}
</td>
</tr>
<tr>
<td>Horizontal FOV</td>
<td>{{ videoFormat.horizontalFOV !== undefined ? videoFormat.horizontalFOV.toFixed(2) + "°" : "-" }}</td>
</tr>
<tr>
<td>Vertical FOV</td>
<td>{{ videoFormat.verticalFOV !== undefined ? videoFormat.verticalFOV.toFixed(2) + "°" : "-" }}</td>
</tr>
<tr>
<td>Diagonal FOV</td>
<td>{{ videoFormat.diagonalFOV !== undefined ? videoFormat.diagonalFOV.toFixed(2) + "°" : "-" }}</td>
</tr>
</tbody>
</template>
</v-simple-table>
<hr style="width: 100%" class="ma-6" />
<v-card-subtitle>Per Observation Details</v-card-subtitle>
<v-data-table
dense
style="width: 100%"
class="pl-2 pr-2"
:headers="[
{ text: 'Observation Id', value: 'index' },
{ text: 'Mean Reprojection Error', value: 'mean' }
]"
:items="getObservationDetails()"
item-key="index"
show-expand
expand-icon="mdi-eye"
>
<template #expanded-item="{ headers, item }">
<td :colspan="headers.length">
<div style="display: flex; justify-content: center; width: 100%">
<img :src="item.snapshotSrc" alt="observation image" class="snapshot-preview pt-2 pb-2" />
</div>
</td>
</template>
</v-data-table>
</v-row>
<v-row v-else class="pt-2 mb-0 pb-0">
The selected video format doesn't have any additional information as it has yet to be calibrated.
</v-row>
</v-card>
</template>
<style scoped>
.v-data-table {
background-color: #006492 !important;
}
.snapshot-preview {
max-width: 55%;
}
@media only screen and (max-width: 512px) {
.snapshot-preview {
max-width: 100%;
}
}
</style>

View File

@@ -41,7 +41,12 @@ const fpsTooLow = computed<boolean>(() => {
</script>
<template>
<v-card class="mb-3 pb-3 pa-4" color="primary" dark>
<v-card
id="camera-settings-camera-view-card"
class="camera-settings-camera-view-card mb-3 pb-3 pa-4"
color="primary"
dark
>
<v-card-title
class="pb-0 mb-2 pl-4 pt-1"
style="min-height: 50px; justify-content: space-between; align-content: center"

View File

@@ -4,6 +4,7 @@ import type {
CameraCalibrationResult,
CameraSettings,
ConfigurableCameraSettings,
Resolution,
RobotOffsetType,
VideoFormat
} from "@/types/SettingTypes";
@@ -13,6 +14,7 @@ import type { WebsocketCameraSettingsUpdate } from "@/types/WebsocketDataTypes";
import { WebsocketPipelineType } from "@/types/WebsocketDataTypes";
import type { ActiveConfigurablePipelineSettings, ActivePipelineSettings, PipelineType } from "@/types/PipelineTypes";
import axios from "axios";
import { resolutionsAreEqual } from "@/lib/PhotonUtils";
interface CameraSettingsStore {
cameras: CameraSettings[];
@@ -41,18 +43,22 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
return this.currentCameraSettings.validVideoFormats[this.currentPipelineSettings.cameraVideoModeIndex];
},
isCurrentVideoFormatCalibrated(): boolean {
return this.currentCameraSettings.completeCalibrations.some(
(v) =>
v.resolution.width === this.currentVideoFormat.resolution.width &&
v.resolution.height === this.currentVideoFormat.resolution.height
return this.currentCameraSettings.completeCalibrations.some((v) =>
resolutionsAreEqual(v.resolution, this.currentVideoFormat.resolution)
);
},
cameraNames(): string[] {
return this.cameras.map((c) => c.nickname);
},
currentCameraName(): string {
return this.cameraNames[useStateStore().currentCameraIndex];
},
pipelineNames(): string[] {
return this.currentCameraSettings.pipelineNicknames;
},
currentPipelineName(): string {
return this.pipelineNames[useStateStore().currentCameraIndex];
},
isDriverMode(): boolean {
return this.currentCameraSettings.currentPipelineIndex === WebsocketPipelineType.DriverMode;
},
@@ -67,6 +73,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
updateCameraSettingsFromWebsocket(data: WebsocketCameraSettingsUpdate[]) {
this.cameras = data.map<CameraSettings>((d) => ({
nickname: d.nickname,
uniqueName: d.uniqueName,
fov: {
value: d.fov,
managedByVendor: !d.isFovConfigurable
@@ -92,16 +99,7 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
standardDeviation: v.standardDeviation,
mean: v.mean
})),
completeCalibrations: d.calibrations.map<CameraCalibrationResult>((calib) => ({
resolution: {
height: calib.height,
width: calib.width
},
distCoeffs: calib.distCoeffs,
standardDeviation: calib.standardDeviation,
perViewErrors: calib.perViewErrors,
intrinsics: calib.intrinsics
})),
completeCalibrations: d.calibrations,
isCSICamera: d.isCSICamera,
pipelineNicknames: d.pipelineNicknames,
currentPipelineIndex: d.currentPipelineIndex,
@@ -360,6 +358,16 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
};
return axios.post("/calibration/importFromCalibDB", payload, { headers: { "Content-Type": "text/plain" } });
},
importCalibrationFromData(
data: { calibration: CameraCalibrationResult },
cameraIndex: number = useStateStore().currentCameraIndex
) {
const payload = {
...data,
cameraIndex: cameraIndex
};
return axios.post("/calibration/importFromData", payload);
},
/**
* Take a snapshot for the calibration processes
*
@@ -408,6 +416,12 @@ export const useCameraSettingsStore = defineStore("cameraSettings", {
cameraIndex: cameraIndex
};
useStateStore().websocket?.send(payload, true);
},
getCalibrationCoeffs(
resolution: Resolution,
cameraIndex: number = useStateStore().currentCameraIndex
): CameraCalibrationResult | undefined {
return this.cameras[cameraIndex].completeCalibrations.find((v) => resolutionsAreEqual(v.resolution, resolution));
}
}
});

View File

@@ -1,3 +1,26 @@
export interface Quaternion {
X: number;
Y: number;
Z: number;
W: number;
}
export interface Translation3d {
x: number;
y: number;
z: number;
}
export interface Rotation3d {
quaternion: Quaternion;
}
export interface Pose3d {
translation: Translation3d;
rotation: Rotation3d;
}
// TODO update backend to serialize this using correct layout
export interface Transform3d {
x: number;
y: number;
@@ -11,13 +34,6 @@ export interface Transform3d {
angle_z: number;
}
export interface Quaternion {
X: number;
Y: number;
Z: number;
W: number;
}
export interface AprilTagFieldLayout {
field: {
length: number;
@@ -25,16 +41,7 @@ export interface AprilTagFieldLayout {
};
tags: {
ID: number;
pose: {
translation: {
x: number;
y: number;
z: number;
};
rotation: {
quaternion: Quaternion;
};
};
pose: Pose3d;
}[];
}

View File

@@ -1,4 +1,5 @@
import { type ActivePipelineSettings, DefaultAprilTagPipelineSettings } from "@/types/PipelineTypes";
import type { Pose3d } from "@/types/PhotonTrackingTypes";
export interface GeneralSettings {
version?: string;
@@ -77,16 +78,59 @@ 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 JsonMatOfDouble {
rows: number;
cols: number;
type: CvType;
data: number[];
}
export interface JsonImageMat {
rows: number;
cols: number;
type: CvType;
data: string; // base64 encoded
}
export interface CvPoint3 {
x: number;
y: number;
z: number;
}
export interface CvPoint {
x: number;
y: number;
}
export interface BoardObservation {
locationInObjectSpace: CvPoint3[];
locationInImageSpace: CvPoint[];
reprojectionErrors: CvPoint[];
optimisedCameraToObject: Pose3d;
includeObservationInCalibration: boolean;
snapshotName: string;
snapshotData: JsonImageMat;
}
export interface CameraCalibrationResult {
resolution: Resolution;
distCoeffs: number[];
standardDeviation: number;
perViewErrors: number[] | null;
intrinsics: number[];
cameraIntrinsics: JsonMatOfDouble;
distCoeffs: JsonMatOfDouble;
observations: BoardObservation[];
}
export interface ConfigurableCameraSettings {
@@ -95,6 +139,7 @@ export interface ConfigurableCameraSettings {
export interface CameraSettings {
nickname: string;
uniqueName: string;
fov: {
value: number;
@@ -117,6 +162,7 @@ export interface CameraSettings {
export const PlaceholderCameraSettings: CameraSettings = {
nickname: "Placeholder Camera",
uniqueName: "Placeholder Name",
fov: {
value: 70,
managedByVendor: true
@@ -142,7 +188,45 @@ export const PlaceholderCameraSettings: CameraSettings = {
pixelFormat: "RGB"
}
],
completeCalibrations: [],
completeCalibrations: [
{
resolution: { width: 1920, height: 1080 },
cameraIntrinsics: {
rows: 1,
cols: 1,
type: 1,
data: [1, 2, 3, 4, 5, 6, 7, 8, 9]
},
distCoeffs: {
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 }
],
includeObservationInCalibration: false,
snapshotName: "img0.png",
snapshotData: { rows: 480, cols: 640, type: CvType.CV_8U, data: "" }
}
]
}
],
pipelineNicknames: ["Placeholder Pipeline"],
lastPipelineIndex: 0,
currentPipelineIndex: 0,

View File

@@ -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";
@@ -20,16 +27,6 @@ export interface WebsocketNumberPair {
second: number;
}
export interface WebsocketCompleteCalib {
distCoeffs: number[];
height: number;
width: number;
standardDeviation: number;
// perViewErrors not set in test mode
perViewErrors: number[] | null;
intrinsics: number[];
}
export type WebsocketVideoFormat = Record<
number,
{
@@ -47,7 +44,7 @@ export type WebsocketVideoFormat = Record<
>;
export interface WebsocketCameraSettingsUpdate {
calibrations: WebsocketCompleteCalib[];
calibrations: CameraCalibrationResult[];
currentPipelineIndex: number;
currentPipelineSettings: ActivePipelineSettings;
fov: number;
@@ -55,6 +52,7 @@ export interface WebsocketCameraSettingsUpdate {
isFovConfigurable: boolean;
isCSICamera: boolean;
nickname: string;
uniqueName: string;
outputStreamPort: number;
pipelineNicknames: string[];
videoFormatList: WebsocketVideoFormat;

View File

@@ -28,6 +28,7 @@ import org.photonvision.common.hardware.Platform;
import org.photonvision.common.networking.NetworkUtils;
import org.photonvision.common.util.SerializationUtils;
import org.photonvision.raspi.LibCameraJNILoader;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.processes.VisionModule;
import org.photonvision.vision.processes.VisionModuleManager;
import org.photonvision.vision.processes.VisionSource;
@@ -166,13 +167,14 @@ public class PhotonConfiguration {
public double fov;
public String nickname;
public String uniqueName;
public HashMap<String, Object> currentPipelineSettings;
public int currentPipelineIndex;
public List<String> pipelineNicknames;
public HashMap<Integer, HashMap<String, Object>> videoFormatList;
public int outputStreamPort;
public int inputStreamPort;
public List<HashMap<String, Object>> calibrations;
public List<CameraCalibrationCoefficients> calibrations;
public boolean isFovConfigurable = true;
public boolean isCSICamera;
}

View File

@@ -184,7 +184,7 @@ public class NTDataPublisher implements CVPipelineResultConsumer {
&& result.inputAndOutputFrame.frameStaticProperties.cameraCalibration != null) {
var fsp = result.inputAndOutputFrame.frameStaticProperties;
ts.cameraIntrinsicsPublisher.accept(fsp.cameraCalibration.getIntrinsicsArr());
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getExtrinsicsArr());
ts.cameraDistortionPublisher.accept(fsp.cameraCalibration.getDistCoeffsArr());
} else {
ts.cameraIntrinsicsPublisher.accept(new double[] {});
ts.cameraDistortionPublisher.accept(new double[] {});

View File

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

View File

@@ -356,6 +356,10 @@ public class TestUtils {
return getCoeffs(LIFECAM_480P_CAL_FILE, testMode);
}
public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
}
public static CameraCalibrationCoefficients getLaptop() {
return getCoeffs("laptop.json", true);
}
@@ -389,8 +393,4 @@ public class TestUtils {
.resolve("testimages")
.resolve(WPI2022Image.kTerminal22ft6in.path);
}
public static CameraCalibrationCoefficients get2023LifeCamCoeffs(boolean testMode) {
return getCoeffs(LIFECAM_1280P_CAL_FILE, testMode);
}
}

View File

@@ -32,6 +32,7 @@ import java.io.FileOutputStream;
import java.io.IOException;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
public class JacksonUtils {
public static class UIMap extends HashMap<String, Object> {}
@@ -61,6 +62,19 @@ public class JacksonUtils {
saveJsonString(json, path, forceSync);
}
public static <T> T deserialize(Map<?, ?> s, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();
ObjectMapper objectMapper =
JsonMapper.builder()
.configure(JsonReadFeature.ALLOW_JAVA_COMMENTS, true)
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.activateDefaultTyping(ptv, ObjectMapper.DefaultTyping.JAVA_LANG_OBJECT)
.build();
return objectMapper.convertValue(s, ref);
}
public static <T> T deserialize(String s, Class<T> ref) throws IOException {
PolymorphicTypeValidator ptv =
BasicPolymorphicTypeValidator.builder().allowIfBaseType(ref).build();

View File

@@ -23,10 +23,12 @@ import edu.wpi.first.math.geometry.CoordinateSystem;
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,23 @@ public class MathUtils {
var axis = rotation.getAxis().times(angle);
rvecOutput.put(0, 0, axis.getData());
}
/**
* Convert an Opencv rvec+tvec pair to a Pose3d.
*
* @param rVec Axis-angle rotation vector, where norm(rVec) is the angle about a unit vector in
* the direction of rVec
* @param tVec 3D translation
* @return Pose3d representing the same rigid transform
*/
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);
}
}

View File

@@ -0,0 +1,71 @@
/*
* 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 <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.geometry.Pose3d;
import java.util.List;
import org.opencv.core.Point;
import org.opencv.core.Point3;
public final class BoardObservation {
// Expected feature 3d location in the camera frame
@JsonProperty("locationInObjectSpace")
public List<Point3> locationInObjectSpace;
// Observed location in pixel space
@JsonProperty("locationInImageSpace")
public List<Point> locationInImageSpace;
// (measured location in pixels) - (expected from FK)
@JsonProperty("reprojectionErrors")
public List<Point> reprojectionErrors;
// Solver optimized board poses
@JsonProperty("optimisedCameraToObject")
public Pose3d optimisedCameraToObject;
// If we should use this observation when re-calculating camera calibration
@JsonProperty("includeObservationInCalibration")
public boolean includeObservationInCalibration;
@JsonProperty("snapshotName")
public String snapshotName;
@JsonProperty("snapshotData")
public JsonImageMat snapshotData;
@JsonCreator
public BoardObservation(
@JsonProperty("locationInObjectSpace") List<Point3> locationInObjectSpace,
@JsonProperty("locationInImageSpace") List<Point> locationInImageSpace,
@JsonProperty("reprojectionErrors") List<Point> reprojectionErrors,
@JsonProperty("optimisedCameraToObject") Pose3d optimisedCameraToObject,
@JsonProperty("includeObservationInCalibration") boolean includeObservationInCalibration,
@JsonProperty("snapshotName") String snapshotName,
@JsonProperty("snapshotData") JsonImageMat snapshotData) {
this.locationInObjectSpace = locationInObjectSpace;
this.locationInImageSpace = locationInImageSpace;
this.reprojectionErrors = reprojectionErrors;
this.optimisedCameraToObject = optimisedCameraToObject;
this.includeObservationInCalibration = includeObservationInCalibration;
this.snapshotName = snapshotName;
this.snapshotData = snapshotData;
}
}

View File

@@ -20,50 +20,72 @@ package org.photonvision.vision.calibration;
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 java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
import org.photonvision.vision.opencv.Releasable;
@JsonIgnoreProperties(ignoreUnknown = true)
public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("resolution")
public final Size resolution;
@JsonProperty("cameraIntrinsics")
public final JsonMat cameraIntrinsics;
public final JsonMatOfDouble cameraIntrinsics;
@JsonProperty("cameraExtrinsics")
@JsonAlias({"cameraExtrinsics", "distCoeffs"})
public final JsonMat distCoeffs;
@JsonProperty("distCoeffs")
@JsonAlias({"distCoeffs", "distCoeffs"})
public final JsonMatOfDouble distCoeffs;
@JsonProperty("perViewErrors")
public final double[] perViewErrors;
@JsonProperty("observations")
public final List<BoardObservation> observations;
@JsonProperty("standardDeviation")
public final double standardDeviation;
@JsonProperty("calobjectWarp")
public final double[] calobjectWarp;
@JsonIgnore private final double[] intrinsicsArr = new double[9];
@JsonIgnore private final double[] distCoeffsArr = new double[5];
@JsonIgnore private final double[] extrinsicsArr = new double[5];
/**
* Contains all camera calibration data for a particular resolution of a camera. Designed for use
* with standard opencv camera calibration matrices. For details on the layout of camera
* intrinsics/distortion matrices, see:
* https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga3207604e4b1a1758aa66acb6ed5aa65d
*
* @param resolution The resolution this applies to. We don't assume camera binning or try
* rescaling calibration
* @param cameraIntrinsics Camera intrinsics parameters matrix, in the standard opencv form.
* @param distCoeffs Camera distortion coefficients array. Variable length depending on order of
* distortion model
* @param calobjectWarp Board deformation parameters, for calibrators that can estimate that. See:
* https://mrcal.secretsauce.net/formulation.html#board-deformation
* @param observations List of snapshots used to construct this calibration
*/
@JsonCreator
public CameraCalibrationCoefficients(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics,
@JsonProperty("cameraExtrinsics") JsonMat distCoeffs,
@JsonProperty("perViewErrors") double[] perViewErrors,
@JsonProperty("standardDeviation") double standardDeviation) {
@JsonProperty("cameraIntrinsics") JsonMatOfDouble cameraIntrinsics,
@JsonProperty("distCoeffs") JsonMatOfDouble distCoeffs,
@JsonProperty("calobjectWarp") double[] calobjectWarp,
@JsonProperty("observations") List<BoardObservation> observations) {
this.resolution = resolution;
this.cameraIntrinsics = cameraIntrinsics;
this.distCoeffs = distCoeffs;
this.perViewErrors = perViewErrors;
this.standardDeviation = standardDeviation;
this.calobjectWarp = calobjectWarp;
// 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);
getDistCoeffsMat().get(0, 0, extrinsicsArr);
getDistCoeffsMat().get(0, 0, distCoeffsArr);
}
@JsonIgnore
@@ -82,18 +104,13 @@ public class CameraCalibrationCoefficients implements Releasable {
}
@JsonIgnore
public double[] getExtrinsicsArr() {
return extrinsicsArr;
public double[] getDistCoeffsArr() {
return distCoeffsArr;
}
@JsonIgnore
public double[] getPerViewErrors() {
return perViewErrors;
}
@JsonIgnore
public double getStandardDeviation() {
return standardDeviation;
public List<BoardObservation> getPerViewErrors() {
return observations;
}
@Override
@@ -130,14 +147,14 @@ public class CameraCalibrationCoefficients implements Releasable {
dist_coefs.get(4).doubleValue(),
};
var cam_jsonmat = new JsonMat(3, 3, cam_arr);
var distortion_jsonmat = new JsonMat(1, 5, dist_array);
var cam_jsonmat = new JsonMatOfDouble(3, 3, cam_arr);
var distortion_jsonmat = new JsonMatOfDouble(1, 5, dist_array);
var error = json.get("avg_reprojection_error").asDouble();
var width = json.get("img_size").get(0).doubleValue();
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, new double[0], List.of());
}
}

View File

@@ -0,0 +1,79 @@
/*
* 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 <https://www.gnu.org/licenses/>.
*/
package org.photonvision.vision.calibration;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Base64;
import org.opencv.core.Mat;
import org.opencv.core.MatOfByte;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.vision.opencv.Releasable;
/** JSON-serializable image. Data is stored as base64-encoded PNG data. */
public class JsonImageMat implements Releasable {
public final int rows;
public final int cols;
public final int type;
// We store image data as a base64-encoded PNG inside a Java string. This lets us serialize it
// without too much overhead and still use JSON.
public final String data;
// Cached matrices to avoid object recreation
@JsonIgnore private Mat wrappedMat = null;
public JsonImageMat(Mat mat) {
this.rows = mat.rows();
this.cols = mat.cols();
this.type = mat.type();
// Convert from Mat -> png byte array -> base64
var buf = new MatOfByte();
Imgcodecs.imencode(".png", mat, buf);
data = Base64.getEncoder().encodeToString(buf.toArray());
buf.release();
}
public JsonImageMat(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@JsonProperty("data") String data) {
this.rows = rows;
this.cols = cols;
this.type = type;
this.data = data;
}
@JsonIgnore
public Mat getAsMat() {
if (wrappedMat == null) {
// Convert back from base64 string -> png -> Mat
var bytes = Base64.getDecoder().decode(data);
var pngData = new MatOfByte(bytes);
this.wrappedMat = Imgcodecs.imdecode(pngData, Imgcodecs.IMREAD_COLOR);
}
return this.wrappedMat;
}
@Override
public void release() {
if (wrappedMat != null) wrappedMat.release();
}
}

View File

@@ -29,7 +29,8 @@ import org.opencv.core.MatOfDouble;
import org.photonvision.common.dataflow.structures.Packet;
import org.photonvision.vision.opencv.Releasable;
public class JsonMat implements Releasable {
/** JSON-serializable image. Data is stored as a raw JSON array. */
public class JsonMatOfDouble implements Releasable {
public final int rows;
public final int cols;
public final int type;
@@ -41,11 +42,11 @@ public class JsonMat implements Releasable {
private MatOfDouble wrappedMatOfDouble;
public JsonMat(int rows, int cols, double[] data) {
public JsonMatOfDouble(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
}
public JsonMat(
public JsonMatOfDouble(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@@ -84,9 +85,9 @@ public class JsonMat implements Releasable {
return Arrays.copyOfRange(data, 0, dataLen);
}
public static JsonMat fromMat(Mat mat) {
public static JsonMatOfDouble fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
return new JsonMatOfDouble(mat.rows(), mat.cols(), getDataFromMat(mat));
}
@JsonIgnore

View File

@@ -22,20 +22,23 @@ import com.fasterxml.jackson.databind.ObjectMapper;
import java.util.ArrayList;
import java.util.List;
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.BoardObservation;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.calibration.JsonMat;
import org.photonvision.vision.calibration.JsonImageMat;
import org.photonvision.vision.calibration.JsonMatOfDouble;
import org.photonvision.vision.pipe.CVPipe;
public class Calibrate3dPipe
extends CVPipe<
List<Triple<Size, Mat, Mat>>,
List<FindBoardCornersPipe.FindBoardCornersPipeResult>,
CameraCalibrationCoefficients,
Calibrate3dPipe.CalibratePipeParams> {
// Camera matrix stores the center of the image and focal length across the x and y-axis in a 3x3
@@ -69,31 +72,34 @@ public class Calibrate3dPipe
* @return Result of processing.
*/
@Override
protected CameraCalibrationCoefficients process(List<Triple<Size, Mat, Mat>> in) {
protected CameraCalibrationCoefficients process(
List<FindBoardCornersPipe.FindBoardCornersPipeResult> in) {
in =
in.stream()
.filter(
it ->
it != null
&& it.getLeft() != null
&& it.getMiddle() != null
&& it.getRight() != null)
&& it.imagePoints != null
&& it.objectPoints != null
&& it.size != null)
.collect(Collectors.toList());
List<Mat> objPoints = in.stream().map(it -> it.objectPoints).collect(Collectors.toList());
List<Mat> imgPts = in.stream().map(it -> it.imagePoints).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,
imgPts,
new Size(in.get(0).getLeft().width, in.get(0).getLeft().height),
new Size(in.get(0).size.width, in.get(0).size.height),
cameraMatrix,
distortionCoefficients,
rvecs,
@@ -106,15 +112,59 @@ public class Calibrate3dPipe
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);
JsonMatOfDouble cameraMatrixMat = JsonMatOfDouble.fromMat(cameraMatrix);
JsonMatOfDouble distortionCoefficientsMat = JsonMatOfDouble.fromMat(distortionCoefficients);
// For each observation, calc reprojection error
Mat jac_temp = new Mat();
List<BoardObservation> 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<Point>();
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));
JsonImageMat image = null;
var inputImage = in.get(i).inputImage;
if (inputImage != null) {
image = new JsonImageMat(inputImage);
}
observations.add(
new BoardObservation(
i_objPts, i_imgPts, reprojectionError, camToBoard, true, "img" + i + ".png", image));
}
jac_temp.release();
// Standard deviation of results
double stdDev = calculateSD(perViewErrorsArray);
try {
// Print calibration successful
logger.info(
@@ -126,32 +176,12 @@ public class Calibrate3dPipe
+ 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, new double[0], observations);
}
public static class CalibratePipeParams {

View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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<Mat, List<TrackedTarget>>, DrawCalibrationPipe.DrawCalibrationPipeParams> {
@Override
protected Void process(Pair<Mat, List<TrackedTarget>> 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;
}
}
}

View File

@@ -18,19 +18,21 @@
package org.photonvision.vision.pipe.impl;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.frame.FrameDivisor;
import org.photonvision.vision.opencv.Releasable;
import org.photonvision.vision.pipe.CVPipe;
import org.photonvision.vision.pipeline.UICalibrationData;
public class FindBoardCornersPipe
extends CVPipe<
Pair<Mat, Mat>, Triple<Size, Mat, Mat>, FindBoardCornersPipe.FindCornersPipeParams> {
Pair<Mat, Mat>,
FindBoardCornersPipe.FindBoardCornersPipeResult,
FindBoardCornersPipe.FindCornersPipeParams> {
private static final Logger logger =
new Logger(FindBoardCornersPipe.class, LogGroup.VisionModule);
@@ -112,10 +114,7 @@ public class FindBoardCornersPipe
* @return All valid Mats for camera calibration
*/
@Override
protected Triple<Size, Mat, Mat> process(Pair<Mat, Mat> in) {
// Create the object points
createObjectPoints();
protected FindBoardCornersPipeResult process(Pair<Mat, Mat> in) {
return findBoardCorners(in);
}
@@ -217,7 +216,7 @@ public class FindBoardCornersPipe
*
* @return Frame resolution, object points, board corners
*/
private Triple<Size, Mat, Mat> findBoardCorners(Pair<Mat, Mat> in) {
private FindBoardCornersPipeResult findBoardCorners(Pair<Mat, Mat> in) {
createObjectPoints();
var inFrame = in.getLeft();
@@ -228,10 +227,15 @@ public class FindBoardCornersPipe
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));
// Note that opencv will copy the frame if no resize is requested; we can skip this since we
// don't need that copy. See:
// https://github.com/opencv/opencv/blob/a8ec6586118c3f8e8f48549a85f2da7a5b78bcc9/modules/imgproc/src/resize.cpp#L4185
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 +248,13 @@ public class FindBoardCornersPipe
}
} 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 +267,23 @@ public class FindBoardCornersPipe
// Get the size of the inFrame
this.imageSize = new Size(inFrame.width(), inFrame.height());
// Do sub corner pix for drawing chessboard
// Do sub corner pix for drawing chessboard when using OpenCV
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);
return new FindBoardCornersPipeResult(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,
@@ -331,4 +326,27 @@ public class FindBoardCornersPipe
return divisor == other.divisor;
}
}
public static class FindBoardCornersPipeResult implements Releasable {
public Size size;
public MatOfPoint2f objectPoints;
public MatOfPoint2f imagePoints;
// Set later only if we need it
public Mat inputImage = null;
public FindBoardCornersPipeResult(
Size size, MatOfPoint2f objectPoints, MatOfPoint2f imagePoints) {
this.size = size;
this.objectPoints = objectPoints;
this.imagePoints = imagePoints;
}
@Override
public void release() {
objectPoints.release();
imagePoints.release();
if (inputImage != null) inputImage.release();
}
}
}

View File

@@ -18,39 +18,38 @@
package org.photonvision.vision.pipeline;
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.Point;
import org.opencv.core.Size;
import org.opencv.imgcodecs.Imgcodecs;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.dataflow.DataChangeService;
import org.photonvision.common.dataflow.events.OutgoingUIEvent;
import org.photonvision.common.logging.LogGroup;
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.BoardObservation;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.frame.Frame;
import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.opencv.ImageRotationMode;
import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
import org.photonvision.vision.pipe.impl.CalculateFPSPipe;
import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
import org.photonvision.vision.pipeline.result.CVPipelineResult;
import org.photonvision.vision.pipeline.result.CalibrationPipelineResult;
public class Calibrate3dPipeline
extends CVPipeline<CVPipelineResult, Calibration3dPipelineSettings> {
// 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();
@@ -59,7 +58,7 @@ public class Calibrate3dPipeline
private boolean takeSnapshot = false;
// Output of the corners
final List<Triple<Size, Mat, Mat>> foundCornersList;
final List<FindBoardCornersPipeResult> foundCornersList;
/// Output of the calibration, getter method is set for this.
private CVPipeResult<CameraCalibrationCoefficients> calibrationOutput;
@@ -68,16 +67,13 @@ public class Calibrate3dPipeline
private boolean calibrating = false;
// Path to save images
private final Path imageDir = ConfigManager.getInstance().getCalibDir();
private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.NONE;
public Calibrate3dPipeline() {
this(12);
public Calibrate3dPipeline(String uniqueName) {
this(12, uniqueName);
}
public Calibrate3dPipeline(int minSnapshots) {
public Calibrate3dPipeline(int minSnapshots, String uniqueName) {
super(PROCESSING_TYPE);
this.settings = new Calibration3dPipelineSettings();
this.foundCornersList = new ArrayList<>();
@@ -99,11 +95,6 @@ public class Calibrate3dPipeline
new Calibrate3dPipe.CalibratePipeParams(
new Size(frameStaticProperties.imageWidth, frameStaticProperties.imageHeight));
calibrate3dPipe.setParams(calibratePipeParams);
// if (cameraQuirks.hasQuirk(CameraQuirk.PiCam) && LibCameraJNI.isSupported()) {
// LibCameraJNI.setRotation(settings.inputImageRotationMode.value);
// // LibCameraJNI.setShouldCopyColor(true);
// }
}
@Override
@@ -114,12 +105,22 @@ public class Calibrate3dPipeline
return new CVPipelineResult(0, 0, null, frame);
}
if (getSettings().inputImageRotationMode != ImageRotationMode.DEG_0) {
// All this calibration assumes zero rotation. If we want a rotation, it should be applied at
// the output
logger.error(
"Input image rotation was non-zero! Calibration wasn't designed to deal with this. Attempting to manually change back to zero");
getSettings().inputImageRotationMode = ImageRotationMode.DEG_0;
return new CVPipelineResult(0, 0, List.of(), frame);
}
long sumPipeNanosElapsed = 0L;
// Check if the frame has chessboard corners
var outputColorCVMat = new CVMat();
inputColorMat.copyTo(outputColorCVMat.getMat());
var findBoardResult =
FindBoardCornersPipeResult findBoardResult =
findBoardCornersPipe.run(Pair.of(inputColorMat, outputColorCVMat.getMat())).output;
var fpsResult = calculateFPSPipe.run(null);
@@ -130,10 +131,10 @@ public class Calibrate3dPipeline
takeSnapshot = false;
if (findBoardResult != null) {
// Only copy the image into the result when we absolutely must
findBoardResult.inputImage = inputColorMat.clone();
foundCornersList.add(findBoardResult);
Imgcodecs.imwrite(
Path.of(imageDir.toString(), "img" + foundCornersList.size() + ".jpg").toString(),
inputColorMat);
// update the UI
broadcastState();
@@ -143,19 +144,18 @@ public class Calibrate3dPipeline
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(),
new Frame(
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties),
getCornersList());
}
public void deleteSavedImages() {
imageDir.toFile().mkdirs();
imageDir.toFile().mkdir();
FileUtils.deleteDirectory(imageDir);
List<List<Point>> getCornersList() {
return foundCornersList.stream()
.map(it -> it.imagePoints.toList())
.collect(Collectors.toList());
}
public boolean hasEnough() {
@@ -188,16 +188,12 @@ public class Calibrate3dPipeline
takeSnapshot = true;
}
public double[] perViewErrors() {
return calibrationOutput.output.perViewErrors;
public List<BoardObservation> perViewErrors() {
return calibrationOutput.output.observations;
}
public void finishCalibration() {
foundCornersList.forEach(
it -> {
it.getMiddle().release();
it.getRight().release();
});
foundCornersList.forEach(it -> it.release());
foundCornersList.clear();
broadcastState();

View File

@@ -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 @@ public class OutputStreamPipeline {
resizeImagePipe.setParams(
new ResizeImagePipe.ResizeImageParams(settings.streamingFrameDivisor));
drawCalibrationPipe.setParams(
new DrawCalibrationPipe.DrawCalibrationPipeParams(settings.streamingFrameDivisor));
}
public CVPipelineResult process(
@@ -149,8 +153,9 @@ public class OutputStreamPipeline {
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 class OutputStreamPipeline {
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) {

View File

@@ -17,17 +17,17 @@
package org.photonvision.vision.pipeline;
import java.util.Map;
public class UICalibrationData {
public final int videoModeIndex;
public int videoModeIndex;
public int count;
public final int minCount;
public final boolean hasEnough;
public final double squareSizeIn;
public final int patternWidth;
public final int patternHeight;
public final BoardType boardType; //
public int minCount;
public boolean hasEnough;
public double squareSizeIn;
public int patternWidth;
public int patternHeight;
public BoardType boardType;
public UICalibrationData() {}
public UICalibrationData(
int count,
@@ -53,18 +53,6 @@ public class UICalibrationData {
DOTBOARD
}
public static UICalibrationData fromMap(Map<String, Object> map) {
return new UICalibrationData(
((Number) map.get("count")).intValue(),
((Number) map.get("videoModeIndex")).intValue(),
((Number) map.get("minCount")).intValue(),
(boolean) map.get("hasEnough"),
((Number) map.get("squareSizeIn")).doubleValue(),
((Number) map.get("patternWidth")).intValue(),
((Number) map.get("patternHeight")).intValue(),
BoardType.values()[(int) map.get("boardType")]);
}
@Override
public String toString() {
return "UICalibrationData{"

View File

@@ -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 <https://www.gnu.org/licenses/>.
*/
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<TrackedTarget> cornersToTarget(List<List<Point>> corners) {
return corners.stream().map(TrackedTarget::new).collect(Collectors.toList());
}
public CalibrationPipelineResult(
double latencyNanos, double fps, Frame outputFrame, List<List<Point>> corners) {
super(latencyNanos, fps, cornersToTarget(corners), outputFrame);
}
}

View File

@@ -37,7 +37,7 @@ public class PipelineManager {
public static final int CAL_3D_INDEX = -2;
protected final List<CVPipelineSettings> 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. */
@@ -56,21 +56,23 @@ public class PipelineManager {
/**
* Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
* pipelines.
*
* @param userPipelines Pipelines to add to the manager.
*/
public PipelineManager(
DriverModePipelineSettings driverSettings, List<CVPipelineSettings> userPipelines) {
PipelineManager(
DriverModePipelineSettings driverSettings,
List<CVPipelineSettings> userPipelines,
String uniqueName) {
this.userPipelineSettings = new ArrayList<>(userPipelines);
// This is to respect the default res idx for vendor cameras
this.driverModePipeline.setSettings(driverSettings);
if (userPipelines.isEmpty()) addPipeline(PipelineType.Reflective);
calibration3dPipeline = new Calibrate3dPipeline(uniqueName);
}
public PipelineManager(CameraConfiguration config) {
this(config.driveModeSettings, config.pipelineSettings);
this(config.driveModeSettings, config.pipelineSettings, config.uniqueName);
}
/**

View File

@@ -26,6 +26,7 @@ import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.function.BiConsumer;
import org.opencv.core.Size;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.configuration.ConfigManager;
import org.photonvision.common.configuration.PhotonConfiguration;
@@ -335,7 +336,10 @@ public class VisionModule {
public void startCalibration(UICalibrationData data) {
var settings = pipelineManager.calibration3dPipeline.getSettings();
pipelineManager.calibration3dPipeline.deleteSavedImages();
var videoMode = visionSource.getSettables().getAllVideoModes().get(data.videoModeIndex);
var resolution = new Size(videoMode.width, videoMode.height);
settings.cameraVideoModeIndex = data.videoModeIndex;
visionSource.getSettables().setVideoModeIndex(data.videoModeIndex);
logger.info(
@@ -347,6 +351,7 @@ public class VisionModule {
settings.boardHeight = data.patternHeight;
settings.boardWidth = data.patternWidth;
settings.boardType = data.boardType;
settings.resolution = resolution;
// Disable gain if not applicable
if (!cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
@@ -501,6 +506,7 @@ public class VisionModule {
ret.fov = visionSource.getSettables().getFOV();
ret.isCSICamera = visionSource.getCameraConfiguration().cameraType == CameraType.ZeroCopyPicam;
ret.nickname = visionSource.getSettables().getConfiguration().nickname;
ret.uniqueName = visionSource.getSettables().getConfiguration().uniqueName;
ret.currentPipelineSettings =
SerializationUtils.objectToHashMap(pipelineManager.getCurrentPipelineSettings());
ret.currentPipelineIndex = pipelineManager.getCurrentPipelineIndex();
@@ -528,20 +534,7 @@ public class VisionModule {
ret.outputStreamPort = this.outputStreamPort;
ret.inputStreamPort = this.inputStreamPort;
var calList = new ArrayList<HashMap<String, Object>>();
for (var c : visionSource.getSettables().getConfiguration().calibrations) {
var internalMap = new HashMap<String, Object>();
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);
calList.add(internalMap);
}
ret.calibrations = calList;
ret.calibrations = visionSource.getSettables().getConfiguration().calibrations;
ret.isFovConfigurable =
!(ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV()

View File

@@ -26,6 +26,7 @@ import org.photonvision.common.dataflow.events.DataChangeEvent;
import org.photonvision.common.dataflow.events.IncomingWebSocketEvent;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
@@ -103,9 +104,15 @@ public class VisionModuleChangeSubscriber extends DataChangeSubscriber {
parentModule.saveAndBroadcastAll();
return;
case "startCalibration":
var data = UICalibrationData.fromMap((Map<String, Object>) newPropValue);
parentModule.startCalibration(data);
parentModule.saveAndBroadcastAll();
try {
var data =
JacksonUtils.deserialize(
(Map<String, Object>) newPropValue, UICalibrationData.class);
parentModule.startCalibration(data);
parentModule.saveAndBroadcastAll();
} catch (Exception e) {
logger.error("Error deserailizing start-cal request", e);
}
return;
case "saveInputSnapshot":
parentModule.saveInputSnapshot();

View File

@@ -146,6 +146,14 @@ public class TrackedTarget implements Releasable {
m_skew = 0;
}
public TrackedTarget(List<Point> 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,

View File

@@ -24,10 +24,8 @@ import edu.wpi.first.math.util.Units;
import java.io.File;
import java.nio.file.Path;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.apache.commons.lang3.tuple.Triple;
import org.junit.jupiter.api.BeforeAll;
import org.junit.jupiter.api.Test;
import org.opencv.calib3d.Calib3d;
@@ -44,6 +42,7 @@ import org.photonvision.vision.frame.FrameThresholdType;
import org.photonvision.vision.opencv.CVMat;
import org.photonvision.vision.pipe.impl.Calibrate3dPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe;
import org.photonvision.vision.pipe.impl.FindBoardCornersPipe.FindBoardCornersPipeResult;
public class Calibrate3dPipeTest {
@BeforeAll
@@ -66,7 +65,7 @@ public class Calibrate3dPipeTest {
new FindBoardCornersPipe.FindCornersPipeParams(
11, 4, UICalibrationData.BoardType.DOTBOARD, 15, FrameDivisor.NONE));
List<Triple<Size, Mat, Mat>> foundCornersList = new ArrayList<>();
List<FindBoardCornersPipeResult> foundCornersList = new ArrayList<>();
for (var f : frames) {
var copy = new Mat();
@@ -78,9 +77,7 @@ public class Calibrate3dPipeTest {
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);
for (var f : frames) {
f.release();
@@ -94,7 +91,7 @@ public class Calibrate3dPipeTest {
File dir = new File(TestUtils.getDotBoardImagesPath().toAbsolutePath().toString());
File[] directoryListing = dir.listFiles();
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "unique_name_lol");
calibration3dPipeline.getSettings().boardHeight = 11;
calibration3dPipeline.getSettings().boardWidth = 4;
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.DOTBOARD;
@@ -117,7 +114,7 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
calibration3dPipeline.removeSnapshot(0);
@@ -132,20 +129,16 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
var cal = calibration3dPipeline.tryCalibration();
calibration3dPipeline.finishCalibration();
assertNotNull(cal);
assertNotNull(cal.perViewErrors);
System.out.println("Per View Errors: " + Arrays.toString(cal.perViewErrors));
assertNotNull(cal.observations);
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());
// Confirm we didn't get leaky on our mat usage
// assertTrue(CVMat.getMatCount() == startMatCount); // TODO Figure out why this doesn't work in
@@ -260,7 +253,7 @@ public class Calibrate3dPipeTest {
assertTrue(directoryListing.length >= 25);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20);
Calibrate3dPipeline calibration3dPipeline = new Calibrate3dPipeline(20, "test_squares_common");
calibration3dPipeline.getSettings().boardType = UICalibrationData.BoardType.CHESSBOARD;
calibration3dPipeline.getSettings().resolution = imgRes;
calibration3dPipeline.getSettings().boardHeight = (int) Math.round(boardDim.height);
@@ -288,7 +281,7 @@ public class Calibrate3dPipeTest {
assertTrue(
calibration3dPipeline.foundCornersList.stream()
.map(Triple::getRight)
.map(it -> it.imagePoints)
.allMatch(it -> it.width() > 0 && it.height() > 0));
var cal = calibration3dPipeline.tryCalibration();
@@ -298,7 +291,7 @@ public class Calibrate3dPipeTest {
// 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 +303,8 @@ public class Calibrate3dPipeTest {
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("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());
// Confirm we didn't get leaky on our mat usage
// assertEquals(startMatCount, CVMat.getMatCount()); // TODO Figure out why this doesn't

View File

@@ -29,7 +29,8 @@ 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(), "meme_name");
manager.addPipeline(PipelineType.Reflective, "Another");
// We now have ["New Pipeline", "Another"]

View File

@@ -131,7 +131,7 @@ public class SimCameraProperties {
for (int j = 0; j < jsonIntrinsicsNode.size(); j++) {
jsonIntrinsics[j] = jsonIntrinsicsNode.get(j).asDouble();
}
var jsonDistortNode = calib.get("cameraExtrinsics").get("data");
var jsonDistortNode = calib.get("distCoeffs").get("data");
double[] jsonDistortion = new double[jsonDistortNode.size()];
for (int j = 0; j < jsonDistortNode.size(); j++) {
jsonDistortion[j] = jsonDistortNode.get(j).asDouble();

View File

@@ -460,7 +460,7 @@ public class RequestHandler {
}
}
public static void onCalibrationImportRequest(Context ctx) {
public static void onCalibDBCalibrationImportRequest(Context ctx) {
var data = ctx.body();
try {
@@ -492,6 +492,38 @@ public class RequestHandler {
}
}
public static void onDataCalibrationImportRequest(Context ctx) {
try {
var data = kObjectMapper.readTree(ctx.body());
int cameraIndex = data.get("cameraIndex").asInt();
var coeffs =
kObjectMapper.convertValue(data.get("calibration"), CameraCalibrationCoefficients.class);
var uploadCalibrationEvent =
new IncomingWebSocketEvent<>(
DataChangeDestination.DCD_ACTIVEMODULE,
"calibrationUploaded",
coeffs,
cameraIndex,
null);
DataChangeService.getInstance().publishEvent(uploadCalibrationEvent);
ctx.status(200);
ctx.result("Calibration imported successfully from imported data!");
logger.info("Calibration imported successfully from imported data!");
} catch (JsonProcessingException e) {
ctx.status(400);
ctx.result("The provided calibration data was malformed");
logger.error("The provided calibration data was malformed", e);
} catch (Exception e) {
ctx.status(500);
ctx.result("An error occurred while uploading calibration data");
logger.error("An error occurred while uploading calibration data", e);
}
}
public static void onProgramRestartRequest(Context ctx) {
// TODO, check if this was successful or not
ctx.status(204);
@@ -567,6 +599,50 @@ public class RequestHandler {
ctx.json(snapshots);
}
public static void onCameraCalibImagesRequest(Context ctx) {
try {
HashMap<String, HashMap<String, ArrayList<HashMap<String, Object>>>> snapshots =
new HashMap<>();
var cameraDirs = ConfigManager.getInstance().getCalibDir().toFile().listFiles();
if (cameraDirs != null) {
var camData = new HashMap<String, ArrayList<HashMap<String, Object>>>();
for (var cameraDir : cameraDirs) {
var resolutionDirs = cameraDir.listFiles();
if (resolutionDirs == null) continue;
for (var resolutionDir : resolutionDirs) {
var calibImages = resolutionDir.listFiles();
if (calibImages == null) continue;
var resolutionImages = new ArrayList<HashMap<String, Object>>();
for (var calibImg : calibImages) {
var snapshotData = new HashMap<String, Object>();
var bufferedImage = ImageIO.read(calibImg);
var buffer = new ByteArrayOutputStream();
ImageIO.write(bufferedImage, "png", buffer);
byte[] data = buffer.toByteArray();
snapshotData.put("snapshotData", data);
snapshotData.put("snapshotFilename", calibImg.getName());
resolutionImages.add(snapshotData);
}
camData.put(resolutionDir.getName(), resolutionImages);
}
var cameraName = cameraDir.getName();
snapshots.put(cameraName, camData);
}
}
ctx.json(snapshots);
} 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.
*

View File

@@ -38,6 +38,10 @@ public class Server {
corsContainer.add(CorsPluginConfig::anyHost);
});
// Increase the upload size limit (arbitrary, but need to be able to deal with large
// calibration JSONs)
javalinConfig.http.maxRequestSize = (long) (50 * 1e6);
javalinConfig.requestLogger.http(
(ctx, ms) -> {
StringJoiner joiner =
@@ -90,6 +94,7 @@ public class Server {
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);
@@ -101,7 +106,9 @@ public class Server {
// Calibration
app.post("/api/calibration/end", RequestHandler::onCalibrationEndRequest);
app.post("/api/calibration/importFromCalibDB", RequestHandler::onCalibrationImportRequest);
app.post(
"/api/calibration/importFromCalibDB", RequestHandler::onCalibDBCalibrationImportRequest);
app.post("/api/calibration/importFromData", RequestHandler::onDataCalibrationImportRequest);
app.start(port);
}

View File

@@ -19,7 +19,7 @@
1.0
]
},
"cameraExtrinsics": {
"distCoeffs": {
"rows": 1,
"cols": 5,
"type": 6,

View File

@@ -19,7 +19,7 @@
1.0
]
},
"cameraExtrinsics": {
"distCoeffs": {
"rows": 1,
"cols": 5,
"type": 6,

View File

@@ -19,7 +19,7 @@
1.0
]
},
"cameraExtrinsics": {
"distCoeffs": {
"rows": 1,
"cols": 5,
"type": 6,

View File

@@ -12,7 +12,7 @@
382.51346080446376, 0.0, 0.0, 1.0
]
},
"cameraExtrinsics": {
"distCoeffs": {
"rows": 1,
"cols": 5,
"type": 6,

File diff suppressed because one or more lines are too long

File diff suppressed because one or more lines are too long

File diff suppressed because one or more lines are too long

View File

@@ -19,7 +19,7 @@
1.0
]
},
"cameraExtrinsics": {
"distCoeffs": {
"rows": 1,
"cols": 5,
"type": 6,