summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
Diffstat
-rw-r--r--tracker-easy/tracker-easy.cpp203
-rw-r--r--tracker-easy/tracker-easy.h8
2 files changed, 132 insertions, 79 deletions
diff --git a/tracker-easy/tracker-easy.cpp b/tracker-easy/tracker-easy.cpp
index 68729567..220765cc 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
{
@@ -115,9 +118,11 @@ namespace EasyTracker
}
///
+ /// Create our model from settings specifications
+ ///
void Tracker::UpdateModel()
{
- infout << std::chrono::system_clock::now().time_since_epoch().count() << ": Update model\n";
+ infout << "Update model";
QMutexLocker lock(&iProcessLock);
// Construct the points defining the object we want to detect based on settings.
@@ -134,7 +139,11 @@ namespace EasyTracker
{
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
+ }
}
// Default to Cap for now
else //if (iSettings.active_model_panel == Cap)
@@ -171,7 +180,92 @@ namespace EasyTracker
}
- const int KMinVertexCount = 3;
+ 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)
+ {
+
+ }
+
+
+ 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]);
+ }
+
+
+ }
+
+
+
///
///
///
@@ -199,71 +293,13 @@ namespace EasyTracker
int rightPointIndex = -1;
int leftPointIndex = -1;
int centerPointIndex = -1;
-
-
+ int topRightPointIndex = -1;
+ int topLeftPointIndex = -1;
if (success)
{
- //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;
- topPointIndex = i;
- }
- }
-
-
- int maxX = 0;
-
- // Find right most point
- for (int i = 0; i < iPoints.size(); i++)
- {
- // Excluding top most point
- if (i != topPointIndex && iPoints[i].x > maxX)
- {
- maxX = iPoints[i].x;
- rightPointIndex = 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 != topPointIndex && i != rightPointIndex && iPoints[i].x < minX)
- {
- leftPointIndex = 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 != topPointIndex && i != rightPointIndex && i != leftPointIndex)
- {
- centerPointIndex = i;
- }
- }
-
- // Order matters
- iTrackedPoints.push_back(iPoints[topPointIndex]);
- iTrackedPoints.push_back(iPoints[rightPointIndex]);
- iTrackedPoints.push_back(iPoints[leftPointIndex]);
- if (iModel.size() > iTrackedPoints.size())
- {
- // We are tracking more than 3 points
- iTrackedPoints.push_back(iPoints[centerPointIndex]);
- }
-
+ // 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
@@ -381,22 +417,35 @@ namespace EasyTracker
// Pass solution through our kalman filter
iKf.Update(translation[0], translation[1], translation[2], angles[2], angles[0], angles[1]);
- // We succeded in finding a solution to our PNP problem
- ever_success.store(true, std::memory_order_relaxed);
+ // 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
+ {
+ 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();
+ }
- // Send solution data back to main thread
- QMutexLocker l2(&iDataLock);
- iBestAngles = angles;
- iBestTranslation = translation;
- iBestTime.start();
}
}
}
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
@@ -425,8 +474,6 @@ namespace EasyTracker
iPreview.DrawCross(iPoints[centerPointIndex], color);
}
-
-
// Render our deadzone rects
for (const cv::Rect& rect : iTrackedRects)
{
@@ -561,7 +608,7 @@ namespace EasyTracker
///
void Tracker::UpdateSettings()
{
- infout << std::chrono::system_clock::now().time_since_epoch().count() << ": Update Setting\n";
+ infout << "Update Setting";
QMutexLocker l(&iProcessLock);
iPointExtractor.UpdateSettings();
iSolver = iSettings.PnpSolver;
diff --git a/tracker-easy/tracker-easy.h b/tracker-easy/tracker-easy.h
index f36ea598..e1dcee55 100644
--- a/tracker-easy/tracker-easy.h
+++ b/tracker-easy/tracker-easy.h
@@ -68,7 +68,11 @@ namespace EasyTracker
private:
void UpdateModel();
void CreateCameraIntrinsicsMatrices();
- void ProcessFrame();
+ void ProcessFrame();
+ void MatchVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex, int& aTopRight, int& aTopLeft);
+ void MatchThreeOrFourVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aCenterIndex);
+ void MatchFiveVertices(int& aTopIndex, int& aRightIndex, int& aLeftIndex, int& aTopRight, int& aTopLeft);
+
//
@@ -118,6 +122,8 @@ namespace EasyTracker
int iSkippedFrameCount = 0;
int iFps = 0;
int iSkippedFps = 0;
+ uint iBadSolutionCount = 0;
+ uint iGoodSolutionCount = 0;
//
KalmanFilterPose iKf;
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-05 23:21:01 +0000