diff options
| -rw-r--r-- | ftnoir_tracker_aruco/aruco-trackercontrols.ui | 28 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 30 | ||||
| -rw-r--r-- | ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 2 |
3 files changed, 53 insertions, 7 deletions
diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui index 76d750af..8cf28057 100644 --- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui +++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui @@ -10,7 +10,7 @@ <x>0</x> <y>0</y> <width>704</width> - <height>273</height> + <height>308</height> </rect> </property> <property name="sizePolicy"> @@ -294,6 +294,32 @@ </property> </widget> </item> + <item row="3" column="0"> + <widget class="QLabel" name="label_9"> + <property name="text"> + <string>Pitch</string> + </property> + </widget> + </item> + <item row="3" column="1"> + <widget class="QDoubleSpinBox" name="pitch_deg"> + <property name="suffix"> + <string>°</string> + </property> + <property name="decimals"> + <number>2</number> + </property> + <property name="minimum"> + <double>-60.000000000000000</double> + </property> + <property name="maximum"> + <double>60.000000000000000</double> + </property> + <property name="value"> + <double>0.000000000000000</double> + </property> + </widget> + </item> </layout> </widget> </item> diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index 41bf0edd..ddbdadb0 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -128,6 +128,8 @@ void Tracker::load_settings() { headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble(); } + headpitch = iniFile.value("pitch", 0).toDouble(); + iniFile.endGroup(); } @@ -328,17 +330,27 @@ void Tracker::run() cv::Rodrigues(rvec, rotation_matrix); - cv::Vec3d foo = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); - { + const double beta = headpitch * M_PI / 180; + double pitch[] = { + 1, 0, 0, + 0, cos(beta), -sin(beta), + 0, sin(beta), cos(beta) + }; + cv::Mat rot(3, 3, CV_64F, pitch); + tvec = rot * tvec; + rotation_matrix = rot * rotation_matrix; + + cv::Vec3d euler = cv::RQDecomp3x3(rotation_matrix, junk1, junk2); + QMutexLocker lck(&mtx); for (int i = 0; i < 3; i++) pose[i] = tvec.at<double>(i); - pose[Yaw] = foo[1]; - pose[Pitch] = -foo[0]; - pose[Roll] = foo[2]; + pose[Yaw] = euler[1]; + pose[Pitch] = -euler[0]; + pose[Roll] = euler[2]; } std::vector<cv::Point2f> repr2; @@ -346,6 +358,11 @@ void Tracker::run() 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); + } + last_centroid = repr2[0]; } else @@ -527,6 +544,8 @@ void TrackerControls::loadSettings() headpos[i]->setValue(iniFile.value(QString("headpos-%1").arg(i)).toDouble()); } + ui.pitch_deg->setValue(iniFile.value("pitch", 0).toDouble()); + iniFile.endGroup(); settingsDirty = false; } @@ -565,6 +584,7 @@ void TrackerControls::save() iniFile.setValue("enable-ty", ui.ty->checkState() != Qt::Unchecked ? true : false); iniFile.setValue("enable-tz", ui.tz->checkState() != Qt::Unchecked ? true : false); iniFile.setValue("resolution", ui.resolution->currentIndex()); + iniFile.setValue("pitch", ui.pitch_deg->value()); QDoubleSpinBox* headpos[] = { ui.cx, diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h index 3a2ebeea..1518ba4d 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h @@ -40,7 +40,7 @@ private: int force_fps, force_width, force_height; double pose[6]; cv::Mat frame; - double headpos[3]; + double headpos[3], headpitch; cv::VideoCapture camera; }; |