CVTrackedFeatures Class Reference

CVTrackedFeatures is the main part of the AR Christoffelturm scene. More...

#include <CVTrackedFeatures.h>

Inheritance diagram for CVTrackedFeatures:

Classes
struct	SLFeatureMarker2D
	Data of a 2D marker image. More...
struct	SLFrameData
	Feature date for a video frame. More...

Public Member Functions
	CVTrackedFeatures (string markerFilename)
	~CVTrackedFeatures ()
	Show statistics if program terminates. More...
bool	track (CVMat imageGray, CVMat image, CVCalibration *calib) final
bool	forceRelocation ()
CVDetectDescribeType	type ()
void	forceRelocation (bool fR)
void	type (CVDetectDescribeType ddType)
	Setter of the feature detector & descriptor type. More...
Public Member Functions inherited from CVTracked
	CVTracked ()
virtual	~CVTracked ()=default
void	drawDetection (bool draw)
bool	isVisible ()
bool	drawDetection ()
CVMatx44f	objectViewMat ()

Private Member Functions
void	loadMarker (string markerFilename)
	Loads the marker image form the filesystem. More...
void	initFeaturesOnMarker ()
void	relocate ()
void	tracking ()
void	drawDebugInformation (bool drawDetection)
void	transferFrameData ()
void	detectKeypointsAndDescriptors ()
CVVDMatch	getFeatureMatches ()
bool	calculatePose ()
void	optimizeMatches ()
bool	trackWithOptFlow (CVMat rvec, CVMat tvec)

Private Attributes
cv::Ptr< cv::DescriptorMatcher >	_matcher
	Descriptor matching algorithm. More...
CVCalibration *	_calib
	Current calibration in use. More...
int	_frameCount
	NO. of frames since process start. More...
bool	_isTracking
	True if tracking. More...
SLFeatureMarker2D	_marker
	2D marker data More...
SLFrameData	_currentFrame
	The current video frame data. More...
SLFrameData	_prevFrame
	The previous video frame data. More...
bool	_forceRelocation
	Force relocation every frame (no opt. flow tracking) More...
CVFeatureManager	_featureManager
	Feature detector-descriptor wrapper instance. More...

Additional Inherited Members
Static Public Member Functions inherited from CVTracked
static cv::Matx44f	createGLMatrix (const CVMat &tVec, const CVMat &rVec)
	Create an OpenGL 4x4 matrix from an OpenCV translation & rotation vector. More...
static void	createRvecTvec (const CVMatx44f &glMat, CVMat &tVec, CVMat &rVec)
	Creates the OpenCV rvec & tvec vectors from an column major OpenGL 4x4 matrix. More...
static CVMatx44f	calcObjectMatrix (const CVMatx44f &cameraObjectMat, const CVMatx44f &objectViewMat)
static CVVec3f	averageVector (vector< CVVec3f > vectors, vector< float > weights)
static SLQuat4f	averageQuaternion (vector< SLQuat4f > quaternions, vector< float > weights)
static void	resetTimes ()
	Resets all static variables. More...
Static Public Attributes inherited from CVTracked
static AvgFloat	trackingTimesMS
	Averaged time for video tracking in ms. More...
static AvgFloat	detectTimesMS
	Averaged time for video feature detection & description in ms. More...
static AvgFloat	detect1TimesMS
	Averaged time for video feature detection subpart 1 in ms. More...
static AvgFloat	detect2TimesMS
	Averaged time for video feature detection subpart 2 in ms. More...
static AvgFloat	matchTimesMS
	Averaged time for video feature matching in ms. More...
static AvgFloat	optFlowTimesMS
	Averaged time for video feature optical flow tracking in ms. More...
static AvgFloat	poseTimesMS
	Averaged time for video feature pose estimation in ms. More...
Protected Attributes inherited from CVTracked
bool	_isVisible
	Flag if marker is visible. More...
bool	_drawDetection
	Flag if detection should be drawn into image. More...
CVMatx44f	_objectViewMat
	view transformation matrix More...
HighResTimer	_timer
	High resolution timer. More...

Detailed Description

CVTrackedFeatures is the main part of the AR Christoffelturm scene.

The implementation tries to find a valid pose based on feature points in realtime. The feature matching algorithm checks the points of the current camera frame with against a reference. There are two important parts of this procedure: The relocalisation, which will be called if we have to find the pose with no hint where the camera could be. The other one is called feature tracking: If a pose was found, the implementation tries to track them and update the pose respectively.

Definition at line 66 of file CVTrackedFeatures.h.

Constructor & Destructor Documentation

CVTrackedFeatures()

CVTrackedFeatures::CVTrackedFeatures ( string  markerFilename )

explicit

Definition at line 42 of file CVTrackedFeatures.cpp.

