diff options
Diffstat (limited to 'tracker-easy/ftnoir_tracker_pt.cpp')
| -rw-r--r-- | tracker-easy/ftnoir_tracker_pt.cpp | 374 |
1 files changed, 374 insertions, 0 deletions
diff --git a/tracker-easy/ftnoir_tracker_pt.cpp b/tracker-easy/ftnoir_tracker_pt.cpp new file mode 100644 index 00000000..947970c3 --- /dev/null +++ b/tracker-easy/ftnoir_tracker_pt.cpp @@ -0,0 +1,374 @@ +/* Copyright (c) 2012 Patrick Ruoff + * Copyright (c) 2014-2016 Stanislaw Halik <sthalik@misaki.pl> + * Copyright (c) 2019 Stephane Lenclud + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "ftnoir_tracker_pt.h" +#include "video/video-widget.hpp" +#include "compat/math-imports.hpp" +#include "compat/check-visible.hpp" + +#include "pt-api.hpp" + +#include <QHBoxLayout> +#include <QDebug> +#include <QFile> +#include <QCoreApplication> + +#include <opencv2\calib3d.hpp> + +#include <iostream> + +using namespace options; + +namespace pt_impl { + +EasyTracker::EasyTracker(pointer<pt_runtime_traits> const& traits) : + traits { traits }, + s { traits->get_module_name() }, + point_extractor { traits->make_point_extractor() }, + camera { traits->make_camera() }, + frame { traits->make_frame() }, + preview_frame { traits->make_preview(preview_width, preview_height) } +{ + cv::setBreakOnError(true); + cv::setNumThreads(1); + + connect(s.b.get(), &bundle_::saving, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); + connect(s.b.get(), &bundle_::reloading, this, &EasyTracker::maybe_reopen_camera, Qt::DirectConnection); + + connect(&s.fov, value_::value_changed<int>(), this, &EasyTracker::set_fov, Qt::DirectConnection); + set_fov(s.fov); +} + +EasyTracker::~EasyTracker() +{ + requestInterruption(); + wait(); + + QMutexLocker l(&camera_mtx); + camera->stop(); +} + + +// Compute Euler angles from ratation matrix +cv::Vec3f EulerAngles(cv::Mat &R) +{ + + float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0)); + + bool singular = sy < 1e-6; // If + + float x, y, z; + if (!singular) + { + x = atan2(R.at<double>(2, 1), R.at<double>(2, 2)); + y = atan2(-R.at<double>(2, 0), sy); + z = atan2(R.at<double>(1, 0), R.at<double>(0, 0)); + } + else + { + x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1)); + y = atan2(-R.at<double>(2, 0), sy); + z = 0; + } + + // Convert to degrees + return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI); +} + + +void getEulerAngles(cv::Mat &rotCamerMatrix, cv::Vec3d &eulerAngles) +{ + + cv::Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ; + double* _r = rotCamerMatrix.ptr<double>(); + double projMatrix[12] = { _r[0],_r[1],_r[2],0, + _r[3],_r[4],_r[5],0, + _r[6],_r[7],_r[8],0 }; + + cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix), + cameraMatrix, + rotMatrix, + transVect, + rotMatrixX, + rotMatrixY, + rotMatrixZ, + eulerAngles); +} + + +void EasyTracker::run() +{ + maybe_reopen_camera(); + + while(!isInterruptionRequested()) + { + pt_camera_info info; + bool new_frame = false; + + { + QMutexLocker l(&camera_mtx); + + if (camera) + std::tie(new_frame, info) = camera->get_frame(*frame); + } + + if (new_frame) + { + const bool preview_visible = check_is_visible(); + + if (preview_visible) + *preview_frame = *frame; + + iImagePoints.clear(); + point_extractor->extract_points(*frame, *preview_frame, points, iImagePoints); + point_count.store(points.size(), std::memory_order_relaxed); + + const bool success = points.size() >= KPointCount; + + int topPointIndex = -1; + + { + QMutexLocker l(¢er_lock); + + if (success) + { + ever_success.store(true, std::memory_order_relaxed); + + // Solve P3P problem with OpenCV + + // Construct the points defining the object we want to detect based on settings. + // We are converting them from millimeters to centimeters. + // TODO: Need to support clip too. That's cap only for now. + // s.active_model_panel != PointModel::Clip + + std::vector<cv::Point3f> objectPoints; + objectPoints.push_back(cv::Point3f(s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Right + objectPoints.push_back(cv::Point3f(-s.cap_x/10.0, s.cap_z / 10.0, -s.cap_y / 10.0)); // Left + objectPoints.push_back(cv::Point3f(0, 0, 0)); // Top + + //Bitmap origin is top left + std::vector<cv::Point2f> trackedPoints; + // Stuff bitmap point in there making sure they match the order of the object point + // Find top most point, that's the one with min Y as we assume our guy's head is not up side down + + int minY = std::numeric_limits<int>::max(); + for (int i = 0; i < 3; i++) + { + if (iImagePoints[i][1]<minY) + { + minY = iImagePoints[i][1]; + topPointIndex = i; + } + } + + int rightPointIndex = -1; + int maxX = 0; + + // Find right most point + for (int i = 0; i < 3; i++) + { + // Excluding top most point + if (i!=topPointIndex && iImagePoints[i][0] > maxX) + { + maxX = iImagePoints[i][0]; + rightPointIndex = i; + } + } + + // Find left most point + int leftPointIndex = -1; + for (int i = 0; i < 3; i++) + { + // Excluding top most point + if (i != topPointIndex && i != rightPointIndex) + { + leftPointIndex = i; + break; + } + } + + // + trackedPoints.push_back(cv::Point2f(iImagePoints[rightPointIndex][0], iImagePoints[rightPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iImagePoints[leftPointIndex][0], iImagePoints[leftPointIndex][1])); + trackedPoints.push_back(cv::Point2f(iImagePoints[topPointIndex][0], iImagePoints[topPointIndex][1])); + + std::cout << "Object: " << objectPoints << "\n"; + std::cout << "Points: " << trackedPoints << "\n"; + + + // Create our camera matrix + // TODO: Just do that once, use data member instead + // Double or Float? + cv::Mat cameraMatrix; + cameraMatrix.create(3, 3, CV_64FC1); + cameraMatrix.setTo(cv::Scalar(0)); + cameraMatrix.at<double>(0, 0) = camera->info.focalLengthX; + cameraMatrix.at<double>(1, 1) = camera->info.focalLengthY; + cameraMatrix.at<double>(0, 2) = camera->info.principalPointX; + cameraMatrix.at<double>(1, 2) = camera->info.principalPointY; + cameraMatrix.at<double>(2, 2) = 1; + + // Create distortion cooefficients + cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1); + // As per OpenCV docs they should be thus: k1, k2, p1, p2, k3, k4, k5, k6 + distCoeffs.at<double>(0, 0) = 0; // Radial first order + distCoeffs.at<double>(1, 0) = camera->info.radialDistortionSecondOrder; // Radial second order + distCoeffs.at<double>(2, 0) = 0; // Tangential first order + distCoeffs.at<double>(3, 0) = 0; // Tangential second order + distCoeffs.at<double>(4, 0) = 0; // Radial third order + distCoeffs.at<double>(5, 0) = camera->info.radialDistortionFourthOrder; // Radial fourth order + distCoeffs.at<double>(6, 0) = 0; // Radial fith order + distCoeffs.at<double>(7, 0) = camera->info.radialDistortionSixthOrder; // Radial sixth order + + // Define our solution arrays + // They will receive up to 4 solutions for our P3P problem + + + // TODO: try SOLVEPNP_AP3P too + iAngles.clear(); + iBestSolutionIndex = -1; + int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P); + + if (solutionCount > 0) + { + std::cout << "Solution count: " << solutionCount << "\n"; + int minPitch = std::numeric_limits<int>::max(); + // Find the solution we want + for (int i = 0; i < solutionCount; i++) + { + std::cout << "Translation:\n"; + std::cout << iTranslations.at(i); + std::cout << "\n"; + std::cout << "Rotation:\n"; + //std::cout << rvecs.at(i); + cv::Mat rotationCameraMatrix; + cv::Rodrigues(iRotations[i], rotationCameraMatrix); + cv::Vec3d angles; + getEulerAngles(rotationCameraMatrix,angles); + iAngles.push_back(angles); + + // Check if pitch is closest to zero + int absolutePitch = std::abs(angles[0]); + if (minPitch > absolutePitch) + { + minPitch = absolutePitch; + iBestSolutionIndex = i; + } + + //cv::Vec3f angles=EulerAngles(quaternion); + std::cout << angles; + std::cout << "\n"; + } + + std::cout << "\n"; + + } + + } + + // Send solution data back to main thread + QMutexLocker l2(&data_lock); + if (iBestSolutionIndex != -1) + { + iBestAngles = iAngles[iBestSolutionIndex]; + iBestTranslation = iTranslations[iBestSolutionIndex]; + } + + } + + if (preview_visible) + { + if (topPointIndex != -1) + { + // Render a cross to indicate which point is the head + preview_frame->draw_head_center(points[topPointIndex][0], points[topPointIndex][1]); + } + + widget->update_image(preview_frame->get_bitmap()); + + auto [ w, h ] = widget->preview_size(); + if (w != preview_width || h != preview_height) + { + preview_width = w; preview_height = h; + preview_frame = traits->make_preview(w, h); + } + } + } + } +} + +bool EasyTracker::maybe_reopen_camera() +{ + QMutexLocker l(&camera_mtx); + + return camera->start(s.camera_name, + s.cam_fps, s.cam_res_x, s.cam_res_y); +} + +void EasyTracker::set_fov(int value) +{ + QMutexLocker l(&camera_mtx); + camera->set_fov(value); +} + +module_status EasyTracker::start_tracker(QFrame* video_frame) +{ + //video_frame->setAttribute(Qt::WA_NativeWindow); + + widget = std::make_unique<video_widget>(video_frame); + layout = std::make_unique<QHBoxLayout>(video_frame); + layout->setContentsMargins(0, 0, 0, 0); + layout->addWidget(widget.get()); + video_frame->setLayout(layout.get()); + //video_widget->resize(video_frame->width(), video_frame->height()); + video_frame->show(); + + start(QThread::HighPriority); + + return {}; +} + +void EasyTracker::data(double *data) +{ + if (ever_success.load(std::memory_order_relaxed)) + { + // Get data back from tracker thread + QMutexLocker l(&data_lock); + data[Yaw] = iBestAngles[1]; + data[Pitch] = iBestAngles[0]; + data[Roll] = iBestAngles[2]; + data[TX] = iBestTranslation[0]; + data[TY] = iBestTranslation[1]; + data[TZ] = iBestTranslation[2]; + } +} + +bool EasyTracker::center() +{ + QMutexLocker l(¢er_lock); + //TODO: Do we need to do anything there? + return false; +} + +int EasyTracker::get_n_points() +{ + return (int)point_count.load(std::memory_order_relaxed); +} + +bool EasyTracker::get_cam_info(pt_camera_info& info) +{ + QMutexLocker l(&camera_mtx); + bool ret; + + std::tie(ret, info) = camera->get_info(); + return ret; +} + + +} // ns pt_impl |