From d32ba5032b9506e89590d15f82b6d4a484f2bd04 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 20 Jul 2015 05:06:44 +0200 Subject: add my copyright to some more code --- ftnoir_tracker_pt/pt_video_widget.cpp | 3 --- ftnoir_tracker_pt/pt_video_widget.h | 11 +++-------- 2 files changed, 3 insertions(+), 11 deletions(-) (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/pt_video_widget.cpp b/ftnoir_tracker_pt/pt_video_widget.cpp index 12f01413..15fc5a86 100644 --- a/ftnoir_tracker_pt/pt_video_widget.cpp +++ b/ftnoir_tracker_pt/pt_video_widget.cpp @@ -9,9 +9,6 @@ #include "pt_video_widget.h" -#include -#include - using namespace cv; using namespace std; diff --git a/ftnoir_tracker_pt/pt_video_widget.h b/ftnoir_tracker_pt/pt_video_widget.h index 09818254..af1d60fd 100644 --- a/ftnoir_tracker_pt/pt_video_widget.h +++ b/ftnoir_tracker_pt/pt_video_widget.h @@ -1,4 +1,5 @@ /* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014 Stanislaw Halik * * Permission to use, copy, modify, and/or distribute this software for any * purpose with or without fee is hereby granted, provided that the above @@ -8,15 +9,9 @@ #pragma once #include -#include -#include +#include #include -#ifndef OPENTRACK_API -# include -# include -#else -# include -#endif +#include #include #include #include -- cgit v1.2.3 From e9b42b3d4eabd1d4fbcdfb990c26f286259be8d2 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 20 Jul 2015 08:09:44 +0200 Subject: camera: more robust get_info() If camera was barely created and there's no info, grab the first frame to get resolution data. --- ftnoir_tracker_pt/camera.cpp | 10 ++++++++++ ftnoir_tracker_pt/camera.h | 4 ++-- 2 files changed, 12 insertions(+), 2 deletions(-) (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/camera.cpp b/ftnoir_tracker_pt/camera.cpp index 9168a3e4..2e745f2a 100644 --- a/ftnoir_tracker_pt/camera.cpp +++ b/ftnoir_tracker_pt/camera.cpp @@ -61,6 +61,16 @@ void Camera::set_res(int x_res, int y_res) } } +CamInfo Camera::get_info() +{ + if (cam_info.res_x == 0 || cam_info.res_y == 0) + { + cv::Mat tmp; + _get_frame(&tmp); + } + return cam_info; +} + bool Camera::get_frame(float dt, cv::Mat* frame) { bool new_frame = _get_frame(frame); diff --git a/ftnoir_tracker_pt/camera.h b/ftnoir_tracker_pt/camera.h index 63614ded..2c42652a 100644 --- a/ftnoir_tracker_pt/camera.h +++ b/ftnoir_tracker_pt/camera.h @@ -54,8 +54,8 @@ public: bool get_frame(float dt, cv::Mat* frame); // WARNING: returned references are valid as long as object - const CamInfo& get_info() const { return cam_info; } - const CamInfo& get_desired() const { return cam_desired; } + CamInfo get_info(); + CamInfo get_desired() const { return cam_desired; } protected: // get a frame from the camera -- cgit v1.2.3 From e89c2b1034692eb1365715a089b7035e6d837719 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 20 Jul 2015 08:13:06 +0200 Subject: pt: change point internal representation Store points as pixel coordinates, not -0.5->0.5. This has no effect on tracking as POSIT is robust enough not to care. --- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 11 +++++------ ftnoir_tracker_pt/point_extractor.cpp | 3 +-- 2 files changed, 6 insertions(+), 8 deletions(-) (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index 8ccfe2c4..b8f40e50 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -58,7 +58,7 @@ float Tracker_PT::get_focal_length() static constexpr double pi = 3.1415926f; const double diag = sqrt(w * w + h * h)/w, diag_fov = static_cast(s.fov) * pi / 180.; const double fov = 2.*atan(tan(diag_fov/2.0)/sqrt(1. + diag*diag)); - return .5 / tan(.5 * fov); + return w*.5 / tan(.5 * fov); } void Tracker_PT::run() @@ -108,18 +108,17 @@ void Tracker_PT::run() for (unsigned i = 0; i < points.size(); i++) { auto& p = points[i]; - auto p2 = cv::Point(p[0] * frame.cols + frame.cols/2, -p[1] * frame.cols + frame.rows/2); cv::Scalar color(0, 255, 0); if (i == points.size()-1) color = cv::Scalar(0, 0, 255); cv::line(frame, - cv::Point(p2.x - 20, p2.y), - cv::Point(p2.x + 20, p2.y), + cv::Point(p[0] - 20, p[1]), + cv::Point(p[0] + 20, p[1]), color, 4); cv::line(frame, - cv::Point(p2.x, p2.y - 20), - cv::Point(p2.x, p2.y + 20), + cv::Point(p[0], p[1] - 20), + cv::Point(p[0], p[1] + 20), color, 4); } diff --git a/ftnoir_tracker_pt/point_extractor.cpp b/ftnoir_tracker_pt/point_extractor.cpp index e81e3aa0..cc9dbce1 100644 --- a/ftnoir_tracker_pt/point_extractor.cpp +++ b/ftnoir_tracker_pt/point_extractor.cpp @@ -211,8 +211,7 @@ std::vector PointExtractor::extract_points(Mat& frame) for (auto& b : simple_blob::merge(blobs)) { auto pos = b.effective_pos(); - Vec2f p((pos[0] - W/2)/W, -(pos[1] - H/2)/W); - points.push_back(p); + points.push_back(pos); } vector channels_; -- cgit v1.2.3 From 9cc86cf3569ef6e68c4779ee0d96d1ae4ef77ca6 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 20 Jul 2015 08:14:15 +0200 Subject: pt: use calibration Issue: #190 --- ftnoir_tracker_pt/ftnoir_tracker_pt.cpp | 35 +++++++++++++++++++++++++++++---- ftnoir_tracker_pt/ftnoir_tracker_pt.h | 1 + 2 files changed, 32 insertions(+), 4 deletions(-) (limited to 'ftnoir_tracker_pt') diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp index b8f40e50..be946018 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.cpp @@ -12,6 +12,7 @@ #include #include #include "opentrack/camera-names.hpp" +#include "opentrack/opencv-calibration.hpp" using namespace std; using namespace cv; @@ -53,6 +54,9 @@ void Tracker_PT::reset_command(Command command) float Tracker_PT::get_focal_length() { + QMutexLocker l(&camera_mtx); + if (!intrinsics.empty()) + return intrinsics.at(0, 0); CamInfo info = camera.get_info(); const int w = info.res_x, h = info.res_y; static constexpr double pi = 3.1415926f; @@ -92,7 +96,18 @@ void Tracker_PT::run() ever_success |= success; if (success) - point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose, s.init_phase_timeout); + { + if (!intrinsics.empty()) + { + std::vector points_; + cv::undistortPoints(points, points_, intrinsics, dist_coeffs); + point_tracker.track(points_, PointModel(s), get_focal_length(), s.dynamic_pose, s.init_phase_timeout); + } + else + { + point_tracker.track(points, PointModel(s), get_focal_length(), s.dynamic_pose, s.init_phase_timeout); + } + } { Affine X_CM = pose(); @@ -101,7 +116,6 @@ void Tracker_PT::run() cv::Vec3f p = X_GH.t; // head (center?) position in global space float fx = get_focal_length(); cv::Vec2f p_(p[0] / p[2] * fx, p[1] / p[2] * fx); // projected to screen - points.push_back(p_); } @@ -138,10 +152,23 @@ void Tracker_PT::apply_settings() { qDebug()<<"Tracker:: Applying settings"; QMutexLocker l(&camera_mtx); - QMutexLocker lock(&mutex); camera.set_device_index(camera_name_to_index(s.camera_name)); camera.set_res(s.cam_res_x, s.cam_res_y); camera.set_fps(s.cam_fps); + cv::Mat intrinsics_ = cv::Mat::eye(3, 3, CV_32FC1); + cv::Mat dist_coeffs_ = cv::Mat::zeros(5, 1, CV_32FC1); + intrinsics = cv::Mat(); + dist_coeffs = cv::Mat(); + auto info = camera.get_info(); + if (info.res_x == 0 || info.res_y == 0) + return; + if (get_camera_calibration(s.camera_name, intrinsics_, dist_coeffs_, info.res_x, info.res_y, s.fov)) + { + intrinsics = intrinsics_.clone(); + dist_coeffs = dist_coeffs_.clone(); + qDebug() << s.camera_name << "calibrated"; + } + qDebug()<<"Tracker::apply ends"; } @@ -156,8 +183,8 @@ void Tracker_PT::start_tracker(QFrame *parent_window) video_layout->addWidget(video_widget); video_frame->setLayout(video_layout); video_widget->resize(video_frame->width(), video_frame->height()); - apply_settings(); camera.start(); + apply_settings(); start(); } diff --git a/ftnoir_tracker_pt/ftnoir_tracker_pt.h b/ftnoir_tracker_pt/ftnoir_tracker_pt.h index 21da9a2c..aeda7dd7 100644 --- a/ftnoir_tracker_pt/ftnoir_tracker_pt.h +++ b/ftnoir_tracker_pt/ftnoir_tracker_pt.h @@ -78,6 +78,7 @@ private: Timer time; volatile bool ever_success; + cv::Mat intrinsics, dist_coeffs; static constexpr double rad2deg = 180.0/3.14159265; static constexpr double deg2rad = 3.14159265/180.0; -- cgit v1.2.3