Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpical] Refactor to use WPILib libraries and modern C++ conventions and improve UX (#7796)

Browse Source 
wpical was unable to use wpimath and its dependent libraries because
Ceres was compiled with a different version of Eigen. Now that the Ceres
build has been redone and shipped in #8151, we can now use wpimath and
our C++ apriltag wrapper in wpical, allowing for major refactors. This
includes:
* Using `to_json` and `from_json` specializations to concisely serialize
and deserialize all JSON files instead of manually handling JSON.
* Removal of the `Fieldmap` and `Pose` classes, which were duplicates of
the `AprilTagFieldLayout` and `Pose3d` classes respectively.
* Using `AprilTagDetector` instead of the raw libapriltag library.
* Using `Pose3d` instead of raw Eigen matrices.

In addition, several other refactors were made to make the code more
readable and to fix several UX issues and crashes. This includes:
* Eagerly parsing every JSON file when selected by the user. This means
JSON files are only parsed once on selection, instead of every time a
downstream function wants to use the data. This also means invalid JSON
can be detected upfront and a specific error shown immediately instead
of a catch all error when trying to calibrate.
* Using `std::optional` to indicate a calibration failed instead of
status codes.
* Processing videos on separate threads to not block the UI thread and
take advantage of parallelization for camera calibration. (2x speedup on
my laptop)
* Removing the OpenCV calibration option, since mrcal should be better
in every scenario.
* Showing a progress bar for camera calibration.
* Breaking up the massive `DisplayGui` function into separate functions
which contain code for different popups. This also allowed for better
organization and scoping of static variables.
* Renaming variables to make their purpose more clear.
* Displaying the tags present in a field layout when trying to combine
multiple field layouts.

Fixes #7722.
This commit is contained in:
Gold856
2025-12-31 12:28:51 -05:00
committed by GitHub
parent 8fbaf4c2f5
commit ae84f6d2c5
14 changed files with 1334 additions and 1559 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
tools/wpical/BUILD.bazel
View File

@@ -177,6 +177,7 @@ wpilib_cc_library(
exclude = ["src/main/native/cpp/WPIcal.cpp"],
) + [
":generate-resources",
":generate-version",
],
copts = copts,
cxxopts = cxxopts,
@@ -212,6 +213,7 @@ cc_binary(
}),
deps = [
":wpical_lib",
"//glass",
"//thirdparty/imgui_suite",
],
)

6
tools/wpical/CMakeLists.txt
View File

@@ -79,7 +79,7 @@ else()
-Wno-missing-field-initializers
)
endif()
target_compile_options(wpical PRIVATE ${compile_flags})
set_target_properties(${wpical_thirdparty_src} PROPERTIES COMPILE_FLAGS ${compile_flags})
find_package(OpenCV REQUIRED)
find_package(Ceres CONFIG REQUIRED)
@@ -89,7 +89,9 @@ find_package(SuiteSparse_config CONFIG REQUIRED)
target_link_libraries(
wpical
apriltag
wpimath
${OpenCV_LIBS}
libglass
wpigui
wpiutil
SuiteSparse::CHOLMOD_static
@@ -126,7 +128,9 @@ if(WITH_TESTS)
wpical_test
googletest
apriltag
wpimath
${OpenCV_LIBS}
libglass
wpigui
wpiutil
SuiteSparse::CHOLMOD_static

6
tools/wpical/build.gradle
View File

@@ -157,10 +157,11 @@ model {
return
}
lib project: ':apriltag', library: 'apriltag', linkage: 'static'
lib project: ':glass', library: 'glass', linkage: 'static'
lib project: ':wpimath', library: 'wpimath', linkage: 'static'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'ceres')
nativeUtils.useRequiredLibrary(it, 'opencv_static')
it.cppCompiler.define 'OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT'
@@ -210,10 +211,11 @@ model {
return
}
lib project: ':apriltag', library: 'apriltag', linkage: 'static'
lib project: ':glass', library: 'glass', linkage: 'static'
lib project: ':wpimath', library: 'wpimath', linkage: 'static'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'ceres')
nativeUtils.useRequiredLibrary(it, 'opencv_static')
it.cppCompiler.define 'OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT'

1083
tools/wpical/src/main/native/cpp/WPIcal.cpp
View File

File diff suppressed because it is too large Load Diff

664
tools/wpical/src/main/native/cpp/cameracalibration.cpp
View File

