summaryrefslogtreecommitdiffhomepage
path: root/ftnoir_tracker_aruco
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2013-07-27 10:35:19 +0200
committerStanislaw Halik <sthalik@misaki.pl>2013-07-27 10:35:19 +0200
commit2f5eb5c882f26a0bfe2f7f3be87e5ce27f862ef9 (patch)
tree1fe9f7d76dde075466674715de0ea5a0242d851a /ftnoir_tracker_aruco
parente4f1be83f42742a2fc9a1606695d89a442999d1c (diff)
Use extrinsic guess, fix coord handedness, get rid of marker size
Diffstat (limited to 'ftnoir_tracker_aruco')
-rw-r--r--ftnoir_tracker_aruco/aruco-trackercontrols.ui35
-rw-r--r--ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp33
2 files changed, 22 insertions, 46 deletions
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
index 173accdf..a04f2659 100644
--- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui
+++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui
@@ -442,41 +442,6 @@
<double>0.000000000000000</double>
</property>
</widget>
- <widget class="QLabel" name="label_6">
- <property name="geometry">
- <rect>
- <x>390</x>
- <y>100</y>
- <width>58</width>
- <height>16</height>
- </rect>
- </property>
- <property name="text">
- <string>Marker size</string>
- </property>
- </widget>
- <widget class="QSpinBox" name="markerSize">
- <property name="geometry">
- <rect>
- <x>455</x>
- <y>97</y>
- <width>70</width>
- <height>22</height>
- </rect>
- </property>
- <property name="suffix">
- <string> cm</string>
- </property>
- <property name="minimum">
- <number>1</number>
- </property>
- <property name="maximum">
- <number>100</number>
- </property>
- <property name="value">
- <number>10</number>
- </property>
- </widget>
</widget>
<resources/>
<connections/>
diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
index fb5d91f7..236fc684 100644
--- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
+++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp
@@ -186,7 +186,7 @@ void Tracker::run()
aruco::MarkerDetector detector;
detector.setDesiredSpeed(3);
- cv::Mat color, grayscale;
+ cv::Mat color, grayscale, rvec, tvec;
while (!stop)
{
@@ -206,29 +206,44 @@ void Tracker::run()
for (int i = 0; i < 5; i++)
dist_coeffs.at<float>(i) = dc[i];
- aruco::CameraParameters params(intrinsics, dist_coeffs, cv::Size(grayscale.cols, grayscale.rows));
-
std::vector< aruco::Marker > markers;
- detector.detect(grayscale, markers, params, marker_size / 100., false);
+ detector.detect(grayscale, markers, cv::Mat(), cv::Mat(), -1, false);
QMutexLocker lck(&mtx);
- if (markers.size() == 1) {
+ if (markers.size() == 1 && markers[0].size() == 4) {
const aruco::Marker& m = markers.at(0);
+ const float halfSize = 1;
+
+ cv::Mat ObjPoints(4,3,CV_32FC1);
+ ObjPoints.at<float>(1,0)=-halfSize;
+ ObjPoints.at<float>(1,1)=-halfSize;
+ ObjPoints.at<float>(1,2)=0;
+ ObjPoints.at<float>(2,0)=halfSize;
+ ObjPoints.at<float>(2,1)=-halfSize;
+ ObjPoints.at<float>(2,2)=0;
+ ObjPoints.at<float>(3,0)=halfSize;
+ ObjPoints.at<float>(3,1)=halfSize;
+ ObjPoints.at<float>(3,2)=0;
+ ObjPoints.at<float>(0,0)=-halfSize;
+ ObjPoints.at<float>(0,1)=halfSize;
+ ObjPoints.at<float>(0,2)=0;
+
+ cv::solvePnP(ObjPoints, m, intrinsics, dist_coeffs, rvec, tvec, tvec.rows == 3 || tvec.cols == 3);
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(m.Rvec, rotation_matrix);
+ cv::Rodrigues(rvec, rotation_matrix);
cv::Vec3d foo = cv::RQDecomp3x3(rotation_matrix, junk1, junk2);
for (int i = 0; i < 3; i++)
{
pose[i+3] = foo[i];
- pose[i] = m.Tvec.at<float>(i);
+ pose[i] = tvec.at<double>(i);
}
}
@@ -402,8 +417,6 @@ void TrackerControls::loadSettings()
ui.doubleSpinBox_4->setValue(iniFile.value("dc3").toDouble());
ui.doubleSpinBox_5->setValue(iniFile.value("dc4").toDouble());
- ui.markerSize->setValue(iniFile.value("marker-size", 10).toInt());
-
iniFile.endGroup();
settingsDirty = false;
}
@@ -449,8 +462,6 @@ void TrackerControls::save()
iniFile.setValue("dc3", ui.doubleSpinBox_4->value());
iniFile.setValue("dc4", ui.doubleSpinBox_5->value());
- iniFile.setValue("marker-size", ui.markerSize->value());
-
iniFile.endGroup();
settingsDirty = false;
}