summaryrefslogtreecommitdiffhomepage
path: root/tracker-aruco
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-aruco')
-rw-r--r--tracker-aruco/ftnoir_tracker_aruco.cpp62
-rw-r--r--tracker-aruco/ftnoir_tracker_aruco.h7
2 files changed, 36 insertions, 33 deletions
diff --git a/tracker-aruco/ftnoir_tracker_aruco.cpp b/tracker-aruco/ftnoir_tracker_aruco.cpp
index 65056a63..d9579743 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.cpp
+++ b/tracker-aruco/ftnoir_tracker_aruco.cpp
@@ -43,7 +43,6 @@ Tracker::Tracker() :
obj_points(4),
intrinsics(decltype(intrinsics)::eye()),
dist_coeffs(decltype(dist_coeffs)::zeros()),
- centroid { cv::Point3f(0, 0, 0) },
rmat(decltype(rmat)::eye()),
roi_points(4),
last_roi(65535, 65535, 0, 0),
@@ -98,25 +97,29 @@ void Tracker::getRT(cv::Matx33d& r_, cv::Vec3d& t_)
bool Tracker::detect_with_roi()
{
- if (last_roi.width > 0)
+ if (last_roi.width > 1 && last_roi.height > 1)
{
- detector.setMinMaxSize(std::min(1.f, std::max(0.01f, size_min * grayscale.cols / last_roi.width)),
- std::max(.01f, std::min(1.0f, size_max * grayscale.cols / last_roi.width)));
+ float min = std::min(1.f, std::max(.01f, size_min * grayscale.cols / last_roi.width));
+ float max = std::max(.01f, std::min(1.f, size_max * grayscale.cols / last_roi.width));
+ detector.setMinMaxSize(min, max);
- detector.detect(grayscale(last_roi), markers, cv::Mat(), cv::Mat(), -1, false);
+ cv::Mat grayscale_ = grayscale(last_roi);
+
+ detector.detect(grayscale_, markers, cv::Mat(), cv::Mat(), -1, false);
if (markers.size() == 1 && markers[0].size() == 4)
{
- auto& m = markers.at(0);
+ auto& m = markers[0];
for (unsigned i = 0; i < 4; i++)
{
- auto& p = m.at(i);
+ auto& p = m[i];
p.x += last_roi.x;
p.y += last_roi.y;
}
return true;
}
}
+
last_roi = cv::Rect(65535, 65535, 0, 0);
return false;
}
@@ -203,13 +206,14 @@ void Tracker::draw_ar(bool ok)
{
if (ok)
{
- const auto& m = markers.at(0);
+ const auto& m = markers[0];
for (unsigned i = 0; i < 4; i++)
cv::line(frame, m[i], m[(i+1)%4], cv::Scalar(0, 0, 255), 2, 8);
}
char buf[32];
- ::sprintf(buf, "Hz: %d", (unsigned short)cur_fps);
+ ::snprintf(buf, sizeof(buf)-1, "Hz: %d", (int)(unsigned short)cur_fps);
+ buf[sizeof(buf)-1] = '\0';
cv::putText(frame, buf, cv::Point(10, 32), cv::FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 1);
}
@@ -219,10 +223,10 @@ void Tracker::clamp_last_roi()
last_roi.x = 0;
if (last_roi.y < 0)
last_roi.y = 0;
- if (last_roi.width < 0)
- last_roi.width = 0;
- if (last_roi.height < 0)
- last_roi.height = 0;
+ if (last_roi.width < 1)
+ last_roi.width = 1;
+ if (last_roi.height < 1)
+ last_roi.height = 1;
if (last_roi.x >= color.cols-1)
last_roi.x = color.cols-1;
if (last_roi.width >= color.cols-1)
@@ -231,6 +235,9 @@ void Tracker::clamp_last_roi()
last_roi.y = color.rows-1;
if (last_roi.height >= color.rows-1)
last_roi.height = color.rows-1;
+
+ last_roi.width -= last_roi.x;
+ last_roi.height -= last_roi.y;
}
void Tracker::set_points()
@@ -251,30 +258,30 @@ void Tracker::set_points()
void Tracker::draw_centroid()
{
repr2.clear();
+
+ static const std::vector<cv::Point3f> centroid { 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);
+ cv::circle(frame, repr2[0], 4, s, -1);
}
-bool Tracker::set_last_roi()
+void Tracker::set_last_roi()
{
roi_projection.clear();
- if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec_, tvec_, false, cv::SOLVEPNP_ITERATIVE))
- return false;
-
+ using f = float;
+ cv::Point3f h(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z));
for (unsigned i = 0; i < 4; i++)
{
- using f = float;
- cv::Point3f pt(obj_points[i] - cv::Point3f(f(s.headpos_x), f(s.headpos_y), f(s.headpos_z)));
+ cv::Point3f pt(obj_points[i] - h);
roi_points[i] = pt * c_search_window;
}
- cv::projectPoints(roi_points, rvec_, tvec_, intrinsics, dist_coeffs, roi_projection);
- set_roi_from_projection();
+ cv::projectPoints(roi_points, rvec, tvec, intrinsics, dist_coeffs, roi_projection);
- return true;
+ set_roi_from_projection();
}
void Tracker::set_rmat()
@@ -316,9 +323,6 @@ void Tracker::set_roi_from_projection()
last_roi.height = max_y;
}
- last_roi.width -= last_roi.x;
- last_roi.height -= last_roi.y;
-
clamp_last_roi();
}
@@ -346,9 +350,11 @@ void Tracker::run()
}
cv::cvtColor(color, grayscale, cv::COLOR_RGB2GRAY);
+
color.copyTo(frame);
set_intrinsics();
+
update_fps(alpha_);
markers.clear();
@@ -362,9 +368,7 @@ void Tracker::run()
if (!cv::solvePnP(obj_points, markers[0], intrinsics, dist_coeffs, rvec, tvec, false, cv::SOLVEPNP_ITERATIVE))
goto fail;
- if (!set_last_roi())
- goto fail;
-
+ set_last_roi();
draw_centroid();
set_rmat();
}
diff --git a/tracker-aruco/ftnoir_tracker_aruco.h b/tracker-aruco/ftnoir_tracker_aruco.h
index fb207f6f..94a96e4c 100644
--- a/tracker-aruco/ftnoir_tracker_aruco.h
+++ b/tracker-aruco/ftnoir_tracker_aruco.h
@@ -70,7 +70,7 @@ private:
void clamp_last_roi();
void set_points();
void draw_centroid();
- bool set_last_roi();
+ void set_last_roi();
void set_rmat();
void set_roi_from_projection();
@@ -90,10 +90,9 @@ private:
aruco::MarkerDetector detector;
std::vector<aruco::Marker> markers;
cv::Vec3d t;
- cv::Vec3d rvec, tvec, rvec_, tvec_;
+ cv::Vec3d rvec, tvec;
std::vector<cv::Point2f> roi_projection;
std::vector<cv::Point2f> repr2;
- std::vector<cv::Point3f> centroid;
cv::Matx33d m_r, m_q, rmat;
cv::Vec3d euler;
std::vector<cv::Point3f> roi_points;
@@ -104,7 +103,7 @@ private:
static constexpr float size_min = 0.05f;
static constexpr float size_max = 0.3f;
- static constexpr double alpha_ = .985;
+ static constexpr double alpha_ = .95;
};
class TrackerControls : public ITrackerDialog, protected camera_dialog
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-21 13:55:10 +0000