@@ -4,38 +4,38 @@
#include "cameracalibration.hpp"
#include <fstream>
#include <iostream>
#include <memory>
#include <queue>
#include <string>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <mrcal_wrapper.h>
#include <opencv2/core/eigen.hpp>
#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
static bool filter(std::vector<cv::Point2f> charuco_corners,
std::vector<int> charuco_ids,
std::vector<std::vector<cv::Point2f>> marker_corners,
std::vector<int> marker_ids, int board_width,
int board_height) {
if (charuco_ids.empty() || charuco_corners.empty()) {
static bool filter(std::vector<cv::Point2f> charucoCorners,
std::vector<int> charucoIds, std::vector<int> markerIds,
int boardWidth, int boardHeight) {
if (charucoIds.empty() || charucoCorners.empty()) {
return false;
}
if (charuco_corners.size() < 10 || charuco_ids.size() < 10) {
if (charucoCorners.size() < 10 || charucoIds.size() < 10) {
return false;
}
for (int i = 0; i < charuco_ids.size(); i++) {
if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) {
for (int i = 0; i < charucoIds.size(); i++) {
if (charucoIds.at(i) > (boardWidth - 1) * (boardHeight - 1) - 1) {
return false;
}
}
for (int i = 0; i < marker_ids.size(); i++) {
if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) {
for (int i = 0; i < markerIds.size(); i++) {
if (markerIds.at(i) > ((boardWidth * boardHeight) / 2) - 1) {
return false;
}
}
@@ -43,362 +43,310 @@ static bool filter(std::vector<cv::Point2f> charuco_corners,
return true;
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
float marker_width, int board_width,
int board_height, bool show_debug_window) {
// ChArUco Board
cv::aruco::Dictionary aruco_dict =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
cv::Size(board_width, board_height), square_width * 0.0254,
marker_width * 0.0254, aruco_dict);
cv::aruco::CharucoDetector charuco_detector(*charuco_board);
// Video capture
cv::VideoCapture video_capture(input_video);
cv::Size frame_shape;
std::vector<std::vector<cv::Point2f>> all_charuco_corners;
std::vector<std::vector<int>> all_charuco_ids;
std::vector<std::vector<cv::Point3f>> all_obj_points;
std::vector<std::vector<cv::Point2f>> all_img_points;
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
cv::Mat debug_image = frame;
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
frame_shape = frame_gray.size();
std::vector<cv::Point2f> charuco_corners;
std::vector<int> charuco_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
std::vector<int> marker_ids;
std::vector<cv::Point3f> obj_points;
std::vector<cv::Point2f> img_points;
charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
marker_corners, marker_ids);
if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
board_width, board_height)) {
continue;
}
charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
img_points);
all_charuco_corners.push_back(charuco_corners);
all_charuco_ids.push_back(charuco_ids);
all_obj_points.push_back(obj_points);
all_img_points.push_back(img_points);
if (show_debug_window) {
cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
charuco_ids);
cv::imshow("Frame", debug_image);
if (cv::waitKey(1) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
// Calibrate
cv::Mat camera_matrix, dist_coeffs;
std::vector<cv::Mat> r_vecs, t_vecs;
std::vector<double> std_dev_intrinsics, std_dev_extrinsics, per_view_errors;
double repError;
try {
// see https://stackoverflow.com/a/75865177
int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU;
repError = cv::calibrateCamera(
all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs,
r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags);
} catch (...) {
std::cout << "calibration failed" << std::endl;
return 1;
}
std::vector<double> matrix = {camera_matrix.begin<double>(),
camera_matrix.end<double>()};
std::vector<double> distortion = {dist_coeffs.begin<double>(),
dist_coeffs.end<double>()};
camera_model.intrinsic_matrix = Eigen::Matrix<double, 3, 3>(matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion.data());
camera_model.avg_reprojection_error = repError;
return 0;
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
float marker_width, int board_width,
int board_height, double imagerWidthPixels,
double imagerHeightPixels,
bool show_debug_window) {
// ChArUco Board
cv::aruco::Dictionary aruco_dict =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
cv::Size(board_width, board_height), square_width * 0.0254,
marker_width * 0.0254, aruco_dict);
cv::aruco::CharucoDetector charuco_detector(*charuco_board);
// Video capture
cv::VideoCapture video_capture(input_video);
cv::Size frame_shape;
// Detection output
std::vector<mrcal_point3_t> observation_boards;
std::vector<mrcal_pose_t> frames_rt_toref;
cv::Size boardSize(board_width - 1, board_height - 1);
cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
cv::Mat debug_image = frame;
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
frame_shape = frame_gray.size();
std::vector<cv::Point2f> charuco_corners;
std::vector<int> charuco_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
std::vector<int> marker_ids;
std::vector<cv::Point3f> obj_points;
std::vector<cv::Point2f> img_points;
std::vector<mrcal_point3_t> points((board_width - 1) * (board_height - 1));
charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
marker_corners, marker_ids);
if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
board_width, board_height)) {
continue;
}
charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
img_points);
for (int i = 0; i < charuco_ids.size(); i++) {
int id = charuco_ids.at(i);
points[id].x = img_points.at(i).x;
points[id].y = img_points.at(i).y;
points[id].z = 1.0f;
}
for (int i = 0; i < points.size(); i++) {
if (points[i].z != 1.0f) {
points[i].x = -1.0f;
points[i].y = -1.0f;
points[i].z = -1.0f;
}
}
frames_rt_toref.push_back(
getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000));
observation_boards.insert(observation_boards.end(), points.begin(),
points.end());
if (show_debug_window) {
cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
charuco_ids);
cv::imshow("Frame", debug_image);
if (cv::waitKey(1) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
if (observation_boards.empty()) {
std::cout << "calibration failed" << std::endl;
return 1;
} else {
std::cout << "points detected" << std::endl;
}
std::unique_ptr<mrcal_result> cal_result =
mrcal_main(observation_boards, frames_rt_toref, boardSize,
square_width * 0.0254, imagerSize, 1000);
auto& stats = *cal_result;
namespace wpical {
CameraModel MrcalResultToCameraModel(mrcal_result& stats) {
// Camera matrix and distortion coefficients
std::vector<double> camera_matrix = {
// fx 0 cx
stats.intrinsics[0], 0, stats.intrinsics[2],
// 0 fy cy
0, stats.intrinsics[1], stats.intrinsics[3],
// 0 0 1
0, 0, 1};
std::vector<double> distortion_coefficients = {stats.intrinsics[4],
stats.intrinsics[5],
stats.intrinsics[6],
stats.intrinsics[7],
stats.intrinsics[8],
stats.intrinsics[9],
stats.intrinsics[10],
stats.intrinsics[11],
0.0,
0.0,
0.0,
0.0,
0.0,
0.0};
camera_model.intrinsic_matrix =
Eigen::Matrix<double, 3, 3>(camera_matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
camera_model.avg_reprojection_error = stats.rms_error;
return 0;
return {Eigen::Matrix<double, 3, 3, Eigen::RowMajor>{
// fx 0 cx
{stats.intrinsics[0], 0, stats.intrinsics[2]},
// 0 fy cy
{0, stats.intrinsics[1], stats.intrinsics[3]},
// 0 0 1
{0, 0, 1}},
Eigen::Matrix<double, 8, 1>{
stats.intrinsics[4], stats.intrinsics[5], stats.intrinsics[6],
stats.intrinsics[7], stats.intrinsics[8], stats.intrinsics[9],
stats.intrinsics[10], stats.intrinsics[11]},
stats.rms_error};
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
int board_width, int board_height,
double imagerWidthPixels,
double imagerHeightPixels,
bool show_debug_window) {
// Video capture
cv::VideoCapture video_capture(input_video);
/**
* Container for data that's shared between threads.
*/
class Data {
public:
/**
* Adds a frame to the queue for workers to grab from.
* @param mat The mat to queue up.
*/
void AddFrame(cv::Mat&& mat);
// Detection output
std::vector<mrcal_point3_t> observation_boards;
std::vector<mrcal_pose_t> frames_rt_toref;
/**
* Returns a frame from the queue.
* @return A frame, or std::nullopt if the queue is empty.
*/
std::optional<cv::Mat> GetFrame();
cv::Size boardSize(board_width - 1, board_height - 1);
cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
std::optional<CameraModel> cameraModel;
std::queue<cv::Mat> queue;
wpi::util::mutex mutex;
};
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
void Data::AddFrame(cv::Mat&& mat) {
std::scoped_lock lock(mutex);
queue.push(std::move(mat));
}
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
std::vector<cv::Point2f> corners;
bool found = cv::findChessboardCorners(
frame_gray, boardSize, corners,
cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
if (found) {
std::vector<mrcal_point3_t> current_points;
for (int i = 0; i < corners.size(); i++) {
current_points.push_back(
mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}});
}
frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize,
imagerSize, square_width * 0.0254,
1000));
observation_boards.insert(observation_boards.end(),
current_points.begin(), current_points.end());
}
if (show_debug_window) {
cv::drawChessboardCorners(frame, boardSize, corners, found);
cv::imshow("Checkerboard Detection", frame);
if (cv::waitKey(30) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
if (observation_boards.empty()) {
std::cout << "calibration failed" << std::endl;
return 1;
std::optional<cv::Mat> Data::GetFrame() {
std::scoped_lock lock(mutex);
if (queue.empty()) {
return std::nullopt;
} else {
std::cout << "points detected" << std::endl;
cv::Mat mat = queue.front();
queue.pop();
return mat;
}
}
class Worker {
public:
/**
* @param data A pointer to the queue of Mats.
* @param squareWidth The width of the full square in meters
* @param markerWidth The width of the marker in meters
* @param boardWidth How many markers wide the board is
* @param boardHeight How many markers tall the board is
*/
explicit Worker(std::shared_ptr<Data> data, float squareWidth,
float markerWidth, int boardWidth, int boardHeight);
~Worker();
void ProcessNextImage(cv::Mat image);
auto GetData() {
return std::make_pair(m_observationBoards, m_framesRtToref);
}
std::unique_ptr<mrcal_result> cal_result =
mrcal_main(observation_boards, frames_rt_toref, boardSize,
square_width * 0.0254, imagerSize, 1000);
int GetProcessedFrameCount();
auto& stats = *cal_result;
void Stop();
// Camera matrix and distortion coefficients
std::vector<double> camera_matrix = {
// fx 0 cx
stats.intrinsics[0], 0, stats.intrinsics[2],
// 0 fy cy
0, stats.intrinsics[1], stats.intrinsics[3],
// 0 0 1
0, 0, 1};
private:
std::atomic_bool m_isDone{false};
std::atomic_int m_framesProcessed{0};
std::function<std::optional<cv::Mat>()> m_getMat;
int m_boardWidth;
int m_boardHeight;
double m_squareWidth;
std::vector<mrcal_point3_t> m_observationBoards;
std::vector<mrcal_pose_t> m_framesRtToref;
// ChArUco Board
cv::aruco::Dictionary m_arucoDict =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
cv::aruco::CharucoBoard m_charucoBoard;
cv::aruco::CharucoDetector m_charucoDetector;
};
std::vector<double> distortion_coefficients = {stats.intrinsics[4],
stats.intrinsics[5],
stats.intrinsics[6],
stats.intrinsics[7],
stats.intrinsics[8],
stats.intrinsics[9],
stats.intrinsics[10],
stats.intrinsics[11],
0.0,
0.0,
0.0,
0.0,
0.0,
0.0};
// Save calibration output
camera_model.intrinsic_matrix =
Eigen::Matrix<double, 3, 3>(camera_matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
camera_model.avg_reprojection_error = stats.rms_error;
return 0;
Worker::Worker(std::shared_ptr<Data> data, float squareWidth, float markerWidth,
int boardWidth, int boardHeight)
: m_boardWidth(boardWidth),
m_boardHeight(boardHeight),
m_squareWidth(squareWidth * 0.0254),
m_charucoBoard(cv::Size(boardWidth, boardHeight), squareWidth * 0.0254,
markerWidth * 0.0254, m_arucoDict),
m_charucoDetector(m_charucoBoard) {
std::thread([this, data] {
while (!m_isDone) {
std::optional<cv::Mat> mat = data->GetFrame();
if (mat) {
ProcessNextImage(*mat);
++m_framesProcessed;
} else {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
}).detach();
}
Worker::~Worker() {
Stop();
}
void Worker::ProcessNextImage(cv::Mat image) {
cv::Size imageSize = image.size();
std::vector<cv::Point2f> charucoCorners;
std::vector<int> charucoIds;
std::vector<std::vector<cv::Point2f>> markerCorners;
std::vector<int> markerIds;
std::vector<cv::Point3f> objPoints;
std::vector<cv::Point2f> imgPoints;
std::vector<mrcal_point3_t> points((m_boardWidth - 1) * (m_boardHeight - 1));
m_charucoDetector.detectBoard(image, charucoCorners, charucoIds,
markerCorners, markerIds);
if (!filter(charucoCorners, charucoIds, markerIds, m_boardWidth,
m_boardHeight)) {
return;
}
m_charucoBoard.matchImagePoints(charucoCorners, charucoIds, objPoints,
imgPoints);
for (int i = 0; i < charucoIds.size(); i++) {
int id = charucoIds.at(i);
points[id].x = imgPoints.at(i).x;
points[id].y = imgPoints.at(i).y;
points[id].z = 1.0f;
}
for (int i = 0; i < points.size(); i++) {
if (points[i].z != 1.0f) {
points[i].x = -1.0f;
points[i].y = -1.0f;
points[i].z = -1.0f;
}
}
cv::Size boardSize(m_boardWidth - 1, m_boardHeight - 1);
m_framesRtToref.push_back(
getSeedPose(points.data(), boardSize, imageSize, m_squareWidth, 1000));
m_observationBoards.insert(m_observationBoards.end(), points.begin(),
points.end());
}
int Worker::GetProcessedFrameCount() {
return m_framesProcessed;
}
void Worker::Stop() {
m_isDone = true;
}
CameraCalibrator::CameraCalibrator(size_t numWorkers, double squareWidth,
double markerWidth, int boardWidth,
int boardHeight, std::string& videoPath)
: m_state(std::make_shared<Data>()) {
for (size_t i = 0; i < numWorkers; i++) {
m_workers.emplace_back(std::make_unique<Worker>(
m_state, squareWidth, markerWidth, boardWidth, boardHeight));
}
cv::VideoCapture cap{videoPath};
m_totalFrames = cap.get(cv::CAP_PROP_FRAME_COUNT);
std::thread([this, boardHeight, boardWidth, squareWidth, state = m_state,
capture = std::move(cap)]() mutable {
cv::Size frameShape{
static_cast<int>(capture.get(cv::CAP_PROP_FRAME_WIDTH)),
static_cast<int>(capture.get(cv::CAP_PROP_FRAME_HEIGHT))};
while (capture.grab() && !m_isFinished) {
cv::Mat frame;
capture.retrieve(frame);
cv::Mat frameGray;
cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY);
state->AddFrame(std::move(frameGray));
}
while (TotalFramesProcessed() < TotalFrames() && !m_isFinished) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
if (m_isFinished) {
state->cameraModel = std::nullopt;
return;
}
std::vector<mrcal_point3_t> allObservationBoards;
std::vector<mrcal_pose_t> allFramesRtToref;
for (auto& worker : m_workers) {
auto data = worker->GetData();
allObservationBoards.insert(allObservationBoards.end(),
data.first.begin(), data.first.end());
allFramesRtToref.insert(allFramesRtToref.end(), data.second.begin(),
data.second.end());
}
if (allObservationBoards.empty()) {
m_isFinished = true;
return;
}
auto result = mrcal_main(allObservationBoards, allFramesRtToref,
cv::Size(boardWidth - 1, boardHeight - 1),
squareWidth * 0.0254, frameShape, 1000);
state->cameraModel = MrcalResultToCameraModel(*result);
m_isFinished = true;
}).detach();
}
CameraCalibrator::~CameraCalibrator() {
Stop();
}
bool CameraCalibrator::IsFinished() {
return m_isFinished;
}
std::optional<CameraModel> CameraCalibrator::GetCameraModel() {
return m_state->cameraModel;
}
int CameraCalibrator::TotalFramesProcessed() {
int totalProcessedFrames = 0;
for (auto& workers : m_workers) {
totalProcessedFrames += workers->GetProcessedFrameCount();
}
return totalProcessedFrames;
}
int CameraCalibrator::TotalFrames() {
return m_totalFrames;
}
void CameraCalibrator::Stop() {
m_isFinished = true;
for (auto& workers : m_workers) {
workers->Stop();
}
}
void to_json(wpi::util::json& json, const CameraModel& cameraModel) {
std::vector<double> cameraMatrix(
cameraModel.intrinsicMatrix.data(),
cameraModel.intrinsicMatrix.data() + cameraModel.intrinsicMatrix.size());
std::vector<double> distortionCoefficients(
cameraModel.distortionCoefficients.data(),
cameraModel.distortionCoefficients.data() +
cameraModel.distortionCoefficients.size());
json = {{"camera_matrix", cameraMatrix},
{"distortion_coefficients", distortionCoefficients},
{"avg_reprojection_error", cameraModel.avgReprojectionError}};
}
void from_json(const wpi::util::json& json, CameraModel& cameraModel) {
bool isCalibdb = json.contains("camera");
Eigen::Matrix3d cameraMatrix;
std::vector<double> distortionCoeffs;
auto mat = json.at("camera_matrix");
auto distortionCoefficients = json.at("distortion_coefficients");
if (isCalibdb) {
// OpenCV format has data key
if (mat.contains("data")) {
auto data = mat.at("data").get<std::vector<double>>();
cameraMatrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>{data.data()};
} else {
for (int i = 0; i < cameraMatrix.rows(); i++) {
for (int j = 0; j < cameraMatrix.cols(); j++) {
cameraMatrix(i, j) = mat[i][j];
}
}
}
// OpenCV format has data key
if (distortionCoefficients.contains("data")) {
distortionCoeffs =
distortionCoefficients.at("data").get<std::vector<double>>();
} else {
distortionCoeffs = distortionCoefficients.get<std::vector<double>>();
}
} else {
cameraMatrix = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>{
mat.get<std::vector<double>>().data()};
distortionCoeffs = distortionCoefficients.get<std::vector<double>>();
}
// CalibDB generates JSONs with 5 values. Just zero out the remaining 3 to get
// it to 8
distortionCoeffs.resize(8, 0);
cameraModel = {cameraMatrix,
Eigen::Matrix<double, 8, 1>{distortionCoeffs.data()},
json.at("avg_reprojection_error")};
}
} // namespace wpical

572
tools/wpical/src/main/native/cpp/fieldcalibration.cpp
View File

@@ -4,11 +4,8 @@
#include "fieldcalibration.hpp"
#include <algorithm>
#include <filesystem>
#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <string>
#include <utility>
@@ -19,13 +16,19 @@
#include <ceres/ceres.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include "apriltag.h"
#include "tag36h11.h"
#include "cameracalibration.hpp"
#include "wpi/apriltag/AprilTag.hpp"
#include "wpi/apriltag/AprilTagDetector.hpp"
#include "wpi/apriltag/AprilTagDetector_cv.hpp"
#include "wpi/apriltag/AprilTagFieldLayout.hpp"
#include "wpi/math/geometry/Rotation3d.hpp"
#include "wpi/math/geometry/Translation3d.hpp"
struct Pose {
Eigen::Vector3d p;
@@ -34,9 +37,9 @@ struct Pose {
};
struct Constraint {
int id_begin;
int id_end;
Pose t_begin_end;
int idBegin;
int idEnd;
Pose tBeginEnd;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
@@ -90,401 +93,243 @@ class PoseGraphError {
const double tagSizeMeters = 0.1651;
inline cameracalibration::CameraModel load_camera_model(std::string path) {
Eigen::Matrix<double, 3, 3> camera_matrix;
Eigen::Matrix<double, 8, 1> camera_distortion;
inline Eigen::Matrix4d EstimateTagPose(std::span<double, 8> tagCorners,
const wpical::CameraModel& cameraModel,
double tagSize) {
cv::Mat cameraMatrix;
cv::Mat cameraDistortion;
std::ifstream file(path);
cv::eigen2cv(cameraModel.intrinsicMatrix, cameraMatrix);
cv::eigen2cv(cameraModel.distortionCoefficients, cameraDistortion);
wpi::util::json json_data;
try {
json_data = wpi::util::json::parse(file);
} catch (const wpi::util::json::parse_error& e) {
std::cout << e.what() << std::endl;
std::array<cv::Point2d, 4> corners;
for (int i = 0; i < 4; i++) {
corners[i] = {tagCorners[i * 2], tagCorners[i * 2 + 1]};
}
bool isCalibdb = json_data.contains("camera");
std::vector<cv::Point3f> points3dBoxBase = {
cv::Point3f(-tagSize / 2.0, tagSize / 2.0, 0.0),
cv::Point3f(tagSize / 2.0, tagSize / 2.0, 0.0),
cv::Point3f(tagSize / 2.0, -tagSize / 2.0, 0.0),
cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, 0.0)};
if (!isCalibdb) {
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
camera_matrix(i, j) =
json_data["camera_matrix"][(i * camera_matrix.cols()) + j];
}
}
std::vector<double> rVec;
std::vector<double> tVec;
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
camera_distortion(i, j) = json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
}
}
} else {
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
try {
camera_matrix(i, j) = json_data["camera_matrix"][i][j];
} catch (...) {
camera_matrix(i, j) = json_data["camera_matrix"]["data"]
[(i * camera_matrix.cols()) + j];
}
}
}
cv::solvePnP(points3dBoxBase, corners, cameraMatrix, cameraDistortion, rVec,
tVec, false, cv::SOLVEPNP_SQPNP);
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
try {
camera_distortion(i, j) =
json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
} catch (...) {
camera_distortion(i, j) = 0.0;
}
}
}
}
cv::Mat rMat;
cv::Rodrigues(rVec, rMat);
cameracalibration::CameraModel camera_model{
camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
return camera_model;
}
inline cameracalibration::CameraModel load_camera_model(
wpi::util::json json_data) {
// Camera matrix
Eigen::Matrix<double, 3, 3> camera_matrix;
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
camera_matrix(i, j) =
json_data["camera_matrix"][(i * camera_matrix.cols()) + j];
}
}
// Distortion coefficients
Eigen::Matrix<double, 8, 1> camera_distortion;
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
camera_distortion(i, j) = json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
}
}
cameracalibration::CameraModel camera_model{
camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
return camera_model;
}
inline std::map<int, wpi::util::json> load_ideal_map(std::string path) {
std::ifstream file(path);
wpi::util::json json_data = wpi::util::json::parse(file);
std::map<int, wpi::util::json> ideal_map;
for (const auto& element : json_data["tags"]) {
ideal_map[element["ID"]] = element;
}
return ideal_map;
}
Eigen::Matrix<double, 4, 4> get_tag_transform(
std::map<int, wpi::util::json>& ideal_map, int tag_id) {
Eigen::Matrix<double, 4, 4> transform =
Eigen::Matrix<double, 4, 4>::Identity();
transform.block<3, 3>(0, 0) =
Eigen::Quaternion<double>(
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"])
.toRotationMatrix();
transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"];
transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"];
transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"];
return transform;
}
inline Eigen::Matrix<double, 4, 4> estimate_tag_pose(
apriltag_detection_t* tag_detection,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size) {
cv::Mat camera_matrix_cv;
cv::Mat camera_distortion_cv;
cv::eigen2cv(camera_matrix, camera_matrix_cv);
cv::eigen2cv(camera_distortion, camera_distortion_cv);
std::vector<cv::Point2f> points_2d = {
cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]),
cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]),
cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]),
cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])};
std::vector<cv::Point3f> points_3d_box_base = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
std::vector<double> r_vec;
std::vector<double> t_vec;
cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv,
camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP);
cv::Mat r_mat;
cv::Rodrigues(r_vec, r_mat);
Eigen::Matrix<double, 4, 4> camera_to_tag{
{r_mat.at<double>(0, 0), r_mat.at<double>(0, 1), r_mat.at<double>(0, 2),
t_vec.at(0)},
{r_mat.at<double>(1, 0), r_mat.at<double>(1, 1), r_mat.at<double>(1, 2),
t_vec.at(1)},
{r_mat.at<double>(2, 0), r_mat.at<double>(2, 1), r_mat.at<double>(2, 2),
t_vec.at(2)},
Eigen::Matrix<double, 4, 4> cameraToTag{
{rMat.at<double>(0, 0), rMat.at<double>(0, 1), rMat.at<double>(0, 2),
tVec.at(0)},
{rMat.at<double>(1, 0), rMat.at<double>(1, 1), rMat.at<double>(1, 2),
tVec.at(1)},
{rMat.at<double>(2, 0), rMat.at<double>(2, 1), rMat.at<double>(2, 2),
tVec.at(2)},
{0.0, 0.0, 0.0, 1.0}};
return camera_to_tag;
return cameraToTag;
}
inline void draw_tag_cube(cv::Mat& frame,
Eigen::Matrix<double, 4, 4> camera_to_tag,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion,
double tag_size) {
cv::Mat camera_matrix_cv;
cv::Mat camera_distortion_cv;
inline void DrawTagCube(cv::Mat& frame, Eigen::Matrix4d cameraToTag,
const wpical::CameraModel& cameraModel,
double tagSize) {
cv::Mat cameraMatrix;
cv::Mat cameraDistortion;
cv::eigen2cv(camera_matrix, camera_matrix_cv);
cv::eigen2cv(camera_distortion, camera_distortion_cv);
cv::eigen2cv(cameraModel.intrinsicMatrix, cameraMatrix);
cv::eigen2cv(cameraModel.distortionCoefficients, cameraDistortion);
std::vector<cv::Point3f> points_3d_box_base = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
std::vector<cv::Point3f> points3dBoxBase = {
cv::Point3f(-tagSize / 2.0, tagSize / 2.0, 0.0),
cv::Point3f(tagSize / 2.0, tagSize / 2.0, 0.0),
cv::Point3f(tagSize / 2.0, -tagSize / 2.0, 0.0),
cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, 0.0)};
std::vector<cv::Point3f> points_3d_box_top = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)};
std::vector<cv::Point3f> points3dBoxTop = {
cv::Point3f(-tagSize / 2.0, tagSize / 2.0, -tagSize),
cv::Point3f(tagSize / 2.0, tagSize / 2.0, -tagSize),
cv::Point3f(tagSize / 2.0, -tagSize / 2.0, -tagSize),
cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, -tagSize)};
std::vector<cv::Point2f> points_2d_box_base = {
std::vector<cv::Point2f> points2dBoxBase = {
cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
cv::Point2f(0.0, 0.0)};
std::vector<cv::Point2f> points_2d_box_top = {
std::vector<cv::Point2f> points2dBoxTop = {
cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
cv::Point2f(0.0, 0.0)};
Eigen::Matrix<double, 3, 3> r_vec = camera_to_tag.block<3, 3>(0, 0);
Eigen::Matrix<double, 3, 1> t_vec = camera_to_tag.block<3, 1>(0, 3);
Eigen::Matrix<double, 3, 3> rVec = cameraToTag.block<3, 3>(0, 0);
Eigen::Matrix<double, 3, 1> tVec = cameraToTag.block<3, 1>(0, 3);
cv::Mat r_vec_cv;
cv::Mat t_vec_cv;
cv::Mat rVecCv;
cv::Mat tVecCv;
cv::eigen2cv(r_vec, r_vec_cv);
cv::eigen2cv(t_vec, t_vec_cv);
cv::eigen2cv(rVec, rVecCv);
cv::eigen2cv(tVec, tVecCv);
cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv,
camera_distortion_cv, points_2d_box_base);
cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv,
camera_distortion_cv, points_2d_box_top);
cv::projectPoints(points3dBoxBase, rVecCv, tVecCv, cameraMatrix,
cameraDistortion, points2dBoxBase);
cv::projectPoints(points3dBoxTop, rVecCv, tVecCv, cameraMatrix,
cameraDistortion, points2dBoxTop);
for (int i = 0; i < 4; i++) {
cv::Point2f& point_base = points_2d_box_base.at(i);
cv::Point2f& point_top = points_2d_box_top.at(i);
cv::Point2f& pointBase = points2dBoxBase.at(i);
cv::Point2f& pointTop = points2dBoxTop.at(i);
cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5);
cv::line(frame, pointBase, pointTop, cv::Scalar(0, 255, 255), 5);
int i_next = (i + 1) % 4;
cv::Point2f& point_base_next = points_2d_box_base.at(i_next);
cv::Point2f& point_top_next = points_2d_box_top.at(i_next);
int next = (i + 1) % 4;
cv::Point2f& pointBaseNext = points2dBoxBase.at(next);
cv::Point2f& pointTopNext = points2dBoxTop.at(next);
cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5);
cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5);
cv::line(frame, pointBase, pointBaseNext, cv::Scalar(0, 255, 255), 5);
cv::line(frame, pointTop, pointTopNext, cv::Scalar(0, 255, 255), 5);
}
}
inline bool process_video_file(
apriltag_detector_t* tag_detector,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size,
inline bool ProcessVideoFile(
wpi::apriltag::AprilTagDetector& detector,
const wpical::CameraModel& cameraModel, double tagSize,
const std::string& path,
std::map<int, Pose, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose>>>& poses,
std::vector<Constraint, Eigen::aligned_allocator<Constraint>>& constraints,
bool show_debug_window) {
if (show_debug_window) {
bool showDebugWindow) {
if (showDebugWindow) {
cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL);
}
cv::VideoCapture video_input(path);
cv::VideoCapture videoInput(path);
if (!video_input.isOpened()) {
std::cout << "Unable to open video " << path << std::endl;
if (!videoInput.isOpened()) {
return false;
}
cv::Mat frame;
cv::Mat frame_gray;
cv::Mat frame_debug;
int frame_num = 0;
while (video_input.read(frame)) {
std::cout << "Processing " << path << " - Frame " << frame_num++
<< std::endl;
cv::Mat frameGray;
cv::Mat frameDebug;
while (videoInput.read(frame)) {
// Convert color frame to grayscale frame
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY);
// Clone color frame for debugging
frame_debug = frame.clone();
frameDebug = frame.clone();
// Detect tags
image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols,
frame_gray.data};
zarray_t* tag_detections =
apriltag_detector_detect(tag_detector, &tag_image);
auto results = wpi::apriltag::AprilTagDetect(detector, frameGray);
// Skip this loop if there are no tags detected
if (zarray_size(tag_detections) == 0) {
std::cout << "No tags detected" << std::endl;
if (results.empty()) {
continue;
}
// Find detection with the smallest tag ID
apriltag_detection_t* tag_detection_min = nullptr;
zarray_get(tag_detections, 0, &tag_detection_min);
for (int i = 0; i < zarray_size(tag_detections); i++) {
apriltag_detection_t* tag_detection_i;
zarray_get(tag_detections, i, &tag_detection_i);
if (tag_detection_i->id < tag_detection_min->id) {
tag_detection_min = tag_detection_i;
auto tagDetectionMin = results.front();
for (auto detection : results) {
if (detection->GetId() < tagDetectionMin->GetId()) {
tagDetectionMin = detection;
}
}
Eigen::Matrix<double, 4, 4> camera_to_tag_min = estimate_tag_pose(
tag_detection_min, camera_matrix, camera_distortion, tag_size);
std::array<double, 8> cornerBuf;
Eigen::Matrix4d cameraToTagMin;
try {
cameraToTagMin = EstimateTagPose(tagDetectionMin->GetCorners(cornerBuf),
cameraModel, tagSize);
} catch (...) {
// SQPNP failed, probably because the camera model is bad
return false;
}
// Find transformation from smallest tag ID
for (int i = 0; i < zarray_size(tag_detections); i++) {
apriltag_detection_t* tag_detection_i;
zarray_get(tag_detections, i, &tag_detection_i);
for (auto detection : results) {
// Add tag ID to poses
if (poses.find(tag_detection_i->id) == poses.end()) {
poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0),
Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
if (poses.find(detection->GetId()) == poses.end()) {
poses[detection->GetId()] = {Eigen::Vector3d(0.0, 0.0, 0.0),
Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
}
std::array<double, 8> corners;
// Estimate camera to tag pose
Eigen::Matrix<double, 4, 4> caamera_to_tag = estimate_tag_pose(
tag_detection_i, camera_matrix, camera_distortion, tag_size);
Eigen::Matrix4d cameraToTag;
try {
cameraToTag = EstimateTagPose(detection->GetCorners(corners),
cameraModel, tagSize);
} catch (...) {
// SQPNP failed, probably because the camera model is bad
return false;
}
// Draw debug cube
if (show_debug_window) {
draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix,
camera_distortion, tag_size);
if (showDebugWindow) {
DrawTagCube(frameDebug, cameraToTag, cameraModel, tagSize);
}
// Skip finding transformation from smallest tag ID to itself
if (tag_detection_i->id == tag_detection_min->id) {
if (detection->GetId() == tagDetectionMin->GetId()) {
continue;
}
Eigen::Matrix<double, 4, 4> tag_min_to_tag =
camera_to_tag_min.inverse() * caamera_to_tag;
Eigen::Matrix4d tagMinToTag = cameraToTagMin.inverse() * cameraToTag;
// Constraint
Constraint constraint;
constraint.id_begin = tag_detection_min->id;
constraint.id_end = tag_detection_i->id;
constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3);
constraint.t_begin_end.q =
Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0));
constraint.idBegin = tagDetectionMin->GetId();
constraint.idEnd = detection->GetId();
constraint.tBeginEnd.p = tagMinToTag.block<3, 1>(0, 3);
constraint.tBeginEnd.q =
Eigen::Quaterniond(tagMinToTag.block<3, 3>(0, 0));
constraints.push_back(constraint);
}
apriltag_detections_destroy(tag_detections);
// Show debug
if (show_debug_window) {
cv::imshow("Processing Frame", frame_debug);
if (showDebugWindow) {
cv::imshow("Processing Frame", frameDebug);
cv::waitKey(1);
}
}
video_input.release();
if (show_debug_window) {
videoInput.release();
if (showDebugWindow) {
cv::destroyAllWindows();
}
return true;
}
int fieldcalibration::calibrate(std::string input_dir_path,
wpi::util::json& output_json,
std::string camera_model_path,
std::string ideal_map_path, int pinned_tag_id,
bool show_debug_window) {
wpical::FieldCalibrator::~FieldCalibrator() {
if (m_processingThread.joinable()) {
m_processingThread.detach();
}
}
std::optional<wpi::apriltag::AprilTagFieldLayout> wpical::calibrate(
std::string inputDirPath, wpical::CameraModel& cameraModel,
const wpi::apriltag::AprilTagFieldLayout& idealLayout, int pinnedTagId,
bool showDebugWindow) {
// Silence OpenCV logging
cv::utils::logging::setLogLevel(
cv::utils::logging::LogLevel::LOG_LEVEL_SILENT);
// Load camera model
Eigen::Matrix3d camera_matrix;
Eigen::Matrix<double, 8, 1> camera_distortion;
try {
auto camera_model = load_camera_model(camera_model_path);
camera_matrix = camera_model.intrinsic_matrix;
camera_distortion = camera_model.distortion_coefficients;
} catch (...) {
return 1;
}
wpi::util::json json = wpi::util::json::parse(std::ifstream(ideal_map_path));
if (!json.contains("tags")) {
return 1;
}
// Load ideal field map
std::map<int, wpi::util::json> ideal_map;
try {
ideal_map = load_ideal_map(ideal_map_path);
} catch (...) {
return 1;
}
bool pinned_tag_found = false;
// Check if pinned tag is in ideal map
for (const auto& [tag_id, tag_json] : ideal_map) {
if (tag_id == pinned_tag_id) {
pinned_tag_found = true;
bool pinnedTagFound = false;
// Check if pinned tag is in ideal layout
for (const auto& tag : idealLayout.GetTags()) {
if (tag.ID == pinnedTagId) {
pinnedTagFound = true;
break;
}
}
if (!pinned_tag_found) {
return 1;
if (!pinnedTagFound) {
return std::nullopt;
}
// Apriltag detector
apriltag_detector_t* tag_detector = apriltag_detector_create();
tag_detector->nthreads = 8;
apriltag_family_t* tag_family = tag36h11_create();
apriltag_detector_add_family(tag_detector, tag_family);
wpi::apriltag::AprilTagDetector detector;
detector.SetConfig({.numThreads = 8});
detector.AddFamily("tag36h11");
// Find tag poses
std::map<int, Pose, std::less<int>,
@@ -492,53 +337,44 @@ int fieldcalibration::calibrate(std::string input_dir_path,
poses;
std::vector<Constraint, Eigen::aligned_allocator<Constraint>> constraints;
for (const auto& entry :
std::filesystem::directory_iterator(input_dir_path)) {
for (const auto& entry : std::filesystem::directory_iterator(inputDirPath)) {
if (entry.path().filename().string()[0] == '.') {
continue;
}
const std::string path = entry.path().string();
bool success = process_video_file(tag_detector, camera_matrix,
camera_distortion, tagSizeMeters, path,
poses, constraints, show_debug_window);
if (!success) {
std::cout << "Unable to process video " << path << std::endl;
return 1;
if (!ProcessVideoFile(detector, cameraModel, tagSizeMeters,
entry.path().string(), poses, constraints,
showDebugWindow)) {
return std::nullopt;
}
}
// Build optimization problem
ceres::Problem problem;
ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold;
ceres::Manifold* quaternionManifold = new ceres::EigenQuaternionManifold;
for (const auto& constraint : constraints) {
auto pose_begin_iter = poses.find(constraint.id_begin);
auto pose_end_iter = poses.find(constraint.id_end);
auto poseBeginIter = poses.find(constraint.idBegin);
auto poseEndIter = poses.find(constraint.idEnd);
ceres::CostFunction* cost_function =
PoseGraphError::Create(constraint.t_begin_end);
ceres::CostFunction* costFunction =
PoseGraphError::Create(constraint.tBeginEnd);
problem.AddResidualBlock(cost_function, nullptr,
pose_begin_iter->second.p.data(),
pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(),
pose_end_iter->second.q.coeffs().data());
problem.AddResidualBlock(
costFunction, nullptr, poseBeginIter->second.p.data(),
poseBeginIter->second.q.coeffs().data(), poseEndIter->second.p.data(),
poseEndIter->second.q.coeffs().data());
problem.SetManifold(pose_begin_iter->second.q.coeffs().data(),
quaternion_manifold);
problem.SetManifold(pose_end_iter->second.q.coeffs().data(),
quaternion_manifold);
problem.SetManifold(poseBeginIter->second.q.coeffs().data(),
quaternionManifold);
problem.SetManifold(poseEndIter->second.q.coeffs().data(),
quaternionManifold);
}
// Pin tag
auto pinned_tag_iter = poses.find(pinned_tag_id);
if (pinned_tag_iter != poses.end()) {
problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data());
problem.SetParameterBlockConstant(
pinned_tag_iter->second.q.coeffs().data());
auto pinnedTagIter = poses.find(pinnedTagId);
if (pinnedTagIter != poses.end()) {
problem.SetParameterBlockConstant(pinnedTagIter->second.p.data());
problem.SetParameterBlockConstant(pinnedTagIter->second.q.coeffs().data());
}
// Solve
@@ -550,62 +386,30 @@ int fieldcalibration::calibrate(std::string input_dir_path,
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << std::endl;
Eigen::Matrix4d correctionA;
correctionA << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
// Output
std::map<int, wpi::util::json> observed_map = ideal_map;
Eigen::Matrix4d correctionB;
correctionB << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1;
Eigen::Matrix<double, 4, 4> correction_a;
correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
Eigen::Matrix4d pinnedTagTransform =
idealLayout.GetTagPose(pinnedTagId)->ToMatrix();
Eigen::Matrix<double, 4, 4> correction_b;
correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1;
Eigen::Matrix<double, 4, 4> pinned_tag_transform =
get_tag_transform(ideal_map, pinned_tag_id);
for (const auto& [tag_id, pose] : poses) {
std::vector<wpi::apriltag::AprilTag> tags;
for (const auto& [tagId, pose] : poses) {
// Transformation from pinned tag
Eigen::Matrix<double, 4, 4> transform =
Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix4d transform = Eigen::Matrix4d::Identity();
transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix();
transform.block<3, 1>(0, 3) = pose.p;
// Transformation from world
Eigen::Matrix<double, 4, 4> corrected_transform =
pinned_tag_transform * correction_a * transform * correction_b;
Eigen::Quaternion<double> corrected_transform_q(
corrected_transform.block<3, 3>(0, 0));
observed_map[tag_id]["pose"]["translation"]["x"] =
corrected_transform(0, 3);
observed_map[tag_id]["pose"]["translation"]["y"] =
corrected_transform(1, 3);
observed_map[tag_id]["pose"]["translation"]["z"] =
corrected_transform(2, 3);
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] =
corrected_transform_q.x();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] =
corrected_transform_q.y();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] =
corrected_transform_q.z();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] =
corrected_transform_q.w();
Eigen::Matrix4d correctedTransform =
pinnedTagTransform * correctionA * transform * correctionB;
// TODO: remove variable when clang 16 is available on Mac
wpi::apriltag::AprilTag tag{tagId, wpi::math::Pose3d{correctedTransform}};
tags.emplace_back(tag);
}
wpi::util::json observed_map_json;
for (const auto& [tag_id, tag_json] : observed_map) {
observed_map_json["tags"].push_back(tag_json);
}
observed_map_json["field"] = {
{"length", static_cast<double>(json.at("field").at("length"))},
{"width", static_cast<double>(json.at("field").at("width"))}};
output_json = observed_map_json;
return 0;
return wpi::apriltag::AprilTagFieldLayout{tags, idealLayout.GetFieldLength(),
idealLayout.GetFieldWidth()};
}

67
tools/wpical/src/main/native/cpp/fmap.cpp
View File

@@ -5,36 +5,49 @@
#include "fmap.hpp"
#include <string>
#include <utility>
#include <vector>
wpi::util::json fmap::singleTag(int tag, const tag::Pose& tagpose) {
std::vector<double> transform = {};
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
transform.push_back(tagpose.transformMatrixFmap(i, j));
}
}
#include <Eigen/Core>
return {{"family", "apriltag3_36h11_classic"},
{"id", tag},
{"size", 165.1},
{"transform", transform},
{"unique", true}};
#include "wpi/apriltag/AprilTagFieldLayout.hpp"
using namespace fmap;
Fieldmap::Fieldmap(std::string type, std::vector<Fiducial> fiducials)
: type(std::move(type)), fiducials(std::move(fiducials)) {}
Fieldmap::Fieldmap(const wpi::apriltag::AprilTagFieldLayout& layout)
: type("frc") {
// https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-map-specification
for (auto tag : layout.GetTags()) {
// TODO: remove variable when clang 16 is available on Mac
Fiducial fiducial{"apriltag3_36h11_classic", tag.ID, 165.1,
tag.pose.ToMatrix(), 1};
fiducials.emplace_back(fiducial);
}
}
wpi::util::json fmap::convertfmap(const wpi::util::json& json) {
std::string fmapstart = "{\"fiducials\":[";
std::string fmapend = "],\"type\":\"frc\"}";
Fieldmap fieldmap(json);
for (int i = 0; i < fieldmap.getNumTags(); i++) {
fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump();
if (i != fieldmap.getNumTags() - 1) {
fmapstart += ",";
}
}
return wpi::util::json::parse(fmapstart.append(fmapend));
void fmap::to_json(wpi::util::json& json, const Fiducial& layout) {
json = {{"family", layout.family},
{"id", layout.id},
{"size", layout.size},
{"transform", std::vector<double>{layout.transform.data(),
layout.transform.data() +
layout.transform.size()}},
{"unique", layout.unique}};
}
void fmap::from_json(const wpi::util::json& json, Fiducial& layout) {
auto vec = json.at("transform").get<std::vector<double>>();
layout = {json.at("family"), json.at("id"), json.at("size"),
Eigen::Matrix4d{vec.data()}, json.at("unique")};
}
void fmap::to_json(wpi::util::json& json, const Fieldmap& layout) {
json = {{"type", "frc"}, {"fiducials", layout.fiducials}};
}
void fmap::from_json(const wpi::util::json& json, Fieldmap& layout) {
layout = {json.at("type"), json.at("fiducials")};
}

46
tools/wpical/src/main/native/cpp/tagpose.cpp
View File

@@ -1,46 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "tagpose.hpp"
#include "wpi/util/deprecated.hpp"
WPI_IGNORE_DEPRECATED
namespace tag {
Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w,
double x, double y, double z, double field_length_meters,
double field_width_meters) {
tagId = tag_id;
xPos = xpos;
yPos = ypos;
zPos = zpos;
quaternion = Eigen::Quaterniond(w, x, y, z);
rotationMatrix = quaternion.toRotationMatrix();
transformMatrixFmap.setZero();
transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix;
transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0);
transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0);
transformMatrixFmap(2, 3) = zpos;
transformMatrixFmap(3, 3) = 1;
transformMatrixFmap(3, 0) = 0;
transformMatrixFmap(3, 1) = 0;
transformMatrixFmap(3, 2) = 0;
Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
rollRot = eulerAngles[0];
pitchRot = eulerAngles[1];
yawRot = eulerAngles[2];
}
wpi::util::json Pose::toJson() {
return {{"ID", tagId},
{"pose",
{{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}},
{"rotation",
{{"quaternion",
{{"W", quaternion.w()},
{"X", quaternion.x()},
{"Y", quaternion.y()},
{"Z", quaternion.z()}}}}}}}};
}
} // namespace tag

116
tools/wpical/src/main/native/include/cameracalibration.hpp
View File

@@ -4,49 +4,97 @@
#pragma once
#include <fstream>
#include <memory>
#include <optional>
#include <string>
#include <thread>
#include <vector>
#include <Eigen/Core>
#include <mrcal_wrapper.h>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/objdetect/charuco_detector.hpp>
#include <opencv2/videoio.hpp>
#include "wpi/util/json.hpp"
#include "wpi/util/mutex.hpp"
namespace cameracalibration {
namespace wpical {
struct CameraModel {
Eigen::Matrix<double, 3, 3> intrinsic_matrix;
Eigen::Matrix<double, 8, 1> distortion_coefficients;
double avg_reprojection_error;
Eigen::Matrix<double, 3, 3, Eigen::RowMajor> intrinsicMatrix;
Eigen::Matrix<double, 8, 1> distortionCoefficients;
double avgReprojectionError = -1;
};
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, float marker_width, int board_width,
int board_height, bool show_debug_window);
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, float marker_width, int board_width,
int board_height, double imagerWidthPixels,
double imagerHeightPixels, bool show_debug_window);
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, int board_width, int board_height,
double imagerWidthPixels, double imagerHeightPixels,
bool show_debug_window);
static void dumpJson(CameraModel& camera_model,
const std::string& output_file_path) {
std::vector<double> camera_matrix(camera_model.intrinsic_matrix.data(),
camera_model.intrinsic_matrix.data() +
camera_model.intrinsic_matrix.size());
std::vector<double> distortion_coefficients(
camera_model.distortion_coefficients.data(),
camera_model.distortion_coefficients.data() +
camera_model.distortion_coefficients.size());
CameraModel MrcalResultToCameraModel(mrcal_result& stats);
wpi::util::json result = {
{"camera_matrix", camera_matrix},
{"distortion_coefficients", distortion_coefficients},
{"avg_reprojection_error", camera_model.avg_reprojection_error}};
class Data;
class Worker;
std::ofstream output_file(output_file_path);
output_file << result.dump(4) << std::endl;
output_file.close();
}
} // namespace cameracalibration
class CameraCalibrator {
public:
/**
* Opens the video at the path, and performs camera calibration using the
* frames in the video.
*
* @param numWorkers The number of threads to spawn.
* @param squareWidth The width of the full square in meters.
* @param markerWidth The width of the marker in meters.
* @param boardWidth How many markers wide the board is.
* @param boardHeight How many markers tall the board is.
* @param videoPath The absolute path to the video.
*/
CameraCalibrator(size_t numWorkers, double squareWidth, double markerWidth,
int boardWidth, int boardHeight, std::string& videoPath);
~CameraCalibrator();
/**
* Returns whether or not all frames have been processed or whether or not the
* camera calibrator has been stopped.
* @return true if all frames have been processed or if the camera calibrator
* has been stopped, false otherwise.
*/
bool IsFinished();
/**
* Gets the camera model.
* @return std::nullopt if the camera calibrator is not finished processing or
* if calibration failed. Returns CameraModel if it's finished and calibration
* was successful. If calibration is successful, there will always be a valid
* CameraModel before IsFinished returns true.
*/
std::optional<CameraModel> GetCameraModel();
/**
* Returns the total number of frames that have been processed.
* @return The total number of processed frames.
*/
int TotalFramesProcessed();
/**
* Returns the total number of frames in the video.
* @return The total number of frames.
*/
int TotalFrames();
/**
* Stops all worker threads.
*/
void Stop();
private:
// Ensures that shared state lives until everything else is destroyed
std::shared_ptr<Data> m_state;
std::atomic_bool m_isFinished{false};
std::atomic_int m_totalFrames;
std::vector<std::shared_ptr<Worker>> m_workers;
};
void to_json(wpi::util::json& json, const CameraModel& cameraModel);
void from_json(const wpi::util::json& json, CameraModel& cameraModel);
} // namespace wpical

41
tools/wpical/src/main/native/include/fieldcalibration.hpp
View File

@@ -4,13 +4,42 @@
#pragma once
#include <optional>
#include <string>
#include <thread>
#include "cameracalibration.hpp"
#include "wpi/util/json.hpp"
#include "wpi/apriltag/AprilTagFieldLayout.hpp"
namespace fieldcalibration {
int calibrate(std::string input_dir_path, wpi::util::json& output_json,
std::string camera_model_path, std::string ideal_map_path,
int pinned_tag_id, bool show_debug_window);
} // namespace fieldcalibration
namespace wpical {
std::optional<wpi::apriltag::AprilTagFieldLayout> calibrate(
std::string inputDirPath, wpical::CameraModel& cameraModel,
const wpi::apriltag::AprilTagFieldLayout& idealLayout, int pinnedTagId,
bool showDebugWindow);
class FieldCalibrator {
public:
~FieldCalibrator();
bool IsFinished() { return m_isFinished; }
std::optional<wpi::apriltag::AprilTagFieldLayout> GetAprilTagFieldLayout() {
return m_fieldLayout;
}
void Calibrate(std::string inputDirPath, wpical::CameraModel& cameraModel,
const wpi::apriltag::AprilTagFieldLayout& idealLayout,
int pinnedTagId, bool showDebugWindow) {
m_processingThread = std::thread([=, this]() mutable {
this->m_fieldLayout = calibrate(inputDirPath, cameraModel, idealLayout,
pinnedTagId, showDebugWindow);
this->m_isFinished = true;
});
}
private:
std::atomic_bool m_isFinished{false};
std::thread m_processingThread;
std::optional<wpi::apriltag::AprilTagFieldLayout> m_fieldLayout;
};
} // namespace wpical

82
tools/wpical/src/main/native/include/fieldmap.hpp
View File

@@ -1,82 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <cmath>
#include <map>
#include "tagpose.hpp"
#include "wpi/util/json.hpp"
class Fieldmap {
public:
Fieldmap() = default;
explicit Fieldmap(const wpi::util::json& json) {
double field_length_meters =
static_cast<double>(json.at("field").at("length"));
double field_width_meters =
static_cast<double>(json.at("field").at("width"));
for (const auto& tag : json.at("tags").items()) {
double tag_id = static_cast<int>(tag.value().at("ID"));
double tagXPos =
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
double tagYPos =
static_cast<double>(tag.value().at("pose").at("translation").at("y"));
double tagZPos =
static_cast<double>(tag.value().at("pose").at("translation").at("z"));
double tagWQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("W"));
double tagXQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("X"));
double tagYQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Y"));
double tagZQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
tagMap.emplace(
tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
tagXQuat, tagYQuat, tagZQuat, field_length_meters,
field_width_meters));
}
fieldLength = field_length_meters;
fieldWidth = field_width_meters;
}
const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
int getNumTags() const { return tagMap.size(); }
bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
wpi::util::json toJson() {
wpi::util::json json;
for (auto& [key, val] : tagMap) {
json["tags"].push_back(val.toJson());
}
json["field"]["length"] = fieldLength;
json["field"]["width"] = fieldWidth;
return json;
}
static double minimizeAngle(double angle) {
angle = std::fmod(angle, 360);
if (angle > 180) {
return angle - 360;
} else if (angle < -180) {
return angle + 360;
}
return angle;
}
void replaceTag(int tag_id, tag::Pose pose) {
tagMap.erase(tag_id);
tagMap.emplace(tag_id, pose);
}
private:
double fieldLength;
double fieldWidth;
std::map<int, tag::Pose> tagMap;
};

36
tools/wpical/src/main/native/include/fmap.hpp
View File

@@ -4,11 +4,39 @@
#pragma once
#include "fieldmap.hpp"
#include "tagpose.hpp"
#include <string>
#include <vector>
#include <Eigen/Core>
#include "wpi/apriltag/AprilTagFieldLayout.hpp"
#include "wpi/util/json.hpp"
namespace fmap {
wpi::util::json singleTag(int tag, const tag::Pose& tagpose);
wpi::util::json convertfmap(const wpi::util::json& json);
struct Fiducial {
std::string family;
int id;
double size;
Eigen::Matrix4d transform;
int unique = 1;
};
class Fieldmap {
public:
Fieldmap() = default;
Fieldmap(std::string type, std::vector<Fiducial> fiducials);
explicit Fieldmap(const wpi::apriltag::AprilTagFieldLayout& layout);
std::string type;
std::vector<Fiducial> fiducials;
};
void to_json(wpi::util::json& json, const Fiducial& layout);
void from_json(const wpi::util::json& json, Fiducial& layout);
void to_json(wpi::util::json& json, const Fieldmap& layout);
void from_json(const wpi::util::json& json, Fieldmap& layout);
} // namespace fmap

25
tools/wpical/src/main/native/include/tagpose.hpp
View File

@@ -1,25 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "wpi/util/json.hpp"
namespace tag {
class Pose {
public:
Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
double y, double z, double field_length_meters,
double field_width_meters);
int tagId;
double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
Eigen::Quaterniond quaternion;
Eigen::Matrix3d rotationMatrix;
Eigen::Matrix4d transformMatrixFmap;
wpi::util::json toJson();
};
} // namespace tag

