From feb7026316a4f2ad551b4ea87226c264c5277ca4 Mon Sep 17 00:00:00 2001
From: Stéphane Lenclud <github@lenclud.com>
Date: Sun, 31 Mar 2019 09:40:37 +0200
Subject: First solveP3P results that are looking consistent. Translation
 vector in meters seems to be spot on. Rotation angles still need to be
 computed. Radial distortion still need to be taken into account.

---
 tracker-kinect-face/camera_kinect_ir.cpp | 33 +++++++++++++++++++++++++++++---
 1 file changed, 30 insertions(+), 3 deletions(-)

(limited to 'tracker-kinect-face/camera_kinect_ir.cpp')

diff --git a/tracker-kinect-face/camera_kinect_ir.cpp b/tracker-kinect-face/camera_kinect_ir.cpp
index b1975db9..8499003b 100644
--- a/tracker-kinect-face/camera_kinect_ir.cpp
+++ b/tracker-kinect-face/camera_kinect_ir.cpp
@@ -114,7 +114,7 @@ inline void SafeRelease(Interface *& pInterfaceToRelease)
     }
 }
 
-bool CameraKinectIr::start(const info& args)
+bool CameraKinectIr::start(info& aInfo)
 {
     stop();
 
@@ -146,6 +146,11 @@ bool CameraKinectIr::start(const info& args)
         }
 
         SafeRelease(pInfraredFrameSource);
+
+        if (SUCCEEDED(hr))
+        {
+            iKinectSensor->get_CoordinateMapper(&iCoordinateMapper);
+        }
     }
 
 
@@ -153,6 +158,27 @@ bool CameraKinectIr::start(const info& args)
     {
         WaitForFirstFrame();
         bool success = iMatFrame.ptr() != nullptr;
+        if (success)
+        {
+            // Provide frame info
+            aInfo.width = width;
+            aInfo.height = height;
+
+            CameraIntrinsics intrinsics;
+            hr = iCoordinateMapper->GetDepthCameraIntrinsics(&intrinsics);
+            if (SUCCEEDED(hr))
+            {
+                aInfo.focalLengthX = intrinsics.FocalLengthX;
+                aInfo.focalLengthY = intrinsics.FocalLengthY;
+                aInfo.principalPointX = intrinsics.PrincipalPointX;
+                aInfo.principalPointY = intrinsics.PrincipalPointY;
+                aInfo.radialDistortionFourthOrder = intrinsics.RadialDistortionFourthOrder;
+                aInfo.radialDistortionSecondOrder = intrinsics.RadialDistortionSecondOrder;
+                aInfo.radialDistortionSixthOrder = intrinsics.RadialDistortionSixthOrder;                
+            }
+
+        }
+
         return success;
     }
 
@@ -172,6 +198,7 @@ void CameraKinectIr::stop()
         iKinectSensor->Close();
     }
 
+    SafeRelease(iCoordinateMapper);
     SafeRelease(iKinectSensor);
 
     // Free up our memory buffer if any
@@ -253,9 +280,9 @@ bool CameraKinectIr::get_frame_(cv::Mat& frame)
             // For scalling to have more precission in the range we are interrested in
             min = max - 255;            
             // See: https://stackoverflow.com/questions/14539498/change-type-of-mat-object-from-cv-32f-to-cv-8u/14539652
-            raw.convertTo(raw8, CV_8U, 255.0 / (max - min), -255.0*min / (max - min));
+            raw.convertTo(iRaw8, CV_8U, 255.0 / (max - min), -255.0*min / (max - min));
             // Second convert to RGB
-            cv::cvtColor(raw8, frame, cv::COLOR_GRAY2BGR);
+            cv::cvtColor(iRaw8, frame, cv::COLOR_GRAY2BGR);
             //
             success = true;
         }
-- 
cgit v1.2.3