trying to find the root of the pose matrix issue

webarkit · Nov 3, 2023 · 0647381 · 0647381
1 parent 5e2d37b
commit 0647381
Show file tree

Hide file tree

Showing 3 changed files with 47 additions and 4 deletions.
diff --git a/WebARKit/WebARKitPattern.cpp b/WebARKit/WebARKitPattern.cpp
@@ -2,12 +2,12 @@
 #include <iostream>
 #include <opencv2/calib3d.hpp>
 
-void WebARKitPatternTrackingInfo::computePose(const WebARKitPattern& pattern, std::vector<cv::Point2f>& imgPoints,
+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(pattern.points3d, imgPoints, caMatrix, distCoeffs, rvec, tvec);
+    cv::solvePnPRansac(treeDPoints, imgPoints, caMatrix, distCoeffs, rvec, tvec);
 
     cv::Mat rMat;
     cv::Rodrigues(rvec, rMat);

diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp
@@ -31,6 +31,27 @@ class WebARKitTracker::WebARKitTrackerImpl {
         _camera->setupCamera(frameWidth, frameHeight);
         _camera->printSettings();
         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];
+            }
+        }*/
+
+        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);
     }
@@ -215,10 +236,17 @@ 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);
-                _patternTrackingInfo.computePose(_pattern, warpedCorners, m_camMatrix, m_distortionCoeff);
+                //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;
@@ -323,6 +351,21 @@ class WebARKitTracker::WebARKitTrackerImpl {
     return warpedPoints;
 }
 
+/*
+std::vector<cv::Point3f> getSelectedFeatures3D(cv::Mat& H)
+{
+    std::vector<cv::Point3f> treeDPoints;
+    //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.points3d, treeDPoints, H);
+    //return treeDPoints;
+//}*/
+
     bool _valid;
 
     bool _isDetected;

diff --git a/WebARKit/include/WebARKitPattern.h b/WebARKit/include/WebARKitPattern.h
@@ -27,7 +27,7 @@ struct WebARKitPatternTrackingInfo
   /**
    * Compute pattern pose using PnP algorithm
    */
-  void computePose(const WebARKitPattern& pattern, std::vector<cv::Point2f>& imgPoints, cv::Mat& caMatrix, cv::Mat& distCoeffs);
+  void computePose(std::vector<cv::Point3f>& treeDPoints, std::vector<cv::Point2f>& imgPoints, cv::Mat& caMatrix, cv::Mat& distCoeffs);
 };
 
 #endif // WEBARKITPATTERN_H