Skip to content

Commit

Permalink
Add ChArUco support for MRcal camera calibration.
Browse files Browse the repository at this point in the history
  • Loading branch information
@ElliotScher
ElliotScher committed Aug 20, 2024
1 parent f189ccd commit b065565
Showing 1 changed file with 152 additions and 98 deletions.
250 changes: 152 additions & 98 deletions wpical/src/main/native/cpp/cameracalibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,38 @@

#include "cameracalibration.h"

#include <iostream>
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()) {
return false;
}

if (charuco_corners.size() < 10 || charuco_ids.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) {
return false;
}
}

for (int i = 0; i < marker_ids.size(); i++) {
if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) {
return false;
}
}

return true;
}

int cameracalibration::calibrate(const std::string& input_video,
float square_width, float marker_width,
int board_width, int board_height,
bool show_debug_window) {
// Aruco Board
// 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(
Expand All @@ -23,26 +48,22 @@ int cameracalibration::calibrate(const std::string& input_video,
int frame_count = 0;
cv::Size frame_shape;

// Detection output
std::vector<cv::Point2f> all_corners;
std::vector<int> all_ids;
std::vector<int> all_counter;
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.isOpened()) {
while (video_capture.grab()) {
cv::Mat frame;
video_capture >> frame;
video_capture.retrieve(frame);

cv::Mat debug_image = frame;

if (frame.empty()) {
break;
}

// Limit FPS
frame_count++;

if (frame_count % 10 != 0) {
continue;
}

// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
Expand All @@ -54,23 +75,30 @@ int cameracalibration::calibrate(const std::string& input_video,
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);

cv::Mat debug_image = frame;
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);

if (!charuco_ids.empty() && charuco_ids.size() >= 4) {
all_corners.insert(all_corners.end(), charuco_corners.begin(),
charuco_corners.end());
all_ids.insert(all_ids.end(), charuco_ids.begin(), charuco_ids.end());
all_counter.push_back(charuco_ids.size());
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);
}

if (show_debug_window) {
cv::imshow("Frame", debug_image);
}
if (cv::waitKey(1) == 'q') {
Expand All @@ -85,15 +113,13 @@ int cameracalibration::calibrate(const std::string& input_video,
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;

std::vector<std::vector<cv::Point2f>> corners = {all_corners};
std::vector<std::vector<int>> ids = {all_ids};
double repError;

try {
cv::aruco::calibrateCameraCharuco(
corners, ids, charuco_board, frame_shape, camera_matrix, dist_coeffs,
r_vecs, t_vecs, std_dev_intrinsics, std_dev_extrinsics, per_view_errors,
cv::CALIB_RATIONAL_MODEL);
int flags = cv::CALIB_RATIONAL_MODEL;
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;
Expand All @@ -106,13 +132,11 @@ int cameracalibration::calibrate(const std::string& input_video,
{"distortion_coefficients",
std::vector<double>(dist_coeffs.begin<double>(),
dist_coeffs.end<double>())},
{"avg_reprojection_error", cv::mean(per_view_errors)[0]}};
{"avg_reprojection_error", repError}};

size_t lastSeparatorPos = input_video.find_last_of("/\\");
std::string output_file_path;

// If a separator is found, return the substring from the beginning to the
// last separator
if (lastSeparatorPos != std::string::npos) {
output_file_path = input_video.substr(0, lastSeparatorPos)
.append("/camera calibration.json");
Expand All @@ -126,121 +150,151 @@ int cameracalibration::calibrate(const std::string& input_video,
}

int cameracalibration::calibrate(const std::string& input_video,
float square_width, int board_width,
int board_height, double imagerWidthPixels,
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);
int frame_count = 0;
cv::Size frame_shape;

// Detection output
std::vector<std::vector<cv::Point2f>> all_corners;
std::vector<std::vector<mrcal_point3_t>> all_points;
std::vector<mrcal_point3_t> observation_boards;
std::vector<mrcal_pose_t> frames_rt_toref;

// Dimensions
cv::Size boardSize(board_width - 1, board_height - 1);
cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);

while (video_capture.isOpened()) {
while (video_capture.grab()) {
cv::Mat frame;
video_capture >> frame;
video_capture.retrieve(frame);

cv::Mat debug_image = frame;

if (frame.empty()) {
break;
}

// frame_count++;
// if (frame_count % 10 != 0)
// {
// continue;
// }

// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);

cv::Mat debug_image = frame;
frame_shape = frame_gray.size();

std::vector<cv::Point2f> corners;
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;

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

bool found = cv::findChessboardCorners(
frame_gray, boardSize, corners,
cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
img_points);

cv::drawChessboardCorners(frame, boardSize, corners, found);
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;
}

if (found) {
all_corners.push_back(corners);
for (int i = 0; i < (sizeof(points) / sizeof(points[0])); i++) {
if (points[i].z != 1.0f) {
points[i].x = -1.0f;
points[i].y = -1.0f;
points[i].z = -1.0f;
}
}

std::vector<mrcal_point3_t> points_vector(
points, points + (sizeof(points) / sizeof(points[0])));

frames_rt_toref.push_back(getSeedPose(points_vector.data(), boardSize,
imagerSize, square_width, 1000));
observation_boards.insert(observation_boards.end(), points_vector.begin(),
points_vector.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);
}
// Exit loop if 'q' is pressed
if (cv::waitKey(30) == 'q') {
if (cv::waitKey(1) == 'q') {
break;
}
}

video_capture.release();
cv::destroyAllWindows();

for (int i = 0; i < all_corners.size(); i++) {
std::vector<mrcal_point3_t> points;
for (int j = 0; j < all_corners.at(i).size(); j++) {
points.push_back(mrcal_point3_t{all_corners.at(i).at(j).x,
all_corners.at(i).at(j).y, 1.0f});
}
all_points.push_back(points);
}

std::vector<mrcal_point3_t> observations_board;
std::vector<mrcal_pose_t> frames_rt_toref;

if (all_points.size() == 0) {
if (observation_boards.empty()) {
std::cout << "calibration failed" << std::endl;
return 1;
} else {
std::cout << "points detected" << std::endl;
}

for (int i = 0; i < all_points.size(); i++) {
size_t total_points =
board_width * boardSize.height * all_points.at(i).size();
observations_board.reserve(total_points);
frames_rt_toref.reserve(all_points.at(i).size());

auto ret = getSeedPose(all_points.at(i).data(), boardSize, imagerSize,
square_width * 0.0254, 1000);

observations_board.insert(observations_board.end(),
all_points.at(i).begin(), all_points.at(i).end());
frames_rt_toref.push_back(ret);
}

auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize,
square_width * 0.0254, imagerSize, 1000);
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;

std::vector<double> cameraMatrix = {
// 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> distortionCoefficients = {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};

wpi::json result = {{"camera_matrix", cameraMatrix},
{"distortion_coefficients", distortionCoefficients},
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
wpi::json result = {{"camera_matrix", camera_matrix},
{"distortion_coefficients", distortion_coefficients},
{"avg_reprojection_error", stats.rms_error}};

size_t lastSeparatorPos = input_video.find_last_of("/\\");
Expand Down

0 comments on commit b065565

Please sign in to comment.