【ORB_SLAM2源码解读】金字塔图像分块并提取Fast特征点（６）

视频讲解

【ORB_SLAM2源码解读】金字塔图像分块并提取Fast特征点（６）

图文讲解

// QQ 952233821 WX slamshizhanjiaocheng
cv::Mat image;
allKeypoints.resize(nlevels);
const float W = 30;
// for 循环遍历金字塔每层图像, 计算特征点（分块）, 特征点均匀化（四叉树）
for (int level = 0; level < nlevels; ++level)
{
//        rectangle(mvImagePyramid[level], Point(19, 19), Point(mvImagePyramid[level].cols-EDGE_THRESHOLD, mvImagePyramid[level].rows-EDGE_THRESHOLD), Scalar(0), 1);
//        cv::imwrite( to_string(level) + "_rectangle_19.png", mvImagePyramid[level]);
    // 3 fast 角点像素半径为3
    const int minBorderX = EDGE_THRESHOLD-3;// 16=19-3
    const int minBorderY = minBorderX;// 16=19-3
    const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;// 736=752-19+3
    const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;// 464=480-19+3
    rectangle(mvImagePyramid[level], Point(minBorderX, minBorderY), Point(maxBorderX, maxBorderY), Scalar(0), 1);
//        cv::imwrite(to_string(level) + "_rectangle_16.png", mvImagePyramid[level]);

    const float width = (maxBorderX-minBorderX);// 原始图像宽752 可提取特征图像宽 752-32=720
    const float height = (maxBorderY-minBorderY);// 原始图像高480 可提取特征图像宽 480-32=448

    // 宽720 分24个图像块
    const int nCols = width/W;
    // 高448 分14个图像块剩余余28个像素 每块高30个像素
    const int nRows = height/W;

    // 每块图像宽720/24=30个像素
    const int wCell = ceil(width/nCols);
    // 每块图像高448/14=32个像素
    const int hCell = ceil(height/nRows);

    // 存储提取到的所有特征点
    vector<cv::KeyPoint> vToDistributeKeys;
    vToDistributeKeys.reserve(nfeatures*10);
    // 双层for循环遍历某层图像的所有小图像块(宽30*高32pixel)
    // 外层for循环遍历行, 内层for循环遍历列
    for(int i=0; i<nRows; i++){
        // 计算每个图像块左上角的iniY坐标
        const float iniY =minBorderY + i*hCell;// iniY=16 = minBorderY=6 + i=0×hCell=32
        // 可以提取特征点的图像范围是从(19, 19)开始的, 然后为了考虑边界像素提取Fast特征点的时候是以特征点为中心半径为3个像素的圆计算的
        // 计算每个图像块左下角的maxY坐标
        float maxY = iniY + 3 + hCell + 3;// maxY=54 = iniY=16 + 3 + hCell=32 + 3
        // 计算到图像块边缘像素坐标和图像边界之间已经不足3个像素时, 就没有计算的必要了, 因为提取提取Fast特征点以特征点为中心半径为3个像素的圆计算的
        if(iniY>=maxBorderY-3)
            continue;
        // 计算到图像块边缘像素坐标大于图像边界时直接赋值为可以提取特征点的图像的边界
        if(maxY>maxBorderY)
            maxY = maxBorderY;

        // 外层for循环遍历行, 内层for循环遍历列
        for(int j=0; j<nCols; j++){
            // 计算每个图像块左上角的iniX坐标
            const float iniX = minBorderX+j*wCell;// iniX=16 = minBorderX=16 + j=0×wCell=30
            // 计算每个图像块右上角的maxX坐标
            float maxX = iniX+3+wCell+3;//maxX=52 = iniX=16 + j=0×wCell=30
            // 计算到图像块边缘像素坐标和图像边界之间已经不足6个像素时,停止当前图像块的计算,继续下一个图像块
            if(iniX>=maxBorderX-6)
                continue;
            // 计算到图像块边缘像素坐标大于图像边界时直接赋值为可以提取特征点的图像的边界
            if(maxX>maxBorderX)
                maxX = maxBorderX;

            vector<cv::KeyPoint> vKeysCell;
            // std::cout << "iniX " << iniX << " iniY " << iniY << " maxX " << maxX << " maxY " << maxY << std::endl;
            // 绘制提取特征的小图像块
            rectangle(mvImagePyramid[level], Point(iniX, iniY), Point(maxX, maxY), Scalar(0), 1);
//                cv::imwrite(to_string(level) + to_string(i) + to_string(j) +".png", mvImagePyramid[level]);

            FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true);
//                cv::imwrite(to_string(level) + to_string(i) + to_string(j) + "_fast.png", mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX));

            if(vKeysCell.empty())
            {
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),vKeysCell,minThFAST,true);
            }

            if(!vKeysCell.empty()){
                for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++){
                    // 像素相对与图像块的坐标转换到相对与检测特征点全图像的的像素坐标
//                        std::cout << "(*vit).pt.x " << (*vit).pt.x << " (*vit).pt.y " << (*vit).pt.y << std::endl;
                    (*vit).pt.x+=j*wCell;
                    (*vit).pt.y+=i*hCell;
//                        std::cout << "(*vit).pt.x " << (*vit).pt.x << " (*vit).pt.y " << (*vit).pt.y << std::endl;
                    vToDistributeKeys.push_back(*vit);
                    image = mvImagePyramid[level].rowRange(minBorderY,maxBorderY).colRange(minBorderX,maxBorderX);
                    // 绘制提取到的特征点
                    cv::circle(image, (*vit).pt,1,cv::Scalar(0),-1);
                }
                cv::imwrite( to_string(level) + "_keypoint.png", image);
            }

        }
    }