diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 32e2c3b..e6ec908 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -5,11 +5,11 @@ jobs: build: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Node.js - uses: actions/setup-node@v3 + uses: actions/setup-node@v4 with: - node-version: '16.x' + node-version: '20.x' - name: Build and test WebARKitLib run: | cd tests && mkdir build && cd build && cmake -DEMSCRIPTEN_COMP=0 .. && make && ./webarkit_test diff --git a/WebARKit/CMakeLists.txt b/WebARKit/CMakeLists.txt index 23d82ea..a809470 100644 --- a/WebARKit/CMakeLists.txt +++ b/WebARKit/CMakeLists.txt @@ -29,8 +29,11 @@ FetchContent_MakeAvailable(build_opencv) get_filename_component(PARENT_DIR ./ ABSOLUTE) set(WEBARKIT_HEADERS +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.h +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.h ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.h -${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking//WebARKitEnums.h +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitEnums.h +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.h ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h ${PARENT_DIR}/include/WebARKitCamera.h @@ -41,7 +44,10 @@ ${PARENT_DIR}/include/WebARKitPattern.h ) set(SOURCE +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.cpp +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.cpp ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp +${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.cpp ${PARENT_DIR}/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp ${PARENT_DIR}/WebARKitCamera.cpp ${PARENT_DIR}/WebARKitLog.cpp diff --git a/WebARKit/WebARKitManager.cpp b/WebARKit/WebARKitManager.cpp index 1d1a227..0914f7e 100644 --- a/WebARKit/WebARKitManager.cpp +++ b/WebARKit/WebARKitManager.cpp @@ -101,6 +101,10 @@ cv::Mat WebARKitManager::getPoseMatrix() { return m_tracker->getPoseMatrix(); } +float* WebARKitManager::getPoseMatrix2() { + return m_tracker->getPoseMatrix2(); +} + cv::Mat WebARKitManager::getGLViewMatrix() { return m_tracker->getGLViewMatrix(); } diff --git a/WebARKit/WebARKitPattern.cpp b/WebARKit/WebARKitPattern.cpp index 4f00fbc..1131c70 100644 --- a/WebARKit/WebARKitPattern.cpp +++ b/WebARKit/WebARKitPattern.cpp @@ -7,18 +7,32 @@ WebARKitPatternTrackingInfo::WebARKitPatternTrackingInfo() { m_scale = 1.0f; } +void WebARKitPatternTrackingInfo::cameraPoseFromPoints(cv::Mat& pose, const std::vector& objPts, + const std::vector& imgPts, 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::solvePnPRansac(objPts, imgPts, caMatrix, distCoeffs, rvec, tvec); + + // Assemble pose matrix from rotation and translation vectors. + cv::Mat rMat; + Rodrigues(rvec, rMat); + cv::hconcat(rMat, tvec, pose); +}; + void WebARKitPatternTrackingInfo::computePose(std::vector& treeDPoints, std::vector& 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 = 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); + // cv::hconcat(rMat, tvec, pose3d); for (unsigned int row = 0; row < 3; ++row) { for (unsigned int col = 0; col < 3; ++col) { @@ -31,10 +45,16 @@ void WebARKitPatternTrackingInfo::computePose(std::vector& treeDPoi invertPose(); } -void WebARKitPatternTrackingInfo::computeGLviewMatrix() { - cv::transpose(pose3d , glViewMatrix); +void WebARKitPatternTrackingInfo::getTrackablePose(cv::Mat& pose) { + //float transMat [3][4]; + cv::Mat poseOut; + pose.convertTo(poseOut, CV_32FC1); + //std::cout << "poseOut: " << poseOut << std::endl; + memcpy(transMat, poseOut.ptr(0), 3*4*sizeof(float)); } +void WebARKitPatternTrackingInfo::computeGLviewMatrix() { cv::transpose(pose3d, glViewMatrix); } + void WebARKitPatternTrackingInfo::invertPose() { /*cv::Mat invertPose(3, 4, CV_64FC1); diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.cpp new file mode 100644 index 0000000..e2d6223 --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.cpp @@ -0,0 +1,60 @@ +/* + * TrackedPoint.cpp + * artoolkitX + * + * This file is part of artoolkitX. + * + * artoolkitX is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * artoolkitX is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with artoolkitX. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2018 Realmax, Inc. + * Copyright 2015 Daqri, LLC. + * Copyright 2010-2015 ARToolworks, Inc. + * + * Author(s): Philip Lamb, Daniel Bell. + * Modified for WebARKit by @kalwalt - Walter Perdan - 2024 + * + */ + +#include + +bool TrackedPoint::IsTracking() +{ + return tracking; +} + +void TrackedPoint::SetTracking(bool newTracking) +{ + tracking = newTracking; +} + +bool TrackedPoint::IsSelected() +{ + return selected; +} + +void TrackedPoint::SetSelected(bool newSelected) +{ + selected = newSelected; +} diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.cpp new file mode 100644 index 0000000..e77a5fc --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.cpp @@ -0,0 +1,194 @@ +/* + * TrackingPointSelector.cpp + * artoolkitX + * + * This file is part of artoolkitX. + * + * artoolkitX is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * artoolkitX is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with artoolkitX. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2018 Realmax, Inc. + * Copyright 2015 Daqri, LLC. + * Copyright 2010-2015 ARToolworks, Inc. + * + * Author(s): Philip Lamb, Daniel Bell. + * Modified for WebARKit by @kalwalt - Walter Perdan - 2024 + * + */ + +#include + +TrackingPointSelector::TrackingPointSelector() +{ +} + +TrackingPointSelector::TrackingPointSelector(std::vector pts, int width, int height, int markerTemplateWidth) : + _reset(false), + _pts(pts) +{ + DistributeBins(width, height, markerTemplateWidth); +} + +/** + @brief Iterates over \_pts and for each one, provided it doesn't intersect the image border, + creates a TrackedPoint representing a template () with a serially-increasing id from 0, and puts + it into the trackingPointsBin structure (a vector of pairs of (binIndex, trackingPoint). + */ +void TrackingPointSelector::DistributeBins(int width, int height, int markerTemplateWidth) +{ + int numberOfBins = 10; + + // Split width and height dimensions into 10 bins each, for total of 100 bins. + int totalXBins = width/numberOfBins; + int totalYBins = height/numberOfBins; + // Init empty bins. + for (int i = 0; i < (numberOfBins * numberOfBins); i++) { + trackingPointBin.insert(std::pair >(i, std::vector())); + } + + // Iterate the points and add points to each bin. + for (int i = 0, id = 0; i < _pts.size(); i++) { + int bx = (int)_pts[i].x/totalXBins; + int by = (int)_pts[i].y/totalYBins; + int index = bx + (by * numberOfBins); + + cv::Rect templateRoi = cv::Rect(_pts[i].x - markerTemplateWidth, _pts[i].y - markerTemplateWidth, markerTemplateWidth*2, markerTemplateWidth*2); + bool is_inside = (templateRoi & cv::Rect(0, 0, width, height)) == templateRoi; // templateRoi must not intersect image boundary. + if (is_inside) { + TrackedPoint newPt; + newPt.id = id; + newPt.pt = _pts[i]; + newPt.pt3d = cv::Point3f(_pts[i].x, _pts[i].y, 0); + newPt.markerRoi = templateRoi; + trackingPointBin[index].push_back(newPt); + id++; + } + } +} + +void TrackingPointSelector::SetHomography(cv::Mat newHomography) +{ + _homography = newHomography; +} + +/// @return 3x3 cv::Mat (of type CV_64FC1, i.e. double) containing the homography. +cv::Mat TrackingPointSelector::GetHomography() +{ + return _homography; +} + +void TrackingPointSelector::UpdatePointStatus(std::vector status) +{ + int index = 0; + for (std::vector::iterator it = _selectedPts.begin(); it != _selectedPts.end(); ++it) { + if (it->tracking) { + it->SetTracking((int)status[index++]); + } + } +} + +void TrackingPointSelector::ResetSelection() +{ + _reset = true; +} + +std::vector TrackingPointSelector::GetInitialFeatures() +{ + if (!_reset) return GetTrackedFeatures(); + _reset = false; + + // Reset state of all points to not selected and not tracking. + _selectedPts.clear(); + for (auto &bin : trackingPointBin) { + for (auto &trackPt : bin.second) { + trackPt.SetSelected(false); + trackPt.SetTracking(false); + } + } + + // Selects a random template from each bin for tracking. + std::vector ret; + for (auto &bin : trackingPointBin) { + size_t pointCount = bin.second.size(); + if (pointCount > 0) { // If there are points in the bin. + // Select a random point from the bin. + int tIndex = pointCount > 1 ? rng.uniform(0, static_cast(bin.second.size())) : 0; + bin.second[tIndex].SetSelected(true); + bin.second[tIndex].SetTracking(true); + _selectedPts.push_back(bin.second[tIndex]); + + ret.push_back(bin.second[tIndex].pt); + } + } + return ret; +} + +std::vector TrackingPointSelector::GetTrackedFeatures() +{ + std::vector selectedPoints; + for (std::vector::iterator it = _selectedPts.begin(); it != _selectedPts.end(); ++it) { + if (it->IsTracking()) { + selectedPoints.push_back(it->pt); + } + } + return selectedPoints; +} + +std::vector TrackingPointSelector::GetTrackedFeatures3d() +{ + std::vector selectedPoints; + for (std::vector::iterator it = _selectedPts.begin(); it != _selectedPts.end(); ++it) { + if (it->IsTracking()) { + selectedPoints.push_back(it->pt3d); + } + } + return selectedPoints; +} + +std::vector TrackingPointSelector::GetTrackedFeaturesWarped() +{ + std::vector selectedPoints = GetTrackedFeatures(); + std::vector warpedPoints; + perspectiveTransform(selectedPoints, warpedPoints, _homography); + return warpedPoints; +} + +std::vector TrackingPointSelector::GetAllFeatures() +{ + std::vector allBinnedPoints; + for (auto &track : trackingPointBin) { + for (auto &trackPt : track.second) { + allBinnedPoints.push_back(trackPt.pt); + } + } + return allBinnedPoints; +} + +void TrackingPointSelector::CleanUp() +{ + _selectedPts.clear(); + _pts.clear(); + trackingPointBin.clear(); + _homography.release(); +} diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp index 89539e9..a06b027 100644 --- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.cpp @@ -2,17 +2,24 @@ extern const double DEFAULT_NN_MATCH_RATIO = 0.7f; 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 DEFAULT_MAX_FEATURES = 800; +extern const int TEBLID_MAX_FEATURES = 1000; extern const int N = 10; extern const int MIN_NUM_MATCHES = 8; +extern const int minRequiredDetectedFeatures = 50; ///< Minimum number of detected features required to consider a target matched. +extern const int markerTemplateWidth = 15; ///< Width in pixels of image patches used in template matching. 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); +extern const int searchRadius = 15; +extern const int match_method = cv::TM_SQDIFF_NORMED; +extern const cv::Size featureImageMinSize(640, 480); ///< Minimum size when downscaling incoming images used for feature tracking. extern const double featureDetectPyramidLevel = 1.05f; ///> Scale factor applied to image pyramid to determine image to perform feature matching upon. extern const int featureBorder = 8; extern const cv::Size blurSize(3, 3); +extern const double ransac_thresh = 2.5f; +extern cv::RNG rng( 0xFFFFFFFF ); extern const double m_pi = 3.14159265358979323846; extern const std::string WEBARKIT_HEADER_VERSION_STRING = "1.0.0"; /*@ diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.cpp new file mode 100644 index 0000000..7d856fa --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.cpp @@ -0,0 +1,20 @@ +#include + +namespace webarkit { +namespace homography { + +WebARKitHomographyInfo::WebARKitHomographyInfo() { validHomography = false; } + +WebARKitHomographyInfo::WebARKitHomographyInfo(cv::Mat hom, std::vector newStatus, + std::vector matches) { + homography = hom; + status = newStatus; + inlier_matches = matches; + if (matches.size() > 4) { + validHomography = true; + } +} + +} // namespace homography + +} // namespace webarkit diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp index 0d566da..0c338e1 100644 --- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.cpp @@ -1,13 +1,26 @@ +#include +#include #include +#include #include + namespace webarkit { class WebARKitTracker::WebARKitTrackerImpl { public: + bool _trackVizActive; + TrackerVisualization _trackViz; + WebARKitTrackerImpl() - : corners(4), initialized(false), output(17, 0.0), _valid(false), _isDetected(false), numMatches(0), - minNumMatches(MIN_NUM_MATCHES), _nn_match_ratio(0.7f) { + : corners(4), initialized(false), output(17, 0.0), _valid(false), _maxNumberOfMarkersToTrack(1), + _currentlyTrackedMarkers(0), _frameCount(0), _frameSizeX(0), + _frameSizeY(0), + _featureDetectPyrLevel(0), + _featureDetectScaleFactor(cv::Vec2f(1.0f, 1.0f)), + _isDetected(false), _isTracking(false), numMatches(0), + minNumMatches(MIN_NUM_MATCHES), _nn_match_ratio(0.7f), _trackVizActive(false), + _trackViz(TrackerVisualization()) { m_camMatrix = cv::Matx33d::zeros(); m_distortionCoeff = cv::Mat::zeros(4, 1, cv::DataType::type); }; @@ -15,6 +28,23 @@ class WebARKitTracker::WebARKitTrackerImpl { ~WebARKitTrackerImpl() = default; void initialize(webarkit::TRACKER_TYPE trackerType, int frameWidth, int frameHeight) { + _frameSizeX = frameWidth; + _frameSizeY = frameHeight; + + // Calculate image downsamping factor. 0 = no size change, 1 = half width and height, 2 = quarter width and height etc. + double xmin_log2 = std::log2(static_cast(featureImageMinSize.width)); + double ymin_log2 = std::log2(static_cast(featureImageMinSize.height)); + _featureDetectPyrLevel = std::min(std::floor(std::log2(static_cast(_frameSizeX)) - xmin_log2), std::floor(std::log2(static_cast(_frameSizeY)) - ymin_log2)); + + // Calculate the exact scale factor using the same calculation pyrDown uses. + int xScaled = _frameSizeX; + int yScaled = _frameSizeY; + for (int i = 1; i <= _featureDetectPyrLevel; i++) { + xScaled = (xScaled + 1) / 2; + yScaled = (yScaled + 1) / 2; + _featureDetectScaleFactor = cv::Vec2f((float)_frameSizeX / (float)xScaled, (float)_frameSizeY / (float)yScaled); + } + setDetectorType(trackerType); if (trackerType == webarkit::TEBLID_TRACKER) { _nn_match_ratio = TEBLID_NN_MATCH_RATIO; @@ -25,7 +55,7 @@ class WebARKitTracker::WebARKitTrackerImpl { _nn_match_ratio = DEFAULT_NN_MATCH_RATIO; minNumMatches = 15; } - WEBARKIT_LOGi("Min Num Matches: %d\n", minNumMatches); + WEBARKIT_LOGd("Min Num Matches: %d\n", minNumMatches); _camera->setupCamera(frameWidth, frameHeight); _camera->printSettings(); @@ -47,17 +77,21 @@ class WebARKitTracker::WebARKitTrackerImpl { } webarkit::cameraProjectionMatrix(camData, 0.1, 1000.0, frameWidth, frameHeight, m_cameraProjectionMatrix); + + _pyramid.clear(); + _prevPyramid.clear(); + _currentlyTrackedMarkers = 0; } - template - void initTracker(T refData, size_t refCols, size_t refRows, ColorSpace colorSpace) { + template void initTracker(T refData, size_t refCols, size_t refRows, ColorSpace colorSpace) { WEBARKIT_LOGi("Init Tracker!\n"); cv::Mat refGray = convert2Grayscale(refData, refCols, refRows, colorSpace); + refGray.copyTo(_image); cv::Mat trackerFeatureMask = createTrackerFeatureMask(refGray); - if(!extractFeatures(refGray, trackerFeatureMask, refKeyPts, refDescr)) { + if (!extractFeatures(refGray, trackerFeatureMask, refKeyPts, refDescr)) { WEBARKIT_LOGe("No features detected!\n"); return; }; @@ -95,17 +129,20 @@ class WebARKitTracker::WebARKitTrackerImpl { _bBox.push_back(cv::Point2f(refCols, refRows)); _bBox.push_back(cv::Point2f(0, refRows)); + _trackSelection = TrackingPointSelector(Points(refKeyPts), refCols, refRows, markerTemplateWidth); + initialized = true; WEBARKIT_LOGi("Tracker ready!\n"); } - bool extractFeatures(const cv::Mat& grayImage, cv::Mat& featureMask, std::vector& keypoints, cv::Mat& descriptors) const { + bool extractFeatures(const cv::Mat& grayImage, cv::Mat& featureMask, std::vector& keypoints, + cv::Mat& descriptors) const { assert(!grayImage.empty()); assert(grayImage.channels() == 1); this->_featureDetector->detect(grayImage, keypoints, featureMask); - if (keypoints.empty()){ + if (keypoints.empty()) { WEBARKIT_LOGe("No keypoints detected!\n"); return false; } @@ -115,14 +152,14 @@ class WebARKitTracker::WebARKitTrackerImpl { return false; } return true; - } + } - void processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace, bool enableBlur) { + void processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace, + bool enableBlur) { cv::Mat grayFrame = convert2Grayscale(frameData, frameCols, frameRows, colorSpace); if (enableBlur) { cv::blur(grayFrame, grayFrame, blurSize); - } - buildImagePyramid(grayFrame); + } processFrame(grayFrame); grayFrame.release(); }; @@ -131,6 +168,8 @@ class WebARKitTracker::WebARKitTrackerImpl { cv::Mat getPoseMatrix() { return _patternTrackingInfo.pose3d; }; + float* getPoseMatrix2() { return (float*)_patternTrackingInfo.transMat; } + cv::Mat getGLViewMatrix() { return _patternTrackingInfo.glViewMatrix; }; std::array getCameraProjectionMatrix() { return m_cameraProjectionMatrix; }; @@ -138,146 +177,215 @@ class WebARKitTracker::WebARKitTrackerImpl { bool isValid() { return _valid; }; protected: - bool resetTracking(cv::Mat& currIm) { - if (!initialized) { - WEBARKIT_LOGe("Reference image not found. AR is unintialized!\n"); - return NULL; + bool RunTemplateMatching(cv::Mat frame, int trackableId) { + // std::cout << "Starting template match" << std::endl; + std::vector finalTemplatePoints, finalTemplateMatchPoints; + // Get a handle on the corresponding points from current image and the marker + // std::vector trackablePoints = _trackables[trackableId]._trackSelection.GetTrackedFeatures(); + // std::vector trackablePointsWarped = + // _trackables[trackableId]._trackSelection.GetTrackedFeaturesWarped(); + std::vector trackablePoints = _trackSelection.GetTrackedFeatures(); + std::vector trackablePointsWarped = _trackSelection.GetTrackedFeaturesWarped(); + // Create an empty result image - May be able to pre-initialize this container + + int n = (int)trackablePointsWarped.size(); + if (_trackVizActive) { + _trackViz.templateMatching = {}; + _trackViz.templateMatching.templateMatchingCandidateCount = n; } - WEBARKIT_LOGi("Reset Tracking!\n"); - - clear_output(); - - _isDetected = false; - - cv::Mat frameDescr; - std::vector frameKeyPts; - bool valid; - - cv::Mat featureMask = createFeatureMask(currIm); - - if(!extractFeatures(currIm, featureMask, frameKeyPts, frameDescr)) { - WEBARKIT_LOGe("No features detected in resetTracking!\n"); - return false; - }; - - if (!_isDetected) { - std::vector refPts; - - getMatches(frameDescr, frameKeyPts, refPts, framePts); - numMatches = framePts.size(); - - WEBARKIT_LOG("Num Matches: %d\n", numMatches); - - if (numMatches >= minNumMatches) { - m_H = cv::findHomography(refPts, framePts, cv::RANSAC); - if ((valid = homographyValid(m_H))) { - _isDetected = true; - //numMatches = framePts.size(); - perspectiveTransform(_bBox, _bBoxTransformed, m_H); + for (int j = 0; j < n; j++) { + auto pt = trackablePointsWarped[j]; + // if (cv::pointPolygonTest(_trackables[trackableId]._bBoxTransformed, trackablePointsWarped[j], true) > 0) + // { + if (cv::pointPolygonTest(_bBoxTransformed, trackablePointsWarped[j], true) > 0) { + auto ptOrig = trackablePoints[j]; + + cv::Rect templateRoi = GetTemplateRoi(pt); + cv::Rect frameROI(0, 0, frame.cols, frame.rows); + if (IsRoiValidForFrame(frameROI, templateRoi)) { + // cv::Rect markerRoi(0, 0, _trackables[trackableId]._image.cols, + // _trackables[trackableId]._image.rows); + cv::Rect markerRoi(0, 0, _image.cols, _image.rows); + + std::vector vertexPoints = GetVerticesFromPoint(ptOrig); + std::vector vertexPointsResults; + // perspectiveTransform(vertexPoints, vertexPointsResults, + // _trackables[trackableId]._trackSelection.GetHomography()); + perspectiveTransform(vertexPoints, vertexPointsResults, _trackSelection.GetHomography()); + + cv::Rect srcBoundingBox = cv::boundingRect(cv::Mat(vertexPointsResults)); + + vertexPoints.clear(); + vertexPoints = GetVerticesFromTopCorner(srcBoundingBox.x, srcBoundingBox.y, srcBoundingBox.width, + srcBoundingBox.height); + // perspectiveTransform(vertexPoints, vertexPointsResults, + // _trackables[trackableId]._trackSelection.GetHomography().inv()); + perspectiveTransform(vertexPoints, vertexPointsResults, _trackSelection.GetHomography().inv()); + + std::vector testVertexPoints = FloorVertexPoints(vertexPointsResults); + std::vector finalWarpPoints = + GetVerticesFromTopCorner(0, 0, srcBoundingBox.width, srcBoundingBox.height); + cv::Mat templateHomography = + findHomography(testVertexPoints, finalWarpPoints, cv::RANSAC, ransac_thresh); + + if (!templateHomography.empty()) { + cv::Rect templateBoundingBox = cv::boundingRect(cv::Mat(vertexPointsResults)); + cv::Rect searchROI = InflateRoi(templateRoi, searchRadius); + if (IsRoiValidForFrame(frameROI, searchROI)) { + searchROI = searchROI & frameROI; + templateBoundingBox = templateBoundingBox & markerRoi; + + if (templateBoundingBox.area() > 0 && searchROI.area() > templateBoundingBox.area()) { + cv::Mat searchImage = frame(searchROI); + // cv::Mat templateImage = _trackables[trackableId]._image(templateBoundingBox); + cv::Mat templateImage = _image(templateBoundingBox); + cv::Mat warpedTemplate; + + warpPerspective(templateImage, warpedTemplate, templateHomography, + srcBoundingBox.size()); + cv::Mat matchResult = MatchTemplateToImage(searchImage, warpedTemplate); + + if (!matchResult.empty()) { + double minVal; + double maxVal; + cv::Point minLoc, maxLoc, matchLoc; + minMaxLoc(matchResult, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat()); + if (minVal < 0.5) { + matchLoc = minLoc; + matchLoc.x += searchROI.x + (warpedTemplate.cols / 2); + matchLoc.y += searchROI.y + (warpedTemplate.rows / 2); + finalTemplatePoints.push_back(ptOrig); + finalTemplateMatchPoints.push_back(matchLoc); + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedTemplateMinimumCorrelationCount++; + } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedTemplateMatchCount++; + } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedTemplateBigEnoughTestCount++; + } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedSearchROIInFrameTestCount++; + } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedGotHomogTestCount++; + } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedROIInFrameTestCount++; } + } else { + if (_trackVizActive) + _trackViz.templateMatching.failedBoundsTestCount++; } } + bool gotHomography = updateTrackableHomography(trackableId, finalTemplatePoints, finalTemplateMatchPoints); + if (!gotHomography) { + // _trackables[trackableId]._isTracking = false; + // _trackables[trackableId]._isDetected = false; + _isTracking = false; + _isDetected = false; + _currentlyTrackedMarkers--; + } + if (_trackVizActive) { + _trackViz.templateMatching.templateMatchingOK = gotHomography; + } + return gotHomography; + } - return valid; - }; + void processFrame(cv::Mat& frame) { + buildImagePyramid(frame); - bool track() { if (!initialized) { WEBARKIT_LOGe("Reference image not found. AR is unintialized!\n"); - return NULL; + assert(initialized == false); } - WEBARKIT_LOGi("Start tracking!\n"); - clear_output(); - - // use optical flow to track keypoints - std::vector err; - std::vector status; - std::vector currPts, goodPtsCurr, goodPtsPrev; - bool valid; - calcOpticalFlowPyrLK(_prevPyramid, _pyramid, framePts, currPts, status, err, winSize, maxLevel, termcrit, 0, - 0.001); - - // calculate average variance - double mean, avg_variance = 0.0; - double sum = 0.0; - double diff; - std::vector diffs; - for (size_t i = 0; i < framePts.size(); ++i) { - if (status[i]) { - goodPtsCurr.push_back(currPts[i]); - goodPtsPrev.push_back(framePts[i]); - - diff = sqrt(pow(currPts[i].x - framePts[i].x, 2.0) + pow(currPts[i].y - framePts[i].y, 2.0)); - sum += diff; - diffs.push_back(diff); - } + if (_trackVizActive) { + memset(_trackViz.bounds, 0, 8 * sizeof(float)); + _trackViz.opticalFlowTrackablePoints.clear(); + _trackViz.opticalFlowTrackedPoints.clear(); + _trackViz.opticalFlowOK = false; } - mean = sum / diffs.size(); - for (int i = 0; i < goodPtsCurr.size(); ++i) { - avg_variance += pow(diffs[i] - mean, 2); - } - avg_variance /= diffs.size(); - - if ((goodPtsCurr.size() > numMatches / 2) && (1.75 > avg_variance)) { - cv::Mat transform = estimateAffine2D(goodPtsPrev, goodPtsCurr); - - // add row of {0,0,1} to transform to make it 3x3 - cv::Mat row = cv::Mat::zeros(1, 3, CV_64F); - row.at(0, 2) = 1.0; - transform.push_back(row); - - // update homography matrix - m_H = transform * m_H; + WEBARKIT_LOGd("Reset Tracking!\n"); - // set old points to new points - framePts = goodPtsCurr; - std::vector warpedCorners; - - if ((valid = homographyValid(m_H))) { - fill_output(m_H); - - warpedCorners = getSelectedFeaturesWarped(m_H); + clear_output(); - _patternTrackingInfo.computePose(_pattern.points3d, warpedCorners, m_camMatrix, m_distortionCoeff); + _isDetected = false; - _patternTrackingInfo.computeGLviewMatrix(); + cv::Mat frameDescr; + std::vector frameKeyPts; - _isDetected = true; + // This if cond. doesn't works as expected in artoolkitx. This make the tracking process unstable. + // if (_currentlyTrackedMarkers < _maxNumberOfMarkersToTrack) { + /*cv::Mat detectionFrame; + if (_featureDetectPyrLevel < 1) { + detectionFrame = frame; } else { - _isDetected = false; + cv::Mat srcFrame = frame; + for (int pyrLevel = 1; pyrLevel <= _featureDetectPyrLevel; pyrLevel++) { + cv::pyrDown(srcFrame, detectionFrame, cv::Size(0, 0)); + srcFrame = detectionFrame; + } + }*/ + + cv::Mat featureMask = createFeatureMask(frame); + + if (!extractFeatures(frame, featureMask, frameKeyPts, frameDescr)) { + WEBARKIT_LOGe("No features detected in extractFeatures!\n"); + // return false; + }; + //if (!_isDetected) { + WEBARKIT_LOGd("frameKeyPts.size() = %d\n", frameKeyPts.size()); + if (static_cast(frameKeyPts.size()) > minRequiredDetectedFeatures) { + MatchFeatures(frameKeyPts, frameDescr); + } + //} ref -> if (_currentlyTrackedMarkers < _maxNumberOfMarkersToTrack) { + int i = 0; + if (_isDetected) { + WEBARKIT_LOGd("Start tracking!\n"); + if (_frameCount > 0 && _prevPyramid.size() > 0) { + // if (_prevPyramid.size() > 0) { + // std::cout << "Starting Optical Flow" << std::endl; + std::vector trackablePoints = _trackSelection.GetInitialFeatures(); + std::vector trackablePointsWarped = _trackSelection.GetTrackedFeaturesWarped(); + if (!runOpticalFlow(i, trackablePoints, trackablePointsWarped)) { + WEBARKIT_LOGd("Optical flow failed.\n"); + } else { + if (_trackVizActive) { _trackViz.opticalFlowOK = true;} + // Refine optical flow with template match. + if (!RunTemplateMatching(frame, i)) { + WEBARKIT_LOGd("Template matching failed."); + } + // std::cout << "Optical flow ok." << std::endl; + } } } - //swapImagePyramid(); - - return valid; - }; - - void processFrame(cv::Mat& frame) { - if (!this->_valid) { - this->_valid = resetTracking(frame); - } else { - this->_valid = track(); + if (_isDetected || _isTracking) { + cv::Mat _pose; + std::vector imgPoints = _trackSelection.GetTrackedFeaturesWarped(); + std::vector objPoints = _trackSelection.GetTrackedFeatures3d(); + _patternTrackingInfo.cameraPoseFromPoints(_pose, objPoints, imgPoints, m_camMatrix, m_distortionCoeff); + // _patternTrackingInfo.computePose(_pattern.points3d, warpedCorners, m_camMatrix, m_distortionCoeff); + _patternTrackingInfo.getTrackablePose(_pose); + fill_output(m_H); + WEBARKIT_LOGi("Marker tracked ! Num. matches : %d\n", numMatches); } - WEBARKIT_LOG("Marker detected : %s\n", _isDetected ? "true" : "false"); - swapImagePyramid(); - }; - bool homographyValid(cv::Mat& H) { - if (H.empty()) { - return false; - } - const double det = H.at(0, 0) * H.at(1, 1) - H.at(1, 0) * H.at(0, 1); - return 1 / N < fabs(det) && fabs(det) < N; - }; + swapImagePyramid(); + _frameCount++; + } void fill_output(cv::Mat& H) { - std::vector warped(4); - cv::perspectiveTransform(corners, warped, H); - output[0] = H.at(0, 0); output[1] = H.at(0, 1); output[2] = H.at(0, 2); @@ -288,39 +396,251 @@ class WebARKitTracker::WebARKitTrackerImpl { output[7] = H.at(2, 1); output[8] = H.at(2, 2); - output[9] = warped[0].x; - output[10] = warped[0].y; - output[11] = warped[1].x; - output[12] = warped[1].y; - output[13] = warped[2].x; - output[14] = warped[2].y; - output[15] = warped[3].x; - output[16] = warped[3].y; + output[9] = _bBoxTransformed[0].x; + output[10] = _bBoxTransformed[0].y; + output[11] = _bBoxTransformed[1].x; + output[12] = _bBoxTransformed[1].y; + output[13] = _bBoxTransformed[2].x; + output[14] = _bBoxTransformed[2].y; + output[15] = _bBoxTransformed[3].x; + output[16] = _bBoxTransformed[3].y; }; - void clear_output() { - output = std::vector(17, 0.0); - }; + void clear_output() { std::fill(output.begin(), output.end(), 0); }; void buildImagePyramid(cv::Mat& frame) { cv::buildOpticalFlowPyramid(frame, _pyramid, winSize, maxLevel); } void swapImagePyramid() { _pyramid.swap(_prevPyramid); } - void getMatches(const cv::Mat& frameDescr, std::vector& frameKeyPts, std::vector& refPoints, std::vector& framePoints) { - std::vector> knnMatches; - _matcher->knnMatch(frameDescr, refDescr, knnMatches, 2); + void MatchFeatures(const std::vector& newFrameFeatures, cv::Mat newFrameDescriptors) { + int maxMatches = 0; + int bestMatchIndex = -1; + std::vector finalMatched1, finalMatched2; + // for (int i = 0; i < _trackables.size(); i++) { + if (!_isDetected) { + std::vector> matches = getMatches(newFrameDescriptors); + numMatches = matches.size(); + WEBARKIT_LOGd("Num Matches: %d\n", numMatches); + + if (matches.size() > minRequiredDetectedFeatures) { + std::vector matched1, matched2; + std::vector status; + int totalGoodMatches = 0; + for (unsigned int j = 0; j < matches.size(); j++) { + // Ratio Test for outlier removal, removes ambiguous matches. + if (matches[j][0].distance < _nn_match_ratio * matches[j][1].distance) { + matched1.push_back(newFrameFeatures[matches[j][0].queryIdx]); + matched2.push_back(refKeyPts[matches[j][0].trainIdx]); + status.push_back(1); + totalGoodMatches++; + } else { + status.push_back(0); + } + } + // Measure goodness of match by most number of matching features. + // This allows for maximum of a single marker to match each time. + // TODO: Would a better metric be percentage of marker features matching? + if (totalGoodMatches > maxMatches) { + finalMatched1 = matched1; + finalMatched2 = matched2; + maxMatches = totalGoodMatches; + // bestMatchIndex = i; + } + } + } + // } // end for cycle - framePoints.clear(); + if (maxMatches > 0) { + for (int i = 0; i < finalMatched1.size(); i++) { + finalMatched1[i].pt.x *= _featureDetectScaleFactor[0]; + finalMatched1[i].pt.y *= _featureDetectScaleFactor[1]; + } - // find the best matches - for (size_t i = 0; i < knnMatches.size(); ++i) { - if (knnMatches[i][0].distance < _nn_match_ratio * knnMatches[i][1].distance) { - framePoints.push_back(frameKeyPts[knnMatches[i][0].queryIdx].pt); - refPoints.push_back(refKeyPts[knnMatches[i][0].trainIdx].pt); + homography::WebARKitHomographyInfo homoInfo = + getHomographyInliers(Points(finalMatched2), Points(finalMatched1)); + if (homoInfo.validHomography) { + // std::cout << "New marker detected" << std::endl; + //_trackables[bestMatchIndex]._isDetected = true; + _isDetected = true; + // Since we've just detected the marker, make sure next invocation of + // GetInitialFeatures() for this marker makes a new selection. + //_trackables[bestMatchIndex]._trackSelection.ResetSelection(); + _trackSelection.ResetSelection(); + //_trackables[bestMatchIndex]._trackSelection.SetHomography(homoInfo.homography); + _trackSelection.SetHomography(homoInfo.homography); + + // Use the homography to form the initial estimate of the bounding box. + // This will be refined by the optical flow pass. + // perspectiveTransform(_trackables[bestMatchIndex]._bBox, _trackables[bestMatchIndex]._bBoxTransformed, + // homoInfo.homography); + perspectiveTransform(_bBox, _bBoxTransformed, homoInfo.homography); + if (_trackVizActive) { + for (int i = 0; i < 4; i++) { + // _trackViz.bounds[i][0] = _trackables[bestMatchIndex]._bBoxTransformed[i].x; + // _trackViz.bounds[i][1] = _trackables[bestMatchIndex]._bBoxTransformed[i].y; + _trackViz.bounds[i][0] = _bBoxTransformed[i].x; + _trackViz.bounds[i][1] = _bBoxTransformed[i].y; + } + } + _currentlyTrackedMarkers++; } } } + bool runOpticalFlow(int trackableId, const std::vector& trackablePoints, + const std::vector& trackablePointsWarped) { + std::vector flowResultPoints, trackablePointsWarpedResult; + std::vector statusFirstPass, statusSecondPass; + std::vector err; + cv::calcOpticalFlowPyrLK(_prevPyramid, _pyramid, trackablePointsWarped, flowResultPoints, statusFirstPass, err, + winSize, maxLevel, termcrit, 0, 0.001); + // By using bi-directional optical flow, we improve quality of detected points. + cv::calcOpticalFlowPyrLK(_pyramid, _prevPyramid, flowResultPoints, trackablePointsWarpedResult, + statusSecondPass, err, winSize, maxLevel, termcrit, 0, 0.001); + + // Keep only the points for which flow was found in both temporal directions. + int killed1 = 0; + std::vector filteredTrackablePoints, filteredTrackedPoints; + for (auto j = 0; j != flowResultPoints.size(); ++j) { + if (!statusFirstPass[j] || !statusSecondPass[j]) { + statusFirstPass[j] = (uchar)0; + killed1++; + continue; + } + filteredTrackablePoints.push_back(trackablePoints[j]); + filteredTrackedPoints.push_back(flowResultPoints[j]); + } + + if (_trackVizActive) { + _trackViz.opticalFlowTrackablePoints = filteredTrackablePoints; + _trackViz.opticalFlowTrackedPoints = filteredTrackedPoints; + } + // std::cout << "Optical flow discarded " << killed1 << " of " << flowResultPoints.size() << " points" << + // std::endl; + + if (!updateTrackableHomography(trackableId, filteredTrackablePoints, filteredTrackedPoints)) { + _isDetected = false; + _isTracking = false; + this->_valid = false; + _currentlyTrackedMarkers--; + return false; + } + + _isTracking = true; + return true; + } + + bool updateTrackableHomography(int trackableId, const std::vector& matchedPoints1, + const std::vector& matchedPoints2) { + if (matchedPoints1.size() > 4) { + homography::WebARKitHomographyInfo homoInfo = getHomographyInliers(matchedPoints1, matchedPoints2); + if (homoInfo.validHomography) { + _trackSelection.UpdatePointStatus(homoInfo.status); + _trackSelection.SetHomography(homoInfo.homography); + m_H = homoInfo.homography; + this->_valid = homoInfo.validHomography; + // Update the bounding box. + perspectiveTransform(_bBox, _bBoxTransformed, homoInfo.homography); + if (_trackVizActive) { + for (int i = 0; i < 4; i++) { + // _trackViz.bounds[i][0] = _trackables[trackableId]._bBoxTransformed[i].x; + // _trackViz.bounds[i][1] = _trackables[trackableId]._bBoxTransformed[i].y; + _trackViz.bounds[i][0] = _bBoxTransformed[i].x; + _trackViz.bounds[i][1] = _bBoxTransformed[i].y; + } + } + if (_frameCount > 1) { + // _trackables[trackableId]._trackSelection.ResetSelection(); + _trackSelection.ResetSelection(); + } + return true; + } + } + return false; + } + + std::vector GetVerticesFromPoint(cv::Point ptOrig, int width = markerTemplateWidth, + int height = markerTemplateWidth) { + std::vector vertexPoints; + vertexPoints.push_back(cv::Point2f(ptOrig.x - width / 2, ptOrig.y - height / 2)); + vertexPoints.push_back(cv::Point2f(ptOrig.x + width / 2, ptOrig.y - height / 2)); + vertexPoints.push_back(cv::Point2f(ptOrig.x + width / 2, ptOrig.y + height / 2)); + vertexPoints.push_back(cv::Point2f(ptOrig.x - width / 2, ptOrig.y + height / 2)); + return vertexPoints; + } + + std::vector GetVerticesFromTopCorner(int x, int y, int width, int height) { + std::vector vertexPoints; + vertexPoints.push_back(cv::Point2f(x, y)); + vertexPoints.push_back(cv::Point2f(x + width, y)); + vertexPoints.push_back(cv::Point2f(x + width, y + height)); + vertexPoints.push_back(cv::Point2f(x, y + height)); + return vertexPoints; + } + + cv::Rect GetTemplateRoi(cv::Point2f pt) { + return cv::Rect(pt.x - (markerTemplateWidth / 2), pt.y - (markerTemplateWidth / 2), markerTemplateWidth, + markerTemplateWidth); + } + + cv::Mat MatchTemplateToImage(cv::Mat searchImage, cv::Mat warpedTemplate) { + int result_cols = searchImage.cols - warpedTemplate.cols + 1; + int result_rows = searchImage.rows - warpedTemplate.rows + 1; + if (result_cols > 0 && result_rows > 0) { + cv::Mat result; + result.create(result_rows, result_cols, CV_32FC1); + + double minVal; + double maxVal; + minMaxLoc(warpedTemplate, &minVal, &maxVal, 0, 0, cv::noArray()); + + cv::Mat normSeatchROI; + normalize(searchImage, normSeatchROI, minVal, maxVal, cv::NORM_MINMAX, -1, cv::Mat()); + /// Do the Matching and Normalize + matchTemplate(normSeatchROI, warpedTemplate, result, match_method); + return result; + } else { + // std::cout << "Results image too small" << std::endl; + return cv::Mat(); + } + } + + bool IsRoiValidForFrame(cv::Rect frameRoi, cv::Rect roi) { return (roi & frameRoi) == roi; } + + cv::Rect InflateRoi(cv::Rect roi, int inflationFactor) { + cv::Rect newRoi = roi; + newRoi.x -= inflationFactor; + newRoi.y -= inflationFactor; + newRoi.width += 2 * inflationFactor; + newRoi.height += 2 * inflationFactor; + return newRoi; + } + + std::vector FloorVertexPoints(const std::vector& vertexPoints) { + std::vector testVertexPoints = vertexPoints; + float minX = std::numeric_limits::max(); + float minY = std::numeric_limits::max(); + for (int k = 0; k < testVertexPoints.size(); k++) { + if (testVertexPoints[k].x < minX) { + minX = testVertexPoints[k].x; + } + if (testVertexPoints[k].y < minY) { + minY = testVertexPoints[k].y; + } + } + for (int k = 0; k < testVertexPoints.size(); k++) { + testVertexPoints[k].x -= minX; + testVertexPoints[k].y -= minY; + } + return testVertexPoints; + } + + std::vector> getMatches(const cv::Mat& frameDescr) { + std::vector> knnMatches; + _matcher->knnMatch(frameDescr, refDescr, knnMatches, 2); + return knnMatches; + } + cv::Mat createTrackerFeatureMask(cv::Mat& frame) { cv::Mat featureMask; if (featureMask.empty()) { @@ -343,24 +663,33 @@ class WebARKitTracker::WebARKitTrackerImpl { } std::vector> contours(1); for (int j = 0; j < 4; j++) { - contours[0].push_back(cv::Point(_bBoxTransformed[j].x / featureDetectPyramidLevel, - _bBoxTransformed[j].y / featureDetectPyramidLevel)); - } + // contours[0].push_back(cv::Point(_trackables[i]._bBoxTransformed[j].x/_featureDetectScaleFactor[0],_trackables[i]._bBoxTransformed[j].y/_featureDetectScaleFactor[1])); + contours[0].push_back(cv::Point(_bBoxTransformed[j].x/_featureDetectScaleFactor[0],_bBoxTransformed[j].y/_featureDetectScaleFactor[1])); + } drawContours(featureMask, contours, 0, cv::Scalar(0), -1, 8); } return featureMask; } - std::vector getSelectedFeaturesWarped(cv::Mat& H) { - std::vector warpedPoints; - perspectiveTransform(_pattern.points2d, warpedPoints, H); - return warpedPoints; - } + cv::Mat _image; + + int _currentlyTrackedMarkers; + + int _frameCount; + + int _frameSizeX; + int _frameSizeY; + /// Pyramid level used in downsampling incoming image for feature matching. 0 = no size change, 1 = half width/height, 2 = quarter width/heigh etc. + int _featureDetectPyrLevel; + /// Scale factor applied to images used for feature matching. Will be 2^_featureDetectPyrLevel. + cv::Vec2f _featureDetectScaleFactor; bool _valid; bool _isDetected; + bool _isTracking; + std::vector corners; cv::Mat m_H; @@ -381,12 +710,16 @@ class WebARKitTracker::WebARKitTrackerImpl { WebARKitPatternTrackingInfo _patternTrackingInfo; + TrackingPointSelector _trackSelection; + cv::Matx33d m_camMatrix; cv::Mat m_distortionCoeff; std::array m_cameraProjectionMatrix; private: + int _maxNumberOfMarkersToTrack; + std::vector output; // 9 from homography matrix, 8 from warped corners*/ cv::Ptr _featureDetector; @@ -412,20 +745,26 @@ class WebARKitTracker::WebARKitTrackerImpl { void setDetectorType(webarkit::TRACKER_TYPE trackerType) { _trackerType = trackerType; if (trackerType == webarkit::TRACKER_TYPE::AKAZE_TRACKER) { - this->_featureDetector = cv::AKAZE::create(); - this->_featureDescriptor = cv::AKAZE::create(); + const double akaze_thresh = 3e-4; // AKAZE detection threshold set to locate about 1000 keypoints + cv::Ptr akaze = cv::AKAZE::create(); + akaze->setThreshold(akaze_thresh); + this->_featureDetector = akaze; + this->_featureDescriptor = akaze; } else if (trackerType == webarkit::TRACKER_TYPE::ORB_TRACKER) { this->_featureDetector = cv::ORB::create(DEFAULT_MAX_FEATURES); 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::ORB::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); this->_featureDescriptor = cv::xfeatures2d::TEBLID::create(1.00f); } - _matcher = cv::BFMatcher::create(); + if (trackerType == webarkit::TRACKER_TYPE::AKAZE_TRACKER) { + _matcher = cv::BFMatcher::create(); + } else { + _matcher = cv::BFMatcher::create(cv::NORM_HAMMING); + } }; }; @@ -449,7 +788,8 @@ void WebARKitTracker::initTracker(uchar* refData, size_t refCols, size_t refRows _trackerImpl->initTracker(refData, refCols, refRows, colorSpace); } -void WebARKitTracker::processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace, bool enableBlur) { +void WebARKitTracker::processFrameData(uchar* frameData, size_t frameCols, size_t frameRows, ColorSpace colorSpace, + bool enableBlur) { _trackerImpl->processFrameData(frameData, frameCols, frameRows, colorSpace, enableBlur); } @@ -457,6 +797,8 @@ std::vector WebARKitTracker::getOutputData() { return _trackerImpl->getO cv::Mat WebARKitTracker::getPoseMatrix() { return _trackerImpl->getPoseMatrix(); } +float* WebARKitTracker::getPoseMatrix2() { return _trackerImpl->getPoseMatrix2(); } + cv::Mat WebARKitTracker::getGLViewMatrix() { return _trackerImpl->getGLViewMatrix(); } std::array WebARKitTracker::getCameraProjectionMatrix() { diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.h new file mode 100644 index 0000000..8d6965c --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackedPoint.h @@ -0,0 +1,61 @@ +/* + * TrackedPoint.h + * artoolkitX + * + * This file is part of artoolkitX. + * + * artoolkitX is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * artoolkitX is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with artoolkitX. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2018 Realmax, Inc. + * Copyright 2015 Daqri, LLC. + * Copyright 2010-2015 ARToolworks, Inc. + * + * Author(s): Philip Lamb, Daniel Bell. + * Modified for WebARKit by @kalwalt - Walter Perdan - 2024 + * + */ + +#ifndef TRACKED_POINT_H +#define TRACKED_POINT_H + +#include + +class TrackedPoint +{ +public: + int id; + cv::Point2f pt; + cv::Point3f pt3d; + cv::Rect markerRoi; + bool selected; + bool tracking; + + bool IsTracking(); + void SetTracking(bool newTracking); + bool IsSelected(); + void SetSelected(bool newSelected); +}; + +#endif //TRACKED_POINT diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackerVisualization.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackerVisualization.h new file mode 100644 index 0000000..84d8800 --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackerVisualization.h @@ -0,0 +1,64 @@ +/* + * TrackerVisualization.h + * artoolkitX + * + * This file is part of artoolkitX. + * + * artoolkitX is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * artoolkitX is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with artoolkitX. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2024 Eden Networks Ltd. + * + * Author(s): Philip Lamb. + * + */ + +#ifndef TRACKER_VISUALIZATION_H +#define TRACKER_VISUALIZATION_H + +#include + +class TrackerVisualization +{ +public: + int id; + float bounds[4][2]; + std::vector opticalFlowTrackablePoints; + std::vector opticalFlowTrackedPoints; + bool opticalFlowOK; + struct templateMatching { + int templateMatchingCandidateCount; + int failedBoundsTestCount; + int failedROIInFrameTestCount; + int failedGotHomogTestCount; + int failedSearchROIInFrameTestCount; + int failedTemplateBigEnoughTestCount; + int failedTemplateMatchCount; + int failedTemplateMinimumCorrelationCount; + bool templateMatchingOK; + }; + templateMatching templateMatching; +}; + +#endif // TRACKER_VISUALIZATION_H diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.h new file mode 100644 index 0000000..c6f0094 --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/TrackingPointSelector.h @@ -0,0 +1,94 @@ +/* + * TrackingPointSelector.h + * artoolkitX + * + * This file is part of artoolkitX. + * + * artoolkitX is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * artoolkitX is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with artoolkitX. If not, see . + * + * As a special exception, the copyright holders of this library give you + * permission to link this library with independent modules to produce an + * executable, regardless of the license terms of these independent modules, and to + * copy and distribute the resulting executable under terms of your choice, + * provided that you also meet, for each linked independent module, the terms and + * conditions of the license of that module. An independent module is a module + * which is neither derived from nor based on this library. If you modify this + * library, you may extend this exception to your version of the library, but you + * are not obligated to do so. If you do not wish to do so, delete this exception + * statement from your version. + * + * Copyright 2018 Realmax, Inc. + * Copyright 2015 Daqri, LLC. + * Copyright 2010-2015 ARToolworks, Inc. + * + * Author(s): Philip Lamb, Daniel Bell. + * Modified for WebARKit by @kalwalt - Walter Perdan - 2024 + * + */ + +#ifndef TRACKINGPOINTSELECTOR_H +#define TRACKINGPOINTSELECTOR_H +#include +#include +#include +#include + +/** + @brief Class used to manage selection of tracking points based on image templates (i.e. unique pixel patches). + */ +class TrackingPointSelector +{ +public: + TrackingPointSelector(); + + TrackingPointSelector(std::vector pts, int width, int height, int markerTemplateWidth); + + void DistributeBins(int width, int height, int markerTemplateWidth); + + void SetHomography(cv::Mat newHomography); + + cv::Mat GetHomography(); + + void UpdatePointStatus(std::vector status); + + /** + @brief Signal that the next call to GetInitialFeatures should return a new selection. + */ + void ResetSelection(); + + /** + @brief If reset, then selects an initial random template from each bin for tracking, + and returns this set. If not reset then returns the same set as GetTrackedFeatures. + */ + std::vector GetInitialFeatures(); + + std::vector GetTrackedFeatures(); + + std::vector GetTrackedFeatures3d(); + + std::vector GetTrackedFeaturesWarped(); + + /// Get all points from all bins that are candidates for selection. + std::vector GetAllFeatures(); + + void CleanUp(); + +private: + bool _reset; + std::vector _pts; + std::map > trackingPointBin; + cv::Mat _homography; + std::vector _selectedPts; +}; +#endif //TRACKINGPOINTSELECTOR diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.h index efdc36a..d038a64 100644 --- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.h +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitConfig.h @@ -14,12 +14,19 @@ extern const int DEFAULT_MAX_FEATURES; extern const int TEBLID_MAX_FEATURES; extern const int N; extern const int MIN_NUM_MATCHES; +extern const int minRequiredDetectedFeatures; ///< Minimum number of detected features required to consider a target matched. +extern const int markerTemplateWidth; ///< Width in pixels of image patches used in template matching. extern const int maxLevel; ///< Maximum number of levels in optical flow image pyramid. extern const cv::Size winSize; extern const cv::TermCriteria termcrit; +extern const int searchRadius; +extern const int match_method; +extern const cv::Size featureImageMinSize; ///< Minimum size when downscaling incoming images used for feature tracking. extern const double featureDetectPyramidLevel; ///> Scale factor applied to image pyramid to determine image to perform feature matching upon. extern const int featureBorder; extern const cv::Size blurSize; +extern const double ransac_thresh; +extern cv::RNG rng; extern const double m_pi; extern const std::string WEBARKIT_HEADER_VERSION_STRING; extern const int WEBARKIT_HEADER_VERSION_MAJOR; diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.h new file mode 100644 index 0000000..7cfe6d4 --- /dev/null +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitHomographyInfo.h @@ -0,0 +1,24 @@ +#ifndef WEBARKIT_HOMOGRAPHY_INFO_H +#define WEBARKIT_HOMOGRAPHY_INFO_H +#include "WebARKitConfig.h" + +namespace webarkit { +namespace homography { + +class WebARKitHomographyInfo { + public: + WebARKitHomographyInfo(); + + WebARKitHomographyInfo(cv::Mat hom, std::vector newStatus, std::vector matches); + + bool validHomography; + cv::Mat homography; + std::vector status; + std::vector inlier_matches; +}; + +} // namespace homography + +} // namespace webarkit + +#endif \ No newline at end of file diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h index 064a35c..8d2bf61 100644 --- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitTracker.h @@ -32,6 +32,8 @@ class WebARKitTracker { std::vector getOutputData(); cv::Mat getPoseMatrix(); + + float* getPoseMatrix2(); cv::Mat getGLViewMatrix(); diff --git a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h index 5295852..4debaeb 100644 --- a/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h +++ b/WebARKit/WebARKitTrackers/WebARKitOpticalTracking/include/WebARKitTrackers/WebARKitOpticalTracking/WebARKitUtils.h @@ -1,12 +1,78 @@ #ifndef WEBARKIT_UTILS_H #define WEBARKIT_UTILS_H -#include +// #include #include +#include #include namespace webarkit { +static std::vector Points(std::vector keypoints) { + std::vector res; + for (unsigned i = 0; i < keypoints.size(); i++) { + res.push_back(keypoints[i].pt); + } + return res; +} + +/// Method for calculating and validating a homography matrix from a set of corresponding points. +/// pts1 and pts must have the same dimensionality. +/// @returns An WebARKitHomographyInfo instance, with its status vector of the same dimensionality as the pts1 and pts2 +/// vectors. +static homography::WebARKitHomographyInfo getHomographyInliers(std::vector pts1, + std::vector pts2) { + if (pts1.size() < 4) { + return homography::WebARKitHomographyInfo(); + } + + cv::Mat inlier_mask, homography; + homography = findHomography(pts1, pts2, cv::RANSAC, ransac_thresh, inlier_mask); + if (homography.empty()) { + // Failed to find a homography. + return homography::WebARKitHomographyInfo(); + } + + const double det = homography.at(0, 0) * homography.at(1, 1) - + homography.at(1, 0) * homography.at(0, 1); + if (det < 0) { + return homography::WebARKitHomographyInfo(); + } + + const double N1 = sqrt(homography.at(0, 0) * homography.at(0, 0) + + homography.at(1, 0) * homography.at(1, 0)); + if (N1 > 4 || N1 < 0.1) { + return homography::WebARKitHomographyInfo(); + } + + const double N2 = sqrt(homography.at(0, 1) * homography.at(0, 1) + + homography.at(1, 1) * homography.at(1, 1)); + if (N2 > 4 || N2 < 0.1) { + return homography::WebARKitHomographyInfo(); + } + + const double N3 = sqrt(homography.at(2, 0) * homography.at(2, 0) + + homography.at(2, 1) * homography.at(2, 1)); + if (N3 > 0.002) { + return homography::WebARKitHomographyInfo(); + } + + std::vector status; + std::vector inlier_matches; + int linliers = 0; + for (int i = 0; i < pts1.size(); i++) { + if ((int)inlier_mask.at(i, 0) == 1) { + status.push_back((uchar)1); + inlier_matches.push_back(cv::DMatch(i, i, 0)); + linliers++; + } else { + status.push_back((uchar)0); + } + } + // Return homography and corresponding inlier point sets + return homography::WebARKitHomographyInfo(homography, status, inlier_matches); +} + /*static auto im_gray(uchar* data, size_t cols, size_t rows) { uint32_t idx; uchar gray[rows][cols]; diff --git a/WebARKit/include/WebARKitManager.h b/WebARKit/include/WebARKitManager.h index 081f533..d1bdffd 100644 --- a/WebARKit/include/WebARKitManager.h +++ b/WebARKit/include/WebARKitManager.h @@ -104,6 +104,8 @@ class WebARKitManager { cv::Mat getPoseMatrix(); + float* getPoseMatrix2(); + cv::Mat getGLViewMatrix(); std::array getTransformationMatrix(); diff --git a/WebARKit/include/WebARKitPattern.h b/WebARKit/include/WebARKitPattern.h index 9b534ad..0d63ada 100644 --- a/WebARKit/include/WebARKitPattern.h +++ b/WebARKit/include/WebARKitPattern.h @@ -25,18 +25,23 @@ class WebARKitPatternTrackingInfo { cv::Mat homography; std::vector points2d; cv::Mat pose3d; + float transMat [3][4]; cv::Mat glViewMatrix; void setScale(const float scale) { m_scale = scale; } float getScale() { return m_scale; } + void cameraPoseFromPoints(cv::Mat& pose, const std::vector& objPts, const std::vector& imgPts, const cv::Matx33f& caMatrix, const cv::Mat& distCoeffs); + /** * Compute pattern pose using PnP algorithm */ void computePose(std::vector& treeDPoints, std::vector& imgPoints, const cv::Matx33f& caMatrix, const cv::Mat& distCoeffs); + void getTrackablePose(cv::Mat& pose); + void computeGLviewMatrix(); private: diff --git a/tests/webarkit_test.cc b/tests/webarkit_test.cc index 03e32cc..4b0bcda 100644 --- a/tests/webarkit_test.cc +++ b/tests/webarkit_test.cc @@ -31,8 +31,8 @@ INSTANTIATE_TEST_SUITE_P(WebARKitEnumTestSuite, WebARKitEnumTest, TEST(WebARKitConfigTest, TestConfigValues) { EXPECT_EQ(DEFAULT_NN_MATCH_RATIO, 0.7f); EXPECT_EQ(TEBLID_NN_MATCH_RATIO, 0.8f); - EXPECT_EQ(DEFAULT_MAX_FEATURES, 8000); - EXPECT_EQ(TEBLID_MAX_FEATURES, 10000); + EXPECT_EQ(DEFAULT_MAX_FEATURES, 800); + EXPECT_EQ(TEBLID_MAX_FEATURES, 1000); EXPECT_EQ(N, 10); EXPECT_EQ(MIN_NUM_MATCHES, 8); EXPECT_EQ(maxLevel, 3);