/****************************************************************************** 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(); }}
