Feature enhancement: PnP algorithm for pose Matrix calculation and other functions improves

WebARKit/CMakeLists.txt

@@ -36,6 +36,7 @@ ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/
 ${PARENT_DIR}/include/WebARKitCamera.h
 ${PARENT_DIR}/include/WebARKitLog.h
 ${PARENT_DIR}/include/WebARKitManager.h
+${PARENT_DIR}/include/WebARKitPattern.h
 )
 
 set(SOURCE 
@@ -44,6 +45,7 @@ ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
 ${PARENT_DIR}/WebARKitCamera.cpp
 ${PARENT_DIR}/WebARKitLog.cpp
 ${PARENT_DIR}/WebARKitManager.cpp
+${PARENT_DIR}/WebARKitPattern.cpp
 )
 
 add_library(

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

@@ -0,0 +1,17 @@
+#include <WebARKitPattern.h>
+#include <iostream>
+#include <opencv2/calib3d.hpp>
+
+void WebARKitPatternTrackingInfo::computePose(std::vector<cv::Point3f>& treeDPoints, 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(treeDPoints, imgPoints, caMatrix, distCoeffs, rvec, tvec);
+
+    cv::Mat rMat;
+    cv::Rodrigues(rvec, rMat);
+    cv::hconcat(rMat, tvec, pose3d);
+
+    std::cout << "pose3d: " << pose3d.rows << " x " << pose3d.cols << std::endl;
+}

WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp

@@ -7,18 +7,21 @@ class WebARKitTracker::WebARKitTrackerImpl {
   public:
     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();
-          };
+          minNumMatches(MIN_NUM_MATCHES), _nn_match_ratio(0.7f) {
+        m_camMatrix = cv::Mat(3,3, cv::DataType<double>::type);
+        //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;
 
     void initialize(webarkit::TRACKER_TYPE trackerType, int frameWidth, int frameHeight) {
         setDetectorType(trackerType);
         if (trackerType == webarkit::TEBLID_TRACKER) {
             _nn_match_ratio = TEBLID_NN_MATCH_RATIO;
-        }
-        else if(trackerType == webarkit::AKAZE_TRACKER) {
+        } else if (trackerType == webarkit::AKAZE_TRACKER) {
             _nn_match_ratio = DEFAULT_NN_MATCH_RATIO;
             minNumMatches = 40;
         } else {
@@ -27,8 +30,41 @@ 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_camMatrix = cv::Mat(3, 3, CV_64FC1, _camera->getCameraData().data());
+        /*m_camMatrix = cv::Mat(3, 3, CV_64FC1);
+        std::array<double, 9> camData = _camera->getCameraData();
+        for(auto i = 0; i < 3; i++) {
+            for(auto j = 0; j < 3; j++) {
+                //WEBARKIT_LOGi("Camera Matrix: %d\n", camData[i*3+j]);
+                m_camMatrix.at<double>(i, j) = camData[i*3+j];
+            }
+        }*/
+
+        m_camMatrix.at<double>(0,0) = 833.63;
+        m_camMatrix.at<double>(0,1) = 0.0;
+        m_camMatrix.at<double>(0,2) = 353.50;
+        m_camMatrix.at<double>(1,0) = 0.0;
+        m_camMatrix.at<double>(1,1) = 833.63;
+        m_camMatrix.at<double>(1,2) = 464.50;
+        m_camMatrix.at<double>(2,0) = 0.0;
+        m_camMatrix.at<double>(2,1) = 0.0;
+        m_camMatrix.at<double>(2,2) = 1.0;
+
+
+        for(auto i = 0; i < 3; i++) {
+            for(auto j = 0; j < 3; j++) {
+                WEBARKIT_LOGi("Camera Matrix: %.2f\n", m_camMatrix.at<double>(i, j));
+            }
+        }
+
+        for(auto i = 0; i < 6; i++) {
+            for(auto j = 0; j < 1; j++) {
+                WEBARKIT_LOGi("Distortion coefficients: %.2f\n", m_distortionCoeff.at<double>(i, j));
+            }
+        }
+
+        //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,6 +77,29 @@ class WebARKitTracker::WebARKitTrackerImpl {
         this->_featureDetector->detect(refGray, refKeyPts, trackerFeatureMask);
         this->_featureDescriptor->compute(refGray, refKeyPts, refDescr);
 
+        // Normalized dimensions :
+        const float maxSize = std::max(refCols, refRows);
+        const float unitW = refCols / maxSize;
+        const float unitH = refRows / maxSize;
+
+        _pattern.size = cv::Size(refCols, refRows);
+
+        WEBARKIT_LOGd("WebARKitPattern size ready!\n");
+
+        _pattern.points2d.push_back(cv::Point2f(0, 0));
+        _pattern.points2d.push_back(cv::Point2f(refCols, 0));
+        _pattern.points2d.push_back(cv::Point2f(refCols, refRows));
+        _pattern.points2d.push_back(cv::Point2f(0, refRows));
+
+        WEBARKIT_LOGd("WebARKitPattern points2d ready!\n");
+
+        _pattern.points3d.push_back(cv::Point3f(-unitW, -unitH, 0));
+        _pattern.points3d.push_back(cv::Point3f(unitW, -unitH, 0));
+        _pattern.points3d.push_back(cv::Point3f(unitW, unitH, 0));
+        _pattern.points3d.push_back(cv::Point3f(-unitW, unitH, 0));
+
+        WEBARKIT_LOGd("WebARKitPattern points3d ready!\n");
+
         corners[0] = cvPoint(0, 0);
         corners[1] = cvPoint(refCols, 0);
         corners[2] = cvPoint(refCols, refRows);
@@ -73,6 +132,8 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
     std::vector<double> getOutputData() { return output; };
 
+    cv::Mat getPoseMatrix() { return _patternTrackingInfo.pose3d; };
+
     bool isValid() { return _valid; };
 
   protected:
@@ -185,8 +246,18 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
             // set old points to new points
             framePts = goodPtsCurr;
+            std::vector<cv::Point2f> warpedCorners;
+            //std::vector<cv::Point3f> treeDPoints;
             if ((valid = homographyValid(m_H))) {
                 fill_output(m_H);
+                warpedCorners = getSelectedFeaturesWarped(m_H);
+                //treeDPoints = getSelectedFeatures3D(m_H);
+                if(m_camMatrix.empty()) {
+                    WEBARKIT_LOGi("Camera Matrix is empty!\n");
+                }else {
+                    WEBARKIT_LOGi("Camera Matrix: %d\n", m_camMatrix.at<double>(0, 0));
+                    }    
+                _patternTrackingInfo.computePose(_pattern.points3d, warpedCorners, m_camMatrix, m_distortionCoeff);
                 _isDetected = true;
             } else {
                 _isDetected = false;
@@ -277,6 +348,14 @@ class WebARKitTracker::WebARKitTrackerImpl {
         return featureMask;
     }
 
+    std::vector<cv::Point2f> getSelectedFeaturesWarped(cv::Mat& H)
+{
+    std::vector<cv::Point2f> warpedPoints;
+    perspectiveTransform(_pattern.points2d, warpedPoints, H);
+    WEBARKIT_LOGi("warpedPoint(0,0): %.2f, %.2f\n", warpedPoints[0].x, warpedPoints[0].y);
+    return warpedPoints;
+}
+
     bool _valid;
 
     bool _isDetected;
@@ -297,6 +376,10 @@ class WebARKitTracker::WebARKitTrackerImpl {
 
     WebARKitCamera* _camera = new WebARKitCamera();
 
+    WebARKitPattern _pattern;
+
+    WebARKitPatternTrackingInfo _patternTrackingInfo;
+
     cv::Mat m_camMatrix;
     cv::Mat m_distortionCoeff;
 
@@ -333,7 +416,7 @@ class WebARKitTracker::WebARKitTrackerImpl {
             this->_featureDescriptor = cv::ORB::create(DEFAULT_MAX_FEATURES);
         } else if (trackerType == webarkit::TRACKER_TYPE::FREAK_TRACKER) {
             this->_featureDetector = cv::ORB::create(10000);
-            //this->_featureDetector = cv::xfeatures2d::StarDetector::create(DEFAULT_MAX_FEATURES);
+            // this->_featureDetector = cv::xfeatures2d::StarDetector::create(DEFAULT_MAX_FEATURES);
             this->_featureDescriptor = cv::xfeatures2d::FREAK::create();
         } else if (trackerType == webarkit::TRACKER_TYPE::TEBLID_TRACKER) {
             this->_featureDetector = cv::ORB::create(TEBLID_MAX_FEATURES);
@@ -351,7 +434,9 @@ WebARKitTracker::WebARKitTracker(WebARKitTracker&&) = default; // copy construct
 
 WebARKitTracker& WebARKitTracker::operator=(WebARKitTracker&&) = default; // move assignment operator
 
-void WebARKitTracker::initialize(webarkit::TRACKER_TYPE trackerType, int frameWidth, int frameHeight) { _trackerImpl->initialize(trackerType, frameWidth, frameHeight); }
+void WebARKitTracker::initialize(webarkit::TRACKER_TYPE trackerType, int frameWidth, int frameHeight) {
+    _trackerImpl->initialize(trackerType, frameWidth, frameHeight);
+}
 
 void WebARKitTracker::initTracker(uchar* refData, size_t refCols, size_t refRows) {
     _trackerImpl->initTracker(refData, refCols, refRows);
@@ -363,6 +448,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

@@ -4,6 +4,7 @@
 #include "WebARKitEnums.h"
 #include <WebARKitLog.h>
 #include <WebARKitCamera.h>
+#include <WebARKitPattern.h>
 #include <opencv2/xfeatures2d.hpp>
 
 namespace webarkit {
@@ -26,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

@@ -0,0 +1,33 @@
+#ifndef WEBARKITPATTERN_H
+#define WEBARKITPATTERN_H
+
+#include <opencv2/core/core.hpp>
+
+struct WebARKitPattern {
+    cv::Size size;
+
+    //cv::Mat grayImg;
+
+    //std::vector<cv::KeyPoint> keypoints;
+    //cv::Mat descriptors;
+
+    std::vector<cv::Point2f> points2d;
+    std::vector<cv::Point3f> points3d;
+};
+
+/**
+ * Intermediate pattern tracking info structure
+ */
+struct WebARKitPatternTrackingInfo
+{
+  cv::Mat                   homography;
+  std::vector<cv::Point2f>  points2d;
+  cv::Mat                   pose3d;
+
+  /**
+   * Compute pattern pose using PnP algorithm
+   */
+  void computePose(std::vector<cv::Point3f>& treeDPoints, std::vector<cv::Point2f>& imgPoints, cv::Mat& caMatrix, cv::Mat& distCoeffs);
+};
+
+#endif // WEBARKITPATTERN_H

tests/webarkit_test.cc

@@ -137,7 +137,7 @@ TEST(WebARKitTest, InitTrackerTest) {
   }
 
   if(image.empty()) {
-    image = cv::Mat(2048, 1637, CV_8UC4, cv::Scalar(0, 0, 0, 0));
+    image = cv::Mat::zeros(2048, 1637,  CV_8UC4);
   }
 
   ASSERT_FALSE(image.empty());