From a2a32cda8e2ed8931a04a20d7422a45538c6c8b3 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 26 Oct 2013 23:02:42 +0200 Subject: implement head centroid for aruco Signed-off-by: Stanislaw Halik --- ftnoir_tracker_aruco/aruco-trackercontrols.ui | 217 ++++++++------------------ ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp | 84 ++++++---- ftnoir_tracker_aruco/ftnoir_tracker_aruco.h | 2 +- 3 files changed, 124 insertions(+), 179 deletions(-) diff --git a/ftnoir_tracker_aruco/aruco-trackercontrols.ui b/ftnoir_tracker_aruco/aruco-trackercontrols.ui index c007e93b..76d750af 100644 --- a/ftnoir_tracker_aruco/aruco-trackercontrols.ui +++ b/ftnoir_tracker_aruco/aruco-trackercontrols.ui @@ -32,154 +32,7 @@ -1 - - - - Distortion coefficients - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::NoButtons - - - 11 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::NoButtons - - - 11 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::NoButtons - - - 11 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::NoButtons - - - 11 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.000000000000000 - - - - - - - - 0 - 0 - - - - true - - - QAbstractSpinBox::NoButtons - - - 11 - - - -1000.000000000000000 - - - 1000.000000000000000 - - - 0.000000000000000 - - - - + @@ -267,7 +120,7 @@ - + QDialogButtonBox::Cancel|QDialogButtonBox::Ok @@ -378,6 +231,72 @@ + + + + Head centroid position + + + Qt::AlignCenter + + + + Qt::AlignHCenter|Qt::AlignTop + + + + + TX + + + + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + + TY + + + + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + + TZ + + + + + + + -200.000000000000000 + + + 200.000000000000000 + + + + + + diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp index b532ac9c..7ad80b46 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.cpp @@ -120,8 +120,11 @@ void Tracker::load_settings() enableTX = iniFile.value("enable-tx", true).toBool(); enableTY = iniFile.value("enable-ty", true).toBool(); enableTZ = iniFile.value("enable-tz", true).toBool(); - for (int i = 0; i < 5; i++) - dc[i] = iniFile.value(QString("dc%1").arg(i), 0).toFloat(); + + for (int i = 0; i < 3; i++) + { + headpos[i] = iniFile.value(QString("headpos-%1").arg(i), 0).toDouble(); + } iniFile.endGroup(); } @@ -194,6 +197,7 @@ void Tracker::run() double error = 0; std::vector reprojection; auto kernel = cv::createGaussianFilter(CV_8U, cv::Size(5, 5), 0); + cv::Point2f last_centroid; while (!stop) { if (!camera.read(color_)) @@ -218,7 +222,7 @@ void Tracker::run() cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_32FC1); for (int i = 0; i < 5; i++) - dist_coeffs.at(i) = dc[i]; + dist_coeffs.at(i) = 0; std::vector< aruco::Marker > markers; @@ -239,6 +243,8 @@ void Tracker::run() 3); } + cv::circle(frame, last_centroid, 4, cv::Scalar(0, 0, 0), -1); + auto time = cv::getTickCount(); if ((long) (time / freq) != (long) (last_time / freq)) @@ -271,18 +277,18 @@ void Tracker::run() const float size = 7; cv::Mat obj_points(4,3,CV_32FC1); - obj_points.at(1,0)=-size; - obj_points.at(1,1)=-size; - obj_points.at(1,2)=0; - obj_points.at(2,0)=size; - obj_points.at(2,1)=-size; - obj_points.at(2,2)=0; - obj_points.at(3,0)=size; - obj_points.at(3,1)=size; - obj_points.at(3,2)=0; - obj_points.at(0,0)=-size; - obj_points.at(0,1)=size; - obj_points.at(0,2)=0; + obj_points.at(1,0)=-size + headpos[0]; + obj_points.at(1,1)=-size + headpos[1]; + obj_points.at(1,2)=0 + headpos[2]; + obj_points.at(2,0)=size + headpos[0]; + obj_points.at(2,1)=-size + headpos[1]; + obj_points.at(2,2)=0 + headpos[2]; + obj_points.at(3,0)=size + headpos[0]; + obj_points.at(3,1)=size + headpos[1]; + obj_points.at(3,2)=0 + headpos[2]; + obj_points.at(0,0)=-size + headpos[0]; + obj_points.at(0,1)=size + headpos[1]; + obj_points.at(0,2)=0 + headpos[2]; cv::solvePnP(obj_points, m, intrinsics, dist_coeffs, rvec, tvec, false, cv::ITERATIVE); @@ -315,6 +321,14 @@ void Tracker::run() error += std::sqrt(x * x + y * y); } + reprojection.clear(); + reprojection.resize(1); + std::vector centroid; + centroid.push_back(cv::Point3f(0, 0, 0)); + cv::projectPoints(centroid, rvec, tvec, intrinsics, dist_coeffs, reprojection); + + last_centroid = reprojection[0]; + //pose[Yaw] -= atan(pose[TX] / pose[TZ]) * 180 / HT_PI; //pose[Pitch] -= atan(pose[TY] / pose[TZ]) * 180 / HT_PI; } @@ -414,6 +428,9 @@ TrackerControls::TrackerControls() connect(ui.tx, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int))); connect(ui.ty, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int))); connect(ui.tz, SIGNAL(stateChanged(int)), this, SLOT(settingChanged(int))); + connect(ui.cx, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double))); + connect(ui.cy, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double))); + connect(ui.cz, SIGNAL(valueChanged(double)), this, SLOT(settingChanged(double))); //connect(ui.buttonCancel, SIGNAL(clicked()), this, SLOT(doCancel())); //connect(ui.buttonOK, SIGNAL(clicked()), this, SLOT(doOK())); //connect(ui.buttonSettings, SIGNAL(clicked()), this, SLOT(cameraSettings())); @@ -474,13 +491,18 @@ void TrackerControls::loadSettings() ui.ty->setCheckState(iniFile.value("enable-ty", true).toBool() ? Qt::Checked : Qt::Unchecked); ui.tz->setCheckState(iniFile.value("enable-tz", true).toBool() ? Qt::Checked : Qt::Unchecked); ui.resolution->setCurrentIndex(iniFile.value("resolution", 0).toInt()); - - ui.doubleSpinBox->setValue(iniFile.value("dc0").toDouble()); - ui.doubleSpinBox_2->setValue(iniFile.value("dc1").toDouble()); - ui.doubleSpinBox_3->setValue(iniFile.value("dc2").toDouble()); - ui.doubleSpinBox_4->setValue(iniFile.value("dc3").toDouble()); - ui.doubleSpinBox_5->setValue(iniFile.value("dc4").toDouble()); - + + QDoubleSpinBox* headpos[] = { + ui.cx, + ui.cy, + ui.cz + }; + + for (int i = 0; i < 3; i++) + { + headpos[i]->setValue(iniFile.value(QString("headpos-%1").arg(i)).toDouble()); + } + iniFile.endGroup(); settingsDirty = false; } @@ -519,13 +541,17 @@ 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("dc0", ui.doubleSpinBox->value()); - iniFile.setValue("dc1", ui.doubleSpinBox_2->value()); - iniFile.setValue("dc2", ui.doubleSpinBox_3->value()); - iniFile.setValue("dc3", ui.doubleSpinBox_4->value()); - iniFile.setValue("dc4", ui.doubleSpinBox_5->value()); - + + QDoubleSpinBox* headpos[] = { + ui.cx, + ui.cy, + ui.cz + }; + + for (int i = 0; i < 3; i++) + { + iniFile.setValue(QString("headpos-%1").arg(i), headpos[i]->value()); + } iniFile.endGroup(); settingsDirty = false; } diff --git a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h index 8d532b4c..5d3d4ed6 100644 --- a/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h +++ b/ftnoir_tracker_aruco/ftnoir_tracker_aruco.h @@ -35,11 +35,11 @@ private: volatile bool stop; float fov; int camera_index; - float dc[5]; int force_fps, force_width, force_height; void load_settings(); double pose[6]; cv::Mat frame; + double headpos[3]; }; // Widget that has controls for FTNoIR protocol client-settings. -- cgit v1.2.3