getPoseMatrix function and improved computePose

WebARKit/WebARKitManager.cpp

@@ -85,6 +85,10 @@ std::vector<double> WebARKitManager::getOutputData() {
     return m_tracker->getOutputData();
 };
 
+cv::Mat WebARKitManager::getPoseMatrix() {
+    return m_tracker->getPoseMatrix();
+}
+
 bool WebARKitManager::isValid() {
   return m_tracker->isValid();
 }

WebARKit/WebARKitPattern.cpp

@@ -1,18 +1,17 @@
 #include <WebARKitPattern.h>
+#include <iostream>
 #include <opencv2/calib3d.hpp>
 
-void WebARKitPatternTrackingInfo::computePose(const WebARKitPattern& pattern, cv::Mat caMatrix, cv::Mat distCoeffs)
-{
-    cv::Mat Rvec;
-    cv::Mat_<float> Tvec;
-    cv::Mat raux, taux;
+void WebARKitPatternTrackingInfo::computePose(const WebARKitPattern& pattern, std::vector<cv::Point2f>& imgPoints,
+                                              cv::Mat& caMatrix, cv::Mat& distCoeffs) {
+    cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
+    cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
 
-    cv::solvePnPRansac(pattern.points3d, points2d, caMatrix, distCoeffs, raux, taux);
-    raux.convertTo(Rvec, CV_32F);
-    taux.convertTo(Tvec, CV_32F);
+    cv::solvePnPRansac(pattern.points3d, imgPoints, caMatrix, distCoeffs, rvec, tvec);
 
-    cv::Mat_<float> rotMat(3, 3);
-    cv::Rodrigues(Rvec, rotMat);
+    cv::Mat rMat;
+    cv::Rodrigues(rvec, rMat);
+    cv::hconcat(rMat, tvec, pose3d);
 
-    cv::hconcat(rotMat, Tvec, pose3d);
+    std::cout << "pose3d: " << pose3d.rows << "x" << pose3d.cols << std::endl;
 }

WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp

@@ -9,7 +9,10 @@ class WebARKitTracker::WebARKitTrackerImpl {
         : corners(4), initialized(false), output(17, 0.0), _valid(false), _isDetected(false), numMatches(0),
           minNumMatches(MIN_NUM_MATCHES), _nn_match_ratio(0.7f) {
         m_camMatrix = cv::Mat();
-        m_distortionCoeff = cv::Mat();
+        //m_distortionCoeff = cv::Mat();
+        //_patternTrackingInfo.pose3d = cv::Mat::zeros(3, 4, CV_64FC1);
+        //m_distortionCoeff = cv::Mat::zeros(6, 1, CV_64FC1);
+        m_distortionCoeff = cv::Mat::zeros(4,1,cv::DataType<double>::type); 
     };
 
     ~WebARKitTrackerImpl() = default;
@@ -28,7 +31,8 @@ class WebARKitTracker::WebARKitTrackerImpl {
         _camera->setupCamera(frameWidth, frameHeight);
         _camera->printSettings();
         m_camMatrix = cv::Mat(3, 3, CV_64FC1, _camera->getCameraData().data());
-        m_distortionCoeff = cv::Mat(6, 1, CV_64FC1, _camera->getDistortionCoefficients().data());
+        //m_distortionCoeff = cv::Mat(6, 1, CV_64FC1, _camera->getDistortionCoefficients().data());
+        //m_distortionCoeff = cv::Mat::zeros(6, 1, CV_64FC1);
     }
 
     void initTracker(uchar* refData, size_t refCols, size_t refRows) {
@@ -41,7 +45,7 @@ class WebARKitTracker::WebARKitTrackerImpl {
         this->_featureDetector->detect(refGray, refKeyPts, trackerFeatureMask);
         this->_featureDescriptor->compute(refGray, refKeyPts, refDescr);
 
-        // Normalized dimensions : 
+        // Normalized dimensions :
         const float maxSize = std::max(refCols, refRows);
         const float unitW = refCols / maxSize;
         const float unitH = refRows / maxSize;
@@ -96,6 +100,8 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
     std::vector<double> getOutputData() { return output; };
 
+    cv::Mat getPoseMatrix() { return _patternTrackingInfo.pose3d; };
+
     bool isValid() { return _valid; };
 
   protected:
@@ -208,8 +214,11 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
             // set old points to new points
             framePts = goodPtsCurr;
+            std::vector<cv::Point2f> warpedCorners;
             if ((valid = homographyValid(m_H))) {
                 fill_output(m_H);
+                warpedCorners = getSelectedFeaturesWarped(m_H);
+                _patternTrackingInfo.computePose(_pattern, warpedCorners, m_camMatrix, m_distortionCoeff);
                 _isDetected = true;
             } else {
                 _isDetected = false;
@@ -300,6 +309,20 @@ class WebARKitTracker::WebARKitTrackerImpl {
         return featureMask;
     }
 
+    std::vector<cv::Point2f> getSelectedFeaturesWarped(cv::Mat& H)
+{
+    std::vector<cv::Point2f> warpedPoints;
+    //std::vector<cv::Point2f> selectedPoints;
+    /*for(std::vector<TrackedPoint>::iterator it = _selectedPts.begin(); it != _selectedPts.end(); ++it) {
+        if(it->IsTracking()) {
+            selectedPoints.push_back(it->pt);
+        }
+    }*/
+    //perspectiveTransform(selectedPoints, warpedPoints, homography);
+    perspectiveTransform(_pattern.points2d, warpedPoints, H);
+    return warpedPoints;
+}
+
     bool _valid;
 
     bool _isDetected;
@@ -322,6 +345,8 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
     WebARKitPattern _pattern;
 
+    WebARKitPatternTrackingInfo _patternTrackingInfo;
+
     cv::Mat m_camMatrix;
     cv::Mat m_distortionCoeff;
 
@@ -390,6 +415,8 @@ void WebARKitTracker::processFrameData(uchar* frameData, size_t frameCols, size_
 
 std::vector<double> WebARKitTracker::getOutputData() { return _trackerImpl->getOutputData(); }
 
+cv::Mat WebARKitTracker::getPoseMatrix() { return _trackerImpl->getPoseMatrix(); }
+
 bool WebARKitTracker::isValid() { return _trackerImpl->isValid(); }
 
 } // namespace webarkit

WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h

@@ -27,6 +27,8 @@ class WebARKitTracker {
 
     std::vector<double> getOutputData();
 
+    cv::Mat getPoseMatrix();
+
     bool isValid();
 
   private:

WebARKit/include/WebARKitManager.h

@@ -99,6 +99,8 @@ class WebARKitManager {
 
     std::vector<double> getOutputData();
 
+    cv::Mat getPoseMatrix();
+
     bool isValid();
 };
 

WebARKit/include/WebARKitPattern.h

@@ -27,7 +27,7 @@ struct WebARKitPatternTrackingInfo
   /**
    * Compute pattern pose using PnP algorithm
    */
-  void computePose(const WebARKitPattern& pattern, cv::Mat caMatrix, cv::Mat distCoeffs);
+  void computePose(const WebARKitPattern& pattern, std::vector<cv::Point2f>& imgPoints, cv::Mat& caMatrix, cv::Mat& distCoeffs);
 };
 
 #endif // WEBARKITPATTERN_H