diff options
| -rw-r--r-- | ftnoir_tracker_aruco/aruco-trackercontrols.ui | 35 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 33 |
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; } |