基于标记的ID检测opencv实现代码

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/nonfree/features2d.hpp>
#include <opencv2/legacy/legacy.hpp>
#include<vector>
//#include "MarkerDetector.h"
//#include "Marker.h"
using namespace cv;
using namespace std;


typedef std::vector<cv::Point>    PointsVector;
typedef std::vector<PointsVector> ContoursVector;






class Marker  //用到的标记类
{  


public:
  //Marker();
  
  friend bool operator<(const Marker &M1,const Marker&M2);
  friend std::ostream & operator<<(std::ostream &str,const Marker &M);


  static cv::Mat rotate(cv::Mat  in);
  static int hammDistMarker(cv::Mat bits);
  static int mat2id(const cv::Mat &bits);
  //static int getMarkerId(cv::Mat &in,int &nRotations);
  //static int Marker::getMarkerId(cv::Mat &in,int &nRotations);
  static int getMarkerId(cv::Mat &markerImage,int &nRotations);
public:
  
  // Id of  the marker
  int id;
  
  // Marker transformation with regards to the camera
  //Transformation transformation;
  
  std::vector<cv::Point2f> points;


  // Helper function to draw the marker contour over the image
  void drawContour(cv::Mat& image, cv::Scalar color = CV_RGB(0,250,0)) const;
};


//标记检测类


class MarkerDetector
{
public:
  //typedef std::vector<cv::Point>    PointsVector;
  //typedef std::vector<PointsVector> ContoursVector;




  /**
   * Initialize a new instance of marker detector object
   * @calibration[in] - Camera calibration (intrinsic and distortion components) necessary for pose estimation.
   */
  //MarkerDetector(CameraCalibration calibration);
  
  //! Searches for markes and fills the list of transformation for found markers
  //void processFrame(const BGRAVideoFrame& frame);
  
  //const std::vector<Transformation>& getTransformations() const;
  


public:
  //! Main marker detection routine
 // bool findMarkers(const BGRAVideoFrame& frame, std::vector<Marker>& detectedMarkers);


  //! Converts image to grayscale
  void prepareImage(const cv::Mat& bgraMat, cv::Mat& grayscale) const;


  //! Performs binary threshold
  void performThreshold(const cv::Mat& grayscale, cv::Mat& thresholdImg) const;


  //! Detects appropriate contours
  void findContours(cv::Mat& thresholdImg, ContoursVector& contours, int minContourPointsAllowed) const;


  //! Finds marker candidates among all contours
  void findCandidates(const ContoursVector& contours, std::vector<Marker>& detectedMarkers);
  




  //! Tries to recognize markers by detecting marker code 
  void recognizeMarkers(const cv::Mat& grayscale, std::vector<Marker>& detectedMarkers);


  //! Calculates marker poses in 3D
  void estimatePosition(std::vector<Marker>& detectedMarkers);


private:
  float m_minContourLengthAllowed;
  
  cv::Size markerSize;
  cv::Mat camMatrix;
  cv::Mat distCoeff;
  //std::vector<Transformation> m_transformations;
  
  cv::Mat m_grayscaleImage;
  cv::Mat m_thresholdImg;  
  cv::Mat canonicalMarkerImage;


  ContoursVector           m_contours;
  std::vector<cv::Point3f> m_markerCorners3d;
  std::vector<cv::Point2f> m_markerCorners2d;
};














//1.彩色图片转成灰度图
//void prepareImage(const Mat& bgraMat, Mat& grayscale)
//{
// // Convert to grayscale
// cvtColor(bgraMat, grayscale, CV_BGRA2GRAY);
//}
//
2.灰度图二值化
//void performThreshold(const Mat& grayscale, Mat& thresholdImg)
//{
// // Adaptive Threshold,use of all pixels in given radius around the examined pixel
// adaptiveThreshold(grayscale, thresholdImg, 255, ADAPTIVE_THRESH_GAUSSIAN_C,THRESH_BINARY_INV, 7, 7);
//}
//
//
//3.轮廓检测
void findContours(const Mat& thresholdImg, std::vector<std::vector<Point>>& contours, int minContourPointsAllowed)
{
std::vector< std::vector<Point> > allContours;
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]);
}
}
}
//4 计算周长
float perimeter(const vector<Point2f> &a)//求多边形周长。
{
  float sum=0,dx,dy;
  for(size_t i=0;i<a.size();i++)
    {
      size_t i2=(i+1) % a.size();


      dx = a[i].x - a[i2].x;
      dy = a[i].y - a[i2].y;


      sum += sqrt(dx*dx + dy*dy);
    }


  return sum;
}


