/* Copyright (c) 2012 Patrick Ruoff * Copyright (c) 2014-2016 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 * 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 #include #include #include #include #include using namespace options; namespace pt_impl { Tracker_PT::Tracker_PT(pointer 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, &Tracker_PT::maybe_reopen_camera, Qt::DirectConnection); connect(s.b.get(), &bundle_::reloading, this, &Tracker_PT::maybe_reopen_camera, Qt::DirectConnection); connect(&s.fov, value_::value_changed(), this, &Tracker_PT::set_fov, Qt::DirectConnection); set_fov(s.fov); } Tracker_PT::~Tracker_PT() { requestInterruption(); wait(); QMutexLocker l(&camera_mtx); camera->stop(); } // Calculates rotation matrix to euler angles cv::Vec3f EulerAngles(cv::Mat &R) { float sy = sqrt(R.at(0, 0) * R.at(0, 0) + R.at(1, 0) * R.at(1, 0)); bool singular = sy < 1e-6; // If float x, y, z; if (!singular) { x = atan2(R.at(2, 1), R.at(2, 2)); y = atan2(-R.at(2, 0), sy); z = atan2(R.at(1, 0), R.at(0, 0)); } else { x = atan2(-R.at(1, 2), R.at(1, 1)); y = atan2(-R.at(2, 0), sy); z = 0; } // Convert to degrees return cv::Vec3f(x* 180 / CV_PI, y* 180 / CV_PI, z* 180 / CV_PI); } void Tracker_PT::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() >= PointModel::N_POINTS; Affine X_CM; { QMutexLocker l(¢er_lock); if (success) { int dynamic_pose_ms = s.dynamic_pose && s.active_model_panel != PointModel::Clip ? s.init_phase_timeout : 0; point_tracker.track(points, PointModel(s), info, dynamic_pose_ms); ever_success.store(true, std::memory_order_relaxed); // TODO: Solve with OpenCV // Construct the points defining the object we want to detect based on settings. // We are converting them from millimeters to meters. // TODO: Need to support clip too. That's cap only for now. std::vector objectPoints; objectPoints.push_back(cv::Point3f(s.cap_x/1000.0,0,0)); // Right objectPoints.push_back(cv::Point3f(-s.cap_x/1000.0, 0, 0)); // Left objectPoints.push_back(cv::Point3f(0, s.cap_y/1000.0, s.cap_z/1000.0)); // Top std::vector trackedPoints; //TODO: Stuff bitmap point in there making sure they match the order of the object point // Find top most point int topPointIndex = -1; int maxY = 0; for (int i = 0; i < 3; i++) { if (iImagePoints[i][1]>maxY) { maxY = 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])); // 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(0, 0) = camera->info.focalLengthX; cameraMatrix.at(1, 1) = camera->info.focalLengthY; cameraMatrix.at(0, 2) = camera->info.principalPointX; cameraMatrix.at(1, 2) = camera->info.principalPointY; cameraMatrix.at(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(0, 0) = 0; // Radial first order distCoeffs.at(1, 0) = camera->info.radialDistortionSecondOrder; // Radial second order distCoeffs.at(2, 0) = 0; // Tangential first order distCoeffs.at(3, 0) = 0; // Tangential second order distCoeffs.at(4, 0) = 0; // Radial third order distCoeffs.at(5, 0) = camera->info.radialDistortionFourthOrder; // Radial fourth order distCoeffs.at(6, 0) = 0; // Radial fith order distCoeffs.at(7, 0) = camera->info.radialDistortionSixthOrder; // Radial sixth order // Define our solution arrays // They will receive up to 4 solutions for our P3P problem std::vector rvecs, tvecs; // TODO: try SOLVEPNP_AP3P too int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_AP3P); if (solutionCount > 0) { std::cout << "Solution count: " << solutionCount << "\n"; // Find the solution we want for (int i = 0; i < solutionCount; i++) { std::cout << "Translation:\n"; std::cout << tvecs.at(i); std::cout << "\n"; std::cout << "Rotation:\n"; //std::cout << rvecs.at(i); cv::Mat quaternion; cv::Rodrigues(rvecs[i], quaternion); cv::Vec3f angles=EulerAngles(quaternion); std::cout << angles; std::cout << "\n"; } std::cout << "\n"; } // TODO: Work out rotation angles // TODO: Choose the one solution that makes sense for us } QMutexLocker l2(&data_lock); X_CM = point_tracker.pose(); } if (preview_visible) { const f fx = pt_camera_info::get_focal_length(info.fov, info.res_x, info.res_y); Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z)); Affine X_GH = X_CM * X_MH; vec3 p = X_GH.t; // head (center?) position in global space if (p[2] > f(.1)) preview_frame->draw_head_center((p[0] * fx) / p[2], (p[1] * fx) / p[2]); 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 Tracker_PT::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 Tracker_PT::set_fov(int value) { QMutexLocker l(&camera_mtx); camera->set_fov(value); } module_status Tracker_PT::start_tracker(QFrame* video_frame) { //video_frame->setAttribute(Qt::WA_NativeWindow); widget = std::make_unique(video_frame); layout = std::make_unique(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 Tracker_PT::data(double *data) { if (ever_success.load(std::memory_order_relaxed)) { Affine X_CM; { QMutexLocker l(&data_lock); X_CM = point_tracker.pose(); } Affine X_MH(mat33::eye(), vec3(s.t_MH_x, s.t_MH_y, s.t_MH_z)); Affine X_GH(X_CM * X_MH); // translate rotation matrix from opengl (G) to roll-pitch-yaw (E) frame // -z -> x, y -> z, x -> -y mat33 R_EG(0, 0,-1, -1, 0, 0, 0, 1, 0); mat33 R(R_EG * X_GH.R * R_EG.t()); // get translation(s) const vec3& t = X_GH.t; // extract rotation angles auto r00 = (double)R(0, 0); auto r10 = (double)R(1,0), r20 = (double)R(2,0); auto r21 = (double)R(2,1), r22 = (double)R(2,2); double beta = atan2(-r20, sqrt(r21*r21 + r22*r22)); double alpha = atan2(r10, r00); double gamma = atan2(r21, r22); constexpr double rad2deg = 180/M_PI; data[Yaw] = rad2deg * alpha; data[Pitch] = -rad2deg * beta; data[Roll] = rad2deg * gamma; // convert to cm data[TX] = (double)t[0] / 10; data[TY] = (double)t[1] / 10; data[TZ] = (double)t[2] / 10; } } bool Tracker_PT::center() { QMutexLocker l(¢er_lock); point_tracker.reset_state(); return false; } int Tracker_PT::get_n_points() { return (int)point_count.load(std::memory_order_relaxed); } bool Tracker_PT::get_cam_info(pt_camera_info& info) { QMutexLocker l(&camera_mtx); bool ret; std::tie(ret, info) = camera->get_info(); return ret; } Affine Tracker_PT::pose() const { QMutexLocker l(&data_lock); return point_tracker.pose(); } } // ns pt_impl