summaryrefslogtreecommitdiffhomepage
path: root/tracker-easy/tracker-easy.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-easy/tracker-easy.cpp')
-rw-r--r--tracker-easy/tracker-easy.cpp116
1 files changed, 95 insertions, 21 deletions
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index ebc6e128..971127d4 100644
--- a/tracker-easy/tracker-easy.cpp
+++ b/tracker-easy/tracker-easy.cpp
@@ -77,7 +77,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,
@@ -100,11 +99,20 @@ namespace EasyTracker
// 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
+ if (iSettings.active_model_panel == FourPoints)
+ {
+ iModel.push_back(cv::Point3f(iSettings.iFourPointsTopX / 10.0, iSettings.iFourPointsTopY / 10.0, iSettings.iFourPointsTopZ / 10.0)); // Top
+ iModel.push_back(cv::Point3f(iSettings.iFourPointsRightX / 10.0, iSettings.iFourPointsRightY / 10.0, iSettings.iFourPointsRightZ / 10.0)); // Right
+ iModel.push_back(cv::Point3f(iSettings.iFourPointsLeftX / 10.0, iSettings.iFourPointsLeftY / 10.0, iSettings.iFourPointsLeftZ / 10.0)); // Left
+ iModel.push_back(cv::Point3f(iSettings.iFourPointsCenterX / 10.0, iSettings.iFourPointsCenterY / 10.0, iSettings.iFourPointsCenterZ / 10.0)); // Center
+ }
+ else if (iSettings.active_model_panel == Cap)
+ {
+ iModel.push_back(cv::Point3f(0, 0, 0)); // Top
+ 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
+ }
}
///
@@ -149,11 +157,14 @@ 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();
int topPointIndex = -1;
+ int rightPointIndex = -1;
+ int leftPointIndex = -1;
+ int centerPointIndex = -1;
{
QMutexLocker l(&center_lock);
@@ -167,7 +178,7 @@ namespace EasyTracker
// 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++)
+ for (int i = 0; i < iPoints.size(); i++)
{
if (iPoints[i].y < minY)
{
@@ -176,11 +187,11 @@ namespace EasyTracker
}
}
- int rightPointIndex = -1;
+
int maxX = 0;
// Find right most point
- for (int i = 0; i < 3; i++)
+ for (int i = 0; i < iPoints.size(); i++)
{
// Excluding top most point
if (i != topPointIndex && iPoints[i].x > maxX)
@@ -191,21 +202,38 @@ namespace EasyTracker
}
// Find left most point
- int leftPointIndex = -1;
- for (int i = 0; i < 3; i++)
+
+ int minX = std::numeric_limits<int>::max();;
+ for (int i = 0; i < iPoints.size(); i++)
{
- // Excluding top most point
- if (i != topPointIndex && i != rightPointIndex)
+ // Excluding top most point and right most point
+ if (i != topPointIndex && i != rightPointIndex && iPoints[i].x < minX)
{
leftPointIndex = i;
- break;
+ 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 != topPointIndex && i != rightPointIndex && i != leftPointIndex)
+ {
+ centerPointIndex = i;
+ }
+ }
+
+ // Order matters
+ iTrackedPoints.push_back(iPoints[topPointIndex]);
iTrackedPoints.push_back(iPoints[rightPointIndex]);
iTrackedPoints.push_back(iPoints[leftPointIndex]);
- iTrackedPoints.push_back(iPoints[topPointIndex]);
+ if (iModel.size() > iTrackedPoints.size())
+ {
+ // We are tracking more than 3 points
+ iTrackedPoints.push_back(iPoints[centerPointIndex]);
+ }
+
bool movedEnough = true;
// Check if we moved enough since last time we were here
@@ -243,7 +271,32 @@ namespace EasyTracker
iAngles.clear();
iBestSolutionIndex = -1;
// Solve P3P problem with OpenCV
- int solutionCount = cv::solveP3P(iModel, iTrackedPoints, iCameraMatrix, iDistCoeffsMatrix, iRotations, iTranslations, iSolver);
+ int solutionCount = 0;
+ if (iModel.size() == 3)
+ {
+ 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)
+ {
+ solutionCount = 1;
+ iRotations.push_back(rotation);
+ iTranslations.push_back(translation);
+ }
+ }
+
if (solutionCount > 0)
{
@@ -306,13 +359,34 @@ namespace EasyTracker
ss << "FPS: " << iFps << "/" << iSkippedFps;
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);
+ }
+
+
+
// Render our deadzone rects
for (const cv::Rect& rect : iTrackedRects)
{
@@ -462,7 +536,7 @@ namespace EasyTracker
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.");
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-19 12:40:17 +0000