//5到目前位置，我们从一张彩色图片上抠出多个四边形，并且四个顶点按照逆时针排序。 Find closed contours that can be approximated with 4 points


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++)
    {
        // Approximate to a polygon
        double eps = contours[i].size() * 0.05;
        cv::approxPolyDP(contours[i], approxCurve, eps, true);


        // We interested only in polygons that contains only four points
        if (approxCurve.size() != 4)
            continue;


        // And they have to be convex
        if (!cv::isContourConvex(approxCurve))
            continue;


        // Ensure that the distance between consecutive points is large enough
        float minDist = std::numeric_limits<float>::max();


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


        // Check that distance is not very small
        if (minDist < m_minContourLengthAllowed)
            continue;


        // All tests are passed. Save marker candidate:
        Marker m;


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


        // Sort the points in anti-clockwise order
        // Trace a line between the first and second point.
        // If the third point is at the right side, then the points are anti-clockwise
        cv::Point v1 = m.points[1] - m.points[0];
        cv::Point v2 = m.points[2] - m.points[0];


        double o = (v1.x * v2.y) - (v1.y * v2.x);


        if (o < 0.0) //if the third point is in the left side, then sort in anti-clockwise order
            std::swap(m.points[1], m.points[3]);


        possibleMarkers.push_back(m);
    }




    // Remove these elements which corners are too close to each other.  
    // First detect candidates for removal:
    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));
            }
        }
    }


    // Mark for removal the element of the pair with smaller perimeter
    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;
    }


    // Return candidates
    detectedMarkers.clear();
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        if (!removalMask[i])
            detectedMarkers.push_back(possibleMarkers[i]);
    }
}




//7海明码的求取
int Marker::hammDistMarker(cv::Mat bits)
{
  //该矩阵产生：每条stripe有4种可能的数据
  int ids[4][5]=
  {
    {1,0,0,0,0},
    {1,0,1,1,1},
    {0,1,0,0,1},
    {0,1,1,1,0}
  };
  
  int dist=0;
  
  for (int y=0;y<5;y++)
  {
    int minSum=1e5; //hamming distance to each possible word
    
    for (int p=0;p<4;p++)
    {
      int sum=0;
      //now, count
      for (int x=0;x<5;x++)
      {
        sum += bits.at<uchar>(y,x) == ids[p][x] ? 0 : 1;  //拿bitMatrix中每一行同ids中的行依次比较，寻找ids中最贴近bitMatrix第y行的一行ids
      }
      
      if (minSum>sum)
        minSum=sum;
    }
    
    //do the and
    dist += minSum;
  }
 
  return dist;
}






//8
int Marker::mat2id(const cv::Mat &bits)
{
  int val=0;
  for (int y=0;y<5;y++)
  {
    val<<=1;
    if ( bits.at<uchar>(y,1)) val|=1;//每次加10在末尾
    val<<=1;
    if ( bits.at<uchar>(y,3)) val|=1;//每次在末尾加01两位
  }
  return val;
}


//9
cv::Mat Marker::rotate(cv::Mat in)
{
  cv::Mat out;
  in.copyTo(out);
  for (int i=0;i<in.rows;i++)
  {
    for (int j=0;j<in.cols;j++)
    {
      out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
    }
  }
  return out;
}








//10
int Marker::getMarkerId(cv::Mat &markerImage,int &nRotations)

