《Example_MarkerBasedAR》中MarkerDetector.cpp源码及详细中文注释

/*****************************************************************************
*   MarkerDetector.cpp
*   Example_MarkerBasedAR
******************************************************************************
*   by Khvedchenia Ievgen, 5th Dec 2012
*   http://computer-vision-talks.com
******************************************************************************
*   Ch2 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/


// Standard includes:
#include <iostream>
#include <sstream>


// File includes:
#include "MarkerDetector.hpp"
#include "Marker.hpp"
#include "TinyLA.hpp"
#include "DebugHelpers.hpp"

MarkerDetector::MarkerDetector(CameraCalibration calibration)
    : m_minContourLengthAllowed(100)
    , markerSize(100,100)
{
    cv::Mat(3,3, CV_32F, const_cast<float*>(&calibration.getIntrinsic().data[0])).copyTo(camMatrix);//相机的内参数
    cv::Mat(4,1, CV_32F, const_cast<float*>(&calibration.getDistorsion().data[0])).copyTo(distCoeff);//相机的畸变参数

    bool centerOrigin = true;
    if (centerOrigin)//坐标轴是否在标记的中心
    {
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,+0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,+0.5f,0));
    }
    else
    {
        m_markerCorners3d.push_back(cv::Point3f(0,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,1,0));
        m_markerCorners3d.push_back(cv::Point3f(0,1,0));    
    }

    m_markerCorners2d.push_back(cv::Point2f(0,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,markerSize.height-1));
    m_markerCorners2d.push_back(cv::Point2f(0,markerSize.height-1));
}

void MarkerDetector::processFrame(const BGRAVideoFrame& frame)
{
    std::vector<Marker> markers;
    findMarkers(frame, markers);//☆★

    m_transformations.clear();
    for (size_t i=0; i<markers.size(); i++)
    {
        m_transformations.push_back(markers[i].transformation);
    }
}

//可以通过该对象取得旋转矩阵和平移向量
const std::vector<Transformation>& MarkerDetector::getTransformations() const
{
    return m_transformations;
}


bool MarkerDetector::findMarkers(const BGRAVideoFrame& frame, std::vector<Marker>& detectedMarkers)
{
    cv::Mat bgraMat(frame.height, frame.width, CV_8UC4, frame.data, frame.stride);

    // BGRA=>gray
    prepareImage(bgraMat, m_grayscaleImage);

    // 二值化
    performThreshold(m_grayscaleImage, m_thresholdImg);

    // 轮廓检测
    findContours(m_thresholdImg, m_contours, m_grayscaleImage.cols / 5);

    // 寻找具有四个角点的近似轮廓
    findCandidates(m_contours, detectedMarkers);

    // 检测它们是否是指定的标记
    recognizeMarkers(m_grayscaleImage, detectedMarkers);

    // 标记的姿态估计
    estimatePosition(detectedMarkers);

    //根据id进行排序
    std::sort(detectedMarkers.begin(), detectedMarkers.end());
    return false;
}

void MarkerDetector::prepareImage(const cv::Mat& bgraMat, cv::Mat& grayscale) const
{
    // Convert to grayscale
    cv::cvtColor(bgraMat, grayscale, CV_BGRA2GRAY);
}

void MarkerDetector::performThreshold(const cv::Mat& grayscale, cv::Mat& thresholdImg) const
{
    cv::threshold(grayscale, thresholdImg, 127, 255, cv::THRESH_BINARY_INV);

    
//    cv::adaptiveThreshold(grayscale,    // Input image
//    thresholdImg,                       // Result binary image
//    255,         
//    cv::ADAPTIVE_THRESH_GAUSSIAN_C, 
//    cv::THRESH_BINARY_INV, 
//    7, 
//    7  
//    );
    

#ifdef SHOW_DEBUG_IMAGES
    cv::showAndSave("Threshold image", thresholdImg);
#endif
}

void MarkerDetector::findContours(cv::Mat& thresholdImg, ContoursVector& contours, int minContourPointsAllowed) const
{
    // 使用自定义的轮廓数组类型来临时保存检测出的轮廓
    ContoursVector allContours;
    
//    输入图像image必须为一个2值单通道图像
//    检测的轮廓数组，每一个轮廓用一个point类型的vector表示
//    轮廓的检索模式
    
//     CV_RETR_EXTERNAL表示只检测外轮廓
//     CV_RETR_LIST检测的轮廓不建立等级关系
//     CV_RETR_CCOMP建立两个等级的轮廓，上面的一层为外边界，里面的一层为内孔的边界信息。如果内孔内还有一个连通物体，这个物体的边界也在顶层。
//     CV_RETR_TREE建立一个等级树结构的轮廓。具体参考contours.c这个demo
    
//    轮廓的近似办法
    
//     CV_CHAIN_APPROX_NONE存储所有的轮廓点，相邻的两个点的像素位置差不超过1，即max（abs（x1-x2），abs（y2-y1））==1
//     CV_CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标，例如一个矩形轮廓只需4个点来保存轮廓信息
//     CV_CHAIN_APPROX_TC89_L1，CV_CHAIN_APPROX_TC89_KCOS使用teh-Chinl chain 近似算法
//     offset表示代表轮廓点的偏移量，可以设置为任意值。对ROI图像中找出的轮廓，并要在整个图像中进行分析时，这个参数还是很有用的。
    
    cv::findContours(thresholdImg, allContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
    
    // 最终保存轮廓的结构，清空上一次保存的结果
    contours.clear();
    // 提炼上一步得到的轮廓，只有当轮廓面积大于一定阈值时才有保存的价值
    for (size_t i=0; i<allContours.size(); i++)
    {
        int contourSize = allContours[i].size();
        if (contourSize > minContourPointsAllowed)// 大于图像宽度的五分之一
        {
            contours.push_back(allContours[i]);
        }
    }

#ifdef SHOW_DEBUG_IMAGES
    {
        cv::Mat contoursImage(thresholdImg.size(), CV_8UC1);
        contoursImage = cv::Scalar(0);
        cv::drawContours(contoursImage, contours, -1, cv::Scalar(255), 2, CV_AA);
        cv::showAndSave("Contours", contoursImage);
    }
#endif
}

void MarkerDetector::findCandidates
(
    const ContoursVector& contours, 
    std::vector<Marker>& detectedMarkers
) 
{
    std::vector<cv::Point>  approxCurve;
    std::vector<Marker>     possibleMarkers;

    // For each contour, analyze if it is a parallelepiped likely to be the marker
    for (size_t i=0; i<contours.size(); i++)
    {
        // 判断是否是多边形的误差限 
        double eps = contours[i].size() * 0.05;
        
        // 对轮廓曲线进行平滑操作，得到一个在误差限定下的近似多边形
        cv::approxPolyDP(contours[i], approxCurve, eps, true);

        // 仅仅考虑四边形
        if (approxCurve.size() != 4)
            continue;

        // 而且多边形必须是凸面的
        if (!cv::isContourConvex(approxCurve))
            continue;

        // 确保相邻两点之间的距离足够大：大到是一条边，而不是短线段
        float minDist = std::numeric_limits<float>::max();

        for (int i = 0; i < 4; i++)
        {
            cv::Point side = approxCurve[i] - approxCurve[(i+1)%4]; // Point(dx, dy)
            float squaredSideLength = side.dot(side);               // dx*dx+dy*dy
            minDist = std::min(minDist, squaredSideLength);
        }

        if (minDist < m_minContourLengthAllowed) // 100
            continue;

        // 通过上述检查之后，就保存候选的标记:
        Marker m;

        for (int i = 0; i<4; i++)
            m.points.push_back( cv::Point2f(approxCurve[i].x, approxCurve[i].y) );

        // 调整四个点的方向，确保它们是呈逆时针方向的
        // 将第一点分别和第二点和第三点连接成直线
        // 如果第三个点在右侧，那么这些点就是默认的逆时针方向
        cv::Point v1 = m.points[1] - m.points[0];
        cv::Point v2 = m.points[2] - m.points[0];

        // (-1)*(v1.y/v1.x)-(-1)*(v2.y/v2.x)：根据直线的斜率大小，来判断第三个点的位置
        double o = (v1.x * v2.y) - (v1.y * v2.x); 

        if (o < 0.0)
            //如果第三个点在左侧，那么就交换第二个点和第四个点的位置，来调整它们成逆时针方向
            std::swap(m.points[1], m.points[3]);

        possibleMarkers.push_back(m);
    }


    // 检测两个marker是否互相过于接近
    std::vector< std::pair<int,int> > tooNearCandidates;
    for (size_t i=0;i<possibleMarkers.size();i++)
    { 
        const Marker& m1 = possibleMarkers[i];

        // 计算本标记到其他标记最近角点的平均距离
        // calculate the average distance of each corner to the nearest corner of the other marker candidate
        for (size_t j=i+1;j<possibleMarkers.size();j++)
        {
            const Marker& m2 = possibleMarkers[j];

            float distSquared = 0;

            for (int c = 0; c < 4; c++)
            {
                cv::Point v = m1.points[c] - m2.points[c];
                distSquared += v.dot(v);
            }
            // 取相应四个角点距离平方和的平均值
            distSquared /= 4;
            
            // 如果距离太近，则把它们一起加入移除队列，以做进一步的检查（检查其周长大小）
            if (distSquared < 100)
            {
                tooNearCandidates.push_back(std::pair<int,int>(i,j));
            }
        }               
    }

    // 标记需要移除的周长较小的标记 
    std::vector<bool> removalMask (possibleMarkers.size(), false);

    for (size_t i=0; i<tooNearCandidates.size(); i++)
    {
        float p1 = perimeter(possibleMarkers[tooNearCandidates[i].first ].points);
        float p2 = perimeter(possibleMarkers[tooNearCandidates[i].second].points);

        size_t removalIndex;
        if (p1 > p2)
            removalIndex = tooNearCandidates[i].second;
        else
            removalIndex = tooNearCandidates[i].first;

        removalMask[removalIndex] = true;
    }

    // 返回经过提炼的候选标记队列
    detectedMarkers.clear();
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        if (!removalMask[i])
            detectedMarkers.push_back(possibleMarkers[i]);
    }
}

void MarkerDetector::recognizeMarkers(const cv::Mat& grayscale, std::vector<Marker>& detectedMarkers)
{
    std::vector<Marker> goodMarkers;

    // Identify the markers
    for (size_t i=0;i<detectedMarkers.size();i++)
    {
        Marker& marker = detectedMarkers[i];

        // 通过变换的角点坐标，计算得到透视矩阵
        cv::Mat markerTransform = cv::getPerspectiveTransform(marker.points, m_markerCorners2d);

        // 通过透视变换将检测到的标记转换成正视图矩形
        cv::warpPerspective(grayscale, canonicalMarkerImage,  markerTransform, markerSize);

#ifdef SHOW_DEBUG_IMAGES
        {
            cv::Mat markerImage = grayscale.clone();
            marker.drawContour(markerImage);
            cv::Mat markerSubImage = markerImage(cv::boundingRect(marker.points));

            cv::showAndSave("Source marker" + ToString(i),           markerSubImage);
            cv::showAndSave("Marker " + ToString(i) + " after warp", canonicalMarkerImage);
        }
#endif

        int nRotations;
        // 检测候选的标记是哪一种旋转的标记，返回值是id
        int id = Marker::getMarkerId(canonicalMarkerImage, nRotations);
        if (id !=- 1)
        {
            marker.id = id;
            // 根据相机的旋转对标记的四个点进行排序（旋转），这样它们就总保持一个顺序，与相机的方向无关了
            std::rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations, marker.points.end());

            goodMarkers.push_back(marker);
        }
    }  

    // 通过亚像素精度来提取更精确的标记角点
    if (goodMarkers.size() > 0)
    {
        std::vector<cv::Point2f> preciseCorners(4 * goodMarkers.size());

        for (size_t i=0; i<goodMarkers.size(); i++)
        {  
            const Marker& marker = goodMarkers[i];      

            for (int c = 0; c <4; c++)
            {
                preciseCorners[i*4 + c] = marker.points[c];
            }
        }
        
        // 类型
        /*
          CV_TERMCRIT_ITER 用最大迭代次数作为终止条件
          CV_TERMCRIT_EPS 用精度作为迭代条件
          CV_TERMCRIT_ITER+CV_TERMCRIT_EPS 用最大迭代次数或者精度作为迭代条件，决定于哪个条件先满足
         */
        // 迭代的最大次数
        // 特定的阈值
        cv::TermCriteria termCriteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01);
        // 输入图像
        // 输入的角点，也作为输出更精确的角点
        // 领域的大小
        // Sobel算子的大小
        // 像素迭代（扩张）的方法
        cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), termCriteria);

        // 拷贝并保存精确的标记角点 
        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            Marker& marker = goodMarkers[i];      

            for (int c=0;c<4;c++) 
            {
                marker.points[c] = preciseCorners[i*4 + c];
            }      
        }
    }

