summaryrefslogtreecommitdiffhomepage
path: root/tracker-points
diff options
context:
space:
mode:
Diffstat (limited to 'tracker-points')
-rw-r--r--tracker-points/ftnoir_tracker_pt.cpp41
-rw-r--r--tracker-points/ftnoir_tracker_pt.h13
2 files changed, 48 insertions, 6 deletions
diff --git a/tracker-points/ftnoir_tracker_pt.cpp b/tracker-points/ftnoir_tracker_pt.cpp
index 2383694b..8d152f65 100644
--- a/tracker-points/ftnoir_tracker_pt.cpp
+++ b/tracker-points/ftnoir_tracker_pt.cpp
@@ -233,26 +233,39 @@ void Tracker_PT::run()
// Define our solution arrays
// They will receive up to 4 solutions for our P3P problem
- std::vector<cv::Mat> rvecs, tvecs;
+
+
+ // TODO: try SOLVEPNP_AP3P too
+ iAngles.clear();
+ iBestSolutionIndex = -1;
+ int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, iRotations, iTranslations, cv::SOLVEPNP_P3P);
- // TODO: try SOLVEPNP_AP3P too
- int solutionCount = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, rvecs, tvecs, 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 << tvecs.at(i);
+ std::cout << iTranslations.at(i);
std::cout << "\n";
std::cout << "Rotation:\n";
//std::cout << rvecs.at(i);
cv::Mat rotationCameraMatrix;
- cv::Rodrigues(rvecs[i], 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";
@@ -271,6 +284,12 @@ void Tracker_PT::run()
QMutexLocker l2(&data_lock);
X_CM = point_tracker.pose();
+ if (iBestSolutionIndex != -1)
+ {
+ iBestAngles = iAngles[iBestSolutionIndex];
+ iBestTranslation = iTranslations[iBestSolutionIndex];
+ }
+
}
if (preview_visible)
@@ -369,6 +388,16 @@ void Tracker_PT::data(double *data)
data[TX] = (double)t[0] / 10;
data[TY] = (double)t[1] / 10;
data[TZ] = (double)t[2] / 10;
+
+
+ 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];
+
}
}
diff --git a/tracker-points/ftnoir_tracker_pt.h b/tracker-points/ftnoir_tracker_pt.h
index 9b8da4ae..9388bd03 100644
--- a/tracker-points/ftnoir_tracker_pt.h
+++ b/tracker-points/ftnoir_tracker_pt.h
@@ -75,6 +75,19 @@ private:
std::atomic<unsigned> point_count { 0 };
std::atomic<bool> ever_success = false;
mutable QMutex center_lock, data_lock;
+
+ // Translation solutions
+ std::vector<cv::Mat> iTranslations;
+ // Rotation solutions
+ std::vector<cv::Mat> iRotations;
+ // Angle solutions, pitch, yaw, roll, in this order
+ std::vector<cv::Vec3d> iAngles;
+ // The index of our best solution
+ int iBestSolutionIndex = -1;
+ // Best translation
+ cv::Vec3d iBestTranslation;
+ // Best angles
+ cv::Vec3d iBestAngles;
};
} // ns pt_impl