diff options
Diffstat (limited to 'tracker-aruco')
| -rw-r--r-- | tracker-aruco/CMakeLists.txt | 8 | ||||
| -rw-r--r-- | tracker-aruco/ar_video_widget.cpp | 67 | ||||
| -rw-r--r-- | tracker-aruco/ar_video_widget.h | 33 | ||||
| -rw-r--r-- | tracker-aruco/aruco-tracker.qrc | 5 | ||||
| -rw-r--r-- | tracker-aruco/aruco-trackercontrols.ui | 281 | ||||
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.cpp | 444 | ||||
| -rw-r--r-- | tracker-aruco/ftnoir_tracker_aruco.h | 99 | ||||
| -rw-r--r-- | tracker-aruco/images/aruco.png | bin | 0 -> 987 bytes | |||
| -rw-r--r-- | tracker-aruco/include/aruco.h | 134 | ||||
| -rw-r--r-- | tracker-aruco/include/arucofidmarkers.h | 119 | ||||
| -rw-r--r-- | tracker-aruco/include/board.h | 168 | ||||
| -rw-r--r-- | tracker-aruco/include/boarddetector.h | 138 | ||||
| -rw-r--r-- | tracker-aruco/include/cameraparameters.h | 138 | ||||
| -rw-r--r-- | tracker-aruco/include/cvdrawingutils.h | 52 | ||||
| -rw-r--r-- | tracker-aruco/include/exports.h | 46 | ||||
| -rw-r--r-- | tracker-aruco/include/marker.h | 143 | ||||
| -rw-r--r-- | tracker-aruco/include/markerdetector.h | 359 | ||||
| -rw-r--r-- | tracker-aruco/trans_calib.cpp | 41 | ||||
| -rw-r--r-- | tracker-aruco/trans_calib.h | 39 |
19 files changed, 2314 insertions, 0 deletions
diff --git a/tracker-aruco/CMakeLists.txt b/tracker-aruco/CMakeLists.txt new file mode 100644 index 00000000..ed52eee6 --- /dev/null +++ b/tracker-aruco/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(OpenCV 3.0) +set(SDK_ARUCO_LIBPATH "" CACHE FILEPATH "Aruco paper marker tracker static library path") +if(SDK_ARUCO_LIBPATH) + opentrack_boilerplate(opentrack-tracker-aruco) + target_link_libraries(opentrack-tracker-aruco ${SDK_ARUCO_LIBPATH} ${OpenCV_LIBS}) + target_include_directories(opentrack-tracker-aruco SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS}) + link_with_dinput8(opentrack-tracker-aruco) +endif() diff --git a/tracker-aruco/ar_video_widget.cpp b/tracker-aruco/ar_video_widget.cpp new file mode 100644 index 00000000..1c3af262 --- /dev/null +++ b/tracker-aruco/ar_video_widget.cpp @@ -0,0 +1,67 @@ +/* Copyright (c) 2014 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ar_video_widget.h" + +void ArucoVideoWidget::update_image(const cv::Mat& frame) +{ + QMutexLocker foo(&mtx); + if (!fresh) + { + _frame = frame.clone(); + fresh = true; + } +} + +void ArucoVideoWidget::update_and_repaint() +{ + QImage qframe; + + { + QMutexLocker foo(&mtx); + if (_frame.cols*_frame.rows <= 0 || !fresh) + return; + fresh = false; + qframe = QImage(_frame.cols, _frame.rows, QImage::Format_RGB888); + uchar* data = qframe.bits(); + const int pitch = qframe.bytesPerLine(); + unsigned char *input = (unsigned char*)(_frame.data); + const int chans = _frame.channels(); + for (int y = 0; y < _frame.rows; y++) + { + const int step = y * _frame.step; + const int pitch_ = y * pitch; + for (int x = 0; x < _frame.cols; x++) + { + data[pitch_ + x * 3 + 0] = input[step + x * chans + 2]; + data[pitch_ + x * 3 + 1] = input[step + x * chans + 1]; + data[pitch_ + x * 3 + 2] = input[step + x * chans + 0]; + } + } + } + + qframe = qframe.scaled(size(), Qt::IgnoreAspectRatio, Qt::FastTransformation); + + { + QMutexLocker foo(&mtx); + texture = qframe; + } + + update(); +} + +void ArucoVideoWidget::paintEvent(QPaintEvent* e) +{ + QMutexLocker foo(&mtx); + QPainter(this).drawImage(e->rect(), texture); +} + +ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent), fresh(false) +{ + connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint())); + timer.start(60); +} diff --git a/tracker-aruco/ar_video_widget.h b/tracker-aruco/ar_video_widget.h new file mode 100644 index 00000000..6df2d626 --- /dev/null +++ b/tracker-aruco/ar_video_widget.h @@ -0,0 +1,33 @@ +/* Copyright (c) 2014 Stanislaw Halik + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include <QTimer> +#include <QWidget> +#include <QMutex> +#include <QMutexLocker> +#include <QPainter> +#include <QPaintEvent> +#include <opencv/cv.hpp> + +class ArucoVideoWidget : public QWidget +{ + Q_OBJECT +private: + QMutex mtx; + QImage texture; + QTimer timer; + cv::Mat _frame; + bool fresh; +private slots: + void update_and_repaint(); +public: + ArucoVideoWidget(QWidget *parent); + void update_image(const cv::Mat& frame); + void paintEvent( QPaintEvent*) override; +}; diff --git a/tracker-aruco/aruco-tracker.qrc b/tracker-aruco/aruco-tracker.qrc new file mode 100644 index 00000000..70859876 --- /dev/null +++ b/tracker-aruco/aruco-tracker.qrc @@ -0,0 +1,5 @@ +<RCC> + <qresource prefix="/"> + <file>images/aruco.png</file> + </qresource> +</RCC> diff --git a/tracker-aruco/aruco-trackercontrols.ui b/tracker-aruco/aruco-trackercontrols.ui new file mode 100644 index 00000000..7e32936c --- /dev/null +++ b/tracker-aruco/aruco-trackercontrols.ui @@ -0,0 +1,281 @@ +<?xml version="1.0" encoding="UTF-8"?> +<ui version="4.0"> + <class>Form</class> + <widget class="QWidget" name="Form"> + <property name="windowModality"> + <enum>Qt::NonModal</enum> + </property> + <property name="geometry"> + <rect> + <x>0</x> + <y>0</y> + <width>485</width> + <height>202</height> + </rect> + </property> + <property name="sizePolicy"> + <sizepolicy hsizetype="Minimum" vsizetype="Minimum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="windowTitle"> + <string>Tracker settings</string> + </property> + <layout class="QGridLayout" name="gridLayout"> + <item row="1" column="1"> + <widget class="QDialogButtonBox" name="buttonBox"> + <property name="standardButtons"> + <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set> + </property> + </widget> + </item> + <item row="0" column="0"> + <widget class="QFrame" name="frame"> + <layout class="QGridLayout" name="gridLayout_3"> + <item row="1" column="0"> + <widget class="QLabel" name="label_2"> + <property name="text"> + <string>Frames per second</string> + </property> + </widget> + </item> + <item row="3" column="1"> + <widget class="QComboBox" name="resolution"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <item> + <property name="text"> + <string>640x480</string> + </property> + </item> + <item> + <property name="text"> + <string>320x240</string> + </property> + </item> + <item> + <property name="text"> + <string>320x200</string> + </property> + </item> + <item> + <property name="text"> + <string>Default (not recommended!)</string> + </property> + </item> + </widget> + </item> + <item row="2" column="0"> + <widget class="QLabel" name="label_3"> + <property name="text"> + <string>Camera name</string> + </property> + </widget> + </item> + <item row="3" column="0"> + <widget class="QLabel" name="label_4"> + <property name="text"> + <string>Resolution</string> + </property> + </widget> + </item> + <item row="0" column="0"> + <widget class="QLabel" name="label"> + <property name="text"> + <string>Diagonal FOV</string> + </property> + </widget> + </item> + <item row="1" column="1"> + <widget class="QComboBox" name="cameraFPS"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <item> + <property name="text"> + <string notr="true">Default</string> + </property> + </item> + <item> + <property name="text"> + <string>30</string> + </property> + </item> + <item> + <property name="text"> + <string>60</string> + </property> + </item> + <item> + <property name="text"> + <string>125</string> + </property> + </item> + <item> + <property name="text"> + <string>200</string> + </property> + </item> + </widget> + </item> + <item row="2" column="1"> + <widget class="QComboBox" name="cameraName"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + </widget> + </item> + <item row="4" column="0"> + <widget class="QLabel" name="label_6"> + <property name="text"> + <string>Camera settings</string> + </property> + </widget> + </item> + <item row="4" column="1"> + <widget class="QPushButton" name="camera_settings"> + <property name="text"> + <string>Open</string> + </property> + </widget> + </item> + <item row="0" column="1"> + <widget class="QSpinBox" name="cameraFOV"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Preferred" vsizetype="Preferred"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="locale"> + <locale language="English" country="UnitedStates"/> + </property> + <property name="minimum"> + <number>10</number> + </property> + <property name="maximum"> + <number>180</number> + </property> + </widget> + </item> + </layout> + </widget> + </item> + <item row="0" column="1"> + <widget class="QFrame" name="frame_2"> + <layout class="QGridLayout" name="gridLayout_2"> + <item row="1" column="0"> + <widget class="QLabel" name="label_5"> + <property name="text"> + <string>Head X</string> + </property> + </widget> + </item> + <item row="2" column="1"> + <widget class="QDoubleSpinBox" name="cy"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Minimum" vsizetype="Maximum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="minimum"> + <double>-10000.000000000000000</double> + </property> + <property name="maximum"> + <double>10000.000000000000000</double> + </property> + </widget> + </item> + <item row="1" column="1"> + <widget class="QDoubleSpinBox" name="cx"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Minimum" vsizetype="Maximum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="minimum"> + <double>-10000.000000000000000</double> + </property> + <property name="maximum"> + <double>10000.000000000000000</double> + </property> + </widget> + </item> + <item row="3" column="1"> + <widget class="QDoubleSpinBox" name="cz"> + <property name="sizePolicy"> + <sizepolicy hsizetype="Minimum" vsizetype="Maximum"> + <horstretch>0</horstretch> + <verstretch>0</verstretch> + </sizepolicy> + </property> + <property name="minimum"> + <double>-10000.000000000000000</double> + </property> + <property name="maximum"> + <double>10000.000000000000000</double> + </property> + </widget> + </item> + <item row="3" column="0"> + <widget class="QLabel" name="label_8"> + <property name="text"> + <string>Head Z </string> + </property> + </widget> + </item> + <item row="2" column="0"> + <widget class="QLabel" name="label_7"> + <property name="text"> + <string>Head Y</string> + </property> + </widget> + </item> + <item row="4" column="1"> + <widget class="QPushButton" name="btn_calibrate"> + <property name="text"> + <string>Toggle calibration</string> + </property> + <property name="checkable"> + <bool>true</bool> + </property> + </widget> + </item> + </layout> + </widget> + </item> + </layout> + </widget> + <resources/> + <connections/> + <designerdata> + <property name="gridDeltaX"> + <number>10</number> + </property> + <property name="gridDeltaY"> + <number>10</number> + </property> + <property name="gridSnapX"> + <bool>false</bool> + </property> + <property name="gridSnapY"> + <bool>false</bool> + </property> + <property name="gridVisible"> + <bool>true</bool> + </property> + </designerdata> +</ui> diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp new file mode 100644 index 00000000..570c2e0d --- /dev/null +++ b/tracker-aruco/ftnoir_tracker_aruco.cpp @@ -0,0 +1,444 @@ +/* Copyright (c) 2013-2015 Stanislaw Halik <sthalik@misaki.pl> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include <vector> +#include <cstdio> +#include <cmath> +#include <algorithm> +#include <QMutexLocker> +#include "./include/markerdetector.h" +#include "ftnoir_tracker_aruco.h" +#include "opentrack/plugin-api.hpp" +#include <opencv2/core/core.hpp> +#include <opencv2/highgui/highgui.hpp> +#include <opencv2/videoio.hpp> +#include "opentrack/camera-names.hpp" +#include "opentrack-compat/sleep.hpp" + +typedef struct { + int width; + int height; +} resolution_tuple; + +static resolution_tuple resolution_choices[] = { + { 640, 480 }, + { 320, 240 }, + { 320, 200 }, + { 0, 0 } +}; + +Tracker::Tracker() : stop(false), layout(nullptr), videoWidget(nullptr) +{ +} + +Tracker::~Tracker() +{ + stop = true; + wait(); + if (videoWidget) + delete videoWidget; + if(layout) + delete layout; + camera.release(); +} + +void Tracker::start_tracker(QFrame* videoframe) +{ + videoframe->show(); + videoWidget = new ArucoVideoWidget(videoframe); + QHBoxLayout* layout = new QHBoxLayout(); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(videoWidget); + if (videoframe->layout()) + delete videoframe->layout(); + videoframe->setLayout(layout); + videoWidget->show(); + start(); + for (int i = 0; i < 6; i++) + pose[i] = 0; + this->layout = layout; +} + +#define HT_PI 3.1415926535 + +void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_) +{ + QMutexLocker l(&mtx); + + r_ = r; + t_ = t; +} + +void Tracker::run() +{ + int rint = s.resolution; + if (rint < 0 || rint >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple))) + rint = 0; + resolution_tuple res = resolution_choices[rint]; + int fps; + switch (static_cast<int>(s.force_fps)) + { + default: + case 0: + fps = 0; + break; + case 1: + fps = 30; + break; + case 2: + fps = 60; + break; + case 3: + fps = 125; + break; + case 4: + fps = 200; + break; + } + + { + QMutexLocker l(&camera_mtx); + + camera = cv::VideoCapture(camera_name_to_index(s.camera_name)); + if (res.width) + { + camera.set(cv::CAP_PROP_FRAME_WIDTH, res.width); + camera.set(cv::CAP_PROP_FRAME_HEIGHT, res.height); + } + if (fps) + camera.set(cv::CAP_PROP_FPS, fps); + + if (!camera.isOpened()) + { + qDebug() << "aruco tracker: can't open camera"; + return; + } + } + + aruco::MarkerDetector detector; + detector.setDesiredSpeed(3); + + cv::Rect last_roi(65535, 65535, 0, 0); + + auto freq = cv::getTickFrequency(); + auto last_time = cv::getTickCount(); + double cur_fps = 0; + std::vector<int> box_sizes { 5, 7, 9, 11 }; + int box_idx = 0; + double failed = 0; + const double max_failed = 1.25; + cv::Vec3d rvec, tvec; + cv::Mat intrinsics = cv::Mat::eye(3, 3, CV_32FC1); + cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); + bool otsu = false; + + while (!stop) + { + cv::Mat color; + { + QMutexLocker l(&camera_mtx); + + if (!camera.read(color)) + continue; + } + static constexpr int thres_param2 = 5; + cv::Mat grayscale; + cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY); + + const int scale = grayscale.cols > 480 ? 2 : 1; + detector.setThresholdParams(box_sizes[box_idx], thres_param2); + + static constexpr double pi = 3.1415926f; + const int w = grayscale.cols, h = grayscale.rows; + const double diag_fov = static_cast<int>(s.fov) * pi / 180.; + const double fov_w = 2.*atan(tan(diag_fov/2.)/sqrt(1. + h/(double)w * h/(double)w)); + const double fov_h = 2.*atan(tan(diag_fov/2.)/sqrt(1. + w/(double)h * w/(double)h)); + const float focal_length_w = .5 * w / tan(.5 * fov_w); + const float focal_length_h = .5 * h / tan(.5 * fov_h); + + intrinsics.at<float> (0, 0) = focal_length_w; + intrinsics.at<float> (1, 1) = focal_length_h; + intrinsics.at<float> (0, 2) = grayscale.cols/2; + intrinsics.at<float> (1, 2) = grayscale.rows/2; + + std::vector< aruco::Marker > markers; + + const double size_min = 0.05; + const double size_max = 0.3; + + bool roi_valid = false; + + auto time = cv::getTickCount(); + + const double dt = (time - last_time) / freq; + last_time = time; + cur_fps = cur_fps * 0.97 + 0.03 * (dt == 0 ? 0 : 1./dt); + + if (last_roi.width > 0 && last_roi.height) + { + detector.setThresholdParams(box_sizes[box_idx], thres_param2); + detector.setMinMaxSize(std::max(0.01, size_min * grayscale.cols / last_roi.width), + std::min(1.0, size_max * grayscale.cols / last_roi.width)); + + cv::Mat grayscale_ = grayscale(last_roi).clone(); + if (detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false), + markers.size() == 1 && markers[0].size() == 4) + { + failed = std::max(0., failed - dt); + auto& m = markers.at(0); + for (int i = 0; i < 4; i++) + { + auto& p = m.at(i); + p.x += last_roi.x; + p.y += last_roi.y; + } + roi_valid = true; + } + } + + if (!roi_valid) + { + otsu = !otsu; + detector._thresMethod = otsu ? aruco::MarkerDetector::FIXED_THRES : aruco::MarkerDetector::ADPT_THRES; + failed += dt; + if (failed > max_failed) + { + box_idx++; + box_idx %= box_sizes.size(); + qDebug() << "aruco: box size now" << box_sizes[box_idx]; + failed = 0; + } + detector.setThresholdParams(box_sizes[box_idx], thres_param2); + detector.setMinMaxSize(size_min, size_max); + detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false); + } + + if (markers.size() == 1 && markers[0].size() == 4) { + const auto& m = markers.at(0); + for (int i = 0; i < 4; i++) + cv::line(color, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), scale, 8); + } + + char buf[128]; + + frame = color.clone(); + + ::sprintf(buf, "Hz: %d", (int)cur_fps); + cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale*2); + + if (markers.size() == 1 && markers[0].size() == 4) { + const auto& m = markers.at(0); + const float size = 40; + + cv::Mat obj_points(4,3,CV_32FC1); + const int x1=1, x2=2, x3=3, x4=0; + obj_points.at<float>(x1,0)=-size + s.headpos_x; + obj_points.at<float>(x1,1)=-size + s.headpos_y; + obj_points.at<float>(x1,2)= 0 + s.headpos_z; + + obj_points.at<float>(x2,0)=size + s.headpos_x; + obj_points.at<float>(x2,1)=-size + s.headpos_y; + obj_points.at<float>(x2,2)= 0 + s.headpos_z; + + obj_points.at<float>(x3,0)=size + s.headpos_x; + obj_points.at<float>(x3,1)=size + s.headpos_y; + obj_points.at<float>(x3,2)= 0 + s.headpos_z; + + obj_points.at<float>(x4,0)= -size + s.headpos_x; + obj_points.at<float>(x4,1)= size + s.headpos_y; + obj_points.at<float>(x4,2)= 0 + s.headpos_z; + + std::vector<cv::Point2f> img_points = m; + if (!cv::solvePnP(obj_points, img_points, intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE)) + goto fail; + + { + std::vector<cv::Point2f> repr2; + std::vector<cv::Point3f> centroid; + centroid.push_back(cv::Point3f(0, 0, 0)); + cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, repr2); + + { + auto s = cv::Scalar(255, 0, 255); + cv::circle(frame, repr2.at(0), 4, s, -1); + } + } + + for (int i = 0; i < 4; i++) + { + obj_points.at<float>(i, 0) -= s.headpos_x; + obj_points.at<float>(i, 1) -= s.headpos_y; + obj_points.at<float>(i, 2) -= s.headpos_z; + } + + cv::Mat rvec_, tvec_; + cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE); + + cv::Mat roi_points = obj_points * c_search_window; + std::vector<cv::Point2f> roi_projection(4); + cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection); + + last_roi = cv::Rect(color.cols-1, color.rows-1, 0, 0); + + for (int i = 0; i < 4; i++) + { + auto proj = roi_projection[i]; + int min_x = std::min<int>(proj.x, last_roi.x), + min_y = std::min<int>(proj.y, last_roi.y); + + int max_x = std::max<int>(proj.x, last_roi.width), + max_y = std::max<int>(proj.y, last_roi.height); + + last_roi.x = min_x; + last_roi.y = min_y; + + last_roi.width = max_x; + last_roi.height = max_y; + } + + if (last_roi.x < 0) + last_roi.x = 0; + if (last_roi.y < 0) + last_roi.y = 0; + if (last_roi.width < 0) + last_roi.width = 0; + if (last_roi.height < 0) + last_roi.height = 0; + if (last_roi.x >= color.cols-1) + last_roi.x = color.cols-1; + if (last_roi.width >= color.cols-1) + last_roi.width = color.cols-1; + if (last_roi.y >= color.rows-1) + last_roi.y= color.rows-1; + if (last_roi.height >= color.rows-1) + last_roi.height = color.rows-1; + + last_roi.width -= last_roi.x; + last_roi.height -= last_roi.y; + + auto rmat = cv::Matx33d::zeros(); + cv::Matx33d m_r(3, 3, CV_64FC1), m_q(3, 3, CV_64FC1); + cv::Rodrigues(rvec, rmat); + + { + cv::Vec3d euler = cv::RQDecomp3x3(rmat, m_r, m_q); + + QMutexLocker lck(&mtx); + + for (int i = 0; i < 3; i++) + pose[i] = tvec(i); + pose[Yaw] = euler[1]; + pose[Pitch] = -euler[0]; + pose[Roll] = euler[2]; + + r = rmat; + t = cv::Vec3d(tvec[0], -tvec[1], tvec[2]); + } + + if (roi_valid) + cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1); + } + else +fail: + last_roi = cv::Rect(65535, 65535, 0, 0); + + if (frame.rows > 0) + videoWidget->update_image(frame); + } + + // give opencv time to exit camera threads, etc. + portable::sleep(500); +} + +void Tracker::data(double *data) +{ + QMutexLocker lck(&mtx); + + data[Yaw] = pose[Yaw]; + data[Pitch] = pose[Pitch]; + data[Roll] = pose[Roll]; + data[TX] = pose[TX] * .1; + data[TY] = pose[TY] * .1; + data[TZ] = pose[TZ] * .1; +} + +TrackerControls::TrackerControls() +{ + tracker = nullptr; + calib_timer.setInterval(200); + ui.setupUi(this); + setAttribute(Qt::WA_NativeWindow, true); + ui.cameraName->addItems(get_camera_names()); + tie_setting(s.camera_name, ui.cameraName); + tie_setting(s.resolution, ui.resolution); + tie_setting(s.force_fps, ui.cameraFPS); + tie_setting(s.fov, ui.cameraFOV); + tie_setting(s.headpos_x, ui.cx); + tie_setting(s.headpos_y, ui.cy); + tie_setting(s.headpos_z, ui.cz); + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); + connect(ui.btn_calibrate, SIGNAL(clicked()), this, SLOT(toggleCalibrate())); + connect(this, SIGNAL(destroyed()), this, SLOT(cleanupCalib())); + connect(&calib_timer, SIGNAL(timeout()), this, SLOT(update_tracker_calibration())); + connect(ui.camera_settings, SIGNAL(pressed()), this, SLOT(camera_settings())); +} + +void TrackerControls::toggleCalibrate() +{ + if (!calib_timer.isActive()) + { + s.headpos_x = 0; + s.headpos_y = 0; + s.headpos_z = 0; + calibrator.reset(); + calib_timer.start(); + } else { + cleanupCalib(); + + auto pos = calibrator.get_estimate(); + s.headpos_x = pos(0); + s.headpos_y = pos(1); + s.headpos_z = pos(2); + } +} + +void TrackerControls::cleanupCalib() +{ + if (calib_timer.isActive()) + calib_timer.stop(); +} + +void TrackerControls::update_tracker_calibration() +{ + if (calib_timer.isActive() && tracker) + { + cv::Matx33d r; + cv::Vec3d t; + tracker->getRT(r, t); + calibrator.update(r, t); + } +} + +void TrackerControls::doOK() +{ + s.b->save(); + this->close(); +} + +void TrackerControls::doCancel() +{ + s.b->reload(); + this->close(); +} + +void TrackerControls::camera_settings() +{ + open_camera_settings(tracker ? &tracker->camera : nullptr, s.camera_name, tracker ? &tracker->camera_mtx : nullptr); +} + +OPENTRACK_DECLARE_TRACKER(Tracker, TrackerControls, TrackerDll) diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h new file mode 100644 index 00000000..f827da77 --- /dev/null +++ b/tracker-aruco/ftnoir_tracker_aruco.h @@ -0,0 +1,99 @@ +/* Copyright (c) 2013-2015 Stanislaw Halik <sthalik@misaki.pl> + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#pragma once + +#include "ui_aruco-trackercontrols.h" +#include "ar_video_widget.h" +#include <QObject> +#include <QThread> +#include <QMutex> +#include <QHBoxLayout> +#include <QDialog> +#include <QTimer> +#include "opentrack/options.hpp" +#include "trans_calib.h" +#include "opentrack/plugin-api.hpp" +#include "opentrack/opencv-camera-dialog.hpp" + +#include <opencv2/core/core.hpp> +#include <opencv2/highgui/highgui.hpp> + +using namespace options; + +struct settings : opts { + value<int> fov; + value<double> headpos_x, headpos_y, headpos_z; + value<QString> camera_name; + value<int> force_fps, resolution; + settings() : + opts("aruco-tracker"), + fov(b, "field-of-view", 56), + headpos_x(b, "headpos-x", 0), + headpos_y(b, "headpos-y", 0), + headpos_z(b, "headpos-z", 0), + camera_name(b, "camera-name", ""), + force_fps(b, "force-fps", 0), + resolution(b, "force-resolution", 0) + {} +}; + +class TrackerControls; + +class Tracker : protected QThread, public ITracker +{ + Q_OBJECT + friend class TrackerControls; + static constexpr double c_search_window = 2.65; +public: + Tracker(); + ~Tracker() override; + void start_tracker(QFrame* frame) override; + void data(double *data) override; + void run() override; + void getRT(cv::Matx33d &r, cv::Vec3d &t); +private: + cv::VideoCapture camera; + QMutex camera_mtx; + QMutex mtx; + volatile bool stop; + QHBoxLayout* layout; + ArucoVideoWidget* videoWidget; + settings s; + double pose[6]; + cv::Mat frame; + cv::Matx33d r; + cv::Vec3d t; +}; + +class TrackerControls : public ITrackerDialog, protected camera_dialog<Tracker> +{ + Q_OBJECT +public: + TrackerControls(); + void register_tracker(ITracker * x) override { tracker = static_cast<Tracker*>(x); } + void unregister_tracker() override { tracker = nullptr; } +private: + Ui::Form ui; + Tracker* tracker; + settings s; + TranslationCalibrator calibrator; + QTimer calib_timer; +private slots: + void doOK(); + void doCancel(); + void toggleCalibrate(); + void cleanupCalib(); + void update_tracker_calibration(); + void camera_settings(); +}; + +class TrackerDll : public Metadata +{ + QString name() { return QString("aruco -- paper marker tracker"); } + QIcon icon() { return QIcon(":/images/aruco.png"); } +}; diff --git a/tracker-aruco/images/aruco.png b/tracker-aruco/images/aruco.png Binary files differnew file mode 100644 index 00000000..491c39e6 --- /dev/null +++ b/tracker-aruco/images/aruco.png diff --git a/tracker-aruco/include/aruco.h b/tracker-aruco/include/aruco.h new file mode 100644 index 00000000..569b95fb --- /dev/null +++ b/tracker-aruco/include/aruco.h @@ -0,0 +1,134 @@ +/** + +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. + + + + \mainpage ArUco: Augmented Reality library from the University of Cordoba + + +ArUco is a minimal C++ library for detection of Augmented Reality markers based on OpenCv exclusively. + +It is an educational project to show student how to detect augmented reality markers and it is provided under BSD license. + + +\section INTRODUCTION INTRODUCTION + +The library relies on the use of coded markers. Each marker has an unique code indicated by the black and white colors in it. The libary detect borders, and analyzes into the rectangular regions which of them are likely to be markers. Then, a decoding is performed and if the code is valid, it is considered that the rectangle is a marker. + +The codification included into the marker is a slighly modified version of the Hamming Code. It has a total a 25 bits didived in 5 rows of 5 bits each. So, we have 5 words of 5 bits. Each word, contains only 2 bits of real information, the rest is for and error detection/correction (error correction is yet to be done). As a conclusion, a marker contains 10 bits of real information wich allows 1024 different markers. + + +\section BOARDS BOARDS + +Aruco allows the possibility to employ board. Boards are markers composed by an array of markers arranged in a known order. The advantages of using boards instead of simple markers are: + - More robusteness. The misdetection of several markers of the board is not a problem as long as a minimum set of them are detected. + - More precision. Since there are a larger number of corners, camera pose estimation becomes more precise. + + +\section APPLICATIONS APPLICATIONS + +The library comes with five applications that will help you to learn how to use the library: + - aruco_create_marker: which creates marker and saves it in a jpg file you can print. + - aruco_simple : simple test aplication that detects the markers in a image + - aruco_test: this is the main application for detection. It reads images either from the camera of from a video and detect markers. Additionally, if you provide the intrinsics of the camera(obtained by OpenCv calibration) and the size of the marker in meters, the library calculates the marker intrinsics so that you can easily create your AR applications. + - aruco_test_gl: shows how to use the library AR applications using OpenGL for rendering + - aruco_create_board: application that helps you to create a board + - aruco_simple_board: simple test aplication that detects a board of markers in a image + - aruco_test_board: application that detects boards + - aruco_test_board_gl: application that detects boards and uses OpenGL to draw + +\section LIBRARY LIBRARY DESCRIPTION: + +The ArUco library contents are divided in two main directories. The src directory, which contains the library itself. And the utils directory which contains the applications. + +The library main classes are: + - aruco::CameraParameters: represent the information of the camera that captures the images. Here you must set the calibration info. + - aruco::Marker: which represent a marker detected in the image + - aruco::MarkerDetector: that is in charge of deteting the markers in a image Detection is done by simple calling the member funcion ArMarkerDetector::detect(). Additionally, the classes contain members to create the required matrices for rendering using OpenGL. See aruco_test_gl for details + - aruco::BoardConfiguration: A board is an array of markers in a known order. BoardConfiguracion is the class that defines a board by indicating the id of its markers. In addition, it has informacion about the distance between the markers so that extrinsica camera computations can be done. + - aruco::Board: This class defines a board detected in a image. The board has the extrinsic camera parameters as public atributes. In addition, it has a method that allows obtain the matrix for getting its position in OpenGL (see aruco_test_board_gl for details). + - aruco::BoardDetector : This is the class in charge of detecting a board in a image. You must pass to it the set of markers detected by ArMarkerDetector and the BoardConfiguracion of the board you want to detect. This class will do the rest for you, even calculating the camera extrinsics. + + +\section COMPILING COMPILING THE LIBRARY: +\subsection Linux +Go to the aruco library and do +\verbatim +>mkdir build +>cd build +>cmake .. +>make +>make install (optional) +\endverbatim + +NOTE ON OPENGL: The library supports eaily the integration with OpenGL. In order to compile with support for OpenGL, you just have installed in your system the develop packages for GL and glut (or freeglut). + +\subsection WINDOWS + +The library has been compiled using MinGW and codeblocks. Below I describe the best way to compile it that I know. If you know better, please let me know. + - step 1) codeblocks + -# Download codeblocks. I recommend to download the version 10.5 with mingw included (codeblocks-10.05mingw-setup.exe) + -# Install and set the PATH variable so that the codeblock/mingw/bin directory is included. In my case c:/codeblocks/mingw/bin. This will allow cmake to find the compiler. + -# The codeblock program will not find the mingw path by deafult. So, run codeblocks and go to setting->Compuiler debugger and set the correct path to the MinGW dir. + - step 2) cmake + -# Download and install the last version of cmake. + - step 3) OpenCv + -# Download the source code and compile it using cmake and codeblocks. Note: install the library in C:\ if you want it to be easily detected by cmake afterwards + - step 4) aruco + -# Download and decompress. + -# Open cmake gui application and set the path to the main library directory and also set a path where the project is going to be built. + -# Generate the codeblock project. + -# Open the project with codeblock and compile then, install. The programs will be probably generated into the bin directory + +OpenGL: by default, the mingw version installed has not the glut library. So, the opengl programs are not compiled. If you want to compile with OpenGL support, you must install glut, or prefereably freeglut. +Thus, + - Download the library (http://www.martinpayne.me.uk/software/development/GLUT/freeglut-MinGW.zip) for mingw. + - Decompress in a directory X. + - Then, rerun cmake setting the variable GLU_PATH to that directory (>cmake .. -DGLUT_PATH="C:\X") + - Finally, recompile and test. Indeed, you should move the freeglut.dll to the directory with the binaries or to any other place in the PATH. + + +CONCLUSION: Move to Linux, things are simpler :P + + +\section Testing + +For testing the applications, the library provides videos and the corresponding camera parameters of these videos. Into the directories you will find information on how to run the examples. + +\section Final Notes + + - REQUIREMENTS: OpenCv >= 2.1.0. and OpenGL for (aruco_test_gl and aruco_test_board_gl) + - CONTACT: Rafael Munoz-Salinas: rmsalinas@uco.es + - This libary is free software and come with no guaratee! + +*/ + +#include "markerdetector.h" +#include "boarddetector.h" +#include "cvdrawingutils.h" + diff --git a/tracker-aruco/include/arucofidmarkers.h b/tracker-aruco/include/arucofidmarkers.h new file mode 100644 index 00000000..7dad4672 --- /dev/null +++ b/tracker-aruco/include/arucofidmarkers.h @@ -0,0 +1,119 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ + +#ifndef ArucoFiducicalMarkerDetector_H +#define ArucoFiducicalMarkerDetector_H +#include <opencv2/core/core.hpp> +#include "exports.h" +#include "marker.h" +#include "board.h" +namespace aruco { + +class ARUCO_EXPORTS FiducidalMarkers { +public: + /** + * \brief Creates an ar marker with the id specified using a modified version of the hamming code. + * There are two type of markers: a) These of 10 bits b) these of 3 bits. The latter are employed for applications + * that need few marker but they must be small. The two type of markers are distinguished by their ids. While the first type + * of markers have ids in the interval [0-1023], the second type ids in the interval [2000-2006]. + * + * + * 10 bits markers + * ----------------------- + * There are a total of 5 rows of 5 cols. Each row encodes a total of 2 bits, so there are 2^10 bits:(0-1023). + * + * The least significative bytes are first (from left-up to to right-bottom) + * + * Example: the id = 110 (decimal) is be represented in binary as : 00 01 10 11 10. + * + * Then, it will generate the following marker: + * + * -# 1st row encodes 00: 1 0 0 0 0 : hex 0x10 + * -# 2nd row encodes 01: 1 0 1 1 1 : hex 0x17 + * -# 3nd row encodes 10: 0 1 0 0 1 : hex 0x09 + * -# 4th row encodes 11: 0 1 1 1 0 : hex 0x0e + * -# 5th row encodes 10: 0 1 0 0 1 : hex 0x09 + * + * Note that : The first bit, is the inverse of the hamming parity. This avoids the 0 0 0 0 0 to be valid + * These marker are detected by the function getFiduciadlMarker_Aruco_Type1 + */ + static cv::Mat createMarkerImage(int id,int size) throw (cv::Exception); + + /** Detection of fiducidal aruco markers (10 bits) + * @param in input image with the patch that contains the possible marker + * @param nRotations number of 90deg rotations in clockwise direction needed to set the marker in correct position + * @return -1 if the image passed is a not a valid marker, and its id in case it really is a marker + */ + static int detect(const cv::Mat &in,int &nRotations); + + /**Similar to createMarkerImage. Instead of returning a visible image, returns a 8UC1 matrix of 0s and 1s with the marker info + */ + static cv::Mat getMarkerMat(int id) throw (cv::Exception); + + + /**Creates a printable image of a board + * @param gridSize grid layout (numer of sqaures in x and Y) + * @param MarkerSize size of markers sides in pixels + * @param MarkerDistance distance between the markers + * @param TInfo output + * @param excludedIds set of ids excluded from the board + */ + static cv::Mat createBoardImage( cv::Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,vector<int> *excludedIds=NULL ) throw (cv::Exception); + + + /**Creates a printable image of a board in chessboard_like manner + * @param gridSize grid layout (numer of sqaures in x and Y) + * @param MarkerSize size of markers sides in pixels + * @param TInfo output + * @param setDataCentered indicates if the center is set at the center of the board. Otherwise it is the left-upper corner + * + */ + static cv::Mat createBoardImage_ChessBoard( cv::Size gridSize,int MarkerSize, BoardConfiguration& TInfo ,bool setDataCentered=true ,vector<int> *excludedIds=NULL) throw (cv::Exception); + + /**Creates a printable image of a board in a frame fashion + * @param gridSize grid layout (numer of sqaures in x and Y) + * @param MarkerSize size of markers sides in pixels + * @param MarkerDistance distance between the markers + * @param TInfo output + * @param setDataCentered indicates if the center is set at the center of the board. Otherwise it is the left-upper corner + * + */ + static cv::Mat createBoardImage_Frame( cv::Size gridSize,int MarkerSize,int MarkerDistance, BoardConfiguration& TInfo ,bool setDataCentered=true,vector<int> *excludedIds=NULL ) throw (cv::Exception); + +private: + + static vector<int> getListOfValidMarkersIds_random(int nMarkers,vector<int> *excluded) throw (cv::Exception); + static cv::Mat rotate(const cv::Mat & in); + static int hammDistMarker(cv::Mat bits); + static int analyzeMarkerImage(cv::Mat &grey,int &nRotations); + static bool correctHammMarker(cv::Mat &bits); +}; + +} + +#endif diff --git a/tracker-aruco/include/board.h b/tracker-aruco/include/board.h new file mode 100644 index 00000000..ec7dd43f --- /dev/null +++ b/tracker-aruco/include/board.h @@ -0,0 +1,168 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _Aruco_board_h +#define _Aruco_board_h +#include <opencv2/opencv.hpp> +#include <string> +#include <vector> +#include "exports.h" +#include "marker.h" +namespace aruco { +using namespace std; +/** + * 3d representation of a marker + */ +struct ARUCO_EXPORTS MarkerInfo:public vector<cv::Point3f> { + MarkerInfo() {} + MarkerInfo(int _id) {id=_id; } + MarkerInfo(const MarkerInfo&MI): vector<cv::Point3f>(MI){id=MI.id; } + MarkerInfo & operator=(const MarkerInfo&MI){ + vector<cv::Point3f> ::operator=(MI); + id=MI.id; + return *this; + } + int id;//maker id +}; + +/**\brief This class defines a board with several markers. + * A Board contains several markers so that they are more robustly detected. + * + * In general, a board is a set of markers. So BoardConfiguration is only a list + * of the id of the markers along with the position of their corners. + * + * The position of the corners can be specified either in pixels (in a non-specific size) or in meters. + * The first is the typical case in which you generate the image of board and the print it. Since you do not know in advance the real + * size of the markers, their corners are specified in pixels, and then, the translation to meters can be made once you know the real size. + * + * On the other hand, you may want to have the information of your boards in meters. The BoardConfiguration allows you to do so. + * + * The point is in the mInfoType variable. It can be either PIX or METERS according to your needs. + * +*/ + + +class ARUCO_EXPORTS BoardConfiguration: public vector<MarkerInfo> +{ + friend class Board; +public: + enum MarkerInfoType {NONE=-1,PIX=0,METERS=1};//indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally + //variable indicates if the data in MakersInfo is expressed in meters or in pixels so as to do conversion internally + int mInfoType; + /** + */ + BoardConfiguration(); + + /** + */ + BoardConfiguration(const BoardConfiguration &T); + + /** + */ + BoardConfiguration & operator=(const BoardConfiguration &T); + /**Saves the board info to a file + */ + void saveToFile(string sfile)throw (cv::Exception); + /**Reads board info from a file + */ + void readFromFile(string sfile)throw (cv::Exception); + /**Indicates if the corners are expressed in meters + */ + bool isExpressedInMeters()const { + return mInfoType==METERS; + } + /**Indicates if the corners are expressed in meters + */ + bool isExpressedInPixels()const { + return mInfoType==PIX; + } + /**Returns the index of the marker with id indicated, if is in the list + */ + int getIndexOfMarkerId(int id)const; + /**Returns the Info of the marker with id specified. If not in the set, throws exception + */ + const MarkerInfo& getMarkerInfo(int id)const throw (cv::Exception); + /**Set in the list passed the set of the ids + */ + void getIdList(vector<int> &ids,bool append=true)const; +private: + /**Saves the board info to a file + */ + void saveToFile(cv::FileStorage &fs)throw (cv::Exception); + /**Reads board info from a file + */ + void readFromFile(cv::FileStorage &fs)throw (cv::Exception); +}; + +/** +*/ +class ARUCO_EXPORTS Board:public vector<Marker> +{ + +public: + BoardConfiguration conf; + //matrices of rotation and translation respect to the camera + cv::Mat Rvec,Tvec; + /** + */ + Board() + { + Rvec.create(3,1,CV_32FC1); + Tvec.create(3,1,CV_32FC1); + for (int i=0;i<3;i++) + Tvec.at<float>(i,0)=Rvec.at<float>(i,0)=-999999; + } + + /**Given the extrinsic camera parameters returns the GL_MODELVIEW matrix for opengl. + * Setting this matrix, the reference corrdinate system will be set in this board + */ + void glGetModelViewMatrix(double modelview_matrix[16])throw(cv::Exception); + + /** + * Returns position vector and orientation quaternion for an Ogre scene node or entity. + * Use: + * ... + * Ogre::Vector3 ogrePos (position[0], position[1], position[2]); + * Ogre::Quaternion ogreOrient (orientation[0], orientation[1], orientation[2], orientation[3]); + * mySceneNode->setPosition( ogrePos ); + * mySceneNode->setOrientation( ogreOrient ); + * ... + */ + void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception); + + + /**Save this from a file + */ + void saveToFile(string filePath)throw(cv::Exception); + /**Read this from a file + */ + void readFromFile(string filePath)throw(cv::Exception); + +}; +} + +#endif diff --git a/tracker-aruco/include/boarddetector.h b/tracker-aruco/include/boarddetector.h new file mode 100644 index 00000000..619b4798 --- /dev/null +++ b/tracker-aruco/include/boarddetector.h @@ -0,0 +1,138 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _Aruco_BoardDetector_H +#define _Aruco_BoardDetector_H +#include <opencv2/opencv.hpp> +#include "exports.h" +#include "board.h" +#include "cameraparameters.h" +#include "markerdetector.h" + +namespace aruco +{ +using namespace std; +/**\brief This class detects AR boards + * Version 1.2 + * There are two modes for board detection. + * First, the old way. (You first detect markers with MarkerDetector and then call to detect in this class. + * + * Second: New mode, marker detection is included in the class + * \code + + CameraParameters CP; + CP.readFromFile(path_cp) + BoardConfiguration BC; + BC.readFromFile(path_bc); + BoardDetector BD; + BD.setParams(BC,CP); //or only BD.setParams(BC) + //capture image + cv::Mat im; + capture_image(im); + + float prob=BD.detect(im); + if (prob>0.3) + CvDrawingUtils::draw3DAxis(im,BD.getDetectedBoard(),CP); + + \endcode + * +*/ +class ARUCO_EXPORTS BoardDetector +{ +public: + /** See discussion in @see enableRotateXAxis. + * Do not change unless you know what you are doing + */ + BoardDetector(bool setYPerperdicular=true); + + + /** + * Use if you plan to let this class to perform marker detection too + */ + void setParams(const BoardConfiguration &bc,const CameraParameters &cp, float markerSizeMeters=-1); + void setParams(const BoardConfiguration &bc); + /** + * Detect markers, and then, look for the board indicated in setParams() + * @return value indicating the likelihood of having found the marker + */ + float detect(const cv::Mat &im)throw (cv::Exception); + /**Returns a reference to the board detected + */ + Board & getDetectedBoard(){return _boardDetected;} + /**Returns a reference to the internal marker detector + */ + MarkerDetector &getMarkerDetector(){return _mdetector;} + /**Returns the vector of markers detected + */ + vector<Marker> &getDetectedMarkers(){return _vmarkers;} + + + //ALTERNATIVE DETECTION METHOD, BASED ON MARKERS PREVIOUSLY DETECTED + + /** Given the markers detected, determines if there is the board passed + * @param detectedMarkers result provided by aruco::ArMarkerDetector + * @param BConf the board you want to see if is present + * @param Bdetected output information of the detected board + * @param camMatrix camera matrix with intrinsics + * @param distCoeff camera distorsion coeff + * @param camMatrix intrinsic camera information. + * @param distCoeff camera distorsion coefficient. If set Mat() if is assumed no camera distorion + * @param markerSizeMeters size of the marker sides expressed in meters + * @return value indicating the likelihood of having found the marker + */ + float detect(const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected, cv::Mat camMatrix=cv::Mat(),cv::Mat distCoeff=cv::Mat(), float markerSizeMeters=-1 )throw (cv::Exception); + float detect(const vector<Marker> &detectedMarkers,const BoardConfiguration &BConf, Board &Bdetected,const CameraParameters &cp, float markerSizeMeters=-1 )throw (cv::Exception); + + + /** + * By default, the Y axis is set to point up. However this is not the default + * operation mode of opencv, which produces the Z axis pointing up instead. + * So, to achieve this change, we have to rotate the X axis. + */ + void setYPerperdicular(bool enable){_setYPerperdicular=enable;} + + + + +private: + void rotateXAxis(cv::Mat &rotation); + bool _setYPerperdicular; + + //-- Functionality to detect markers inside + bool _areParamsSet; + BoardConfiguration _bconf; + Board _boardDetected; + float _markerSize; + CameraParameters _camParams; + MarkerDetector _mdetector;//internal markerdetector + vector<Marker> _vmarkers;//markers detected in the call to : float detect(const cv::Mat &im); + +}; + +}; +#endif + diff --git a/tracker-aruco/include/cameraparameters.h b/tracker-aruco/include/cameraparameters.h new file mode 100644 index 00000000..083d5f96 --- /dev/null +++ b/tracker-aruco/include/cameraparameters.h @@ -0,0 +1,138 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _Aruco_CameraParameters_H +#define _Aruco_CameraParameters_H +#include "exports.h" +#include <opencv2/core/core.hpp> +#include <string> + +namespace aruco +{ +using namespace std; +/**\brief Parameters of the camera + */ + +class ARUCO_EXPORTS CameraParameters +{ +public: + + // 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) + cv::Mat CameraMatrix; + //4x1 matrix (k1,k2,p1,p2) + cv::Mat Distorsion; + //size of the image + cv::Size CamSize; + + /**Empty constructor + */ + CameraParameters() ; + /**Creates the object from the info passed + * @param cameraMatrix 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) + * @param distorsionCoeff 4x1 matrix (k1,k2,p1,p2) + * @param size image size + */ + CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception); + /**Sets the parameters + * @param cameraMatrix 3x3 matrix (fx 0 cx, 0 fy cy, 0 0 1) + * @param distorsionCoeff 4x1 matrix (k1,k2,p1,p2) + * @param size image size + */ + void setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception); + /**Copy constructor + */ + CameraParameters(const CameraParameters &CI) ; + + /**Indicates whether this object is valid + */ + bool isValid()const { + return CameraMatrix.rows!=0 && CameraMatrix.cols!=0 && Distorsion.rows!=0 && Distorsion.cols!=0 && CamSize.width!=-1 && CamSize.height!=-1; + } + /**Assign operator + */ + CameraParameters & operator=(const CameraParameters &CI); + /**Reads the camera parameters from a file generated using saveToFile. + */ + void readFromFile(string path)throw(cv::Exception); + /**Saves this to a file + */ + void saveToFile(string path,bool inXML=true)throw(cv::Exception); + + /**Reads from a YAML file generated with the opencv2.2 calibration utility + */ + void readFromXMLFile(string filePath)throw(cv::Exception); + + /**Adjust the parameters to the size of the image indicated + */ + void resize(cv::Size size)throw(cv::Exception); + + /**Returns the location of the camera in the reference system given by the rotation and translation vectors passed + * NOT TESTED + */ + static cv::Point3f getCameraLocation(cv::Mat Rvec,cv::Mat Tvec); + + /**Given the intrinsic camera parameters returns the GL_PROJECTION matrix for opengl. + * PLease NOTE that when using OpenGL, it is assumed no camera distorsion! So, if it is not true, you should have + * undistor image + * + * @param orgImgSize size of the original image + * @param size of the image/window where to render (can be different from the real camera image). Please not that it must be related to CamMatrix + * @param proj_matrix output projection matrix to give to opengl + * @param gnear,gfar: visible rendering range + * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order of glDrawPixels: bottom-to-top. + */ + void glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); + + /** + * setup camera for an Ogre project. + * Use: + * ... + * Ogre::Matrix4 PM(proj_matrix[0], proj_matrix[1], ... , proj_matrix[15]); + * yourCamera->setCustomProjectionMatrix(true, PM); + * yourCamera->setCustomViewMatrix(true, Ogre::Matrix4::IDENTITY); + * ... + * As in OpenGL, it assumes no camera distorsion + */ + void OgreGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); + + +private: + //GL routines + + static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception); + static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception); + static double norm( double a, double b, double c ); + static double dot( double a1, double a2, double a3, + double b1, double b2, double b3 ); + + +}; + +} +#endif + + diff --git a/tracker-aruco/include/cvdrawingutils.h b/tracker-aruco/include/cvdrawingutils.h new file mode 100644 index 00000000..ff67242f --- /dev/null +++ b/tracker-aruco/include/cvdrawingutils.h @@ -0,0 +1,52 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _ArUco_DrawUtils_H_ +#define _ArUco_DrawUtils_H_ +#include "exports.h" +#include "aruco.h" +namespace aruco +{ + /**\brief A set of functions to draw in opencv images + */ + class ARUCO_EXPORTS CvDrawingUtils + { + public: + + static void draw3dAxis(cv::Mat &Image,Marker &m,const CameraParameters &CP); + + static void draw3dCube(cv::Mat &Image,Marker &m,const CameraParameters &CP); + + static void draw3dAxis(cv::Mat &Image,Board &m,const CameraParameters &CP); + + static void draw3dCube(cv::Mat &Image,Board &m,const CameraParameters &CP); + + }; +}; + +#endif + diff --git a/tracker-aruco/include/exports.h b/tracker-aruco/include/exports.h new file mode 100644 index 00000000..8f7dab8c --- /dev/null +++ b/tracker-aruco/include/exports.h @@ -0,0 +1,46 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ + + + +#ifndef __OPENARUCO_CORE_TYPES_H__ +#define __OPENARUCO_CORE_TYPES_H__ + +#if !defined _CRT_SECURE_NO_DEPRECATE && _MSC_VER > 1300 +#define _CRT_SECURE_NO_DEPRECATE /* to avoid multiple Visual Studio 2005 warnings */ +#endif + + +#if ((defined WIN32 || defined _WIN32 || defined WINCE) && defined DSO_EXPORTS) || defined(_MSC_VER) + #define ARUCO_EXPORTS __declspec(dllexport) +#else + #define ARUCO_EXPORTS __attribute__ ((visibility ("default"))) +#endif + + +#endif diff --git a/tracker-aruco/include/marker.h b/tracker-aruco/include/marker.h new file mode 100644 index 00000000..efc3e5fe --- /dev/null +++ b/tracker-aruco/include/marker.h @@ -0,0 +1,143 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _Aruco_Marker_H +#define _Aruco_Marker_H +#include <vector> +#include <iostream> +#include <opencv2/core/core.hpp> +#include "exports.h" +#include "cameraparameters.h" +namespace aruco { +using namespace std; +/**\brief This class represents a marker. It is a vector of the fours corners ot the marker + * + */ + +class ARUCO_EXPORTS Marker: public std::vector<cv::Point2f> +{ +public: + //id of the marker + int id; + //size of the markers sides in meters + float ssize; + //matrices of rotation and translation respect to the camera + cv::Mat Rvec,Tvec; + + /** + */ + Marker(); + /** + */ + Marker(const Marker &M); + /** + */ + Marker(const std::vector<cv::Point2f> &corners,int _id=-1); + /** + */ + ~Marker() {} + /**Indicates if this object is valid + */ + bool isValid()const{return id!=-1 && size()==4;} + + /**Draws this marker in the input image + */ + void draw(cv::Mat &in, cv::Scalar color, int lineWidth=1,bool writeId=true)const; + + /**Calculates the extrinsics (Rvec and Tvec) of the marker with respect to the camera + * @param markerSize size of the marker side expressed in meters + * @param CP parmeters of the camera + * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis + */ + void calculateExtrinsics(float markerSize,const CameraParameters &CP,bool setYPerperdicular=true)throw(cv::Exception); + /**Calculates the extrinsics (Rvec and Tvec) of the marker with respect to the camera + * @param markerSize size of the marker side expressed in meters + * @param CameraMatrix matrix with camera parameters (fx,fy,cx,cy) + * @param Distorsion matrix with distorsion parameters (k1,k2,p1,p2) + * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis + */ + void calculateExtrinsics(float markerSize,cv::Mat CameraMatrix,cv::Mat Distorsion=cv::Mat(),bool setYPerperdicular=true)throw(cv::Exception); + + /**Given the extrinsic camera parameters returns the GL_MODELVIEW matrix for opengl. + * Setting this matrix, the reference coordinate system will be set in this marker + */ + void glGetModelViewMatrix( double modelview_matrix[16])throw(cv::Exception); + + /** + * Returns position vector and orientation quaternion for an Ogre scene node or entity. + * Use: + * ... + * Ogre::Vector3 ogrePos (position[0], position[1], position[2]); + * Ogre::Quaternion ogreOrient (orientation[0], orientation[1], orientation[2], orientation[3]); + * mySceneNode->setPosition( ogrePos ); + * mySceneNode->setOrientation( ogreOrient ); + * ... + */ + void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception); + + /**Returns the centroid of the marker + */ + cv::Point2f getCenter()const; + /**Returns the perimeter of the marker + */ + float getPerimeter()const; + /**Returns the area + */ + float getArea()const; + /** + */ + /** + */ + friend bool operator<(const Marker &M1,const Marker&M2) + { + return M1.id<M2.id; + } + /** + */ + friend ostream & operator<<(ostream &str,const Marker &M) + { + str<<M.id<<"="; + for (int i=0;i<4;i++) + str<<"("<<M[i].x<< ","<<M[i].y<<") "; + str<<"Txyz="; + for (int i=0;i<3;i++) + str<<M.Tvec.ptr<float>(0)[i]<<" "; + str<<"Rxyz="; + for (int i=0;i<3;i++) + str<<M.Rvec.ptr<float>(0)[i]<<" "; + + return str; + } + + +private: + void rotateXAxis(cv::Mat &rotation); + +}; + +} +#endif diff --git a/tracker-aruco/include/markerdetector.h b/tracker-aruco/include/markerdetector.h new file mode 100644 index 00000000..8a7e75ca --- /dev/null +++ b/tracker-aruco/include/markerdetector.h @@ -0,0 +1,359 @@ +/***************************** +Copyright 2011 Rafael Muñoz Salinas. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are +permitted provided that the following conditions are met: + + 1. Redistributions of source code must retain the above copyright notice, this list of + conditions and the following disclaimer. + + 2. Redistributions in binary form must reproduce the above copyright notice, this list + of conditions and the following disclaimer in the documentation and/or other materials + provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED +WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF +ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those of the +authors and should not be interpreted as representing official policies, either expressed +or implied, of Rafael Muñoz Salinas. +********************************/ +#ifndef _ARUCO_MarkerDetector_H +#define _ARUCO_MarkerDetector_H +#include <opencv2/core/core.hpp> +#include <cstdio> +#include <iostream> +#include "cameraparameters.h" +#include "exports.h" +#include "marker.h" + +namespace aruco +{ +using namespace std; +/**\brief Main class for marker detection + * + */ +class ARUCO_EXPORTS MarkerDetector +{ + //Represent a candidate to be a maker + class MarkerCandidate: public Marker{ + public: + MarkerCandidate(){} + MarkerCandidate(const Marker &M): Marker(M){} + MarkerCandidate(const MarkerCandidate &M): Marker(M){ + contour=M.contour; + idx=M.idx; + } + MarkerCandidate & operator=(const MarkerCandidate &M){ + (*(Marker*)this)=(*(Marker*)&M); + contour=M.contour; + idx=M.idx; + return *this; + } + + vector<cv::Point> contour;//all the points of its contour + int idx;//index position in the global contour list + }; +public: + + /** + * See + */ + MarkerDetector(); + + /** + */ + ~MarkerDetector(); + + /**Detects the markers in the image passed + * + * If you provide information about the camera parameters and the size of the marker, then, the extrinsics of the markers are detected + * + * @param input input color image + * @param detectedMarkers output vector with the markers detected + * @param camMatrix intrinsic camera information. + * @param distCoeff camera distorsion coefficient. If set Mat() if is assumed no camera distorion + * @param markerSizeMeters size of the marker sides expressed in meters + * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis + */ + void detect(const cv::Mat &input,std::vector<Marker> &detectedMarkers,cv::Mat camMatrix=cv::Mat(),cv::Mat distCoeff=cv::Mat(),float markerSizeMeters=-1,bool setYPerperdicular=true) throw (cv::Exception); + /**Detects the markers in the image passed + * + * If you provide information about the camera parameters and the size of the marker, then, the extrinsics of the markers are detected + * + * @param input input color image + * @param detectedMarkers output vector with the markers detected + * @param camParams Camera parameters + * @param markerSizeMeters size of the marker sides expressed in meters + * @param setYPerperdicular If set the Y axis will be perpendicular to the surface. Otherwise, it will be the Z axis + */ + void detect(const cv::Mat &input,std::vector<Marker> &detectedMarkers, CameraParameters camParams,float markerSizeMeters=-1,bool setYPerperdicular=true) throw (cv::Exception); + + /**This set the type of thresholding methods available + */ + + enum ThresholdMethods {FIXED_THRES,ADPT_THRES,CANNY}; + + + + /**Sets the threshold method + */ + void setThresholdMethod(ThresholdMethods m) { + _thresMethod=m; + } + /**Returns the current threshold method + */ + ThresholdMethods getThresholdMethod()const { + return _thresMethod; + } + /** + * Set the parameters of the threshold method + * We are currently using the Adptive threshold ee opencv doc of adaptiveThreshold for more info + * @param param1: blockSize of the pixel neighborhood that is used to calculate a threshold value for the pixel + * @param param2: The constant subtracted from the mean or weighted mean + */ + void setThresholdParams(double param1,double param2) { + _thresParam1=param1; + _thresParam2=param2; + } + /** + * Set the parameters of the threshold method + * We are currently using the Adptive threshold ee opencv doc of adaptiveThreshold for more info + * param1: blockSize of the pixel neighborhood that is used to calculate a threshold value for the pixel + * param2: The constant subtracted from the mean or weighted mean + */ + void getThresholdParams(double ¶m1,double ¶m2)const { + param1=_thresParam1; + param2=_thresParam2; + } + + + /**Returns a reference to the internal image thresholded. It is for visualization purposes and to adjust manually + * the parameters + */ + const cv::Mat & getThresholdedImage() { + return thres; + } + /**Methods for corner refinement + */ + enum CornerRefinementMethod {NONE,HARRIS,SUBPIX,LINES}; + /** + */ + void setCornerRefinementMethod(CornerRefinementMethod method) { + _cornerMethod=method; + } + /** + */ + CornerRefinementMethod getCornerRefinementMethod()const { + return _cornerMethod; + } + /**Specifies the min and max sizes of the markers as a fraction of the image size. By size we mean the maximum + * of cols and rows. + * @param min size of the contour to consider a possible marker as valid (0,1] + * @param max size of the contour to consider a possible marker as valid [0,1) + * + */ + void setMinMaxSize(float min=0.03,float max=0.5)throw(cv::Exception); + + /**reads the min and max sizes employed + * @param min output size of the contour to consider a possible marker as valid (0,1] + * @param max output size of the contour to consider a possible marker as valid [0,1) + * + */ + void getMinMaxSize(float &min,float &max){min=_minSize;max=_maxSize;} + + /**Enables/Disables erosion process that is REQUIRED for chessboard like boards. + * By default, this property is enabled + */ + void enableErosion(bool enable){_doErosion=enable;} + + /** + * Specifies a value to indicate the required speed for the internal processes. If you need maximum speed (at the cost of a lower detection rate), + * use the value 3, If you rather a more precise and slow detection, set it to 0. + * + * Actually, the main differences are that in highspeed mode, we employ setCornerRefinementMethod(NONE) and internally, we use a small canonical + * image to detect the marker. In low speed mode, we use setCornerRefinementMethod(HARRIS) and a bigger size for the canonical marker image + */ + void setDesiredSpeed(int val); + /** + */ + int getDesiredSpeed()const { + return _speed; + } + + /** + * Allows to specify the function that identifies a marker. Therefore, you can create your own type of markers different from these + * employed by default in the library. + * The marker function must have the following structure: + * + * int myMarkerIdentifier(const cv::Mat &in,int &nRotations); + * + * The marker function receives the image 'in' with the region that migh contain one of your markers. These are the rectangular regions with black + * in the image. + * + * As output your marker function must indicate the following information. First, the output parameter nRotations must indicate how many times the marker + * must be rotated clockwise 90 deg to be in its ideal position. (The way you would see it when you print it). This is employed to know + * always which is the corner that acts as reference system. Second, the function must return -1 if the image does not contains one of your markers, and its id otherwise. + * + */ + void setMakerDetectorFunction(int (* markerdetector_func)(const cv::Mat &in,int &nRotations) ) { + markerIdDetector_ptrfunc=markerdetector_func; + } + + /** Use an smaller version of the input image for marker detection. + * If your marker is small enough, you can employ an smaller image to perform the detection without noticeable reduction in the precision. + * Internally, we are performing a pyrdown operation + * + * @param level number of times the image size is divided by 2. Internally, we are performing a pyrdown. + */ + void pyrDown(unsigned int level){pyrdown_level=level;} + + ///------------------------------------------------- + /// Methods you may not need + /// Thesde methods do the hard work. They have been set public in case you want to do customizations + ///------------------------------------------------- + + /** + * Thesholds the passed image with the specified method. + */ + void thresHold(int method,const cv::Mat &grey,cv::Mat &thresImg,double param1=-1,double param2=-1)throw(cv::Exception); + /** + * Detection of candidates to be markers, i.e., rectangles. + * This function returns in candidates all the rectangles found in a thresolded image + */ + void detectRectangles(const cv::Mat &thresImg,vector<std::vector<cv::Point2f> > & candidates); + + /**Returns a list candidates to be markers (rectangles), for which no valid id was found after calling detectRectangles + */ + const vector<std::vector<cv::Point2f> > &getCandidates() { + return _candidates; + } + + /**Given the iput image with markers, creates an output image with it in the canonical position + * @param in input image + * @param out image with the marker + * @param size of out + * @param points 4 corners of the marker in the image in + * @return true if the operation succeed + */ + bool warp(cv::Mat &in,cv::Mat &out,cv::Size size, std::vector<cv::Point2f> points)throw (cv::Exception); + + + + /** Refine MarkerCandidate Corner using LINES method + * @param candidate candidate to refine corners + */ + void refineCandidateLines(MarkerCandidate &candidate); + + + /**DEPRECATED!!! Use the member function in CameraParameters + * + * Given the intrinsic camera parameters returns the GL_PROJECTION matrix for opengl. + * PLease NOTE that when using OpenGL, it is assumed no camera distorsion! So, if it is not true, you should have + * undistor image + * + * @param CamMatrix arameters of the camera specified. + * @param orgImgSize size of the original image + * @param size of the image/window where to render (can be different from the real camera image). Please not that it must be related to CamMatrix + * @param proj_matrix output projection matrix to give to opengl + * @param gnear,gfar: visible rendering range + * @param invert: indicates if the output projection matrix has to yield a horizontally inverted image because image data has not been stored in the order of glDrawPixels: bottom-to-top. + */ + static void glGetProjectionMatrix( CameraParameters & CamMatrix,cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert=false )throw(cv::Exception); + +private: + + bool _enableCylinderWarp; + bool warp_cylinder ( cv::Mat &in,cv::Mat &out,cv::Size size, MarkerCandidate& mc ) throw ( cv::Exception ); + /** + * Detection of candidates to be markers, i.e., rectangles. + * This function returns in candidates all the rectangles found in a thresolded image + */ + void detectRectangles(const cv::Mat &thresImg,vector<MarkerCandidate> & candidates); +public: + //Current threshold method + ThresholdMethods _thresMethod; + //Threshold parameters + double _thresParam1,_thresParam2; + //Current corner method + CornerRefinementMethod _cornerMethod; + //minimum and maximum size of a contour lenght + float _minSize,_maxSize; + //Speed control + int _speed; + int _markerWarpSize; + bool _doErosion; + //vectr of candidates to be markers. This is a vector with a set of rectangles that have no valid id + vector<std::vector<cv::Point2f> > _candidates; + //level of image reduction + int pyrdown_level; + //Images + cv::Mat grey,thres,thres2,reduced; + //pointer to the function that analizes a rectangular region so as to detect its internal marker + int (* markerIdDetector_ptrfunc)(const cv::Mat &in,int &nRotations); +private: + /** + */ + bool isInto(cv::Mat &contour,std::vector<cv::Point2f> &b); + /** + */ + int perimeter(std::vector<cv::Point2f> &a); + + +// //GL routines +// +// static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception); +// static int arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception); +// static double norm( double a, double b, double c ); +// static double dot( double a1, double a2, double a3, +// double b1, double b2, double b3 ); +// + + //detection of the + void findBestCornerInRegion_harris(const cv::Mat & grey,vector<cv::Point2f> & Corners,int blockSize); + + + // auxiliar functions to perform LINES refinement + void interpolate2Dline( const vector< cv::Point > &inPoints, cv::Point3f &outLine); + cv::Point2f getCrossPoint(const cv::Point3f& line1, const cv::Point3f& line2); + + + /**Given a vector vinout with elements and a boolean vector indicating the lements from it to remove, + * this function remove the elements + * @param vinout + * @param toRemove + */ + template<typename T> + void removeElements(vector<T> & vinout,const vector<bool> &toRemove) + { + //remove the invalid ones by setting the valid in the positions left by the invalids + size_t indexValid=0; + for (size_t i=0;i<toRemove.size();i++) { + if (!toRemove[i]) { + if (indexValid!=i) vinout[indexValid]=vinout[i]; + indexValid++; + } + } + vinout.resize(indexValid); + } + + //graphical debug + void drawApproxCurve(cv::Mat &in,std::vector<cv::Point> &approxCurve ,cv::Scalar color); + void drawContour(cv::Mat &in,std::vector<cv::Point> &contour,cv::Scalar ); + void drawAllContours(cv::Mat input, std::vector<std::vector<cv::Point> > &contours); + void draw(cv::Mat out,const std::vector<Marker> &markers ); + +}; + + + + +} +#endif diff --git a/tracker-aruco/trans_calib.cpp b/tracker-aruco/trans_calib.cpp new file mode 100644 index 00000000..b5148efd --- /dev/null +++ b/tracker-aruco/trans_calib.cpp @@ -0,0 +1,41 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "trans_calib.h" + +TranslationCalibrator::TranslationCalibrator() +{ + reset(); +} + +void TranslationCalibrator::reset() +{ + P = cv::Matx66f::zeros(); + y = cv::Vec6f(0,0,0, 0,0,0); +} + +void TranslationCalibrator::update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k) +{ + cv::Matx<double, 6,3> H_k_T = cv::Matx<double, 6,3>::zeros(); + for (int i=0; i<3; ++i) { + for (int j=0; j<3; ++j) { + H_k_T(i,j) = R_CM_k(j,i); + } + } + for (int i=0; i<3; ++i) + { + H_k_T(3+i,i) = 1.0; + } + P += H_k_T * H_k_T.t(); + y += H_k_T * t_CM_k; +} + +cv::Vec3f TranslationCalibrator::get_estimate() +{ + cv::Vec6f x = P.inv() * y; + return cv::Vec3f(x[0], x[1], x[2]); +} diff --git a/tracker-aruco/trans_calib.h b/tracker-aruco/trans_calib.h new file mode 100644 index 00000000..3d1e56fa --- /dev/null +++ b/tracker-aruco/trans_calib.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2012 Patrick Ruoff + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#ifndef TRANSCALIB_H +#define TRANSCALIB_H + +#include <opencv2/core/core.hpp> + +//----------------------------------------------------------------------------- +// Calibrates the translation from head to model = t_MH +// by recursive least squares / +// kalman filter in information form with identity noise covariance +// measurement equation when head position = t_CH is fixed: +// (R_CM_k , Id)*(-t_MH, t_CH) = t_CM_k + +class TranslationCalibrator +{ +public: + TranslationCalibrator(); + + // reset the calibration process + void reset(); + + // update the current estimate + void update(const cv::Matx33d& R_CM_k, const cv::Vec3d& t_CM_k); + + // get the current estimate for t_MH + cv::Vec3f get_estimate(); + +private: + cv::Matx66f P; // normalized precision matrix = inverse covariance + cv::Vec6f y; // P*(-t_MH, t_CH) +}; + +#endif //TRANSCALIB_H |
