Skip to content

Commit

Permalink
Add backend for more aruco dictionaries.
Browse files Browse the repository at this point in the history
  • Loading branch information
@ElliotScher
ElliotScher committed Jan 15, 2025
1 parent 475f695 commit d0358cc
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 150 deletions.
59 changes: 36 additions & 23 deletions wpical/src/main/native/cpp/WPIcal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,9 @@
#include <wpi/json.h>
#include <wpigui.h>

#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>

namespace gui = wpi::gui;

const char* GetWPILibVersion();
Expand Down Expand Up @@ -192,7 +195,8 @@ static void DisplayGui() {
ImGui::EndMenuBar();

static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
static std::unique_ptr<pfd::open_file> camera_calibration_input_video_selector;
static std::unique_ptr<pfd::open_file>
camera_calibration_input_video_selector;
static std::unique_ptr<pfd::open_file> field_map_selector;
static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
static std::unique_ptr<pfd::open_file> combination_calibrations_selector;
Expand Down Expand Up @@ -427,9 +431,11 @@ static void DisplayGui() {

if (mrcal) {
openFileButton("Select Camera Calibration Video",
selected_camera_calibration_input_video, camera_calibration_input_video_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_calibration_input_video_selector, selected_camera_calibration_input_video);
selected_camera_calibration_input_video,
camera_calibration_input_video_selector, "Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_calibration_input_video_selector,
selected_camera_calibration_input_video);

if (!selected_camera_calibration_input_video.empty()) {
drawCheck();
Expand All @@ -449,20 +455,24 @@ static void DisplayGui() {
ImGui::InputDouble("Image Height (pixels)", &imagerHeight);

ImGui::Separator();
if (ImGui::Button("Calibrate") && !selected_camera_calibration_input_video.empty()) {
if (ImGui::Button("Calibrate") &&
!selected_camera_calibration_input_video.empty()) {
std::cout << "calibration button pressed" << std::endl;
int ret = cameracalibration::calibrate(
selected_camera_calibration_input_video.c_str(), cameraModel, markerWidth,
boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug);
cv::aruco::Dictionary dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
int ret = cameracalibration::calibrate(dictionary,
selected_camera_calibration_input_video.c_str(), cameraModel, squareWidth,
markerWidth, boardWidth, boardHeight, imagerWidth, imagerHeight,
showDebug);
if (ret == 0) {
size_t lastSeparatorPos =
selected_camera_calibration_input_video.find_last_of("/\\");
std::string output_file_path;

if (lastSeparatorPos != std::string::npos) {
output_file_path =
selected_camera_calibration_input_video.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
output_file_path = selected_camera_calibration_input_video
.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
}

selected_camera_intrinsics = output_file_path;
Expand All @@ -479,9 +489,11 @@ static void DisplayGui() {
}
} else {
openFileButton("Select Camera Calibration Video",
selected_camera_calibration_input_video, camera_calibration_input_video_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_calibration_input_video_selector, selected_camera_calibration_input_video);
selected_camera_calibration_input_video,
camera_calibration_input_video_selector, "Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_calibration_input_video_selector,
selected_camera_calibration_input_video);

if (!selected_camera_calibration_input_video.empty()) {
drawCheck();
Expand All @@ -497,20 +509,23 @@ static void DisplayGui() {
ImGui::InputInt("Board Height (squares)", &boardHeight);

ImGui::Separator();
if (ImGui::Button("Calibrate") && !selected_camera_calibration_input_video.empty()) {
if (ImGui::Button("Calibrate") &&
!selected_camera_calibration_input_video.empty()) {
std::cout << "calibration button pressed" << std::endl;
int ret = cameracalibration::calibrate(
selected_camera_calibration_input_video.c_str(), cameraModel, squareWidth,
markerWidth, boardWidth, boardHeight, showDebug);
cv::aruco::Dictionary dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
int ret = cameracalibration::calibrate(dictionary,
selected_camera_calibration_input_video.c_str(), cameraModel,
squareWidth, markerWidth, boardWidth, boardHeight, showDebug);
if (ret == 0) {
size_t lastSeparatorPos =
selected_camera_calibration_input_video.find_last_of("/\\");
std::string output_file_path;

if (lastSeparatorPos != std::string::npos) {
output_file_path =
selected_camera_calibration_input_video.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
output_file_path = selected_camera_calibration_input_video
.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
}

selected_camera_intrinsics = output_file_path;
Expand Down Expand Up @@ -550,8 +565,6 @@ static void DisplayGui() {
ImGui::Text("K5: %.4f", cameraModel.distortion_coefficients(6));
ImGui::Text("K6: %.4f", cameraModel.distortion_coefficients(7));



ImGui::End();

ImGui::SetNextWindowPos(ImVec2(588, 24));
Expand Down
119 changes: 4 additions & 115 deletions wpical/src/main/native/cpp/cameracalibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,6 @@
#include <vector>

#include <mrcal_wrapper.h>
#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>

static bool filter(std::vector<cv::Point2f> charuco_corners,
Expand Down Expand Up @@ -43,13 +41,12 @@ static bool filter(std::vector<cv::Point2f> charuco_corners,
return true;
}

int cameracalibration::calibrate(const std::string& input_video,
int cameracalibration::calibrate(const cv::aruco::Dictionary& aruco_dict,
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);
Expand Down Expand Up @@ -152,15 +149,14 @@ int cameracalibration::calibrate(const std::string& input_video,
return 0;
}

int cameracalibration::calibrate(const std::string& input_video,
int cameracalibration::calibrate(const cv::aruco::Dictionary& aruco_dict,
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);
Expand Down Expand Up @@ -295,110 +291,3 @@ int cameracalibration::calibrate(const std::string& input_video,

return 0;
}

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

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

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

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

// 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;
}
11 changes: 5 additions & 6 deletions wpical/src/main/native/include/cameracalibration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,9 @@

#include <Eigen/Core>
#include <wpi/json.h>
#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>

namespace cameracalibration {
struct CameraModel {
Expand All @@ -18,17 +21,13 @@ struct CameraModel {
double avg_reprojection_error;
};

int calibrate(const std::string& input_video, CameraModel& camera_model,
int calibrate(const cv::aruco::Dictionary& dictionary, 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,
int calibrate(const cv::aruco::Dictionary& dictionary, 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(),
Expand Down
19 changes: 13 additions & 6 deletions wpical/src/test/native/cpp/test_calibrate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,10 @@
#include <gtest/gtest.h>
#include <wpi/json.h>

#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>

const std::string projectRootPath = PROJECT_ROOT_PATH;
const std::string calSavePath =
projectRootPath.substr(0,
Expand All @@ -22,6 +26,9 @@ cameracalibration::CameraModel cameraModel = {

wpi::json output_json;

cv::aruco::Dictionary dictionary =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);

#ifdef __linux__
const std::string fileSuffix = ".avi";
const std::string videoLocation = "/altfieldvideo";
Expand All @@ -30,7 +37,7 @@ const std::string fileSuffix = ".mp4";
const std::string videoLocation = "/fieldvideo";
#endif
TEST(Camera_CalibrationTest, OpenCV_Typical) {
int ret = cameracalibration::calibrate(
int ret = cameracalibration::calibrate(dictionary,
projectRootPath + "/testcalibration" + fileSuffix, cameraModel, 0.709f,
0.551f, 12, 8, false);
cameracalibration::dumpJson(cameraModel,
Expand All @@ -39,23 +46,23 @@ TEST(Camera_CalibrationTest, OpenCV_Typical) {
}

TEST(Camera_CalibrationTest, OpenCV_Atypical) {
int ret = cameracalibration::calibrate(
int ret = cameracalibration::calibrate(dictionary,
projectRootPath + videoLocation + "/long" + fileSuffix, cameraModel,
0.709f, 0.551f, 12, 8, false);
EXPECT_EQ(ret, 1);
}

TEST(Camera_CalibrationTest, MRcal_Typical) {
int ret = cameracalibration::calibrate(
projectRootPath + "/testcalibration" + fileSuffix, cameraModel, .709f, 12,
int ret = cameracalibration::calibrate(dictionary,
projectRootPath + "/testcalibration" + fileSuffix, cameraModel, .709f, 0.551f, 12,
8, 1080, 1920, false);
EXPECT_EQ(ret, 0);
}

TEST(Camera_CalibrationTest, MRcal_Atypical) {
int ret = cameracalibration::calibrate(
int ret = cameracalibration::calibrate(dictionary,
projectRootPath + videoLocation + "/short" + fileSuffix, cameraModel,
0.709f, 12, 8, 1080, 1920, false);
0.709f, 0.551f, 12, 8, 1080, 1920, false);
EXPECT_EQ(ret, 1);
}

Expand Down

0 comments on commit d0358cc

Please sign in to comment.