Aruco

Cmakelist.txt

cmake_minimum_required(VERSION 2.6)project(artag2)find_package(OpenCV REQUIRED )include_directories(${OpenCV_INCLUDE_DIR})link_directories(${OpenCV_LIBRARY_DIR})add_executable(artag2 main.cpp Marker.cpp)target_link_libraries( artag2 ${OpenCV_LIBRARIES} )install(TARGETS artag2 RUNTIME DESTINATION bin)

Marker.hpp

#include "Marker.hpp"using namespace cv;using namespace std;Marker::Marker()     :id(-1) {}bool operator<(const Marker& M1,const Marker& M2) {  return M1.id<M2.id;}Mat Marker::rotate(Mat in)//就是把矩阵旋转90度  {  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);//at<uchar>用来指定某个位置的像素，同时指定数据类型。就是交换元素，怎么交换的？        }    }  return out;}//在信息论中，两个等长字符串之间的汉明距离是两个字符串对应位置的字符不同的个数。换句话说，它就是将一个字符串变换成另外一个字符串所需要替换的字符个数。int Marker::hammDistMarker(Mat bits)//对每个可能的标记方向找到海明距离，和参考标识一致的为0,其他旋转形式的标记不为0,因为经过透射变换后，只能得到四个方向的标记，则旋转四次，找到和参考标识一致的方向。 {  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;//每个元素的海明距离      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;            }          if(minSum>sum)            minSum=sum;        }      dist += minSum;    }  return dist;}int Marker::mat2id(const Mat& bits)//移位，求或，再移位，得到最终的ID {  int val=0;  for(int y=0;y<5;y++)     {      val<<=1;//移位操作      if(bits.at<uchar>(y,1)) val |= 1;      val<<=1;      if(bits.at<uchar>(y,3)) val |= 1;    }  return val;}int Marker::getMarkerId(Mat& markerImage,int &nRotations) {  assert(markerImage.rows == markerImage.cols);//如果它的条件返回错误，则终止程序执行  assert(markerImage.type() == CV_8UC1);  Mat grey = markerImage;  //Threshold image使用Otsu算法移除灰色的像素，只留下黑色和白色像素。  //这是固定阀值方法    //输入图像image必须为一个2值单通道图像    //检测的轮廓数组，每一个轮廓用一个point类型的vector表示    //阀值    //max_value 使用 CV_THRESH_BINARY 和 CV_THRESH_BINARY_INV 的最大值    //type   threshold(grey,grey,125,255,THRESH_BINARY | THRESH_OTSU);//对候选标记区域的灰度图使用大律OSTU算法，求取二值化图，大范围图片用这个算法会影响性能。#ifdef SHOW_DEBUG_IMAGES  imshow("Binary marker",grey);  imwrite("Binary marker" + ".png",grey);   #endif  //所使用的标记都有一个内部的5x5编码，采用的是简单修改的汉明码。简单的说，就是5bits中只有2bits被使用，其他三位都是错误的识别码，也就是说我们至多有1024种不同的标识。我们的汉明码最大的不同是，汉明码的第一位（奇偶校验位的3和5)是反向的。所有ID 0(在汉明码是00000)，在这里是10000，目的是减少环境造成的影响.  //标识被划分为7x7的网格，内部的5x5表示标识内容，额外的是黑色边界，接下来是逐个检查四条边的像素是否都是黑色的，若有不是黑色，那么就不是标识。  int cellSize = markerImage.rows/7;  for(int y=0;y<7;y++)      {      int inc = 6;      if(y == 0 || y == 6) inc=1;//对第一行和最后一行，检查整个边界      for(int x=0;x<7;x+=inc)          {          int cellX = x*cellSize;          int cellY = y*cellSize;          Mat cell = grey(Rect(cellX,cellY,cellSize,cellSize));          int nZ = countNonZero(cell);//统计区域内非0的个数。          if(nZ > (cellSize*cellSize)/2)              {              return -1;//如果边界信息不是黑色的，就不是一个标识。            }        }    }  Mat bitMatrix = Mat::zeros(5,5,CV_8UC1);  //得到信息（对于内部的网格，决定是否是黑色或白色的）就是判断内部5x5的网格都是什么颜色的，得到一个包含信息的矩阵bitMatrix。  for(int y=0;y<5;y++)      {      for(int x=0;x<5;x++)          {          int cellX = (x+1)*cellSize;          int cellY = (y+1)*cellSize;          Mat cell = grey(Rect(cellX,cellY,cellSize,cellSize));          int nZ = countNonZero(cell);          if(nZ > (cellSize*cellSize)/2)            bitMatrix.at<uchar>(y,x) = 1;        }    }  //检查所有的旋转  Mat rotations[4];  int distances[4];  rotations[0] = bitMatrix;  distances[0] = hammDistMarker(rotations[0]);//求没有旋转的矩阵的海明距离。  pair<int,int> minDist(distances[0],0);//把求得的海明距离和旋转角度作为最小初始值对，每个pair都有两个属性值first和second  for(int i=1;i<4;i++)//就是判断这个矩阵与参考矩阵旋转多少度。      {      //计算最近的可能元素的海明距离      rotations[i] = rotate(rotations[i-1]);//每次旋转90度      distances[i] = hammDistMarker(rotations[i]);      if(distances[i] < minDist.first)          {          minDist.first = distances[i];          minDist.second = i;//这个pair的第二个值是代表旋转几次，每次90度。        }    }  nRotations = minDist.second;//这个是将返回的旋转角度值  if(minDist.first == 0)//若海明距离为0,则根据这个旋转后的矩阵计算标识ID      {      return mat2id(rotations[minDist.second]);    }  return -1;}void Marker::drawContour(Mat& image,Scalar color) const//在图像上画线，描绘出轮廓。  {  float thickness = 2;  line(image,points[0],points[1],color,thickness,CV_AA);  line(image,points[1],points[2],color,thickness,CV_AA);  line(image,points[2],points[3],color,thickness,CV_AA);//thickness线宽  line(image,points[3],points[0],color,thickness,CV_AA);//CV_AA是抗锯齿}

