ORB-SLAM2 Notes: Day 8 – KeyPoint Distribution

In the last section, Understanding the QuadTree and KeyPoint Distribution, I explained the principles of quad tree, and how it distributes key points using this data structure.

Next, we’ll help you deeply understand the code of quad tree and distribution algorithm. Note that all the code below has been uploaded to my GitHub.

KeyPointsRegionalQuadTreeNode.h

#ifndef REGIONALQUADTREE_H
#define REGIONALQUADTREE_H

#include <list>
#include <vector>
#include <opencv2/features2d.hpp>

using namespace cv;
using namespace std;

namespace my_ORB_SLAM2 {

// Class of quadtree node.
class KeyPointsRegionalQuadTreeNode {
    public:
        /*
        @brief Set node. */
        KeyPointsRegionalQuadTreeNode() {};
        ~KeyPointsRegionalQuadTreeNode() {};
        
        vector<KeyPoint*> mvpKeyPoints; // Vector of pointers to key points.
        int miMinX, miMinY, miMaxX, miMaxY, miMidX, miMidY; // Coordinates of the box in the original image.
        bool mbLocked = false; // Whether it is changeable.
};

class KeyPointsRegionalQuadTree {
    public:
        /*
        @brief Set quadtree.
        
        @param[in] iWidth Image width.
        @param[in] iHeight Image height.
        @param[in] vKeyPoints A group of key points. */
        KeyPointsRegionalQuadTree(int iWidth, int iHeight, vector<KeyPoint> &vKeyPoints);
        ~KeyPointsRegionalQuadTree() {};

        list<KeyPointsRegionalQuadTreeNode> mlNodes; // Nodes list of this quad tree.
        vector<KeyPoint> &mvKeyPoints; // All key points of this quad tree.

        /*
        @brief Divide the node pointed to by iterator.
            
        @param[in] lNodesIterator Iterator pointing to the node to be divided. */
        list<KeyPointsRegionalQuadTreeNode>::iterator divide(list<KeyPointsRegionalQuadTreeNode>::iterator &lNodesIterator);
};

}

#endif

KeyPointsRegionalQuadTreeNode.cpp

#include <myORB-SLAM2/KeyPointsRegionalQuadTree.h>

namespace my_ORB_SLAM2 {
    /*
    @brief Initialize the QuadTree.
    
    @param[in] iWidth Image width.
    @param[in] iHeight Image height.
    @param[in] vKeyPoints A group of keypoints */
    KeyPointsRegionalQuadTree::KeyPointsRegionalQuadTree(int iWidth, int iHeight, vector<KeyPoint> &vKeyPoints):
    mvKeyPoints(vKeyPoints) {
        // Get the number of keypoints.
        int nKeyPoints = mvKeyPoints.size();
        
        // If the QuadTree has at least one keypoint.
        if(nKeyPoints >= 1) {
            // Set the first node of the quadtree.
            mlNodes.resize(1);
            KeyPointsRegionalQuadTreeNode &node = mlNodes.front();

            // Set the coordinate of this node related to the original image.
            node.miMinX = 0;
            node.miMinY = 0;
            node.miMaxX = iWidth;
            node.miMaxY = iHeight;
            node.miMidX = int(0.5 * (float)iWidth);
            node.miMidY = int(0.5 * (float)iHeight);

            // Reserve keypoints capacity for this node.
            node.mvpKeyPoints.reserve(nKeyPoints);

            // Store all pointers to all keypoints in this node.
            for (KeyPoint &keyPoint : mvKeyPoints) {
                node.mvpKeyPoints.push_back(&keyPoint);
            }

            // Lock this node if it contains only one keypoint.
            node.mbLocked = (nKeyPoints == 1);
        }
    }