{
  assert(markerImage.rows == markerImage.cols);
  assert(markerImage.type() == CV_8UC1);
  
  cv::Mat grey = markerImage;
  //threshold image,参数threshold:阈值125 max_value:255，使用 CV_THRESH_BINARY 和 CV_THRESH_BINARY_INV 的最大值
  cv::threshold(grey, grey, 125, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
    
  //Markers  are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
  //the external border should be entirely black
  
  int cellSize = markerImage.rows / 7;
  
  //检查四周边缘
  for (int y=0;y<7;y++)  //y:row，x:col
  {
    int inc=6;
    
    if (y==0 || y==6) inc=1; //for first and last row, check the whole border
    
//第2,3,4,5,6行，只检查左右两列，第1,7行检查所有列
    for (int x=0;x<7;x+=inc)
    {
      int cellX = x * cellSize;
      int cellY = y * cellSize;
      cv::Mat cell = grey(cv::Rect(cellX,cellY,cellSize,cellSize));
      
      int nZ = cv::countNonZero(cell);


      if (nZ > (cellSize*cellSize) / 2)
      {
        return -1;//can not be a marker because the border element is not black!
      }
    }
  }
  
  cv::Mat bitMatrix = cv::Mat::zeros(5,5,CV_8UC1);
  
  //get information(for each inner square, determine if it is  black or white)  
  for (int y=0;y<5;y++)
  {
    for (int x=0;x<5;x++)
    {
      int cellX = (x+1)*cellSize;
      int cellY = (y+1)*cellSize;
      cv::Mat cell = grey(cv::Rect(cellX,cellY,cellSize,cellSize));
      
      int nZ = cv::countNonZero(cell);
      if (nZ> (cellSize*cellSize) /2) 
        bitMatrix.at<uchar>(y,x) = 1;
    }
  }
  
  //check all possible rotations
  cv::Mat rotations[4];
  int distances[4];
  
  rotations[0] = bitMatrix;  
  distances[0] = hammDistMarker(rotations[0]);
  
  std::pair<int,int> minDist(distances[0],0);
  
  for (int i=1; i<4; i++)
  {
    //get the hamming distance to the nearest possible word
    rotations[i] = rotate(rotations[i-1]);
    distances[i] = hammDistMarker(rotations[i]);
    
    if (distances[i] < minDist.first)
    {
      minDist.first  = distances[i];
      minDist.second = i;
    }
  }
  
  nRotations = minDist.second;


  if (minDist.first == 0)
  {
    return mat2id(rotations[minDist.second]);
  }
  
  return -1;
}


//6/*detectedMarkers为出参*/
void detectMarkers(const Mat& grayscale, vector<Marker>& detectedMarkers)
{
Mat canonicalMarker;
char name[20]="";


vector<Marker> goodMarkers;
vector<Point2f> corners(4);
corners[0] = Point2f(0,0);
corners[1] = Point2f (199,0);
corners[3] = Point2f(0,199);
corners[2] = Point2f(199,199);
// Identify the markers
for (size_t i=0;i<detectedMarkers.size();i++)
{
Marker& marker = detectedMarkers[i];
// Find the perspective transfomation that brings current marker to rectangular form
Mat M = getPerspectiveTransform(marker.points, corners);


// Transform image to get a canonical marker image
warpPerspective(grayscale, canonicalMarker,  M, Size(200,200));
sprintf(name,"warp_%d.jpg",i);
imwrite(name, canonicalMarker);


int nRotations;
int id = Marker::getMarkerId(canonicalMarker,nRotations);//5*5 mask,3 bits校验,2 bits 数据,每个stripe有4种数据,共有5条stripe,故有4^5个ID,白块为1，黑块为0
cout<<"ID:"<<id<<endl;
if (id !=- 1)
{
marker.id = id;
//sort the points so that they are always in the same order no matter the camera orientation
//Rotates the order of the elements in the range [first,last), in such a way that the element pointed by middle becomes the new first element.
rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations, marker.points.end());


goodMarkers.push_back(marker);
}

}


//refine using subpixel accuracy the corners
if (goodMarkers.size() > 0)
{
std::vector<Point2f> preciseCorners(4 * goodMarkers.size());//每个marker四个点


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


for (int c=0;c<4;c++)
{
preciseCorners[i*4+c] = marker.points[c]; //i表示第几个marker，c表示某个marker的第几个点
}
}


//Refines the corner locations.The function iterates to find the sub-pixel accurate location of corners or radial saddle points
cv::TermCriteria termCriteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01);
cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), termCriteria);


//copy back
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];
//cout<<"X:"<<marker.points[c].x<<"Y:"<<marker.points[c].y<<endl;
}      
}
}


画出细化后的矩形图片
/*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));    
}


imwrite("refine.jpg",markerCornersMat);*/


detectedMarkers = goodMarkers;

}