{
    // To match the binary features, we are matching each descriptor in reference with each
    // descriptor in the current frame. The smaller the hamming distance the better the match
    // Hamming distance <-> XOR sum
    _matcher = cv::BFMatcher::create(cv::BFMatcher::BRUTEFORCE_HAMMING, false);
 
    // Initialize some member variables on startup to prevent uncontrolled behaviour
    _currentFrame.foundPose         = false;
    _prevFrame.foundPose            = false;
    _currentFrame.reprojectionError = 0.0f;
    _prevFrame.inlierPoints2D       = CVVPoint2f(nFeatures);
    _forceRelocation                = false;
    _frameCount                     = 0;
 
    loadMarker(std::move(markerFilename));
 
// Create directory for debug output if flag is set
#ifdef DEBUG_OUTPUT_PATH
#    if defined(SL_OS_LINUX) || defined(SL_OS_MACOS) || defined(SL_OS_MACIOS)
    mkdir(DEBUG_OUTPUT_PATH, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
#    elif defined(SL_OS_WINDOWS)
    _mkdir(DEBUG_OUTPUT_PATH);
#    else
#        undef SAVE_SNAPSHOTS_OUTPUT
#    endif
#endif
}

~CVTrackedFeatures()

CVTrackedFeatures::~CVTrackedFeatures

(

)

Show statistics if program terminates.

Definition at line 72 of file CVTrackedFeatures.cpp.

{
#if DO_FEATURE_BENCHMARKING
    Utils::log("");
    Utils::log("");
    Utils::log("------------------------------------------------------------------");
    Utils::log("CVTrackedFeatures statistics");
    Utils::log("------------------------------------------------------------------");
    Utils::log("Avg calculation time per frame                   : %f ms", _trackingTimesMS().average());
    Utils::log("");
    Utils::log("Settings for Pose estimation: ------------------------------------");
    Utils::log("Features                                         : %d", nFeatures);
    Utils::log("Minimal ratio for 2 best matches                 : %f", minRatio);
    Utils::log("RANSAC iterations                                : %d", iterations);
    Utils::log("RANSAC mean reprojection error                   : %f", reprojection_error);
    Utils::log("RANSAC confidence                                : %d", confidence);
    Utils::log("Repose frequency                                 : Each %d point", reposeFrequency);
    Utils::log("Initial patch size for Pose optimization         : %d pixels", initialPatchSize);
    Utils::log("Maximal patch size for Pose optimization         : %d pixels", maxPatchSize);
    Utils::log("");
    Utils::log("Pose information: ------------------------------------------------");
    Utils::log("Avg allmatches to inliers proposition            : %f", sum_allmatches_to_inliers / _frameCount);
    Utils::log("Avg reprojection error (only if POSE)            : %f", sum_reprojection_error / frames_with_pose);
    Utils::log("Pose found                                       : %d of %d frames", frames_with_pose, _frameCount);
    Utils::log("Avg matches                                      : %f", sum_matches / frames_with_pose);
    Utils::log("Avg inlier matches                               : %f", sum_inlier_matches / frames_with_pose);
    Utils::log("Avg more matches with Pose optimization          : %f", sum_poseopt_difference / frames_with_pose);
 
// Only used for testing with slight movements
// Utils::log("Avg Rotation error                               : %f deg", rotationError / frames_with_pose);
// Utils::log("Avg Translation error                            : %f px", translationError / frames_with_pose);
#endif // DO_FEATURE_BENCHMARKING
}

Member Function Documentation

calculatePose()

bool CVTrackedFeatures::calculatePose ( )

private

This method does the most important work of the whole pipeline:

RANSAC: We execute first RANSAC to eliminate wrong feature correspondences (outliers) and only use the correct ones (inliers) for PnP solving (https://en.wikipedia.org/wiki/Perspective-n-Point).

Methods of solvePnP:

• P3P: If we have 3 Points given, we have the minimal form of the PnP problem. We can treat the points as a triangle definition ABC. We have 3 corner points and 3 angles. Because we get many soulutions for the equation, there will be a fourth point which removes the ambiguity. Therefore the OpenCV implementation requires 4 points to use this method.
• EPNP: This method is used if there are n >= 4 points. The reference points are expressed as 4 virtual control points. The coordinates of these points are the unknowns for the equtation.
• ITERATIVE: Calculates pose using the DLT (Direct Linear Transform) method. If there is a homography will be much easier and no DLT will be used. Otherwise we are using the DLT and make a Levenberg-Marquardt optimization. The latter helps to decrease the reprojection error which is the sum of the squared distances between the image and object points.
  
  Overall Steps:

1. Call RANSAC with EPNP: The RANdom Sample Consensus algorithm is called to remove "wrong" point correspondences which makes the solvePnP more robust. The so called inliers are used for calculation, wrong correspondences (outliers) will be ignored. Therefore the method below will first run a solvePnP with the EPNP method and returns the reprojection error. EPNP works like the following:
   • Choose the 4 control pints: C0 as centroid of reference points,
     C1, C2 and C3 from PCA of the reference points
   • Compute barycentric coordinates with the control points
   • Derivate the image reference points with the above
2. Optimize inlier matches
3. Call PnP ITERATIVE: General problem: We have a calibrated cam and sets of corresponding 2D/3D points. We will calculate the rotation and translation in respect to world coordinates.
   • If for no extrinsic guess, begin with computation
   • If planarity is detected, find homography, otherwise use DLT method
   • After sucessful determination of a pose, optimize it with
     Levenberg-Marquardt (iterative part)

Returns

True if the pose was found.

Definition at line 568 of file CVTrackedFeatures.cpp.

{
    // solvePnP crashes if less than 5 points are given
    if (_currentFrame.matches.size() < 10) return false;
 
    float startMS = _timer.elapsedTimeInMilliSec();
 
    // Find 2D/3D correspondences
    // At the moment we are using only the two correspondences like this:
    // KeypointsOriginal <-> KeypointsActualscene
    // Train index --> "CVPoint" in the model
    // Query index --> "CVPoint" in the actual frame
 
    if (_currentFrame.matches.size() < 10)
        return false;
 
    CVVPoint3f modelPoints(_currentFrame.matches.size());
    CVVPoint2f framePoints(_currentFrame.matches.size());
 
    for (size_t i = 0; i < _currentFrame.matches.size(); i++)
    {
        modelPoints[i] = _marker.keypoints3D[(uint)_currentFrame.matches[i].trainIdx];
        framePoints[i] = _currentFrame.keypoints[(uint)_currentFrame.matches[i].queryIdx].pt;
    }
 
    vector<uchar> inliersMask(modelPoints.size());
 
    //////////////////////
    // 1. RANSAC with EPnP
    //////////////////////
 
    bool foundPose = cv::solvePnPRansac(modelPoints,
                                        framePoints,
                                        _calib->cameraMat(),
                                        _calib->distortion(),
                                        _currentFrame.rvec,
                                        _currentFrame.tvec,
                                        _currentFrame.useExtrinsicGuess,
                                        iterations,
                                        reprojection_error,
                                        confidence,
                                        inliersMask,
                                        cv::SOLVEPNP_EPNP);
 
    // Get matches with help of inlier indices
    for (size_t idx : inliersMask)
    {
        _currentFrame.inlierMatches.push_back(_currentFrame.matches[idx]);
        _currentFrame.inlierPoints2D.push_back(framePoints[idx]);
        _currentFrame.inlierPoints3D.push_back(modelPoints[idx]);
    }
 
    // Pose optimization
    if (foundPose)
    {
        // float matchesBefore = (float)_currentFrame.inlierMatches.size();
 
        /////////////////////
        // 2. Optimze Matches
        /////////////////////
 
        optimizeMatches();
 
        ///////////////////////
        // 3. solvePnP Iterativ
        ///////////////////////
 
        foundPose = cv::solvePnP(_currentFrame.inlierPoints3D,
                                 _currentFrame.inlierPoints2D,
                                 _calib->cameraMat(),
                                 _calib->distortion(),
                                 _currentFrame.rvec,
                                 _currentFrame.tvec,
                                 true,
                                 cv::SOLVEPNP_ITERATIVE);
 
#if DO_FEATURE_BENCHMARKING
        sum_matches += _currentFrame.matches.size();
        sum_inlier_matches += _currentFrame.inlierMatches.size();
        sum_allmatches_to_inliers += _currentFrame.inlierMatches.size() /
                                     _currentFrame.matches.size();
        sum_poseopt_difference += _currentFrame.inlierMatches.size() /
                                  matchesBefore;
#endif
    }
 
    CVTracked::poseTimesMS.set(_timer.elapsedTimeInMilliSec() - startMS);
 
    return foundPose;
}

detectKeypointsAndDescriptors()

void CVTrackedFeatures::detectKeypointsAndDescriptors ( )

private

Get keypoints and descriptors in one step. This is a more efficient way since we have to build the scaling pyramide only once. If we detect and describe seperatly, it will lead in two scaling pyramids and is therefore less meaningful.

Definition at line 485 of file CVTrackedFeatures.cpp.

{
    float startMS = _timer.elapsedTimeInMilliSec();
 
    _featureManager.detectAndDescribe(_currentFrame.imageGray,
                                      _currentFrame.keypoints,
                                      _currentFrame.descriptors);
 
    CVTracked::detectTimesMS.set(_timer.elapsedTimeInMilliSec() - startMS);
}

drawDebugInformation()

void CVTrackedFeatures::drawDebugInformation ( bool  drawDetection )

private

Visualizes the following parts of the whole Pose estimation:

• Keypoints
• Inlier matches
• Optical Flow (Small arrows that show how keypoints moved between frames)
• Reprojection with the calculated Pose

Definition at line 305 of file CVTrackedFeatures.cpp.

{
    if (drawDetection)
    {
        for (auto& inlierPoint : _currentFrame.inlierPoints2D)
            circle(_currentFrame.image,
                   inlierPoint,
                   3,
                   cv::Scalar(0, 0, 255));
    }
 
#if DRAW_REPROJECTION_POINTS
    CVMat imgReprojection = _currentFrame.image;
#elif defined(SAVE_SNAPSHOTS_OUTPUT)
    CVMat imgReprojection;
    _currentFrame.image.copyTo(imgReprojection);
#endif
 
#if DRAW_REPROJECTION_POINTS || defined(DEBUG_OUTPUT_PATH)
    if (!_currentFrame.inlierMatches.empty())
    {
        CVVPoint2f projectedPoints(_marker.keypoints3D.size());
 
        cv::projectPoints(_marker.keypoints3D,
                          _currentFrame.rvec,
                          _currentFrame.tvec,
                          _calib->cameraMat(),
                          _calib->distortion(),
                          projectedPoints);
 
        for (size_t i = 0; i < _marker.keypoints3D.size(); i++)
        {
            if (i % reposeFrequency) continue;
 
            CVPoint2f projectedModelPoint = projectedPoints[i];
            CVPoint2f keypointForPose     = _currentFrame.keypoints[_currentFrame.inlierMatches.back().queryIdx].pt;
 
            // draw all projected map features and the original keypoint on video stream
            circle(imgReprojection,
                   projectedModelPoint,
                   2,
                   CV_RGB(255, 0, 0),
                   1,
                   FILLED);
 
            circle(imgReprojection,
                   keypointForPose,
                   5,
                   CV_RGB(0, 0, 255),
                   1,
                   FILLED);
 
            // draw the point index and reprojection error
            putText(imgReprojection,
                    to_string(i),
                    CVPoint2f(projectedModelPoint.x - 2, projectedModelPoint.y - 5),
                    FONT_HERSHEY_SIMPLEX,
                    0.3,
                    CV_RGB(255, 0, 0),
                    1.0);
        }
    }
#endif
 
#if defined(DEBUG_OUTPUT_PATH)
    // Draw reprojection
    CVMat imgOut;
    drawMatches(imgReprojection,
                CVVKeyPoint(),
                _marker.imageDrawing,
                CVVKeyPoint(),
                CVVDMatch(),
                imgOut,
                CV_RGB(255, 0, 0),
                CV_RGB(255, 0, 0));
 
    imwrite(DEBUG_OUTPUT_PATH + to_string(_frameCount) + "_reprojection.png",
            imgOut);
 
    // Draw keypoints
    if (!_currentFrame.keypoints.empty())
    {
        CVMat imgKeypoints;
        drawKeypoints(_currentFrame.imageGray,
                      _currentFrame.keypoints,
                      imgKeypoints);
 
        imwrite(DEBUG_OUTPUT_PATH + to_string(_frameCount) + "_keypoints.png",
                imgKeypoints);
    }
 
    for (size_t i = 0; i < _currentFrame.inlierPoints2D.size(); i++)
        circle(_currentFrame.image,
               _currentFrame.inlierPoints2D[i],
               2,
               Scalar(0, 255, 0));
 
    // Draw matches
    if (!_currentFrame.inlierMatches.empty())
    {
        CVMat imgMatches;
        drawMatches(_currentFrame.imageGray,
                    _currentFrame.keypoints,
                    _marker.imageGray,
                    _marker.keypoints2D,
                    _currentFrame.inlierMatches,
                    imgMatches,
                    CV_RGB(255, 0, 0),
                    CV_RGB(255, 0, 0));
 
        imwrite(DEBUG_OUTPUT_PATH + to_string(_frameCount) + "_matching.png",
                imgMatches);
    }
 
    // Draw optical flow
    if (_isTracking)
    {
        CVMat optFlow, rgb;
        _currentFrame.imageGray.copyTo(optFlow);
        cvtColor(optFlow, rgb, CV_GRAY2BGR);
        for (size_t i = 0; i < _currentFrame.inlierPoints2D.size(); i++)
            cv::arrowedLine(rgb,
                            _prevFrame.inlierPoints2D[i],
                            _currentFrame.inlierPoints2D[i],
                            Scalar(0, 255, 0),
                            1,
                            LINE_8,
                            0,
                            0.2);
 
        imwrite(DEBUG_OUTPUT_PATH + to_string(_frameCount) + "-optflow.png", rgb);
    }
#endif
}

forceRelocation() [1/2]

bool CVTrackedFeatures::forceRelocation ( )

inline

Definition at line 75 of file CVTrackedFeatures.h.

{ return _forceRelocation; }

forceRelocation() [2/2]

void CVTrackedFeatures::forceRelocation ( bool  fR )

inline

Definition at line 79 of file CVTrackedFeatures.h.

{ _forceRelocation = fR; }

getFeatureMatches()

CVVDMatch CVTrackedFeatures::getFeatureMatches ( )

private

Get matching features with the defined feature matcher. Since we are using the k-next-neighbour matcher, we check if the best and second best match are not too identical with the so called ratio test.

Returns

Vector of found matches

Definition at line 501 of file CVTrackedFeatures.cpp.

{
    float startMS = _timer.elapsedTimeInMilliSec();
 
    int        k = 2;
    CVVVDMatch matches;
    _matcher->knnMatch(_currentFrame.descriptors, _marker.descriptors, matches, k);
 
    // Perform ratio test which determines if k matches from the knn matcher
    // are not too similar. If the ratio of the the distance of the two
    // matches is toward 1, the matches are near identically.
    CVVDMatch goodMatches;
    for (auto& match : matches)
    {
        const cv::DMatch& match1 = match[0];
        const cv::DMatch& match2 = match[1];
        if (match2.distance == 0.0f ||
            (match1.distance / match2.distance) < minRatio)
            goodMatches.push_back(match1);
    }
 
    CVTracked::matchTimesMS.set(_timer.elapsedTimeInMilliSec() - startMS);
    return goodMatches;
}

initFeaturesOnMarker()

void CVTrackedFeatures::initFeaturesOnMarker ( )

private

Prepares the reference tracker:

1. Detect and describe the keypoints on the reference image
2. Set up 3D points with predefined scaling
3. Perform optional drawing operations on image

Definition at line 134 of file CVTrackedFeatures.cpp.

{
    assert(!_marker.imageGray.empty() && "Grayscale image is empty!");
 
    // Clear previous initializations
    _marker.keypoints2D.clear();
    _marker.keypoints3D.clear();
    _marker.descriptors.release();
 
    // Detect and compute features in marker image
    _featureManager.detectAndDescribe(_marker.imageGray,
                                      _marker.keypoints2D,
                                      _marker.descriptors);
    // Scaling factor for the 3D point.
    // Width of image is A4 size in image, 297mm is the real A4 height
    float pixelPerMM = (float)_marker.imageGray.cols / 297.0f;
 
    // Calculate 3D-Points based on the detected features
    for (auto& keypoint : _marker.keypoints2D)
    {
        // 2D location in image
        CVPoint2f refImageKeypoint = keypoint.pt;
 
        // CVPoint scaling
        refImageKeypoint /= pixelPerMM;
 
        // Here we can use Z=0 because the tracker is planar
        _marker.keypoints3D.push_back(cv::Point3f(refImageKeypoint.x,
                                                  refImageKeypoint.y,
                                                  0.0f));
    }
 
// Draw points and indices which should be reprojected later.
// Only a few (defined with reposeFrequency)
// points are used for the reprojection.
#if defined(DEBUG_OUTPUT_PATH) || DRAW_REPROJECTION_POINTS
    _marker.imageGray.copyTo(_marker.imageDrawing);
    cvtColor(_marker.imageDrawing, _marker.imageDrawing, cv::COLOR_GRAY2BGR);
 
    for (size_t i = 0; i < _marker.keypoints3D.size(); i++)
    {
        if (i % reposeFrequency)
            continue;
 
        CVPoint2f originalModelPoint = _marker.keypoints2D[i].pt;
 
        circle(_marker.imageDrawing,
               originalModelPoint,
               1,
               CV_RGB(255, 0, 0),
               1,
               FILLED);
 
        putText(_marker.imageDrawing,
                to_string(i),
                CVPoint2f(originalModelPoint.x - 1,
                          originalModelPoint.y - 1),
                FONT_HERSHEY_SIMPLEX,
                0.25,
                CV_RGB(255, 0, 0),
                1);
    }
#endif
}

loadMarker()

void CVTrackedFeatures::loadMarker ( string  markerFilename )

private

Loads the marker image form the filesystem.

Definition at line 107 of file CVTrackedFeatures.cpp.

{
    // Load the file directly
    if (!SLFileStorage::exists(markerFilename, IOK_image))
    {
        string msg = "CVTrackedFeatures::loadMarker: File not found: " +
                     markerFilename;
        Utils::exitMsg("SLProject",
                       msg.c_str(),
                       __LINE__,
                       __FILE__);
    }
 
    CVImage img(markerFilename);
 
#ifndef SL_EMSCRIPTEN
    cvtColor(img.cvMat(), _marker.imageGray, cv::COLOR_RGB2GRAY);
#else
    cvtColor(img.cvMat(), _marker.imageGray, cv::COLOR_RGBA2GRAY);
#endif
}

optimizeMatches()

void CVTrackedFeatures::optimizeMatches ( )

private

To get more matches with the calculated pose, we reproject the reference points to our current frame. Within a predefined patch, we try to rematch not matched features with the reprojected point. If not possible, we increase the patch size until we found a match for the point or we reach a threshold.

Definition at line 664 of file CVTrackedFeatures.cpp.

{
#if DO_FEATURE_BENCHMARKING
    float reprojectionError = 0;
#endif
 
    // 1. Reproject the model points with the calculated POSE
    CVVPoint2f projectedPoints(_marker.keypoints3D.size());
    cv::projectPoints(_marker.keypoints3D,
                      _currentFrame.rvec,
                      _currentFrame.tvec,
                      _calib->cameraMat(),
                      _calib->distortion(),
                      projectedPoints);
 
    CVVKeyPoint    bboxFrameKeypoints;
    vector<size_t> frameIndicesInsideRect;
 
    for (size_t i = 0; i < _marker.keypoints3D.size(); i++)
    {
        // only every reposeFrequency
        if (i % reposeFrequency)
            continue;
 
        // Check if this point has a match inside matches, continue if so
        int alreadyMatched = 0;
        // todo: this is bad, because for every marker keypoint we have to iterate all inlierMatches!
        // better: iterate inlierMatches once at the beginning and mark all marker keypoints as inliers or not!
        for (size_t j = 0; j < _currentFrame.inlierMatches.size(); j++)
        {
            if (_currentFrame.inlierMatches[(uint)j].trainIdx == (int)i)
                alreadyMatched++;
        }
 
        if (alreadyMatched > 0) continue;
 
        // Get the corresponding projected point of the actual (i) modelpoint
        CVPoint2f projectedModelPoint = projectedPoints[i];
        CVVDMatch newMatches;
 
        int patchSize = initialPatchSize;
 
        // Adaptive patch size
        while (newMatches.empty() && patchSize <= maxPatchSize)
        {
            // Increase matches by even number
            patchSize += 2;
            newMatches.clear();
            bboxFrameKeypoints.clear();
            frameIndicesInsideRect.clear();
 
            // 2. Select only before calculated Keypoints within patch
            // with projected "positioning" keypoint as center
            // OpenCV: Top-left origin
            int xTopLeft   = (int)(projectedModelPoint.x - (float)patchSize / 2.0f);
            int yTopLeft   = (int)(projectedModelPoint.y - (float)patchSize / 2.0f);
            int xDownRight = xTopLeft + patchSize;
            int yDownRight = yTopLeft + patchSize;
 
            for (size_t j = 0; j < _currentFrame.keypoints.size(); j++)
            { // bbox check
                if (_currentFrame.keypoints[j].pt.x > xTopLeft &&
                    _currentFrame.keypoints[j].pt.x < xDownRight &&
                    _currentFrame.keypoints[j].pt.y > yTopLeft &&
                    _currentFrame.keypoints[j].pt.y < yDownRight)
                {
                    bboxFrameKeypoints.push_back(_currentFrame.keypoints[j]);
                    frameIndicesInsideRect.push_back(j);
                }
            }
 
            // 3. Match the descriptors of the key points inside
            // the rectangle around the projected map point
            // with the descriptor of the projected map point.
 
            // This is our descriptor for the model point i
            CVMat modelPointDescriptor = _marker.descriptors.row((int)i);
 
            // We extract the descriptors which belong to the key points
            // inside the rectangle around the projected map point
            CVMat bboxPointsDescriptors;
            for (size_t j : frameIndicesInsideRect)
                bboxPointsDescriptors.push_back(_currentFrame.descriptors.row((int)j));
 
            // 4. Match the frame key points inside the rectangle with the projected model point
            _matcher->match(bboxPointsDescriptors, modelPointDescriptor, newMatches);
        }
 
        if (!newMatches.empty())
        {
            for (size_t j = 0; j < frameIndicesInsideRect.size(); j++)
            {
                newMatches[j].trainIdx = (int)i;
                newMatches[j].queryIdx = (int)frameIndicesInsideRect[j];
            }
 
            // 5. Only add the best new match to matches vector
            CVDMatch bestNewMatch;
            bestNewMatch.distance = 0;
 
            for (CVDMatch newMatch : newMatches)
                if (bestNewMatch.distance < newMatch.distance)
                    bestNewMatch = newMatch;
 
            // 6. Only add the best new match to matches vector
            _currentFrame.inlierMatches.push_back(bestNewMatch);
        }
 
        // Get the keypoint which was used for pose estimation
        CVPoint2f keypointForPose = _currentFrame.keypoints[(uint)_currentFrame.inlierMatches.back().queryIdx].pt;
 
#if DO_FEATURE_BENCHMARKING
        reprojectionError += (float)norm(CVMat(projectedModelPoint),
                                         CVMat(keypointForPose));
#endif
 
#if DRAW_PATCHES
        // draw green rectangle around every map point
        rectangle(_currentFrame.image,
                  Point2f(projectedModelPoint.x - (float)patchSize / 2.0f,
                          projectedModelPoint.y - (float)patchSize / 2.0f),
                  Point2f(projectedModelPoint.x + (float)patchSize / 2.0f,
                          projectedModelPoint.y + (float)patchSize / 2.0f),
                  CV_RGB(0, 255, 0));
 
        // draw key points, that lie inside this rectangle
        for (const auto& kPt : bboxFrameKeypoints)
            circle(_currentFrame.image,
                   kPt.pt,
                   1,
                   CV_RGB(0, 0, 255),
                   1,
                   FILLED);
#endif
    }
 
#if DO_FEATURE_BENCHMARKING
    sum_reprojection_error += reprojectionError / _marker.keypoints3D.size();
#endif
 
#if DO_FEATURE_BENCHMARKING
    CVMat prevRmat, currRmat;
    if (_prevFrame.foundPose)
    {
        Rodrigues(_prevFrame.rvec, prevRmat);
        Rodrigues(_currentFrame.rvec, currRmat);
        double rotationError_rad = acos((trace(prevRmat * currRmat).val[0] - 1.0) / 2.0);
        rotationError += rotationError_rad * 180 / 3.14;
        translationError += cv::norm(_prevFrame.tvec, _currentFrame.tvec);
    }
#endif
 
#if DRAW_REPROJECTION_POINTS
    // Draw the projection error for the current frame
    putText(_currentFrame.image,
            "Reprojection error: " + to_string(reprojectionError / _marker.keypoints3D.size()),
            Point2f(20, 20),
            FONT_HERSHEY_SIMPLEX,
            0.5,
            CV_RGB(255, 0, 0),
            2.0);
#endif
 
    // Optimize POSE
    vector<cv::Point3f> modelPoints = vector<cv::Point3f>(_currentFrame.inlierMatches.size());
    vector<cv::Point2f> framePoints = vector<cv::Point2f>(_currentFrame.inlierMatches.size());
    for (size_t i = 0; i < _currentFrame.inlierMatches.size(); i++)
    {
        modelPoints[i] = _marker.keypoints3D[(uint)_currentFrame.inlierMatches[i].trainIdx];
        framePoints[i] = _currentFrame.keypoints[(uint)_currentFrame.inlierMatches[i].queryIdx].pt;
    }
 
    if (modelPoints.empty()) return;
    _currentFrame.inlierPoints3D = modelPoints;
    _currentFrame.inlierPoints2D = framePoints;
}

relocate()

void CVTrackedFeatures::relocate ( )

private

If relocation should be done, the following steps are necessary:

1. Detect keypoints
2. Describe keypoints (Binary descriptors)
3. Match keypoints in current frame and the reference tracker
4. Try to calculate new Pose with Perspective-n-Point algorithm

Definition at line 273 of file CVTrackedFeatures.cpp.

{
    _isTracking = false;
    detectKeypointsAndDescriptors();
    _currentFrame.matches   = getFeatureMatches();
    _currentFrame.foundPose = calculatePose();
 
    // Zero time keeping on the tracking branch
    CVTracked::optFlowTimesMS.set(0);
}

track()

bool CVTrackedFeatures::track ( CVMat  imageGray, CVMat  image, CVCalibration *  calib  )

finalvirtual

The main part of this tracker is to calculate a correct Pose.

Parameters

imageGray	Current grayscale frame
image	Current RGB frame
calib	Calibration information

Returns

So far always false

Implements CVTracked.

Definition at line 218 of file CVTrackedFeatures.cpp.

{
    assert(!image.empty() && "Image is empty");
    assert(!calib->cameraMat().empty() && "Calibration is empty");
    assert(!_marker.imageGray.empty());
 
    // Initialize reference points if program just started
    if (_frameCount == 0)
    {
        _calib = calib;
        initFeaturesOnMarker();
    }
 
    // Copy image matrix into current frame data
    _currentFrame.image     = image;
    _currentFrame.imageGray = imageGray;
 
    // Determine if relocation or feature tracking should be performed
    bool relocationNeeded = _forceRelocation ||
                            !_prevFrame.foundPose ||
                            _prevFrame.inlierMatches.size() < 100 ||
                            frames_since_posefound < 3;
 
    // If relocation condition meets, calculate the Pose with feature detection, otherwise
    // track the previous determined features
    if (relocationNeeded)
        relocate();
    else
        tracking();
 
    if (_currentFrame.foundPose)
    {
        _objectViewMat = createGLMatrix(_currentFrame.tvec, _currentFrame.rvec);
        frames_with_pose++;
    }
 
    // Perform OpenCV drawning if flags are set (see CVTrackedFeatures.h)
    drawDebugInformation(_drawDetection);
 
    // Prepare next frame and transfer necessary data
    transferFrameData();
 
    _frameCount++;
 
    return _currentFrame.foundPose;
}

tracking()

void CVTrackedFeatures::tracking ( )

private

To track the already detected keypoints after a sucessful pose estimation, we track the features with optical flow

Definition at line 288 of file CVTrackedFeatures.cpp.

{
    _isTracking             = true;
    _currentFrame.foundPose = trackWithOptFlow(_prevFrame.rvec, _prevFrame.tvec);
 
    // Zero time keeping on the relocation branch
    CVTracked::detectTimesMS.set(0);
    CVTracked::matchTimesMS.set(0);
}

trackWithOptFlow()

bool CVTrackedFeatures::trackWithOptFlow ( CVMat  rvec, CVMat  tvec  )

private

Tracks the features with Optical Flow (Lucas Kanade). This will only try to predict the new location of keypoints. If they were found, we perform a solvePnP to get the new Pose from feature tracking. The method performs tests if the Pose is good enough (not too much difference between previous and new Pose).

Parameters

rvec	Rotation vector (will be used for extrinsic guess)
tvec	Translation vector (will be used for extrinsic guess)

Returns

True if Pose found, false otherwise

Definition at line 850 of file CVTrackedFeatures.cpp.

{
    if (_prevFrame.inlierPoints2D.size() < 4) return false;
 
    float startMS = _timer.elapsedTimeInMilliSec();
 
    vector<uchar> status;
    vector<float> err;
    CVSize        winSize(15, 15);
 
    cv::TermCriteria criteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS,
                              10,    // terminate after this many iterations, or
                              0.03); // when the search window moves by less than this
 
    // Find closest possible feature points based on optical flow
    CVVPoint2f pred2DPoints(_prevFrame.inlierPoints2D.size());
 
    // todo: do not relate optical flow to previous frame! better to original marker image, otherwise we will drift
    cv::calcOpticalFlowPyrLK(
      _prevFrame.imageGray,      // Previous frame
      _currentFrame.imageGray,   // Current frame
      _prevFrame.inlierPoints2D, // Previous and current keypoints coordinates.The latter will be
      pred2DPoints,              // expanded if more good coordinates are detected during OptFlow
      status,                    // Output vector for keypoint correspondences (1 = match found)
      err,                       // Error size for each flow
      winSize,                   // Search window for each pyramid level
      3,                         // Max levels of pyramid creation
      criteria,                  // Configuration from above
      0,                         // Additional flags
      0.001);                    // Minimal Eigen threshold
 
    // Only use points which are not wrong in any way during the optical flow calculation
    CVVPoint2f frame2DPoints;
    CVVPoint3f model3DPoints;
    for (size_t i = 0; i < status.size(); i++)
    {
        if (status[i])
        {
            frame2DPoints.push_back(pred2DPoints[i]);
            // Original code from Zingg/Tschanz got zero size vector
            // model3DPoints.push_back(_currentFrameFrame.inlierPoints3D[i]);
            model3DPoints.push_back(_prevFrame.inlierPoints3D[i]);
        }
    }
 
    CVTracked::optFlowTimesMS.set(_timer.elapsedTimeInMilliSec() - startMS);
 
    _currentFrame.inlierPoints2D = frame2DPoints;
    _currentFrame.inlierPoints3D = model3DPoints;
 
    if (_currentFrame.inlierPoints2D.size() < _prevFrame.inlierPoints2D.size() * 0.75)
        return false;
 
    /////////////////////
    // Pose Estimation //
    /////////////////////
 
    startMS = _timer.elapsedTimeInMilliSec();
 
    bool foundPose = cv::solvePnP(model3DPoints,
                                  frame2DPoints,
                                  _calib->cameraMat(),
                                  _calib->distortion(),
                                  rvec,
                                  tvec,
                                  true);
    bool poseValid = true;
 
    if (foundPose)
    {
        for (int i = 0; i < tvec.cols; i++)
        {
            if (abs(tvec.at<double>(i, 0) - tvec.at<double>(i, 0)) > abs(tvec.at<double>(i, 0)) * 0.2)
            {
                cout << "translation too large" << endl;
                poseValid = false;
            }
        }
        for (int i = 0; i < rvec.cols; i++)
        {
            if (abs(rvec.at<double>(i, 0) - rvec.at<double>(i, 0)) > 0.174533)
            {
                cout << "rotation too large" << endl;
                poseValid = false;
            }
        }
    }
 
    if (foundPose && poseValid)
    {
        rvec.copyTo(_currentFrame.rvec);
        tvec.copyTo(_currentFrame.tvec);
    }
 
    CVTracked::poseTimesMS.set(_timer.elapsedTimeInMilliSec() - startMS);
 
    return foundPose && poseValid;
}

transferFrameData()

void CVTrackedFeatures::transferFrameData ( )

private

Copies the current frame data to the previous frame data struct for the next frame handling. TODO: more elegant way to do this whole copy action

Definition at line 444 of file CVTrackedFeatures.cpp.

{
    _currentFrame.imageGray.copyTo(_prevFrame.imageGray);
    _currentFrame.image.copyTo(_prevFrame.image);
    _currentFrame.rvec.copyTo(_prevFrame.rvec);
    _currentFrame.tvec.copyTo(_prevFrame.tvec);
 
    _prevFrame.reprojectionError = _currentFrame.reprojectionError;
    _prevFrame.foundPose         = _currentFrame.foundPose;
    _prevFrame.inlierPoints3D    = _currentFrame.inlierPoints3D;
    _prevFrame.inlierPoints2D    = _currentFrame.inlierPoints2D;
 
    if (!_currentFrame.inlierMatches.empty())
        _prevFrame.inlierMatches = _currentFrame.inlierMatches;
 
    _currentFrame.keypoints.clear();
    _currentFrame.matches.clear();
    _currentFrame.inlierMatches.clear();
    _currentFrame.inlierPoints2D.clear();
    _currentFrame.inlierPoints3D.clear();
    _currentFrame.reprojectionError = 0;
 
    _currentFrame.useExtrinsicGuess = _prevFrame.foundPose;
 
    if (_prevFrame.foundPose)
    {
        _currentFrame.rvec = _prevFrame.rvec;
        _currentFrame.tvec = _prevFrame.tvec;
    }
    else
    {
        _currentFrame.rvec = CVMat::zeros(3, 1, CV_64FC1);
        _currentFrame.tvec = CVMat::zeros(3, 1, CV_64FC1);
    }
}

type() [1/2]

CVDetectDescribeType CVTrackedFeatures::type ( )

inline

Definition at line 76 of file CVTrackedFeatures.h.

{ return _featureManager.type(); }

type() [2/2]

void CVTrackedFeatures::type

(

CVDetectDescribeType

ddType

)

Setter of the feature detector & descriptor type.

Definition at line 200 of file CVTrackedFeatures.cpp.

{
    _featureManager.createDetectorDescriptor(ddType);
 
    _currentFrame.foundPose         = false;
    _prevFrame.foundPose            = false;
    _currentFrame.reprojectionError = 0.0f;
 
    // Set the frame counter to 0 to reinitialize in track
    _frameCount = 0;
}

Member Data Documentation

_calib

CVCalibration* CVTrackedFeatures::_calib

private

Current calibration in use.

Definition at line 96 of file CVTrackedFeatures.h.

_currentFrame

SLFrameData CVTrackedFeatures::_currentFrame

private

The current video frame data.

Definition at line 129 of file CVTrackedFeatures.h.

_featureManager

CVFeatureManager CVTrackedFeatures::_featureManager

private

Feature detector-descriptor wrapper instance.

Definition at line 132 of file CVTrackedFeatures.h.

_forceRelocation

bool CVTrackedFeatures::_forceRelocation

private

Force relocation every frame (no opt. flow tracking)

Definition at line 131 of file CVTrackedFeatures.h.

_frameCount

int CVTrackedFeatures::_frameCount

private

NO. of frames since process start.

Definition at line 97 of file CVTrackedFeatures.h.

_isTracking

bool CVTrackedFeatures::_isTracking

private

True if tracking.

Definition at line 98 of file CVTrackedFeatures.h.

_marker

SLFeatureMarker2D CVTrackedFeatures::_marker

private

2D marker data

Definition at line 128 of file CVTrackedFeatures.h.

_matcher

cv::Ptr<cv::DescriptorMatcher> CVTrackedFeatures::_matcher

private

Descriptor matching algorithm.

Definition at line 95 of file CVTrackedFeatures.h.

_prevFrame

SLFrameData CVTrackedFeatures::_prevFrame

private

The previous video frame data.

Definition at line 130 of file CVTrackedFeatures.h.

---

The documentation for this class was generated from the following files:

• CVTrackedFeatures.h
• CVTrackedFeatures.cpp