diff options
Diffstat (limited to 'tracker-easy/tracker-easy.cpp')
| -rw-r--r-- | tracker-easy/tracker-easy.cpp | 599 |
1 files changed, 438 insertions, 161 deletions
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp index ebc6e128..e13bdf37 100644 --- a/tracker-easy/tracker-easy.cpp +++ b/tracker-easy/tracker-easy.cpp @@ -24,11 +24,14 @@ using namespace options; // Disable debug -#define dbgout if (true) {} else std::cout +#define dbgout if (true) {} else std::cout << "\n" <<std::chrono::system_clock::now().time_since_epoch().count() << ": " //#define infout if (true) {} else std::cout // Enable debug //#define dbgout if (false) {} else std::cout -#define infout if (false) {} else std::cout +#define infout if (false) {} else std::cout << "\n" << std::chrono::system_clock::now().time_since_epoch().count() << ": " + +// We need at least 3 vertices to be able to do anything +const int KMinVertexCount = 3; namespace EasyTracker { @@ -49,14 +52,35 @@ namespace EasyTracker //connect(&iSettings.cam_fps, value_::value_changed<int>(), this, &Tracker::SetFps, Qt::DirectConnection); // Make sure deadzones are updated whenever the settings are changed - connect(&iSettings.DeadzoneRectHalfEdgeSize, value_::value_changed<int>(), this, &Tracker::UpdateDeadzones, Qt::DirectConnection); - UpdateDeadzones(iSettings.DeadzoneRectHalfEdgeSize); + connect(&iSettings.DeadzoneRectHalfEdgeSize, value_::value_changed<int>(), this, &Tracker::UpdateSettings, Qt::DirectConnection); + + // Update point extractor whenever some of the settings it needs are changed + connect(&iSettings.iMinBlobSize, value_::value_changed<int>(), this, &Tracker::UpdateSettings, Qt::DirectConnection); + connect(&iSettings.iMaxBlobSize, value_::value_changed<int>(), this, &Tracker::UpdateSettings, Qt::DirectConnection); // Make sure solver is updated whenever the settings are changed - connect(&iSettings.PnpSolver, value_::value_changed<int>(), this, &Tracker::UpdateSolver, Qt::DirectConnection); - UpdateSolver(iSettings.PnpSolver); + connect(&iSettings.PnpSolver, value_::value_changed<int>(), this, &Tracker::UpdateSettings, Qt::DirectConnection); + + // Debug + connect(&iSettings.debug, value_::value_changed<bool>(), this, &Tracker::UpdateSettings, Qt::DirectConnection); + + // Make sure model is updated whenever it is changed + connect(&iSettings.iCustomModelThree, value_::value_changed<bool>(), this, &Tracker::UpdateModel, Qt::DirectConnection); + connect(&iSettings.iCustomModelFour, value_::value_changed<bool>(), this, &Tracker::UpdateModel, Qt::DirectConnection); + connect(&iSettings.iCustomModelFive, value_::value_changed<bool>(), this, &Tracker::UpdateModel, Qt::DirectConnection); + + // Update model logic + #define UM(v) connect(&iSettings.v, value_::value_changed<int>(), this, &Tracker::UpdateModel, Qt::DirectConnection) + UM(iVertexTopX); UM(iVertexTopY); UM(iVertexTopZ); + UM(iVertexTopRightX); UM(iVertexTopRightY); UM(iVertexTopRightZ); + UM(iVertexTopLeftX); UM(iVertexTopLeftY); UM(iVertexTopLeftZ); + UM(iVertexRightX); UM(iVertexRightY); UM(iVertexRightZ); + UM(iVertexLeftX); UM(iVertexLeftY); UM(iVertexLeftZ); + UM(iVertexCenterX); UM(iVertexCenterY); UM(iVertexCenterZ); + + UpdateModel(); - CreateModelFromSettings(); + UpdateSettings(); } Tracker::~Tracker() @@ -77,7 +101,6 @@ namespace EasyTracker // Compute Euler angles from rotation matrix 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, @@ -95,19 +118,6 @@ namespace EasyTracker } /// - void Tracker::CreateModelFromSettings() - { - // 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 - iModel.clear(); - iModel.push_back(cv::Point3f(iSettings.cap_x / 10.0, iSettings.cap_z / 10.0, -iSettings.cap_y / 10.0)); // Right - iModel.push_back(cv::Point3f(-iSettings.cap_x / 10.0, iSettings.cap_z / 10.0, -iSettings.cap_y / 10.0)); // Left - iModel.push_back(cv::Point3f(0, 0, 0)); // Top - } - - /// void Tracker::CreateCameraIntrinsicsMatrices() { // Create our camera matrix @@ -132,11 +142,194 @@ namespace EasyTracker iDistCoeffsMatrix.at<double>(7, 0) = iCameraInfo.radialDistortionSixthOrder; // Radial sixth order } + + void Tracker::MatchVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex, int& aTopRight, int& aTopLeft) + { + if (iModel.size() == 5) + { + MatchFiveVertices(aTopIndex, aRightIndex, aLeftIndex, aTopRight, aTopLeft); + } + else + { + MatchThreeOrFourVertices(aTopIndex, aRightIndex, aLeftIndex, aCenterIndex); + } + } + + + void Tracker::MatchFiveVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aTopRight, int& aTopLeft) + { + //Bitmap origin is top left + iTrackedPoints.clear(); + + int vertexIndices[] = { -1,-1,-1,-1,-1 }; + std::vector<int> indices = { 0,1,2,3,4 }; + + // Tracked points must match the order of the object model points. + // 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 < iPoints.size(); i++) + { + if (iPoints[i].y < minY) + { + minY = iPoints[i].y; + vertexIndices[VertexPosition::Top] = i; + } + } + indices.erase(std::find(indices.begin(), indices.end(), vertexIndices[VertexPosition::Top])); + + // Find right most point + int maxX = 0; + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point + if (i != vertexIndices[VertexPosition::Top] && iPoints[i].x > maxX) + { + maxX = iPoints[i].x; + vertexIndices[VertexPosition::Right] = i; + } + } + indices.erase(std::find(indices.begin(), indices.end(), vertexIndices[VertexPosition::Right])); + + // Find left most point + int minX = std::numeric_limits<int>::max(); + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point and right most point + if (i != vertexIndices[VertexPosition::Top] && i != vertexIndices[VertexPosition::Right] && iPoints[i].x < minX) + { + minX = iPoints[i].x; + vertexIndices[VertexPosition::Left] = i; + } + } + indices.erase(std::find(indices.begin(), indices.end(), vertexIndices[VertexPosition::Left])); + + // Check which of our two remaining points is on the left + int leftIndex = -1; + int rightIndex = -1; + if (iPoints[indices[0]].x > iPoints[indices[1]].x) + { + leftIndex = indices[1]; + rightIndex = indices[0]; + } + else + { + leftIndex = indices[0]; + rightIndex = indices[1]; + } + + // Check which of the left points is at the top + if (iPoints[vertexIndices[VertexPosition::Left]].y < iPoints[leftIndex].y) + { + vertexIndices[VertexPosition::TopLeft] = vertexIndices[VertexPosition::Left]; + vertexIndices[VertexPosition::Left] = leftIndex; + } + else + { + vertexIndices[VertexPosition::TopLeft] = leftIndex; + } + + // Check which of the right points is at the top + if (iPoints[vertexIndices[VertexPosition::Right]].y < iPoints[rightIndex].y) + { + vertexIndices[VertexPosition::TopRight] = vertexIndices[VertexPosition::Right]; + vertexIndices[VertexPosition::Right] = rightIndex; + } + else + { + vertexIndices[VertexPosition::TopRight] = rightIndex; + } + + + // Order matters, see UpdateModel function + iTrackedPoints.push_back(iPoints[vertexIndices[VertexPosition::Top]]); + iTrackedPoints.push_back(iPoints[vertexIndices[VertexPosition::Right]]); + iTrackedPoints.push_back(iPoints[vertexIndices[VertexPosition::Left]]); + iTrackedPoints.push_back(iPoints[vertexIndices[VertexPosition::TopRight]]); + iTrackedPoints.push_back(iPoints[vertexIndices[VertexPosition::TopLeft]]); + + // + aTopIndex = vertexIndices[VertexPosition::Top]; + aRightIndex = vertexIndices[VertexPosition::Right]; + aLeftIndex = vertexIndices[VertexPosition::Left]; + aTopRight = vertexIndices[VertexPosition::TopRight]; + aTopLeft = vertexIndices[VertexPosition::TopLeft]; + + + } + + + void Tracker::MatchThreeOrFourVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex) + { + //Bitmap origin is top left + iTrackedPoints.clear(); + // Tracked points must match the order of the object model points. + // 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 < iPoints.size(); i++) + { + if (iPoints[i].y < minY) + { + minY = iPoints[i].y; + aTopIndex = i; + } + } + + + int maxX = 0; + + // Find right most point + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point + if (i != aTopIndex && iPoints[i].x > maxX) + { + maxX = iPoints[i].x; + aRightIndex = i; + } + } + + // Find left most point + int minX = std::numeric_limits<int>::max(); + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding top most point and right most point + if (i != aTopIndex && i != aRightIndex && iPoints[i].x < minX) + { + aLeftIndex = i; + minX = iPoints[i].x; + } + } + + // Find center point, the last one + for (int i = 0; i < iPoints.size(); i++) + { + // Excluding the three points we already have + if (i != aTopIndex && i != aRightIndex && i != aLeftIndex) + { + aCenterIndex = i; + } + } + + // Order matters + iTrackedPoints.push_back(iPoints[aTopIndex]); + iTrackedPoints.push_back(iPoints[aRightIndex]); + iTrackedPoints.push_back(iPoints[aLeftIndex]); + if (iModel.size() > iTrackedPoints.size()) + { + // We are tracking more than 3 points + iTrackedPoints.push_back(iPoints[aCenterIndex]); + } + } + + + /// /// /// void Tracker::ProcessFrame() { + QMutexLocker l(&iProcessLock); + // Create OpenCV matrix from our frame // TODO: Assert channel size is one or two iMatFrame = cv::Mat(iFrame.height, iFrame.width, CV_MAKETYPE((iFrame.channelSize == 2 ? CV_16U : CV_8U), iFrame.channels), iFrame.data, iFrame.stride); @@ -149,170 +342,208 @@ namespace EasyTracker } iPoints.clear(); - iPointExtractor.ExtractPoints(iMatFrame, (doPreview ? &iPreview.iFrameRgb : nullptr), iPoints); + iPointExtractor.ExtractPoints(iMatFrame, (doPreview ? &iPreview.iFrameRgb : nullptr), iModel.size(), iPoints); - const bool success = iPoints.size() >= KPointCount; + const bool success = iPoints.size() >= iModel.size() && iModel.size() >= KMinVertexCount; int topPointIndex = -1; + int rightPointIndex = -1; + int leftPointIndex = -1; + int centerPointIndex = -1; + int topRightPointIndex = -1; + int topLeftPointIndex = -1; + if (success) { - QMutexLocker l(¢er_lock); - - if (success) + // Lets match our 3D vertices with our image 2D points + MatchVertices(topPointIndex, rightPointIndex, leftPointIndex, centerPointIndex, topRightPointIndex, topLeftPointIndex); + + bool movedEnough = true; + // Check if we moved enough since last time we were here + // This is our deadzone management + if (iDeadzoneHalfEdge != 0 // Check if deazones are enabled + && iTrackedRects.size() == iTrackedPoints.size()) { - ever_success.store(true, std::memory_order_relaxed); - - //Bitmap origin is top left - iTrackedPoints.clear(); - // Tracked points must match the order of the object model points. - // 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++) + movedEnough = false; + for (size_t i = 0; i < iTrackedPoints.size(); i++) { - if (iPoints[i].y < minY) + if (!iTrackedRects[i].contains(iTrackedPoints[i])) { - minY = iPoints[i].y; - topPointIndex = i; + movedEnough = true; + break; } } + } - int rightPointIndex = -1; - int maxX = 0; - - // Find right most point - for (int i = 0; i < 3; i++) + if (!movedEnough) + { + // We are in a dead zone + // However we still have tracking so make sure we don't auto center + QMutexLocker lock(&iDataLock); + iBestTime.start(); + } + else + { + // Build deadzone rectangles if needed + iTrackedRects.clear(); + if (iDeadzoneHalfEdge != 0) // Check if deazones are enabled { - // Excluding top most point - if (i != topPointIndex && iPoints[i].x > maxX) + for (const cv::Point& pt : iTrackedPoints) { - maxX = iPoints[i].x; - rightPointIndex = i; + cv::Rect rect(pt - cv::Point(iDeadzoneHalfEdge, iDeadzoneHalfEdge), cv::Size(iDeadzoneEdge, iDeadzoneEdge)); + iTrackedRects.push_back(rect); } } - // Find left most point - int leftPointIndex = -1; - for (int i = 0; i < 3; i++) + dbgout << "Object: " << iModel << "\n"; + dbgout << "Points: " << iTrackedPoints << "\n"; + + iAngles.clear(); + iBestSolutionIndex = -1; + // Solve P3P problem with OpenCV + int solutionCount = 0; + if (iModel.size() == 3) { - // Excluding top most point - if (i != topPointIndex && i != rightPointIndex) + solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, iSolver); + } + else + { + //Guess extrinsic boolean is only for ITERATIVE method, it will be set to false for all other method + cv::Mat rotation, translation; + // Init only needed for iterative, it's also useless as it is + rotation = cv::Mat::zeros(3, 1, CV_64FC1); + translation = cv::Mat::zeros(3, 1, CV_64FC1); + rotation.setTo(cv::Scalar(0)); + translation.setTo(cv::Scalar(0)); + ///// + iRotations.clear(); + iTranslations.clear(); + bool solved = cv::solvePnP(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, rotation, translation, true, iSolver ); + if (solved) { - leftPointIndex = i; - break; + solutionCount = 1; + iRotations.push_back(rotation); + iTranslations.push_back(translation); } } - // - iTrackedPoints.push_back(iPoints[rightPointIndex]); - iTrackedPoints.push_back(iPoints[leftPointIndex]); - iTrackedPoints.push_back(iPoints[topPointIndex]); + // Reset best solution index + iBestSolutionIndex = -1; - bool movedEnough = true; - // Check if we moved enough since last time we were here - // This is our deadzone management - if (iSettings.DeadzoneRectHalfEdgeSize != 0 // Check if deazones are enabled - && iTrackedRects.size() == iTrackedPoints.size()) + if (solutionCount > 0) { - movedEnough = false; - for (size_t i = 0; i < iTrackedPoints.size(); i++) + dbgout << "Solution count: " << solutionCount << "\n"; + int minPitch = std::numeric_limits<int>::max(); + // Find the solution we want amongst all possible ones + for (int i = 0; i < solutionCount; i++) { - if (!iTrackedRects[i].contains(iTrackedPoints[i])) + dbgout << "Translation:\n"; + dbgout << iTranslations.at(i); + dbgout << "\n"; + dbgout << "Rotation:\n"; + //dbgout << 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) { - movedEnough = true; - break; + // The solution with pitch closest to zero is the one we want + minPitch = absolutePitch; + iBestSolutionIndex = i; } + + dbgout << angles; + dbgout << "\n"; } + + dbgout << "\n"; } - if (movedEnough) + if (iBestSolutionIndex != -1) { - // Build deadzone rectangles if needed - iTrackedRects.clear(); - if (iSettings.DeadzoneRectHalfEdgeSize != 0) // Check if deazones are enabled - { - for (const cv::Point& pt : iTrackedPoints) - { - cv::Rect rect(pt - cv::Point(iDeadzoneHalfEdge, iDeadzoneHalfEdge), cv::Size(iDeadzoneEdge, iDeadzoneEdge)); - iTrackedRects.push_back(rect); - } - } + // Best translation + cv::Vec3d translation = iTranslations[iBestSolutionIndex]; + // Best angles + cv::Vec3d angles = iAngles[iBestSolutionIndex]; - dbgout << "Object: " << iModel << "\n"; - dbgout << "Points: " << iTrackedPoints << "\n"; + // Pass solution through our kalman filter + iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]); - iAngles.clear(); - iBestSolutionIndex = -1; - // Solve P3P problem with OpenCV - int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, iSolver); - - if (solutionCount > 0) + // Check if our solution makes sense + // For now, just discard solutions with extrem pitch + if (std::abs(angles[0]) > 50) //TODO: Put that in settings { - dbgout << "Solution count: " << solutionCount << "\n"; - int minPitch = std::numeric_limits<int>::max(); - // Find the solution we want amongst all possible ones - for (int i = 0; i < solutionCount; i++) - { - dbgout << "Translation:\n"; - dbgout << iTranslations.at(i); - dbgout << "\n"; - dbgout << "Rotation:\n"; - //dbgout << 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) - { - // The solution with pitch closest to zero is the one we want - minPitch = absolutePitch; - iBestSolutionIndex = i; - } - - dbgout << angles; - dbgout << "\n"; - } - - dbgout << "\n"; + infout << "WARNING: discarding solution!"; + iBadSolutionCount++; + } + else + { + iGoodSolutionCount++; + // We succeded in finding a solution to our PNP problem + ever_success.store(true, std::memory_order_relaxed); + + // Send solution data back to main thread + QMutexLocker l2(&iDataLock); + iBestAngles = angles; + iBestTranslation = translation; + iBestTime.start(); } - } - } - - if (iBestSolutionIndex != -1) - { - // Best translation - cv::Vec3d translation = iTranslations[iBestSolutionIndex]; - // Best angles - cv::Vec3d angles = iAngles[iBestSolutionIndex]; - - // Pass solution through our kalman filter - iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]); - - // Send solution data back to main thread - QMutexLocker l2(&data_lock); - iBestAngles = angles; - iBestTranslation = translation; + } } - } if (doPreview) { + double qualityIndex = 1 - (iGoodSolutionCount!=0?(double)iBadSolutionCount / (double)iGoodSolutionCount:0); std::ostringstream ss; - ss << "FPS: " << iFps << "/" << iSkippedFps; + ss << "FPS: " << iFps << "/" << iSkippedFps << " QI: " << qualityIndex; iPreview.DrawInfo(ss.str()); - // + //Color is BGR if (topPointIndex != -1) { // Render a cross to indicate which point is the head - iPreview.DrawCross(iPoints[topPointIndex]); + static const cv::Scalar color(0, 255, 255); // Yellow + iPreview.DrawCross(iPoints[topPointIndex],color); } + if (rightPointIndex != -1) + { + static const cv::Scalar color(255, 0, 255); // Pink + iPreview.DrawCross(iPoints[rightPointIndex], color); + } + + if (leftPointIndex != -1) + { + static const cv::Scalar color(255, 0, 0); // Blue + iPreview.DrawCross(iPoints[leftPointIndex], color); + } + + if (centerPointIndex != -1) + { + static const cv::Scalar color(0, 255, 0); // Green + iPreview.DrawCross(iPoints[centerPointIndex], color); + } + + if (topRightPointIndex != -1) + { + static const cv::Scalar color(0, 0, 255); // Red + iPreview.DrawCross(iPoints[topRightPointIndex], color); + } + + if (topLeftPointIndex != -1) + { + static const cv::Scalar color(255, 255, 0); // Cyan + iPreview.DrawCross(iPoints[topLeftPointIndex], color); + } + + // Render our deadzone rects for (const cv::Rect& rect : iTrackedRects) { @@ -320,7 +551,7 @@ namespace EasyTracker } // Show full size preview pop-up - if (iSettings.debug) + if (iDebug) { cv::imshow("Preview", iPreview.iFrameRgb); cv::waitKey(1); @@ -340,7 +571,7 @@ namespace EasyTracker else { // No preview, destroy preview pop-up - if (iSettings.debug) + if (iDebug) { cv::destroyWindow("Preview"); } @@ -442,27 +673,54 @@ namespace EasyTracker iKf.Init(18, 6, 0, dt); } - void Tracker::UpdateDeadzones(int aHalfEdgeSize) + + /// + /// Create our model from settings specifications + /// + void Tracker::UpdateModel() { - QMutexLocker l(¢er_lock); - iDeadzoneHalfEdge = aHalfEdgeSize; - iDeadzoneEdge = iDeadzoneHalfEdge * 2; - iTrackedRects.clear(); - } + infout << "Update model"; + QMutexLocker lock(&iProcessLock); + // 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. + iModel.clear(); + iModel.push_back(cv::Point3f(iSettings.iVertexTopX / 10.0, iSettings.iVertexTopY / 10.0, iSettings.iVertexTopZ / 10.0)); // Top + iModel.push_back(cv::Point3f(iSettings.iVertexRightX / 10.0, iSettings.iVertexRightY / 10.0, iSettings.iVertexRightZ / 10.0)); // Right + iModel.push_back(cv::Point3f(iSettings.iVertexLeftX / 10.0, iSettings.iVertexLeftY / 10.0, iSettings.iVertexLeftZ / 10.0)); // Left - void Tracker::UpdateSolver(int aSolver) - { - QMutexLocker l(¢er_lock); - iSolver = aSolver; + if (iSettings.iCustomModelFour) + { + iModel.push_back(cv::Point3f(iSettings.iVertexCenterX / 10.0, iSettings.iVertexCenterY / 10.0, iSettings.iVertexCenterZ / 10.0)); // Center + } + else if (iSettings.iCustomModelFive) + { + iModel.push_back(cv::Point3f(iSettings.iVertexTopRightX / 10.0, iSettings.iVertexTopRightY / 10.0, iSettings.iVertexTopRightZ / 10.0)); // Top Right + iModel.push_back(cv::Point3f(iSettings.iVertexTopLeftX / 10.0, iSettings.iVertexTopLeftY / 10.0, iSettings.iVertexTopLeftZ / 10.0)); // Top Left + } } + /// + /// Take a copy of the settings needed by our thread to avoid deadlocks + /// + void Tracker::UpdateSettings() + { + infout << "Update Setting"; + QMutexLocker l(&iProcessLock); + iPointExtractor.UpdateSettings(); + iSolver = iSettings.PnpSolver; + iDeadzoneHalfEdge = iSettings.DeadzoneRectHalfEdgeSize; + iDeadzoneEdge = iDeadzoneHalfEdge * 2; + iTrackedRects.clear(); + iDebug = iSettings.debug; + } - + /// module_status Tracker::start_tracker(QFrame* video_frame) { // Check that we support that solver - if (iSolver!=cv::SOLVEPNP_P3P && iSolver != cv::SOLVEPNP_AP3P) + if (iSolver!=cv::SOLVEPNP_P3P && iSolver != cv::SOLVEPNP_AP3P && iModel.size()==3) { return module_status("Error: Solver not supported use either P3P or AP3P."); } @@ -495,28 +753,47 @@ namespace EasyTracker } // + void FeedData(double* aData, const cv::Vec3d& aAngles, const cv::Vec3d& aTranslation) + { + aData[Yaw] = aAngles[1]; + aData[Pitch] = aAngles[0]; + aData[Roll] = aAngles[2]; + aData[TX] = aTranslation[0]; + aData[TY] = aTranslation[1]; + aData[TZ] = aTranslation[2]; + } + + // // That's called around 250 times per second. // Therefore we better not do anything here other than provide current data. // - void Tracker::data(double *data) + void Tracker::data(double* aData) { 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]; + QMutexLocker l(&iDataLock); + // If there was no new data recently then we provide center data. + // Basically, if our user remove her hat, we will go back to center position until she puts it back on. + if (iSettings.iAutoCenter && iBestTime.elapsed_ms() > iSettings.iAutoCenterTimeout) + { + // Reset to center until we get new data + FeedData(aData, iCenterAngles, iCenterTranslation); + } + else + { + // We got valid data, provide it + FeedData(aData, iBestAngles, iBestTranslation); + } } } bool Tracker::center() { - QMutexLocker l(¢er_lock); - //TODO: Do we need to do anything there? + QMutexLocker l(&iDataLock); + iCenterTranslation = iBestTranslation; + iCenterAngles = iBestAngles; + // Returning false tells the pipeline we want to use the default center behaviour return false; } |