    /*
    @brief Divide the node pointed to by iterator.
        
    @param[in] lNodesIterator Iterator pointing to the node to be divided. */
    list<KeyPointsRegionalQuadTreeNode>::iterator 
    KeyPointsRegionalQuadTree::divide(list<KeyPointsRegionalQuadTreeNode>::iterator &lNodesIterator) {
        // If this node is locked, get next node.
        if(lNodesIterator->mbLocked) { return ++lNodesIterator; }
        
        // Divide the parent node into 4 child nodes, setting their bounding coordinates.
        KeyPointsRegionalQuadTreeNode node00, node01, node10, node11;
        node00.miMinX = lNodesIterator->miMinX; 
        node00.miMinY = lNodesIterator->miMinY; 
        node00.miMaxX = lNodesIterator->miMidX; 
        node00.miMaxY = lNodesIterator->miMidY; 
        node00.miMidX = int((float)(node00.miMaxX + node00.miMinX) * 0.5);
        node00.miMidY = int((float)(node00.miMaxY + node00.miMinY) * 0.5);

        node01.miMinX = lNodesIterator->miMidX; 
        node01.miMinY = lNodesIterator->miMinY; 
        node01.miMaxX = lNodesIterator->miMaxX; 
        node01.miMaxY = lNodesIterator->miMidY; 
        node01.miMidX = int((float)(node01.miMaxX + node01.miMinX) * 0.5);
        node01.miMidY = int((float)(node01.miMaxY + node01.miMinY) * 0.5);

        node10.miMinX = lNodesIterator->miMinX; 
        node10.miMinY = lNodesIterator->miMidY; 
        node10.miMaxX = lNodesIterator->miMidX; 
        node10.miMaxY = lNodesIterator->miMaxY; 
        node10.miMidX = int((float)(node10.miMaxX + node10.miMinX) * 0.5);
        node10.miMidY = int((float)(node10.miMaxY + node10.miMinY) * 0.5);

        node11.miMinX = lNodesIterator->miMidX; 
        node11.miMinY = lNodesIterator->miMidY; 
        node11.miMaxX = lNodesIterator->miMaxX; 
        node11.miMaxY = lNodesIterator->miMaxY; 
        node11.miMidX = int((float)(node11.miMaxX + node11.miMinX) * 0.5);
        node11.miMidY = int((float)(node11.miMaxY + node11.miMinY) * 0.5);

        // Reserve keypoints capacity for these 4 nodes to avoid performance loss due to dynamic memory allocation.
        int nKeyPoints = lNodesIterator->mvpKeyPoints.size();
        node00.mvpKeyPoints.reserve(nKeyPoints);
        node01.mvpKeyPoints.reserve(nKeyPoints);
        node10.mvpKeyPoints.reserve(nKeyPoints);
        node11.mvpKeyPoints.reserve(nKeyPoints);
        
        // Distribute all keypoints from the parent node to its 4 child nodes based on their coordinates.
        int iMidX, iMidY;
        iMidX = lNodesIterator->miMidX;
        iMidY = lNodesIterator->miMidY;
        for(KeyPoint* &keyPoint : lNodesIterator->mvpKeyPoints) {
            if(keyPoint->pt.x < iMidX && keyPoint->pt.y < iMidY) {
                node00.mvpKeyPoints.push_back(keyPoint);
            }
            else if(keyPoint->pt.x >= iMidX && keyPoint->pt.y < iMidY) {
                node01.mvpKeyPoints.push_back(keyPoint);
            }
            else if(keyPoint->pt.x < iMidX && keyPoint->pt.y >= iMidY) {
                node10.mvpKeyPoints.push_back(keyPoint);
            }
            else { node11.mvpKeyPoints.push_back(keyPoint); }
        }

        // Lock each node if it contains only one keypoint.
        node00.mbLocked = (node00.mvpKeyPoints.size() == 1);
        node01.mbLocked = (node01.mvpKeyPoints.size() == 1);
        node10.mbLocked = (node10.mvpKeyPoints.size() == 1);
        node11.mbLocked = (node11.mvpKeyPoints.size() == 1);
        
        // Save these nodes to the QuadTree if they contain more than one keypoint.
        if(node00.mvpKeyPoints.size() > 0)
            mlNodes.push_front(node00);
        
        if(node01.mvpKeyPoints.size() > 0)
            mlNodes.push_front(node01);

        if(node10.mvpKeyPoints.size() > 0)
            mlNodes.push_front(node10);

        if(node11.mvpKeyPoints.size() > 0)
            mlNodes.push_front(node11);

        // Remove the parent node and return an iterator to the next node.
        return mlNodes.erase(lNodesIterator);
    }
}

Keypoint Distribution Algorithm

I designed the KeyPointDistributorPyramid class for keypoints distribution.

KeyPointDistributorPyramid.h

#ifndef KEYPOINTDISTRIBUTOR
#define KEYPOINTDISTRIBUTOR

#include "myORB-SLAM2/KeyPointsRegionalQuadTree.h"

namespace my_ORB_SLAM2 {

class KeyPointDistributorPyramid {
    public:
        /*
        @brief Keypoint distributor incorporating the image pyramid concept. */
        KeyPointDistributorPyramid() {};
        ~KeyPointDistributorPyramid() {};

        /*
        @brief This function distributes the keypoints from each level of the image pyramid.
        
        @param[in, out] vvDistributedKeyPointsPerLevel This parameter saves the distributed keypoints from each level of the image pyramid.
        @param[in] vvKeyPointsPerLevel This parameter saves the keypoints from each level of the image pyramid.
        @param[in] vnFeaturesPerLevel This parameter saves the number of keypoints to be extracted from each level of the image pyramid.
        @param[in] vImagePerLevel This parameter saves the images from each level of the image pyramid. */
        void distribute(
            vector<vector<KeyPoint>> &vvDistributedKeyPointsPerLevel,
            vector<vector<KeyPoint>> &vvKeyPointsPerLevel, 
            vector<int> &vnFeaturesPerLevel,
            vector<Mat> &vImagePerLevel
        );
};

}