147
tools/wpical/src/test/native/cpp/test_calibrate.cpp
View File

@@ -2,6 +2,8 @@
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <fstream>
#include <optional>
#include <string>
#include <gtest/gtest.h>
@@ -9,6 +11,8 @@
#include "cameracalibration.hpp"
#include "fieldcalibration.hpp"
#include "path_lookup.hpp"
#include "wpi/apriltag/AprilTagFieldLayout.hpp"
#include "wpi/apriltag/AprilTagFields.hpp"
#include "wpi/util/json.hpp"
const std::string projectRootPath = PROJECT_ROOT_PATH;
@@ -21,12 +25,6 @@ const std::string calSavePath =
0, projectRootPath.find("/src/main/native/assets")) +
"/build")
: std::string(tmpdir_c_str);
cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
.distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
.avg_reprojection_error = 0.0};
wpi::util::json output_json;
#ifdef __linux__
const std::string fileSuffix = ".avi";
@@ -36,80 +34,93 @@ const std::string fileSuffix = ".mp4";
const std::string videoLocation = "/fieldvideo";
#endif
TEST(Camera_CalibrationTest, OpenCV_Typical) {
int ret = cameracalibration::calibrate(
LookupPath(projectRootPath + "/testcalibration" + fileSuffix),
cameraModel, 0.709f, 0.551f, 12, 8, false);
cameracalibration::dumpJson(cameraModel,
calSavePath + "/cameracalibration.json");
EXPECT_EQ(ret, 0);
TEST(CameraCalibrationTest, Typical) {
auto path = LookupPath(projectRootPath + "/testcalibration" + fileSuffix);
auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path);
while (!calibrator.IsFinished()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
auto ret = calibrator.GetCameraModel();
EXPECT_NE(ret, std::nullopt);
std::ofstream output_file(calSavePath + "/cameracalibration.json");
output_file << wpi::util::json(ret.value()).dump(4) << std::endl;
}
TEST(Camera_CalibrationTest, OpenCV_Atypical) {
int ret = cameracalibration::calibrate(
LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix),
cameraModel, 0.709f, 0.551f, 12, 8, false);
EXPECT_EQ(ret, 1);
TEST(CameraCalibrationTest, Atypical) {
auto path =
LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix);
auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path);
while (!calibrator.IsFinished()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
auto ret = calibrator.GetCameraModel();
EXPECT_EQ(ret, std::nullopt);
}
TEST(Camera_CalibrationTest, MRcal_Typical) {
int ret = cameracalibration::calibrate(
LookupPath(projectRootPath + "/testcalibration" + fileSuffix),
cameraModel, .709f, 12, 8, 1080, 1920, false);
EXPECT_EQ(ret, 0);
TEST(FieldCalibrationTest, Typical) {
auto model = wpi::util::json::parse(
std::ifstream(calSavePath + "/cameracalibration.json"))
.get<wpical::CameraModel>();
auto ret =
wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
3, false);
EXPECT_NE(ret, std::nullopt);
}
TEST(Camera_CalibrationTest, MRcal_Atypical) {
int ret = cameracalibration::calibrate(
LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix),
cameraModel, 0.709f, 12, 8, 1080, 1920, false);
EXPECT_EQ(ret, 1);
TEST(FieldCalibrationTest, Atypical_Bad_Camera_Model) {
wpical::CameraModel model{};
auto ret =
wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
3, false);
EXPECT_EQ(ret, std::nullopt);
}
TEST(Field_CalibrationTest, Typical) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + videoLocation), output_json,
calSavePath + "/cameracalibration.json",
LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
EXPECT_EQ(ret, 0);
TEST(FieldCalibrationTest, Atypical_Bad_Field_Layout) {
auto model = wpi::util::json::parse(
std::ifstream(calSavePath + "/cameracalibration.json"))
.get<wpical::CameraModel>();
auto ret =
wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
wpi::apriltag::AprilTagFieldLayout{}, 3, false);
EXPECT_EQ(ret, std::nullopt);
}
TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + videoLocation), output_json,
LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix),
LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
EXPECT_EQ(ret, 1);
TEST(FieldCalibrationTest, Atypical_Bad_Input_Directory) {
auto model = wpi::util::json::parse(
std::ifstream(calSavePath + "/cameracalibration.json"))
.get<wpical::CameraModel>();
auto ret =
wpical::calibrate(LookupPath(projectRootPath), model,
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
3, false);
EXPECT_EQ(ret, std::nullopt);
}
TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + videoLocation), output_json,
calSavePath + "/cameracalibration.json",
calSavePath + "/cameracalibration.json", 3, false);
EXPECT_EQ(ret, 1);
TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag) {
auto model = wpi::util::json::parse(
std::ifstream(calSavePath + "/cameracalibration.json"))
.get<wpical::CameraModel>();
auto ret =
wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
42, false);
EXPECT_EQ(ret, std::nullopt);
}
TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + ""), output_json,
calSavePath + "/cameracalibration.json",
LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + videoLocation), output_json,
calSavePath + "/cameracalibration.json",
LookupPath(projectRootPath + "/2024-crescendo.json"), 42, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
int ret = fieldcalibration::calibrate(
LookupPath(projectRootPath + videoLocation), output_json,
calSavePath + "/cameracalibration.json",
LookupPath(projectRootPath + "/2024-crescendo.json"), -1, false);
EXPECT_EQ(ret, 1);
TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
auto model = wpi::util::json::parse(
std::ifstream(calSavePath + "/cameracalibration.json"))
.get<wpical::CameraModel>();
auto ret =
wpical::calibrate(LookupPath(projectRootPath + videoLocation), model,
wpi::apriltag::AprilTagFieldLayout::LoadField(
wpi::apriltag::AprilTagField::k2024Crescendo),
-1, false);
EXPECT_EQ(ret, std::nullopt);
}