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

WebARKit/CMakeLists.txt

@@ -14,13 +14,13 @@ if(${EMSCRIPTEN_COMP} EQUAL 1)
   message("Fetching opencv for emscripten compilation from webarkit/opencv-em ...")
   FetchContent_Declare(
     build_opencv
-    URL https://github.com/webarkit/opencv-em/releases/download/0.1.3/opencv-js-4.7.0-emcc-3.1.26.zip
+    URL https://github.com/webarkit/opencv-em/releases/download/0.1.4/opencv-js-4.7.0-emcc-3.1.26.zip
   )
 else()
   message("Fetching opencv from webarkit/opencv-em ...")
   FetchContent_Declare(
     build_opencv
-    URL https://github.com/webarkit/opencv-em/releases/download/0.1.3/opencv-4.7.0.zip
+    URL https://github.com/webarkit/opencv-em/releases/download/0.1.4/opencv-4.7.0.zip
   )
 endif()
 
@@ -35,15 +35,19 @@ ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/
 ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h
 ${PARENT_DIR}/include/WebARKitCamera.h
 ${PARENT_DIR}/include/WebARKitLog.h
+${PARENT_DIR}/include/WebARKitGL.h
 ${PARENT_DIR}/include/WebARKitManager.h
+${PARENT_DIR}/include/WebARKitPattern.h
 )
 
 set(SOURCE 
 ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp
 ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
 ${PARENT_DIR}/WebARKitCamera.cpp
 ${PARENT_DIR}/WebARKitLog.cpp
+${PARENT_DIR}/WebARKitGL.cpp
 ${PARENT_DIR}/WebARKitManager.cpp
+${PARENT_DIR}/WebARKitPattern.cpp
 )
 
 add_library(
@@ -65,4 +69,9 @@ target_include_directories(WebARKitLib PRIVATE
         "${build_opencv_SOURCE_DIR}/libs/opencv/modules/imgproc/include"
         "${build_opencv_SOURCE_DIR}/libs/opencv/modules/video/include"
         "${build_opencv_SOURCE_DIR}/libs/opencv_contrib/modules/xfeatures2d/include"
-        )
+        )
+
+target_link_libraries(WebARKitLib
+jpeg
+"${build_opencv_SOURCE_DIR}/3rdparty/lib/liblibopenjp2.a"
+)

WebARKit/WebARKitGL.cpp