#if SHOW_DEBUG_IMAGES
    {
        cv::Mat markerCornersMat(grayscale.size(), grayscale.type());
        markerCornersMat = cv::Scalar(0);

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            goodMarkers[i].drawContour(markerCornersMat, cv::Scalar(255));    
        }

        cv::showAndSave("Markers refined edges", grayscale * 0.5 + markerCornersMat);
    }
#endif

    detectedMarkers = goodMarkers;
}

// 标记的姿态估计
void MarkerDetector::estimatePosition(std::vector<Marker>& detectedMarkers)
{
    for (size_t i=0; i<detectedMarkers.size(); i++)
    {                   
        Marker& m = detectedMarkers[i];

        cv::Mat Rvec;
        cv::Mat_<float> Tvec;
        cv::Mat raux,taux;// 把点从模型坐标系转到相机坐标系下的旋转向量、平移向量：保存欧几里得变换的结果
        // 根据笛卡尔坐标系的3D坐标和标记的2D角点坐标，以及相机的内参数和畸变参数，求取相机相对于标记的欧几里得变换（刚体变换）
        cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff,raux,taux);
        raux.convertTo(Rvec,CV_32F);
        taux.convertTo(Tvec ,CV_32F);

        cv::Mat_<float> rotMat(3,3);
        cv::Rodrigues(Rvec, rotMat);// 将旋转向量转换成旋转矩阵

        // Copy to transformation matrix
        for (int col=0; col<3; col++)
        {
            for (int row=0; row<3; row++)
            {        
                m.transformation.r().mat[row][col] = rotMat(row,col); // Copy rotation component
            }
            m.transformation.t().data[col] = Tvec(col); // Copy translation component
        }

        // 之前求取的是相机相对于标记的欧几里得变换（刚体变换），可是结果我们是要求标记相对于相机的变换，所以仅需要对该变换求逆即可
        m.transformation = m.transformation.getInverted();
    }
}