//6
//void MarkerDetector::recognizeMarkers(const Mat& grayscale,vector<Marker>& detectedMarkers)
//{  Mat stand=imread("marker1.png");
//   int img_width = stand.cols;
//   int img_height = stand.rows;
//  Mat canonicalMarkerImage;
//  char name[20] = "";
//
//  vector<Marker> goodMarkers;
//
//  /*Identify the markers识别标识 //分析每一个捕获到的标记，去掉透视投影，得到平面／正面的矩形。
//  //为了得到这些矩形的标记图像，我们不得不使用透视变换去恢复(unwarp)输入的图像。这个矩阵应该使用cv::getPerspectiveTransform函数，它首先根据四个对应的点找到透视变换，第一个参数是标记的坐标，第二个是正方形标记图像的坐标。估算的变换将会把标记转换成方形，从而方便我们分析。 */
//  for(size_t i=0;i<detectedMarkers.size();i++)
//    {
//      Marker& marker = detectedMarkers[i];
//
//   
// vector<Point2f> corners(4);
// corners[0] = Point2f(0,0);
// corners[1] = Point2f (199,0);
// corners[3] = Point2f(0,199);
// corners[2] = Point2f(199,199);
//      //找到透视转换矩阵，获得矩形区域的正面视图// 找到透视投影，并把标记转换成矩形，输入图像四边形顶点坐标，输出图像的相应的四边形顶点坐标 
//      // Find the perspective transformation that brings current marker to rectangular form
//      Mat markerTransform = getPerspectiveTransform(marker.points,corners);//输入原始图像和变换之后的图像的对应4个点，便可以得到变换矩阵
//      /* Transform image to get a canonical marker image
//      // Transform image to get a canonical marker image  
//      //输入的图像  
//      //输出的图像  
//      //3x3变换矩阵 */
//      warpPerspective(grayscale,canonicalMarkerImage,markerTransform,Size(200,200));//对图像进行透视变换,这就得到和标识图像一致正面的图像，方向可能不同，看四个点如何排列的了。感觉这个变换后，就得到只有标识图的正面图
//
//       sprintf(name,"warp_%d.jpg",i);
//       imshow(name,canonicalMarkerImage);
//#ifdef SHOW_DEBUG_IMAGES
//         {
//          Mat markerImage = grayscale.clone();
//          marker.drawContour(markerImage);
//          Mat markerSubImage = markerImage(boundingRect(marker.points));
//
//          imshow("Source marker" + ToString(i),markerSubImage);
//          imwrite("Source marker" + ToString(i) + ".png",markerSubImage);   
//
//          imshow("Marker " + ToString(i),canonicalMarkerImage);
//          imwrite("Marker " + ToString(i) + ".png",canonicalMarkerImage);   
//        }
//#endif
//
//  }
//}


void main()
{
Mat image =imread("marker.png");//彩色图片
Mat gray;
Mat thresholdImg;

 


cvtColor(image,gray,CV_BGR2GRAY);//灰度转化
namedWindow("gray");
imshow("gray",gray);


threshold(gray,thresholdImg,110,255,CV_THRESH_BINARY);//二值化
namedWindow("threshold");
imshow("threshold",thresholdImg);




vector<std::vector<Point>> contours;//用于存储轮廓的向量
findContours( thresholdImg, contours, CV_CHAIN_APPROX_NONE);//提取轮廓 轮廓存储到contour里了
 cout<<contours.size()<<endl;
Mat result (image.size(),CV_8U,Scalar(255));
drawContours(result,contours,-1,0,2);
 imshow("画出提取的轮廓",result);


 
 MarkerDetector markerDetector;
std::vector<Marker> detectedMarkers1;
markerDetector.findCandidates( contours,detectedMarkers1); //上面三句找到候选标记
 
for(int i = 0;i < detectedMarkers1.size();i ++)
{
for(int j = 0;j < detectedMarkers1[i].points.size();j++)
{
circle(gray,detectedMarkers1[i].points[j],1,Scalar(255),2);
}
} 
imshow("temp",gray);
// waitKey();
 


cout<<detectedMarkers1[0].points<<endl;


//MarkerDetector markerturn;
//markerturn.recognizeMarkers(gray,detectedMarkers1);
detectMarkers(gray,  detectedMarkers1);
 
 //vector<Marker> detectedMarkers2;
//const vector<Point2f> m_markerCorners2d;
//detectMarkers(gray,detectedMarkers1, m_markerCorners2d);//这个三句是对标记进行旋转处理
//imshow("mark",detectedMarkers2);
//detectedMarkers2[0].drawContour(cv::Mat& image, cv::Scalar color = CV_RGB(0,250,0))<<endl;


 
//Marker goodid;
//int nRotations;
goodid.getMarkerId(cv::Mat &markerImage,nRotations);
//goodid.getMarkerId(gray,nRotations);

waitKey(0);
}