@@ -0,0 +1,81 @@
+#include <WebARKitGL.h>
+
+namespace webarkit {
+
+void arglCameraViewRHf(float para[3][4], float m_modelview[16], const float scale) {
+    m_modelview[0 + 0 * 4] = para[0][0]; // R1C1
+    m_modelview[0 + 1 * 4] = para[0][1]; // R1C2
+    m_modelview[0 + 2 * 4] = para[0][2];
+    m_modelview[0 + 3 * 4] = para[0][3];
+    m_modelview[1 + 0 * 4] = -para[1][0]; // R2
+    m_modelview[1 + 1 * 4] = -para[1][1];
+    m_modelview[1 + 2 * 4] = -para[1][2];
+    m_modelview[1 + 3 * 4] = -para[1][3];
+    m_modelview[2 + 0 * 4] = -para[2][0]; // R3
+    m_modelview[2 + 1 * 4] = -para[2][1];
+    m_modelview[2 + 2 * 4] = -para[2][2];
+    m_modelview[2 + 3 * 4] = -para[2][3];
+    m_modelview[3 + 0 * 4] = 0.0f;
+    m_modelview[3 + 1 * 4] = 0.0f;
+    m_modelview[3 + 2 * 4] = 0.0f;
+    m_modelview[3 + 3 * 4] = 1.0f;
+    if (scale != 0.0f) {
+        m_modelview[12] *= scale;
+        m_modelview[13] *= scale;
+        m_modelview[14] *= scale;
+    }
+}
+
+void arglCameraViewRHf(cv::Mat para, std::array<double, 16>& m_modelview, const double scale) {
+    m_modelview[0 + 0 * 4] = para.at<double>(0, 0); // R1C1
+    m_modelview[0 + 1 * 4] = para.at<double>(0, 1); // R1C2
+    m_modelview[0 + 2 * 4] = para.at<double>(0, 2);
+    m_modelview[0 + 3 * 4] = para.at<double>(0, 3);
+    m_modelview[1 + 0 * 4] = -para.at<double>(1, 0); // R2
+    m_modelview[1 + 1 * 4] = -para.at<double>(1, 1);
+    m_modelview[1 + 2 * 4] = -para.at<double>(1, 2);
+    m_modelview[1 + 3 * 4] = -para.at<double>(1, 3);
+    m_modelview[2 + 0 * 4] = -para.at<double>(2, 0); // R3
+    m_modelview[2 + 1 * 4] = -para.at<double>(2, 1);
+    m_modelview[2 + 2 * 4] = -para.at<double>(2, 2);
+    m_modelview[2 + 3 * 4] = -para.at<double>(2, 3);
+    m_modelview[3 + 0 * 4] = 0.0f;
+    m_modelview[3 + 1 * 4] = 0.0f;
+    m_modelview[3 + 2 * 4] = 0.0f;
+    m_modelview[3 + 3 * 4] = 1.0f;
+    if (scale != 0.0f) {
+        m_modelview[12] *= scale;
+        m_modelview[13] *= scale;
+        m_modelview[14] *= scale;
+    }
+}
+
+void cameraProjectionMatrix(const std::array<double, 9>& calibration, double nearPlane, double farPlane, int screenWidth, int screenHeight, std::array<double, 16>& projectionMatrix)
+{
+  // Camera parameters
+  double f_x = calibration.at(0); // Focal length in x axis
+  double f_y = calibration.at(4); // Focal length in y axis (usually the same?)
+  double c_x = calibration.at(2); // Camera primary point x
+  double c_y = calibration.at(5); // Camera primary point y
+
+  projectionMatrix[0] = -2.0f * f_x / screenWidth;
+  projectionMatrix[1] = 0.0f;
+  projectionMatrix[2] = 0.0f;
+  projectionMatrix[3] = 0.0f;
+
+  projectionMatrix[4] = 0.0f;
+  projectionMatrix[5] = 2.0f * f_y / screenHeight;
+  projectionMatrix[6] = 0.0f;
+  projectionMatrix[7] = 0.0f;
+
+  projectionMatrix[8] = 2.0f * c_x / screenWidth - 1.0f;
+  projectionMatrix[9] = 2.0f * c_y / screenHeight - 1.0f;    
+  projectionMatrix[10] = -( farPlane + nearPlane) / ( farPlane - nearPlane );
+  projectionMatrix[11] = -1.0f;
+
+  projectionMatrix[12] = 0.0f;
+  projectionMatrix[13] = 0.0f;
+  projectionMatrix[14] = -2.0f * farPlane * nearPlane / ( farPlane - nearPlane );        
+  projectionMatrix[15] = 0.0f;
+}
+} // namespace webarkit

WebARKit/WebARKitManager.cpp

@@ -41,13 +41,25 @@ bool WebARKitManager::initialiseBase(webarkit::TRACKER_TYPE trackerType, int fra
     return true;
 }
 