main.cpp

#include <opencv2/core.hpp>#include <opencv2/highgui/highgui.hpp>#include <opencv2/contrib/contrib.hpp>#include <opencv2/imgproc/imgproc.hpp>#include <iostream>#include "Marker.hpp"using namespace cv;using namespace std;void Aruco(Mat src,vector<Marker>& detectedMarkers);float perimeter(const vector<Point2f> &a);void M();vector<Point3f> m_markerCorners3d;vector<Point2f> m_markerCorners2d;Size markerSize(100,100);void M(){  //markerSize(100,100);   m_markerCorners2d.push_back(Point2f(0,0));   m_markerCorners2d.push_back(Point2f(markerSize.width-1,0));   m_markerCorners2d.push_back(Point2f(markerSize.width-1,markerSize.height-1));   m_markerCorners2d.push_back(Point2f(0,markerSize.height-1));}int main(int argv,char** argc){  VideoCapture cap(0);  Mat src;  vector<Marker> markers;  //MarkerDetector markerDetector;  M();  cout<<m_markerCorners2d.size()<<endl;  while(true)  {    cap>>src;    imshow("src",src);    waitKey(10);    Aruco(src,markers);  }  return 0;}void Aruco(Mat src,vector<Marker>& detectedMarkers){  Mat grayscale;  cvtColor(src,grayscale,CV_BGR2GRAY);  Mat thresholdImg;  adaptiveThreshold(grayscale,//Input Image                    thresholdImg,//Result binary image                    255,                    ADAPTIVE_THRESH_GAUSSIAN_C,                    THRESH_BINARY_INV,                    7,                    7                    );  vector<vector<Point> > allContours;  findContours(thresholdImg, allContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);  vector<vector<Point> > contours;  for(size_t i=0;i<allContours.size();i++)    {      int contourSize = allContours[i].size();      if(contourSize > 100)   //minContourPointsAllowed        {          contours.push_back(allContours[i]);        }    }        vector<Point> approxCurve;//返回结果为多边形，用点集表示//相似形状    vector<Marker> possibleMarkers;//可能的标记        for(size_t i=0;i<contours.size();i++)    {      /*近似一个多边形逼近，为了减少轮廓的像素。这样比较好，可筛选出非标记区域，因为标记总能被四个顶点的多边形表示。如果多边形的顶点多于或少于四个，就绝对不是本项目想要的标记。通过点集近似多边形，第三个参数为epsilon代表近似程度，即原始轮廓及近似多边形之间的距离，第四个参数表示多边形是闭合的。*/      double eps = contours[i].size()*0.05;      //输入图像的2维点集，输出结果，估计精度，是否闭合。输出多边形的顶点组成的点集//使多边形边缘平滑，得到近似的多边形       approxPolyDP(contours[i],approxCurve,eps,true);      //我们感兴趣的多边形只有四个顶点      if(approxCurve.size() != 4)        continue;      //检查轮廓是否是凸边形      if(!isContourConvex(approxCurve))        continue;      //确保连续点之间的距离是足够大的。//确保相邻的两点间的距离“足够大”－大到是一条边而不是短线段就是了      //float minDist = numeric_limits<float>::max();//代表float可以表示的最大值，numeric_limits就是模板类，这里表示max（float）;3.4e038      float minDist = 1e10;//这个值就很大了      //求当前四边形各顶点之间的最短距离      for(int i=0;i<4;i++)        {          Point side = approxCurve[i] - approxCurve[(i+1)%4];//这里应该是2维的相减          float squaredSideLength = side.dot(side);//求2维向量的点积，就是XxY          minDist = min(minDist,squaredSideLength);//找出最小的距离        }      //检查距离是不是特别小，小的话就退出本次循环，开始下一次循环      if(minDist<100)//m_minContourLengthAllowed        continue;      //所有的测试通过了，保存标识候选，当四边形大小合适，则将该四边形maker放入possibleMarkers容器内 //保存相似的标记         Marker m;      for(int i=0;i<4;i++)        m.points.push_back(Point2f(approxCurve[i].x,approxCurve[i].y));//vector头文件里面就有这个push_back函数，在vector类中作用为在vector尾部加入一个数据。      /*逆时针保存这些点      //从代码推测，marker中的点集本来就两种序列：顺时针和逆时针，这里要把顺时针的序列改成逆时针，在多边形逼近时，多边形是闭合的，则不是顺时针就是逆时针      //在第一个和第二个点之间跟踪出一条线，如果第三个点在右边，则点是逆时针保存的//逆时针排列这些点,第一个点和第二个点之间连一条线,如果第三个点在边，那么这些点就是逆时针*/      Point v1 = m.points[1] - m.points[0];      Point v2 = m.points[2] - m.points[0];      /*行列式的几何意义是什么呢？有两个解释：一个解释是行列式就是行列式中的行或列向量所构成的超平行多面体的有向面积或有向体积；另一个解释是矩阵A的行列式detA就是线性变换A下的图形面积或体积的伸缩因子。      //以行向量a=(a1,a2)，b=(b1,b2)为邻边的平行四边形的有向面积：若这个平行四边形是由向量沿逆时针方向转到b而得到的，面积取正值；若这个平行四边形是由向量a沿顺时针方向转到而得到的，面积取负值； */      double o = (v1.x * v2.y) - (v1.y * v2.x);      if(o<0.0) //如果第三个点在左边，那么交换第一个点和第三个点，逆时针保存        swap(m.points[1],m.points[3]);      possibleMarkers.push_back(m);//把这个标识放入候选标识向量中    }    vector< pair<int,int> > tooNearCandidates;  for(size_t i=0;i<possibleMarkers.size();i++)    {      const Marker& m1 = possibleMarkers[i];      //计算两个maker四边形之间的距离，四组点之间距离和的平均值，若平均值较小，则认为两个maker很相近,把这一对四边形放入移除队列。//计算每个边角到其他可能标记的最近边角的平均距离      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++)            {              Point v = m1.points[c] - m2.points[c];              //向量的点乘－》两点的距离              distSquared += v.dot(v);            }          distSquared /= 4;          if(distSquared < 100)            {              tooNearCandidates.push_back(pair<int,int>(i,j));            }        }    }    vector<bool> removalMask(possibleMarkers.size(),false);//创建Vector对象，并设置容量。第一个参数是容量，第二个是元素。  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]);    }            Mat canonicalMarkerImage;  char name[20] = "";  vector<Marker> goodMarkers;        for(size_t i=0;i<detectedMarkers.size();i++)    {      Marker& marker = detectedMarkers[i];      //找到透视转换矩阵，获得矩形区域的正面视图// 找到透视投影，并把标记转换成矩形，输入图像四边形顶点坐标，输出图像的相应的四边形顶点坐标       // Find the perspective transformation that brings current marker to rectangular form      Mat markerTransform = getPerspectiveTransform(marker.points,m_markerCorners2d);//输入原始图像和变换之后的图像的对应4个点，便可以得到变换矩阵      cout<<"ssssssssssssss"<<endl;      /* Transform image to get a canonical marker image      // Transform image to get a canonical marker image        //输入的图像        //输出的图像        //3x3变换矩阵 */            warpPerspective(grayscale,canonicalMarkerImage,markerTransform,markerSize);//对图像进行透视变换,这就得到和标识图像一致正面的图像，方向可能不同，看四个点如何排列的了。感觉这个变换后，就得到只有标识图的正面图      // sprintf(name,"warp_%d.jpg",i);      // imwrite(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      int nRotations;      int id = Marker::getMarkerId(canonicalMarkerImage,nRotations);      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);        }    }          if(goodMarkers.size() > 0)    {      //找到所有标记的角点       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_TERMCRIT_ITER 用最大迭代次数作为终止条件          CV_TERMCRIT_EPS 用精度作为迭代条件          CV_TERMCRIT_ITER+CV_TERMCRIT_EPS 用最大迭代次数或者精度作为迭代条件，决定于哪个条件先满足        //迭代的最大次数        //特定的阀值 */      TermCriteria termCriteria = TermCriteria(TermCriteria::MAX_ITER | TermCriteria::EPS,30,0.01);//这个是迭代终止条件，这里是达到30次迭代或者达到0.01精度终止。角点精准化迭代过程的终止条件      /*输入图像        //输入的角点，也作为输出更精确的角点        //接近的大小（Neighborhood size）        //Aperture parameter for the Sobel() operator        //像素迭代（扩张）的方法 */      cornerSubPix(grayscale,preciseCorners,cvSize(5,5),cvSize(-1,-1),termCriteria);//发现亚像素精度的角点位置，第二个参数代表输入的角点的初始位置并输出精准化的坐标。在标记检测的早期的阶段没有使用cornerSubPix函数是因为它的复杂性－调用这个函数处理大量顶点时会耗费大量的处理时间，因此我们只在处理有效标记时使用。      //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;              }        }    }  //画出细化后的矩形图片   Mat markerCornersMat(grayscale.size(),grayscale.type());    markerCornersMat = Scalar(0);    for(size_t i=0;i<goodMarkers.size();i++)    {      goodMarkers[i].drawContour(markerCornersMat,Scalar(255));    }    imshow("Markers refined edges",grayscale*0.5 + markerCornersMat);    //imwrite("Markers refined edges" + ".png",grayscale*0.5 + markerCornersMat);       imwrite("refine.jpg",grayscale*0.5 + markerCornersMat);  detectedMarkers = goodMarkers;    }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;}