summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco
diff options
context:
space:
mode:
Diffstat (limited to 'ftnoir_tracker_aruco')
-rw-r--r--ftnoir_tracker_aruco/ar_video_widget.cpp12
-rw-r--r--ftnoir_tracker_aruco/ar_video_widget.h22
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui425
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp457
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.h59
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco_dll.h3
-rw-r--r--ftnoir_tracker_aruco/include/boarddetector.h2
-rw-r--r--ftnoir_tracker_aruco/include/cameraparameters.h6
-rw-r--r--ftnoir_tracker_aruco/include/cvdrawingutils.h2
-rw-r--r--ftnoir_tracker_aruco/include/exports.h6
-rw-r--r--ftnoir_tracker_aruco/include/marker.h16
-rw-r--r--ftnoir_tracker_aruco/include/markerdetector.h64
-rw-r--r--ftnoir_tracker_aruco/trans_calib.cpp44
-rw-r--r--ftnoir_tracker_aruco/trans_calib.h39
14 files changed, 578 insertions, 579 deletions
diff --git a/ftnoir_tracker_aruco/ar_video_widget.cpp b/ftnoir_tracker_aruco/ar_video_widget.cpp
index 9a089213..61a611ea 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.cpp
+++ b/ftnoir_tracker_aruco/ar_video_widget.cpp
@@ -40,3 +40,15 @@ void ArucoVideoWidget::update_and_repaint()
texture = qframe2;
update();
}
+
+void ArucoVideoWidget::paintEvent(QPaintEvent* e)
+{
+ QMutexLocker foo(&mtx);
+ QPainter(this).drawImage(e->rect(), texture);
+}
+
+ArucoVideoWidget::ArucoVideoWidget(QWidget* parent): QWidget(parent)
+{
+ connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
+ timer.start(60);
+} \ No newline at end of file
diff --git a/ftnoir_tracker_aruco/ar_video_widget.h b/ftnoir_tracker_aruco/ar_video_widget.h
index e2cf4d9f..820ba7d0 100644
--- a/ftnoir_tracker_aruco/ar_video_widget.h
+++ b/ftnoir_tracker_aruco/ar_video_widget.h
@@ -22,26 +22,18 @@
class ArucoVideoWidget : public QWidget
{
Q_OBJECT
-
-public:
- ArucoVideoWidget(QWidget *parent) : QWidget(parent) {
- connect(&timer, SIGNAL(timeout()), this, SLOT(update_and_repaint()));
- timer.start(60);
- }
- void update_image(const cv::Mat& frame);
-protected slots:
- void paintEvent( QPaintEvent* e ) {
- QMutexLocker foo(&mtx);
- QPainter painter(this);
- painter.drawImage(e->rect(), texture);
- }
- void update_and_repaint();
-
+
private:
QMutex mtx;
QImage texture;
QTimer timer;
cv::Mat _frame;
+private slots:
+ void update_and_repaint();
+public:
+ ArucoVideoWidget(QWidget *parent);
+ void update_image(const cv::Mat& frame);
+ void paintEvent( QPaintEvent*) override;
};
#endif // VIDEOWIDGET_H
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 1d5a4241..099dec02 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -9,8 +9,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>615</width>
- <height>326</height>
+ <width>562</width>
+ <height>178</height>
</rect>
</property>
<property name="sizePolicy">
@@ -19,67 +19,160 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="maximumSize">
- <size>
- <width>6666</width>
- <height>6666</height>
- </size>
- </property>
<property name="windowTitle">
- <string>HT tracker settings</string>
+ <string>Tracker settings</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <property name="spacing">
- <number>7</number>
- </property>
- <item row="9" column="1" rowspan="3">
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Head position</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <item row="1" column="1">
+ <widget class="QDialogButtonBox" name="buttonBox">
+ <property name="standardButtons">
+ <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
</property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="2" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>TY</string>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QFrame" name="frame">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="1">
+ <widget class="QDoubleSpinBox" 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">
+ <double>35.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>180.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>52.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="0">
- <widget class="QLabel" name="label_5">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Frames per second</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>TX</string>
+ <string>Horizontal FOV</string>
</property>
</widget>
</item>
<item row="1" column="1">
- <widget class="QDoubleSpinBox" name="cx">
+ <widget class="QComboBox" name="cameraFPS">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="minimum">
- <double>-10000.000000000000000</double>
+ <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>120</string>
+ </property>
+ </item>
+ <item>
+ <property name="text">
+ <string>180</string>
+ </property>
+ </item>
+ </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>
- <property name="maximum">
- <double>10000.000000000000000</double>
+ <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_8">
+ <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>
+ </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>TZ</string>
+ <string>Head X</string>
</property>
</widget>
</item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="cz">
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="cy">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Minimum">
<horstretch>0</horstretch>
@@ -94,8 +187,8 @@
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="cy">
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="cx">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Minimum">
<horstretch>0</horstretch>
@@ -110,264 +203,48 @@
</property>
</widget>
</item>
- </layout>
- </widget>
- </item>
- <item row="0" column="3" rowspan="3" colspan="2">
- <widget class="QGroupBox" name="groupBox">
- <property name="title">
- <string>Enable axes</string>
- </property>
- <layout class="QFormLayout" name="formLayout">
- <property name="horizontalSpacing">
- <number>7</number>
- </property>
- <property name="leftMargin">
- <number>6</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>6</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QCheckBox" name="rx">
- <property name="text">
- <string>RX</string>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="cz">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QCheckBox" name="tx">
- <property name="text">
- <string>TX</string>
+ <property name="minimum">
+ <double>-10000.000000000000000</double>
</property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QCheckBox" name="ry">
- <property name="text">
- <string>RY</string>
+ <property name="maximum">
+ <double>10000.000000000000000</double>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QCheckBox" name="ty">
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_8">
<property name="text">
- <string>TY</string>
+ <string>Head Z </string>
</property>
</widget>
</item>
<item row="2" column="0">
- <widget class="QCheckBox" name="rz">
+ <widget class="QLabel" name="label_7">
<property name="text">
- <string>RZ</string>
+ <string>Head Y</string>
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QCheckBox" name="tz">
+ <item row="4" column="1">
+ <widget class="QPushButton" name="btn_calibrate">
<property name="text">
- <string>TZ</string>
+ <string>Calibrate</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>Frames per second</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1" colspan="2">
- <widget class="QComboBox" name="cameraName"/>
- </item>
- <item row="1" column="1" colspan="2">
- <widget class="QComboBox" name="cameraFPS">
- <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>120</string>
- </property>
- </item>
- <item>
- <property name="text">
- <string>180</string>
- </property>
- </item>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QCheckBox" name="red_only">
- <property name="text">
- <string>Recommended!</string>
- </property>
- </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="0" column="1" colspan="2">
- <widget class="QDoubleSpinBox" name="cameraFOV">
- <property name="locale">
- <locale language="English" country="UnitedStates"/>
- </property>
- <property name="minimum">
- <double>35.000000000000000</double>
- </property>
- <property name="maximum">
- <double>180.000000000000000</double>
- </property>
- <property name="value">
- <double>52.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="2" rowspan="5" colspan="3">
- <widget class="QLabel" name="label_6">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;The ARUCO Library has been developed by the Ava group of the Univeristy of Cordoba, Spain&lt;/p&gt;&lt;p&gt;Rafael Muñoz Salinas &amp;lt;&lt;a href=&quot;mailto:rmsalinas@uco.es&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;rmsalinas@uco.es&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;p&gt;&amp;lt;&lt;a href=&quot;https://github.com/rmsalinas/aruco&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;https://github.com/rmsalinas/aruco&lt;/span&gt;&lt;/a&gt;&amp;gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
- </property>
- <property name="textFormat">
- <enum>Qt::RichText</enum>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
- </property>
- <property name="wordWrap">
- <bool>true</bool>
- </property>
- <property name="margin">
- <number>4</number>
- </property>
- <property name="openExternalLinks">
- <bool>true</bool>
- </property>
- <property name="textInteractionFlags">
- <set>Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Red channel only</string>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="QComboBox" name="resolution">
- <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="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Horizontal FOV</string>
- </property>
- </widget>
- </item>
- <item row="11" column="4">
- <widget class="QDialogButtonBox" name="buttonBox">
- <property name="standardButtons">
- <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QDoubleSpinBox" name="marker_pitch">
- <property name="minimum">
- <double>-180.000000000000000</double>
- </property>
- <property name="maximum">
- <double>180.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Resolution</string>
- </property>
- </widget>
- </item>
- <item row="8" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Marker pitch</string>
- </property>
- </widget>
- </item>
</layout>
</widget>
- <tabstops>
- <tabstop>cameraFOV</tabstop>
- <tabstop>cameraFPS</tabstop>
- <tabstop>cameraName</tabstop>
- <tabstop>resolution</tabstop>
- <tabstop>red_only</tabstop>
- <tabstop>marker_pitch</tabstop>
- <tabstop>cx</tabstop>
- <tabstop>cy</tabstop>
- <tabstop>cz</tabstop>
- <tabstop>rx</tabstop>
- <tabstop>ry</tabstop>
- <tabstop>rz</tabstop>
- <tabstop>tx</tabstop>
- <tabstop>ty</tabstop>
- <tabstop>tz</tabstop>
- <tabstop>buttonBox</tabstop>
- </tabstops>
<resources/>
<connections/>
<designerdata>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index ae7ca0b5..fd8a8ce1 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -5,17 +5,16 @@
* copyright notice and this permission notice appear in all copies.
*/
-#include "ftnoir_tracker_base/ftnoir_tracker_base.h"
-#include "ftnoir_tracker_aruco.h"
-#include "ui_aruco-trackercontrols.h"
-#include "facetracknoir/global-settings.h"
-#include <cmath>
-#include <QMutexLocker>
-#include <aruco.h>
-#include <opencv2/opencv.hpp>
-#include <opencv/highgui.h>
#include <vector>
#include <cstdio>
+#include <cmath>
+#include <algorithm>
+#include <QMutexLocker>
+#include "./include/markerdetector.h"
+#include "ftnoir_tracker_aruco.h"
+#include "facetracknoir/plugin-api.hpp"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
#if defined(_WIN32)
# undef NOMINMAX
@@ -30,51 +29,51 @@
static QList<QString> get_camera_names(void) {
QList<QString> ret;
#if defined(_WIN32)
- // Create the System Device Enumerator.
- HRESULT hr;
- ICreateDevEnum *pSysDevEnum = NULL;
- hr = CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER, IID_ICreateDevEnum, (void **)&pSysDevEnum);
- if (FAILED(hr))
- {
- return ret;
- }
- // Obtain a class enumerator for the video compressor category.
- IEnumMoniker *pEnumCat = NULL;
- hr = pSysDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &pEnumCat, 0);
-
- if (hr == S_OK) {
- // Enumerate the monikers.
- IMoniker *pMoniker = NULL;
- ULONG cFetched;
- while (pEnumCat->Next(1, &pMoniker, &cFetched) == S_OK) {
- IPropertyBag *pPropBag;
- hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void **)&pPropBag);
- if (SUCCEEDED(hr)) {
- // To retrieve the filter's friendly name, do the following:
- VARIANT varName;
- VariantInit(&varName);
- hr = pPropBag->Read(L"FriendlyName", &varName, 0);
- if (SUCCEEDED(hr))
- {
- // Display the name in your UI somehow.
- QString str((QChar*)varName.bstrVal, wcslen(varName.bstrVal));
- ret.append(str);
- }
- VariantClear(&varName);
-
- ////// To create an instance of the filter, do the following:
- ////IBaseFilter *pFilter;
- ////hr = pMoniker->BindToObject(NULL, NULL, IID_IBaseFilter,
- //// (void**)&pFilter);
- // Now add the filter to the graph.
- //Remember to release pFilter later.
- pPropBag->Release();
- }
- pMoniker->Release();
- }
- pEnumCat->Release();
- }
- pSysDevEnum->Release();
+ // Create the System Device Enumerator.
+ HRESULT hr;
+ ICreateDevEnum *pSysDevEnum = NULL;
+ hr = CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER, IID_ICreateDevEnum, (void **)&pSysDevEnum);
+ if (FAILED(hr))
+ {
+ return ret;
+ }
+ // Obtain a class enumerator for the video compressor category.
+ IEnumMoniker *pEnumCat = NULL;
+ hr = pSysDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &pEnumCat, 0);
+
+ if (hr == S_OK) {
+ // Enumerate the monikers.
+ IMoniker *pMoniker = NULL;
+ ULONG cFetched;
+ while (pEnumCat->Next(1, &pMoniker, &cFetched) == S_OK) {
+ IPropertyBag *pPropBag;
+ hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void **)&pPropBag);
+ if (SUCCEEDED(hr)) {
+ // To retrieve the filter's friendly name, do the following:
+ VARIANT varName;
+ VariantInit(&varName);
+ hr = pPropBag->Read(L"FriendlyName", &varName, 0);
+ if (SUCCEEDED(hr))
+ {
+ // Display the name in your UI somehow.
+ QString str((QChar*)varName.bstrVal, wcslen(varName.bstrVal));
+ ret.append(str);
+ }
+ VariantClear(&varName);
+
+ ////// To create an instance of the filter, do the following:
+ ////IBaseFilter *pFilter;
+ ////hr = pMoniker->BindToObject(NULL, NULL, IID_IBaseFilter,
+ //// (void**)&pFilter);
+ // Now add the filter to the graph.
+ //Remember to release pFilter later.
+ pPropBag->Release();
+ }
+ pMoniker->Release();
+ }
+ pEnumCat->Release();
+ }
+ pSysDevEnum->Release();
#else
for (int i = 0; i < 16; i++) {
char buf[128];
@@ -90,15 +89,15 @@ static QList<QString> get_camera_names(void) {
}
typedef struct {
- int width;
- int height;
+ int width;
+ int height;
} resolution_tuple;
static resolution_tuple resolution_choices[] = {
- { 640, 480 },
- { 320, 240 },
- { 320, 200 },
- { 0, 0 }
+ { 640, 480 },
+ { 320, 240 },
+ { 320, 200 },
+ { 0, 0 }
};
Tracker::Tracker() : stop(false), layout(nullptr), videoWidget(nullptr)
@@ -109,8 +108,8 @@ Tracker::~Tracker()
{
stop = true;
wait();
- if (videoWidget)
- delete videoWidget;
+ if (videoWidget)
+ delete videoWidget;
if(layout)
delete layout;
qDebug() << "releasing camera, brace for impact";
@@ -137,12 +136,20 @@ void Tracker::StartTracker(QFrame* videoframe)
#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 res = s.resolution;
- if (res < 0 || res >= (int)(sizeof(resolution_choices) / sizeof(resolution_tuple)))
- res = 0;
- resolution_tuple r = resolution_choices[res];
+ 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))
{
@@ -164,23 +171,19 @@ void Tracker::run()
break;
}
camera = cv::VideoCapture(s.camera_index);
- if (r.width)
+ if (res.width)
{
- camera.set(CV_CAP_PROP_FRAME_WIDTH, r.width);
- camera.set(CV_CAP_PROP_FRAME_HEIGHT, r.height);
+ 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);
-
+
aruco::MarkerDetector detector;
detector.setDesiredSpeed(3);
cv::Rect last_roi(65535, 65535, 0, 0);
- cv::Mat color, color_, grayscale, rvec, tvec;
-
- const double stateful_coeff = 0.88;
-
if (!camera.isOpened())
{
fprintf(stderr, "aruco tracker: can't open camera\n");
@@ -191,23 +194,17 @@ void Tracker::run()
auto last_time = cv::getTickCount();
int cur_fps = 0;
int last_fps = 0;
- cv::Point2f last_centroid;
- bool first = true;
while (!stop)
{
- if (!camera.read(color_))
+ cv::Mat color;
+ if (!camera.read(color))
continue;
auto tm = cv::getTickCount();
- color_.copyTo(color);
- if (s.red_only)
- {
- cv::Mat channel[3];
- cv::split(color, channel);
- grayscale = channel[2];
- } else
- cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
-
+
+ cv::Mat grayscale;
+ cv::cvtColor(color, grayscale, cv::COLOR_BGR2GRAY);
+
const int scale = frame.cols > 480 ? 2 : 1;
detector.setThresholdParams(scale > 1 ? 11 : 7, 4);
@@ -225,27 +222,33 @@ void Tracker::run()
const double size_min = 0.04;
const double size_max = 0.38;
-
- if (last_roi.width > 0 &&
- (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
- markers.size() == 1 && markers[0].size() == 4))
+
+ bool roi_valid = false;
+
+ if (last_roi.width > 0 && last_roi.height)
{
detector.setMinMaxSize(std::max(0.01, size_min * grayscale.cols / last_roi.width),
std::min(1.0, size_max * grayscale.cols / last_roi.width));
- auto& m = markers.at(0);
- for (int i = 0; i < 4; i++)
+ if (detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false),
+ markers.size() == 1 && markers[0].size() == 4)
{
- auto& p = m.at(i);
- p.x += last_roi.x;
- p.y += last_roi.y;
+ 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;
}
}
- else
+
+ if (!roi_valid)
{
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++)
@@ -268,97 +271,122 @@ void Tracker::run()
frame = color.clone();
::sprintf(buf, "Hz: %d", last_fps);
- cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), scale);
+ cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(0, 255, 0), 1);
::sprintf(buf, "Jiffies: %ld", (long) (10000 * (time - tm) / freq));
- cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), scale);
-
+ cv::putText(frame, buf, cv::Point(10, 54), cv::FONT_HERSHEY_PLAIN, scale, cv::Scalar(80, 255, 0), 1);
+
if (markers.size() == 1 && markers[0].size() == 4) {
const auto& m = markers.at(0);
const float size = 40;
-
- const double p = s.marker_pitch;
- const double sq = sin(p * HT_PI / 180);
- const double cq = cos(p * HT_PI / 180);
cv::Mat obj_points(4,3,CV_32FC1);
- obj_points.at<float>(1,0)=-size + s.headpos_x;
- obj_points.at<float>(1,1)=-size * cq + s.headpos_y;
- obj_points.at<float>(1,2)=-size * sq + s.headpos_z;
- obj_points.at<float>(2,0)=size + s.headpos_x;
- obj_points.at<float>(2,1)=-size * cq + s.headpos_y;
- obj_points.at<float>(2,2)=-size * sq + s.headpos_z;
- obj_points.at<float>(3,0)=size + s.headpos_x;
- obj_points.at<float>(3,1)=size * cq + s.headpos_y;
- obj_points.at<float>(3,2)=size * sq + s.headpos_z;
- obj_points.at<float>(0,0)=-size + s.headpos_x;
- obj_points.at<float>(0,1)=size * cq + s.headpos_y;
- obj_points.at<float>(0,2)=size * sq + s.headpos_z;
-
- last_roi = cv::Rect(65535, 65535, 0, 0);
-
+ 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;
+
+ cv::Vec3d rvec, tvec;
+
+ cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::P3P);
+
+ std::vector<cv::Point2f> roi_projection(4);
+
+ {
+ 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++)
{
- auto foo = m.at(i);
- last_roi.x = std::min<int>(foo.x, last_roi.x);
- last_roi.y = std::min<int>(foo.y, last_roi.y);
- last_roi.width = std::max<int>(foo.x, last_roi.width);
- last_roi.height = std::max<int>(foo.y, last_roi.height);
+ 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::P3P);
+ tvec = tvec_;
+ }
+
+ cv::Mat roi_points = obj_points * c_search_window;
+ 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++)
{
- last_roi.width -= last_roi.x;
- last_roi.height -= last_roi.y;
- last_roi.x -= last_roi.width * stateful_coeff;
- last_roi.y -= last_roi.height * stateful_coeff;
- last_roi.width *= stateful_coeff * 3;
- last_roi.height *= stateful_coeff * 3;
- last_roi.x = std::max<int>(0, last_roi.x);
- last_roi.y = std::max<int>(0, last_roi.y);
- last_roi.width = std::min<int>(grayscale.cols - last_roi.x, last_roi.width);
- last_roi.height = std::min<int>(grayscale.rows - last_roi.y, last_roi.height);
+ 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+1 > color.cols)
+ last_roi.width = color.cols-1;
+
+ if (last_roi.height+1 > color.rows)
+ last_roi.height = color.rows-1;
- 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);
+ 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(rotation_matrix, junk1, junk2);
-
- if (fabs(euler[0]) + fabs(s.marker_pitch) > 60)
- {
- first = true;
- qDebug() << "reset levmarq due to pitch breakage";
- }
+ cv::Vec3d euler = cv::RQDecomp3x3(rmat, m_r, m_q);
QMutexLocker lck(&mtx);
for (int i = 0; i < 3; i++)
- pose[i] = tvec.at<double>(i);
-
+ pose[i] = tvec(i);
pose[Yaw] = euler[1];
pose[Pitch] = -euler[0];
pose[Roll] = euler[2];
- }
- 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);
+ r = rmat;
+ t = tvec;
}
-
- last_centroid = repr2[0];
+
+ if (roi_valid)
+ cv::rectangle(frame, last_roi, cv::Scalar(255, 0, 255), 1);
}
else
- {
last_roi = cv::Rect(65535, 65535, 0, 0);
- first = true;
- }
if (frame.rows > 0)
videoWidget->update_image(frame);
@@ -368,28 +396,22 @@ void Tracker::run()
void Tracker::GetHeadPoseData(double *data)
{
QMutexLocker lck(&mtx);
-
- if (s.eyaw)
- data[Yaw] = pose[Yaw];
- if (s.epitch)
- data[Pitch] = pose[Pitch];
- if (s.eroll)
- data[Roll] = pose[Roll];
- if (s.ex)
- data[TX] = pose[TX] * .1;
- if (s.ey)
- data[TY] = pose[TY] * .1;
- if (s.ez)
- data[TZ] = pose[TZ] * .1;
+
+ 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;
}
class TrackerDll : public Metadata
{
- // ITrackerDll interface
- void getFullName(QString *strToBeFilled);
- void getShortName(QString *strToBeFilled);
- void getDescription(QString *strToBeFilled);
- void getIcon(QIcon *icon);
+ // ITrackerDll interface
+ void getFullName(QString *strToBeFilled);
+ void getShortName(QString *strToBeFilled);
+ void getDescription(QString *strToBeFilled);
+ void getIcon(QIcon *icon);
};
//-----------------------------------------------------------------------------
@@ -400,12 +422,12 @@ void TrackerDll::getFullName(QString *strToBeFilled)
void TrackerDll::getShortName(QString *strToBeFilled)
{
- *strToBeFilled = "aruco";
+ *strToBeFilled = "aruco";
}
void TrackerDll::getDescription(QString *strToBeFilled)
{
- *strToBeFilled = "";
+ *strToBeFilled = "";
}
void TrackerDll::getIcon(QIcon *icon)
@@ -417,28 +439,19 @@ void TrackerDll::getIcon(QIcon *icon)
//-----------------------------------------------------------------------------
//#pragma comment(linker, "/export:GetTrackerDll=_GetTrackerDll@0")
-extern "C" FTNOIR_TRACKER_BASE_EXPORT Metadata* CALLING_CONVENTION GetMetadata()
+extern "C" OPENTRACK_EXPORT Metadata* GetMetadata()
{
- return new TrackerDll;
+ return new TrackerDll;
}
//#pragma comment(linker, "/export:GetTracker=_GetTracker@0")
-extern "C" FTNOIR_TRACKER_BASE_EXPORT ITracker* CALLING_CONVENTION GetConstructor()
+extern "C" OPENTRACK_EXPORT ITracker* GetConstructor()
{
return new Tracker;
}
-////////////////////////////////////////////////////////////////////////////////
-// Factory function that creates instances if the Tracker-settings dialog object.
-
-// Export both decorated and undecorated names.
-// GetTrackerDialog - Undecorated name, which can be easily used with GetProcAddress
-// Win32 API function.
-// _GetTrackerDialog@0 - Common name decoration for __stdcall functions in C language.
-//#pragma comment(linker, "/export:GetTrackerDialog=_GetTrackerDialog@0")
-
-extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDialog( )
+extern "C" OPENTRACK_EXPORT ITrackerDialog* GetDialog( )
{
return new TrackerControls;
}
@@ -446,26 +459,54 @@ extern "C" FTNOIR_TRACKER_BASE_EXPORT ITrackerDialog* CALLING_CONVENTION GetDial
TrackerControls::TrackerControls()
{
tracker = nullptr;
- ui.setupUi(this);
+ calib_timer.setInterval(200);
+ ui.setupUi(this);
setAttribute(Qt::WA_NativeWindow, true);
+ ui.cameraName->addItems(get_camera_names());
tie_setting(s.camera_index, ui.cameraName);
- tie_setting(s.resolution, ui.resolution);
+ tie_setting(s.resolution, ui.resolution);
tie_setting(s.force_fps, ui.cameraFPS);
tie_setting(s.fov, ui.cameraFOV);
- tie_setting(s.eyaw, ui.rx);
- tie_setting(s.epitch, ui.ry);
- tie_setting(s.eroll, ui.rz);
- tie_setting(s.ex, ui.tx);
- tie_setting(s.ey, ui.ty);
- tie_setting(s.ez, ui.tz);
tie_setting(s.headpos_x, ui.cx);
tie_setting(s.headpos_y, ui.cy);
tie_setting(s.headpos_z, ui.cz);
- tie_setting(s.red_only, ui.red_only);
- tie_setting(s.marker_pitch, ui.marker_pitch);
connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
- ui.cameraName->addItems(get_camera_names());
+ 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()));
+}
+
+void TrackerControls::toggleCalibrate()
+{
+ if (!calib_timer.isActive())
+ {
+ calibrator.reset();
+ calib_timer.start();
+ } else {
+ cleanupCalib();
+ }
+}
+
+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);
+ auto pos = calibrator.get_estimate() * .1;
+ s.headpos_x = pos(0);
+ s.headpos_y = pos(1);
+ s.headpos_z = pos(2);
+ }
}
void TrackerControls::doOK()
@@ -473,11 +514,11 @@ void TrackerControls::doOK()
s.b->save();
if (tracker)
tracker->reload();
- this->close();
+ this->close();
}
void TrackerControls::doCancel()
{
- s.b->revert();
+ s.b->reload();
this->close();
}
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
index 4cab84b5..3d37dacd 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h
@@ -5,10 +5,8 @@
* copyright notice and this permission notice appear in all copies.
*/
-#ifndef FTNOIR_TRACKER_HT_H
-#define FTNOIR_TRACKER_HT_H
+#pragma once
-#include "ftnoir_tracker_base/ftnoir_tracker_base.h"
#include "ui_aruco-trackercontrols.h"
#include "ar_video_widget.h"
#include <QObject>
@@ -16,18 +14,20 @@
#include <QMutex>
#include <QHBoxLayout>
#include <QDialog>
-#include <opencv2/opencv.hpp>
-#include <opencv/highgui.h>
+#include <QTimer>
#include "facetracknoir/options.h"
+#include "ftnoir_tracker_aruco/trans_calib.h"
+#include "facetracknoir/plugin-api.hpp"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
using namespace options;
struct settings {
pbundle b;
value<double> fov, headpos_x, headpos_y, headpos_z;
value<int> camera_index, force_fps, resolution;
- value<bool> red_only;
- value<bool> eyaw, epitch, eroll, ex, ey, ez;
- value<double> marker_pitch;
settings() :
b(bundle("aruco-tracker")),
fov(b, "field-of-view", 56),
@@ -36,59 +36,52 @@ struct settings {
headpos_z(b, "headpos-z", 0),
camera_index(b, "camera-index", 0),
force_fps(b, "force-fps", 0),
- resolution(b, "force-resolution", 0),
- red_only(b, "red-only", false),
- eyaw(b, "enable-y", true),
- epitch(b, "enable-p", true),
- eroll(b, "enable-r", true),
- ex(b, "enable-x", true),
- ey(b, "enable-y", true),
- ez(b, "enable-z", true),
- marker_pitch(b, "marker-pitch", 0)
+ resolution(b, "force-resolution", 0)
{}
};
class Tracker : protected QThread, public ITracker
{
Q_OBJECT
+ static constexpr double c_search_window = 2.2;
public:
- Tracker();
- virtual ~Tracker();
+ Tracker();
+ ~Tracker() override;
void StartTracker(QFrame* frame);
void GetHeadPoseData(double *data);
void run();
void reload() { s.b->reload(); }
+ void getRT(cv::Matx33d &r, cv::Vec3d &t);
private:
QMutex mtx;
volatile bool stop;
QHBoxLayout* layout;
- ArucoVideoWidget* videoWidget;
+ ArucoVideoWidget* videoWidget;
settings s;
double pose[6];
cv::Mat frame;
cv::VideoCapture camera;
+ cv::Matx33d r;
+ cv::Vec3d t;
};
-// Widget that has controls for FTNoIR protocol client-settings.
class TrackerControls : public QWidget, public ITrackerDialog
{
Q_OBJECT
public:
TrackerControls();
- void registerTracker(ITracker * x) {
- tracker = dynamic_cast<Tracker*>(x);
- }
- void unRegisterTracker() {
- tracker = nullptr;
- }
+ void registerTracker(ITracker * x) { tracker = dynamic_cast<Tracker*>(x); }
+ void unRegisterTracker() { tracker = nullptr; }
private:
- Ui::Form ui;
+ Ui::Form ui;
Tracker* tracker;
settings s;
+ TranslationCalibrator calibrator;
+ QTimer calib_timer;
private slots:
- void doOK();
- void doCancel();
+ void doOK();
+ void doCancel();
+ void toggleCalibrate();
+ void cleanupCalib();
+ void update_tracker_calibration();
};
-
-#endif
-
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco_dll.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco_dll.h
index ffdc5262..66e57100 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco_dll.h
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco_dll.h
@@ -5,8 +5,7 @@
* copyright notice and this permission notice appear in all copies.
*/
-#include "ftnoir_tracker_base/ftnoir_tracker_base.h"
-#include "facetracknoir/global-settings.h"
+#include "facetracknoir/plugin-api.hpp"
//-----------------------------------------------------------------------------
class TrackerDll : public Metadata
diff --git a/ftnoir_tracker_aruco/include/boarddetector.h b/ftnoir_tracker_aruco/include/boarddetector.h
index 4770b5c9..a0ee2361 100644
--- a/ftnoir_tracker_aruco/include/boarddetector.h
+++ b/ftnoir_tracker_aruco/include/boarddetector.h
@@ -134,6 +134,6 @@ private:
};
-}
+};
#endif
diff --git a/ftnoir_tracker_aruco/include/cameraparameters.h b/ftnoir_tracker_aruco/include/cameraparameters.h
index c3381a74..a419afbe 100644
--- a/ftnoir_tracker_aruco/include/cameraparameters.h
+++ b/ftnoir_tracker_aruco/include/cameraparameters.h
@@ -28,7 +28,7 @@ or implied, of Rafael Muñoz Salinas.
#ifndef _Aruco_CameraParameters_H
#define _Aruco_CameraParameters_H
#include "exports.h"
-#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
#include <string>
using namespace std;
namespace aruco
@@ -105,7 +105,7 @@ public:
* @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:
@@ -117,7 +117,7 @@ public:
* 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
diff --git a/ftnoir_tracker_aruco/include/cvdrawingutils.h b/ftnoir_tracker_aruco/include/cvdrawingutils.h
index 38e9986e..ff67242f 100644
--- a/ftnoir_tracker_aruco/include/cvdrawingutils.h
+++ b/ftnoir_tracker_aruco/include/cvdrawingutils.h
@@ -46,7 +46,7 @@ namespace aruco
static void draw3dCube(cv::Mat &Image,Board &m,const CameraParameters &CP);
};
-}
+};
#endif
diff --git a/ftnoir_tracker_aruco/include/exports.h b/ftnoir_tracker_aruco/include/exports.h
index 154605ec..aaeb94e4 100644
--- a/ftnoir_tracker_aruco/include/exports.h
+++ b/ftnoir_tracker_aruco/include/exports.h
@@ -25,7 +25,7 @@ The views and conclusions contained in the software and documentation are those
authors and should not be interpreted as representing official policies, either expressed
or implied, of Rafael Muñoz Salinas.
********************************/
-
+
#ifndef __OPENARUCO_CORE_TYPES_H__
@@ -37,9 +37,9 @@ or implied, of Rafael Muñoz Salinas.
#if (defined WIN32 || defined _WIN32 || defined WINCE) && defined DSO_EXPORTS
- #define ARUCO_EXPORTS __declspec(dllexport)
+ #define ARUCO_EXPORTS __declspec(dllexport)
#else
- #define ARUCO_EXPORTS
+ #define ARUCO_EXPORTS __attribute__ ((visibility ("default")))
#endif
diff --git a/ftnoir_tracker_aruco/include/marker.h b/ftnoir_tracker_aruco/include/marker.h
index dc6bb28c..89961002 100644
--- a/ftnoir_tracker_aruco/include/marker.h
+++ b/ftnoir_tracker_aruco/include/marker.h
@@ -29,7 +29,7 @@ or implied, of Rafael Muñoz Salinas.
#define _Aruco_Marker_H
#include <vector>
#include <iostream>
-#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
#include "exports.h"
#include "cameraparameters.h"
using namespace std;
@@ -81,12 +81,12 @@ public:
* @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:
@@ -97,8 +97,8 @@ public:
* mySceneNode->setOrientation( ogreOrient );
* ...
*/
- void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception);
-
+ void OgreGetPoseParameters( double position[3], double orientation[4] )throw(cv::Exception);
+
/**Returns the centroid of the marker
*/
cv::Point2f getCenter()const;
@@ -132,11 +132,11 @@ public:
return str;
}
-
-
+
+
private:
void rotateXAxis(cv::Mat &rotation);
-
+
};
}
diff --git a/ftnoir_tracker_aruco/include/markerdetector.h b/ftnoir_tracker_aruco/include/markerdetector.h
index 4d6e7b90..6f489c34 100644
--- a/ftnoir_tracker_aruco/include/markerdetector.h
+++ b/ftnoir_tracker_aruco/include/markerdetector.h
@@ -27,7 +27,7 @@ or implied, of Rafael Muñoz Salinas.
********************************/
#ifndef _ARUCO_MarkerDetector_H
#define _ARUCO_MarkerDetector_H
-#include <opencv2/opencv.hpp>
+#include <opencv2/core/core.hpp>
#include <cstdio>
#include <iostream>
#include "cameraparameters.h"
@@ -47,27 +47,25 @@ class ARUCO_EXPORTS MarkerDetector
class MarkerCandidate: public Marker{
public:
MarkerCandidate(){}
- MarkerCandidate(const Marker &M): Marker(M){}
+ MarkerCandidate(const Marker &M): Marker(M){}
MarkerCandidate(const MarkerCandidate &M): Marker(M){
contour=M.contour;
idx=M.idx;
}
- MarkerCandidate operator=(const MarkerCandidate &M){
- if (this == &M)
- return *this;
+ MarkerCandidate & operator=(const MarkerCandidate &M){
(*(Marker*)this)=(*(Marker*)&M);
contour=M.contour;
idx=M.idx;
- return M;
+ return *this;
}
-
+
vector<cv::Point> contour;//all the points of its contour
int idx;//index position in the global contour list
};
public:
/**
- * See
+ * See
*/
MarkerDetector();
@@ -161,17 +159,17 @@ public:
* 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
*/
@@ -210,10 +208,10 @@ public:
markerIdDetector_ptrfunc=markerdetector_func;
}
- /** Use an smaller version of the input image for marker detection.
+ /** 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;}
@@ -247,17 +245,17 @@ public:
* @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);
-
-
+ 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
@@ -308,26 +306,26 @@ private:
*/
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,
+ 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
@@ -353,5 +351,9 @@ private:
void draw(cv::Mat out,const std::vector<Marker> &markers );
};
-}
+
+
+
+
+};
#endif
diff --git a/ftnoir_tracker_aruco/trans_calib.cpp b/ftnoir_tracker_aruco/trans_calib.cpp
new file mode 100644
index 00000000..369de449
--- /dev/null
+++ b/ftnoir_tracker_aruco/trans_calib.cpp
@@ -0,0 +1,44 @@
+/* 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"
+
+using namespace cv;
+
+//-----------------------------------------------------------------------------
+TranslationCalibrator::TranslationCalibrator()
+{
+ reset();
+}
+
+void TranslationCalibrator::reset()
+{
+ P = Matx66f::zeros();
+ y = Vec6f(0,0,0, 0,0,0);
+}
+
+void TranslationCalibrator::update(const Matx33d& R_CM_k, const Vec3d& t_CM_k)
+{
+ Matx<double, 6,3> H_k_T = 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;
+}
+
+Vec3f TranslationCalibrator::get_estimate()
+{
+ Vec6f x = P.inv() * y;
+ return Vec3f(x[0], x[1], x[2]);
+}
diff --git a/ftnoir_tracker_aruco/trans_calib.h b/ftnoir_tracker_aruco/trans_calib.h
new file mode 100644
index 00000000..5a2d7c0f
--- /dev/null
+++ b/ftnoir_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
generated by cgit v1.2.3 (git 2.39.1) at 2025-02-05 04:31:05 +0000