-bool WebARKitManager::initTracker(uchar* refData, size_t refCols, size_t refRows) {
+bool WebARKitManager::initTracker(cv::Mat refData, size_t refCols, size_t refRows, ColorSpace colorSpace) {
+    WEBARKIT_LOGd("WebARKitManager::initTracker(...)\n");
+    if (refData.empty() || refCols <= 0 || refRows <= 0) {
+        WEBARKIT_LOGe("Error initialising tracker.\n");
+        return false;
+    }
+    m_tracker->initTracker(refData, refCols, refRows, colorSpace);
+    state = WAITING_FOR_VIDEO;
+    WEBARKIT_LOGd("WebARKitManager::initTracker() done.\n");
+    return true;
+}
+
+bool WebARKitManager::initTracker(uchar* refData, size_t refCols, size_t refRows, ColorSpace colorSpace) {
     WEBARKIT_LOGd("WebARKitManager::initTracker(...)\n");
     if (!refData || refCols <= 0 || refRows <= 0) {
         WEBARKIT_LOGe("Error initialising tracker.\n");
         return false;
     }
-    m_tracker->initTracker(refData, refCols, refRows);
+    m_tracker->initTracker(refData, refCols, refRows, colorSpace);
     state = WAITING_FOR_VIDEO;
     WEBARKIT_LOGd("WebARKitManager::initTracker() done.\n");
     return true;
@@ -67,7 +79,7 @@ bool WebARKitManager::shutdown() {
     return true;
 };
 
-void WebARKitManager::processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace) {
+void WebARKitManager::processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace, bool enableBlur) {
     WEBARKIT_LOGd("WebARKitManager::processFrameData(...)\n");
     if (state < WAITING_FOR_VIDEO) {
         WEBARKIT_LOGe("processFrameData called without init the tracker. Call first initTracker.\n");
@@ -76,7 +88,7 @@ void WebARKitManager::processFrameData(uchar* frameData, size_t frameCols, size_
         WEBARKIT_LOGe("Error initialising processFrameData.\n");
         //return false;
     }
-  m_tracker->processFrameData(frameData, frameCols, frameRows, colorSpace);
+  m_tracker->processFrameData(frameData, frameCols, frameRows, colorSpace, enableBlur);
   state = DETECTION_RUNNING;
   WEBARKIT_LOGd("WebARKitManager::processFrameData() done\n");
 }
@@ -85,6 +97,24 @@ std::vector<double> WebARKitManager::getOutputData() {
     return m_tracker->getOutputData();
 };
 
+cv::Mat WebARKitManager::getPoseMatrix() {
+    return m_tracker->getPoseMatrix();
+}
+
+cv::Mat WebARKitManager::getGLViewMatrix() {
+    return m_tracker->getGLViewMatrix();
+}
+
+std::array<double, 16> WebARKitManager::getTransformationMatrix() {
+    std::array<double, 16> transformationMatrix;
+    webarkit::arglCameraViewRHf(m_tracker->getPoseMatrix(), transformationMatrix, 1.0f);
+    return transformationMatrix;
+}
+
+std::array<double, 16> WebARKitManager::getCameraProjectionMatrix() {
+    return m_tracker->getCameraProjectionMatrix();
+}
+
 bool WebARKitManager::isValid() {
   return m_tracker->isValid();
 }

WebARKit/WebARKitPattern.cpp

@@ -0,0 +1,58 @@
+#include <WebARKitPattern.h>
+#include <opencv2/calib3d.hpp>
+
+WebARKitPatternTrackingInfo::WebARKitPatternTrackingInfo() {
+    pose3d = cv::Mat::zeros(4, 4, CV_64FC1);
+    glViewMatrix = cv::Mat::zeros(4, 4, CV_64F);
+    m_scale = 1.0f;
+}
+
+void WebARKitPatternTrackingInfo::computePose(std::vector<cv::Point3f>& treeDPoints,
+                                              std::vector<cv::Point2f>& imgPoints, const cv::Matx33f& caMatrix,
+                                              const 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::Mat rvec, tvec;
+
+    cv::solvePnPRansac(treeDPoints, imgPoints, caMatrix, distCoeffs, rvec, tvec);
+
+    cv::Mat rMat;
+    cv::Rodrigues(rvec, rMat);
+    //cv::hconcat(rMat, tvec, pose3d);
+
+    for (unsigned int row = 0; row < 3; ++row) {
+        for (unsigned int col = 0; col < 3; ++col) {
+            pose3d.at<double>(row, col) = rMat.at<double>(row, col);
+        }
+        pose3d.at<double>(row, 3) = tvec.at<double>(row, 0);
+    }
+    pose3d.at<double>(3, 3) = 1.0f;
+
+    invertPose();
+}
+
+void WebARKitPatternTrackingInfo::computeGLviewMatrix() {
+   cv::transpose(pose3d , glViewMatrix);
+}
+
+void WebARKitPatternTrackingInfo::invertPose() {
+
+    /*cv::Mat invertPose(3, 4, CV_64FC1);
+    for (auto j = 0; j < 3; j++) {
+        invertPose.at<double>(j, 0) = pose3d.at<double>(j, 0);
+        invertPose.at<double>(j, 1) = -pose3d.at<double>(j, 1);
+        invertPose.at<double>(j, 2) = -pose3d.at<double>(j, 2);
+        //invertPose.at<double>(j, 3) = pose3d.at<double>(j, 3) * m_scale * 0.001f * 1.64f;
+        invertPose.at<double>(j, 3) = pose3d.at<double>(j, 3);
+    }*/
+
+    cv::Mat cvToGl = cv::Mat::zeros(4, 4, CV_64F);
+    cvToGl.at<double>(0, 0) = 1.0f;
+    cvToGl.at<double>(1, 1) = -1.0f; // Invert the y axis
+    cvToGl.at<double>(2, 2) = -1.0f; // invert the z axis
+    cvToGl.at<double>(3, 3) = 1.0f;
+
+    pose3d = cvToGl * pose3d;
+
+    // pose3d = invertPose;
+}

WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp

@@ -5,7 +5,7 @@ extern const double TEBLID_NN_MATCH_RATIO = 0.8f;
 extern const int DEFAULT_MAX_FEATURES = 8000;
 extern const int TEBLID_MAX_FEATURES = 10000;
 extern const int N = 10;
-extern const int MIN_NUM_MATCHES = 50;
+extern const int MIN_NUM_MATCHES = 8;
 extern const int maxLevel = 3; ///< Maximum number of levels in optical flow image pyramid.
 extern const cv::Size winSize(31, 31);
 extern const cv::TermCriteria termcrit(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);