From 8141c4f07b1ddc4555d10a78ea5c3f482c8be04f Mon Sep 17 00:00:00 2001
From: Stéphane Lenclud <github@lenclud.com>
Date: Thu, 28 Mar 2019 20:54:35 +0100
Subject: Hacking my way through to cv::solveP3P usage.

---
 tracker-points/ftnoir_tracker_pt.cpp | 38 ++++++++++++++++++++++++++++++++++++
 tracker-points/module/CMakeLists.txt |  2 +-
 tracker-points/module/camera.cpp     |  1 -
 tracker-points/module/camera.h       |  2 ++
 tracker-points/pt-api.hpp            |  3 +++
 5 files changed, 44 insertions(+), 2 deletions(-)

diff --git a/tracker-points/ftnoir_tracker_pt.cpp b/tracker-points/ftnoir_tracker_pt.cpp
index ca228a24..e455a9ed 100644
--- a/tracker-points/ftnoir_tracker_pt.cpp
+++ b/tracker-points/ftnoir_tracker_pt.cpp
@@ -18,6 +18,8 @@
 #include <QFile>
 #include <QCoreApplication>
 
+#include <opencv2\calib3d.hpp>
+
 using namespace options;
 
 namespace pt_impl {
@@ -93,6 +95,42 @@ void Tracker_PT::run()
                                         info,
                                         dynamic_pose_ms);
                     ever_success.store(true, std::memory_order_relaxed);
+
+                    // TODO: Solve with OpenCV
+
+                    std::vector<cv::Point3f> objectPoints;
+                    //TODO: Stuff object points in that vector
+                    std::vector<cv::Point2f> trackedPoints;
+                    //TODO: Stuff bitmap point in there making sure they match the order of the object point
+
+                    // Create our camera matrix
+                    // TODO: Just do that once, use data memeber instead
+                    // Double or Float?
+                    cv::Mat cameraMatrix;
+                    cameraMatrix.create(3, 3, CV_64FC1);
+                    cameraMatrix.setTo(cv::Scalar(0));
+                    cameraMatrix.at<double>(0, 0) = camera->info.focalLengthX;
+                    cameraMatrix.at<double>(1, 1) = camera->info.focalLengthX;
+                    cameraMatrix.at<double>(0, 2) = camera->info.principalPointX;
+                    cameraMatrix.at<double>(1, 2) = camera->info.principalPointY;
+                    cameraMatrix.at<double>(2, 2) = 1;
+
+                    // Create distortion cooefficients
+                    cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
+                    for (int i = 0; i < 3; i++)
+                    {
+                        // Put in proper values
+                        distCoeffs.at<double>(i, 0) = 0;
+                    }
+
+                    std::vector<cv::Mat> rvecs, tvecs;
+
+                    // TODO: try SOLVEPNP_AP3P too 
+                    int num_of_solutions = cv::solveP3P(objectPoints, trackedPoints, cameraMatrix, distCoeffs, rvecs, tvecs, cv::SOLVEPNP_P3P);
+
+
+
+
                 }
 
                 QMutexLocker l2(&data_lock);
diff --git a/tracker-points/module/CMakeLists.txt b/tracker-points/module/CMakeLists.txt
index 1eec9616..33410f9b 100644
--- a/tracker-points/module/CMakeLists.txt
+++ b/tracker-points/module/CMakeLists.txt
@@ -2,5 +2,5 @@ find_package(OpenCV QUIET)
 if(OpenCV_FOUND)
     otr_module(tracker-points)
     target_link_libraries(${self} opentrack-tracker-points-base)
-    target_include_directories(${self} PUBLIC "${CMAKE_SOURCE_DIR}/tracker-pt")
+    target_include_directories(${self} PUBLIC "${CMAKE_SOURCE_DIR}/tracker-points")
 endif()
diff --git a/tracker-points/module/camera.cpp b/tracker-points/module/camera.cpp
index a70698de..25f1f8d5 100644
--- a/tracker-points/module/camera.cpp
+++ b/tracker-points/module/camera.cpp
@@ -96,7 +96,6 @@ bool Camera::start(const QString& name, int fps, int res_x, int res_y)
             if (!cap)
                 goto fail;
 
-            camera::info info {};
             info.fps = fps;
             info.width = res_x;
             info.height = res_y;
diff --git a/tracker-points/module/camera.h b/tracker-points/module/camera.h
index 02e2fe4d..65b0e552 100644
--- a/tracker-points/module/camera.h
+++ b/tracker-points/module/camera.h
@@ -35,6 +35,7 @@ struct Camera final : pt_camera
 
     void set_fov(f value) override { fov = value; }
     void show_camera_settings() override;
+    
 
 private:
     using camera = typename video::impl::camera;
@@ -47,6 +48,7 @@ private:
     pt_camera_info cam_desired;
 
     std::unique_ptr<camera> cap;
+    
     pt_settings s;
 
     static constexpr f dt_eps = f{1}/256;
diff --git a/tracker-points/pt-api.hpp b/tracker-points/pt-api.hpp
index 741576a1..a27c7e38 100644
--- a/tracker-points/pt-api.hpp
+++ b/tracker-points/pt-api.hpp
@@ -4,6 +4,7 @@
 
 #include "cv/numeric.hpp"
 #include "options/options.hpp"
+#include "video/camera.hpp"
 
 #include <tuple>
 #include <type_traits>
@@ -87,6 +88,8 @@ struct pt_camera
 
     virtual void set_fov(f value) = 0;
     virtual void show_camera_settings() = 0;
+
+    video::impl::camera::info info;
 };
 
 struct pt_point_extractor : pt_pixel_pos_mixin
-- 
cgit v1.2.3