#endif

KeyPointDistributorPyramid.cpp

#include "myORB-SLAM2/KeyPointDistributorPyramid.h"

namespace my_ORB_SLAM2 {
    /*
    @brief This function distributes the keypoints from each level of the image pyramid.
    
    @param[in, out] vvDistributedKeyPointsPerLevel This parameter saves the distributed keypoints from each level of the image pyramid.
    @param[in] vvKeyPointsPerLevel This parameter saves the keypoints from each level of the image pyramid.
    @param[in] vnFeaturesPerLevel This parameter saves the number of keypoints to be extracted from each level of the image pyramid.
    @param[in] vImagePerLevel This parameter saves the images from each level of the image pyramid. */
    void KeyPointDistributorPyramid::distribute(
        vector<vector<KeyPoint>> &vvDistributedKeyPointsPerLevel,
        vector<vector<KeyPoint>> &vvKeyPointsPerLevel, 
        vector<int> &vnFeaturesPerLevel,
        vector<Mat> &vImagePerLevel
    ) {
        // This vector is resized to correspond the number of levels in the image pyramid. 
        vvDistributedKeyPointsPerLevel.resize(vImagePerLevel.size());

        // This step iterates over each level of the image pyramid.
        for(int iLevel = 0; iLevel < vImagePerLevel.size(); iLevel++) {
            // Initialize the QuadTree.
            int iWidth, iHeight; // Get width and height of the image.
            iWidth = vImagePerLevel[iLevel].cols;
            iHeight = vImagePerLevel[iLevel].rows;

            // Reference to the keypoints at this level.
            vector<KeyPoint> &vKeyPoints = vvKeyPointsPerLevel[iLevel];
            KeyPointsRegionalQuadTree quadTree(iWidth, iHeight, vKeyPoints);

            // Start dividing the QuadTree
            while(true) {
                // Get an iterator for the node list in the QuadTree.
                list<KeyPointsRegionalQuadTreeNode>::iterator lit = 
                quadTree.mlNodes.begin();

                // Count the nodes to be divided.
                int nDistribution = 0;
                while(lit != quadTree.mlNodes.end()) {
                    if(lit->mvpKeyPoints.size() > 1) { nDistribution++; }
                    lit++;
                }

                // Exit the loop if no nodes need division.
                if(nDistribution == 0) { break; }
                
                // Count the nodes in the QuadTree after the next division, if at least one node is divisible.
                int nNextNodes = quadTree.mlNodes.size() + nDistribution * 3;

                // If the node count doesn't meet the target, then the QuadTree divides all the nodes except the nodes with a single keypoint.
                if(nNextNodes < vnFeaturesPerLevel[iLevel]) {
                    lit = quadTree.mlNodes.begin();
                    while(lit != quadTree.mlNodes.end())
                        lit = quadTree.divide(lit);
                }else {
                    // Check if the node count reaches the target.
                    bool bComplete = false;

                    // Sort node list in descending order.
                    quadTree.mlNodes.sort([](const KeyPointsRegionalQuadTreeNode &n1, const KeyPointsRegionalQuadTreeNode &n2) { return n1.mvpKeyPoints.size() > n2.mvpKeyPoints.size();});
                    
                    // Divide the nodes from the beginning of the list.
                    lit = quadTree.mlNodes.begin();
                    while(lit != quadTree.mlNodes.end()) {
                        lit = quadTree.divide(lit);

                        // Stop dividing if the node count meets the target.
                        if(quadTree.mlNodes.size() >= vnFeaturesPerLevel[iLevel]) {
                            bComplete = true;
                            break;
                        }
                    } 
                    
                    // Stop dividing.
                    if(bComplete) { break; }
                }
            }

            // Reserve the keypoint capacity for the vectors from each level of image pyramid.
            vvDistributedKeyPointsPerLevel[iLevel].reserve(quadTree.mlNodes.size());
            for(KeyPointsRegionalQuadTreeNode &node : quadTree.mlNodes) {
                // Get the keypoint with best response within the node.
                float fMaxResponse = -INFINITY;
                KeyPoint* pKeyPoint = nullptr;
                vector<KeyPoint*> &keyPoints = node.mvpKeyPoints;
                for(vector<KeyPoint*>::iterator vit = keyPoints.begin(); vit != keyPoints.end(); ++vit) {
                    if((*vit)->response > fMaxResponse) {
                        fMaxResponse = (*vit)->response;
                        pKeyPoint = (*vit);
                    }
                }

                // Save this keypoint based on its image pyramid level.
                vvDistributedKeyPointsPerLevel[iLevel].push_back(*pKeyPoint);
            }
        }
    };

}

Result

Original Image

Keypoint Distribution

Undistributed Keypoints (left) and Distributed Keypoints (right).