mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
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:
2
.gitignore
vendored
2
.gitignore
vendored
@@ -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
|
||||
|
||||
134
devTools/calibrationUtils.py
Normal file
134
devTools/calibrationUtils.py
Normal 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()
|
||||
@@ -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 {
|
||||
|
||||
@@ -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>
|
||||
@@ -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"
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
@@ -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;
|
||||
}[];
|
||||
}
|
||||
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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[] {});
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
@@ -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 {
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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{"
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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"]
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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.
|
||||
*
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
1.0
|
||||
]
|
||||
},
|
||||
"cameraExtrinsics": {
|
||||
"distCoeffs": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"type": 6,
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
1.0
|
||||
]
|
||||
},
|
||||
"cameraExtrinsics": {
|
||||
"distCoeffs": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"type": 6,
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
1.0
|
||||
]
|
||||
},
|
||||
"cameraExtrinsics": {
|
||||
"distCoeffs": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"type": 6,
|
||||
|
||||
@@ -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
@@ -19,7 +19,7 @@
|
||||
1.0
|
||||
]
|
||||
},
|
||||
"cameraExtrinsics": {
|
||||
"distCoeffs": {
|
||||
"rows": 1,
|
||||
"cols": 5,
|
||||
"type": 6,
|
||||
|
||||
Reference in New Issue
Block a user