From 2a7bb33871dee1124dd1956fe14503845e8cee3f Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 6 Jan 2014 23:45:43 +0100 Subject: aruco: allow for perfect fisheye correction --- ftnoir_tracker_aruco/aruco-trackercontrols.ui | 455 +++++++++++++------------- ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 18 +- ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 4 +- 3 files changed, 249 insertions(+), 228 deletions(-) (limited to 'ftnoir_tracker_aruco') diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui index e699eccc..7dadb58c 100644 --- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui +++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui @@ -10,7 +10,7 @@ 0 0 630 - 315 + 326 @@ -32,169 +32,42 @@ 7 - - - - Enable axes - - - - 7 + + + + + + + + Default - - 6 + + + + 30 - - 0 + + + + 60 - - 6 + + + + 120 - - 0 + + + + 180 - - - - RX - - - - - - - TX - - - - - - - RY - - - - - - - TY - - - - - - - RZ - - - - - - - TZ - - - - - - - - - - Head position - - - Qt::AlignCenter - - - - - - TX - - - - - - - - 0 - 0 - - - - -200.000000000000000 - - - 200.000000000000000 - - - - - - - TY - - - - - - - - 0 - 0 - - - - -200.000000000000000 - - - 200.000000000000000 - - - - - - - TZ - - - - - - - - 0 - 0 - - - - -200.000000000000000 - - - 200.000000000000000 - - - - + - - - - - - - - - - 35.000000000000000 - - - 180.000000000000000 - - - 52.000000000000000 + + + + Frames per second @@ -205,14 +78,7 @@ - - - - Frames per second - - - - + @@ -243,33 +109,41 @@ - - - - - Default - - - - - 30 - - - - - 60 - - - - - 120 - - - - - 180 - - + + + + + + + 35.000000000000000 + + + 180.000000000000000 + + + 52.000000000000000 + + + + + + + Camera name + + + + + + + Red channel only + + + + + + + Horizontal FOV + @@ -296,28 +170,7 @@ - - - - Camera name - - - - - - - Red channel only - - - - - - - Horizontal FOV - - - - + QDialogButtonBox::Cancel|QDialogButtonBox::Ok @@ -331,6 +184,16 @@ + + + + -180.000000000000000 + + + 180.000000000000000 + + + @@ -338,13 +201,164 @@ - - - - -180.000000000000000 + + + + Head position - - 180.000000000000000 + + Qt::AlignCenter + + + + + + TY + + + + + + + TX + + + + + + + + 0 + 0 + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + + TZ + + + + + + + + 0 + 0 + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + + + 0 + 0 + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + + + + + Enable axes + + + + 7 + + + 6 + + + 0 + + + 6 + + + 0 + + + + + RX + + + + + + + TX + + + + + + + RY + + + + + + + TY + + + + + + + RZ + + + + + + + TZ + + + + + + + + + + Check if yaw/X interconnect + + + + + + + Fisheye correction @@ -357,6 +371,7 @@ resolution red_only marker_pitch + fisheye_correction cx cy cz diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 776d443a..67c88e3e 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -18,7 +18,8 @@ #include #if defined(_WIN32) -#define NOMINMAX +# undef NOMINMAX +# define NOMINMAX # define NO_DSHOW_STRSAFE # include #else @@ -322,17 +323,13 @@ void Tracker::run() last_roi.width = std::min(grayscale.cols - last_roi.x, last_roi.width); last_roi.height = std::min(grayscale.rows - last_roi.y, last_roi.height); } - - cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE); + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, !first, cv::ITERATIVE); first = false; - cv::Mat rotation_matrix = cv::Mat::zeros(3, 3, CV_64FC1); - cv::Mat junk1(3, 3, CV_64FC1), junk2(3, 3, CV_64FC1); - cv::Rodrigues(rvec, rotation_matrix); - + { cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); @@ -350,6 +347,12 @@ void Tracker::run() pose[Yaw] = euler[1]; pose[Pitch] = -euler[0]; pose[Roll] = euler[2]; + + if (s.fisheye_correction) + { + pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI; + pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI; + } } std::vector repr2; @@ -473,6 +476,7 @@ TrackerControls::TrackerControls() tie_setting(s.headpos_z, ui.cz); tie_setting(s.red_only, ui.red_only); tie_setting(s.marker_pitch, ui.marker_pitch); + tie_setting(s.fisheye_correction, ui.fisheye_correction); connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); ui.cameraName->addItems(get_camera_names()); diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h index 4cab84b5..e32e304a 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h @@ -28,6 +28,7 @@ struct settings { value red_only; value eyaw, epitch, eroll, ex, ey, ez; value marker_pitch; + value fisheye_correction; settings() : b(bundle("aruco-tracker")), fov(b, "field-of-view", 56), @@ -44,7 +45,8 @@ struct settings { ex(b, "enable-x", true), ey(b, "enable-y", true), ez(b, "enable-z", true), - marker_pitch(b, "marker-pitch", 0) + marker_pitch(b, "marker-pitch", 0), + fisheye_correction(b, "fisheye-correction", false) {} }; -- cgit v1.2.3