aruco码通过识别4个角点进行3D定位，所以结合aruco码的特点，在QR码的外围加上一个黑框用于定位，效果还可以，见下图，代码整理后再发出，这个码可以实现与aruco同样的精度，缺点是太远识别不到QR码。
原理：通过黑框来定位，但是黑框4个角点会顺序会随时发生变化，所以这4个点的顺序需要QR码来保证一定的顺序。如何保证呢？检测到4个点之后，会通过仿射变换把4个点的ROI区域映射到垂直状态正方形的ROI，由于上一步对这4个点进行了相关操作使得这里的QR顺序是一一对应的，然后通过zbar检测的QR码的4个角点的来保证映射过来的顺序，通过一顿胡乱操作，很糙的实现了该功能。
zbar解码后4个点的顺序是按照固定顺序排列的
代码附下：
void MarkerDetector::detect_qr ( const cv::Mat &input,vector<Marker> &detectedMarkers,Mat camMatrix ,Mat distCoeff ,float markerSizeMeters ,bool setYPerpendicular) throw ( cv::Exception )
{
//it must be a 3 channel image
if ( input.type() ==CV_8UC3 ) cv::cvtColor ( input,grey,CV_BGR2GRAY );
else grey=input;
//clear input data
detectedMarkers.clear();
cv::Mat imgToBeThresHolded=grey;
double ThresParam1=_thresParam1,ThresParam2=_thresParam2;
//Must the image be downsampled before continue processing?
if ( pyrdown_level!=0 )
{
reduced=grey;
for ( int i=0;i<pyrdown_level;i++ )
{
cv::Mat tmp;
cv::pyrDown ( reduced,tmp );
reduced=tmp;
}
int red_den=pow ( 2.0f,pyrdown_level );
imgToBeThresHolded=reduced;
ThresParam1/=float ( red_den );
ThresParam2/=float ( red_den );
}
///Do threshold the image and detect contours
thresHold ( _thresMethod,imgToBeThresHolded,thres,ThresParam1,ThresParam2 );
//an erosion might be required to detect chessboard like boards
if ( _doErosion )
{
erode ( thres,thres2,cv::Mat() );
thres2=thres;
// cv::bitwise_xor ( thres,thres2,thres );
}
//find all rectangles in the thresholdes image
vector<MarkerCandidate > MarkerCanditates;
detectRectangles_qr ( thres,MarkerCanditates );
//if the image has been downsampled, then calcualte the location of the corners in the original image
if ( pyrdown_level!=0 )
{
float red_den=pow ( 2.0f,pyrdown_level );
float offInc= ( ( pyrdown_level/2. )-0.5 );
for ( unsigned int i=0;i<MarkerCanditates.size();++i ) {
for ( int c=0;c<4;c++ )
{
MarkerCanditates[i][c].x=MarkerCanditates[i][c].x*red_den+offInc;
MarkerCanditates[i][c].y=MarkerCanditates[i][c].y*red_den+offInc;
}
//do the same with the the contour points
for ( size_t c=0;c<MarkerCanditates[i].contour.size();++c )
{
MarkerCanditates[i].contour[c].x=MarkerCanditates[i].contour[c].x*red_den+offInc;
MarkerCanditates[i].contour[c].y=MarkerCanditates[i].contour[c].y*red_den+offInc;
}
}
}
/*
//zbar::Image::SymbolIterator symbol = img.symbol_begin();
if(img.symbol_begin()==img.symbol_end())
{
cout<<"can't detect QR code！"<<endl;
return;
}
vector<Point2f> Marker_zbar;
Marker maker_;
for(zbar::SymbolIterator symbol = img.symbol_begin();symbol != img.symbol_end(); ++symbol)
{
Marker_zbar.push_back(cv::Point2f(symbol->get_location_x(0),symbol->get_location_y(0)));
Marker_zbar.push_back(cv::Point2f(symbol->get_location_x(1),symbol->get_location_y(1)));
Marker_zbar.push_back(cv::Point2f(symbol->get_location_x(3),symbol->get_location_y(3)));
maker_.message = symbol->get_data();
maker_.id = std::atoi(maker_.message.c_str());
break;
}
///sort the markers
for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
{
int index = 0;//Marker_pair[i].second;
int min_distance = std::numeric_limits<int>::max();
int nRotations = 0;
for(unsigned int k=0; k<4; k++)
{
float dist = sqrt((MarkerCanditates[index][k].x - Marker_zbar[i].x) * (MarkerCanditates[index][k].x - Marker_zbar[i].x)
+ (MarkerCanditates[index][k].y - Marker_zbar[i].y) * (MarkerCanditates[index][k].y - Marker_zbar[i].y));
if(dist < min_distance)
{
min_distance = dist;
nRotations = k;
}
}
switch (i) {
case 0:
nRotations = (-nRotations + 2 + 4)%4;
break;
case 1:
nRotations = (-nRotations + 1 + 4)%4;
break;
case 2:
nRotations = (-nRotations + 3 + 4)%4;
break;
default:
return;
break;
}
std::rotate ( MarkerCanditates[index].begin(),MarkerCanditates[index].begin() +4-nRotations,MarkerCanditates[index].end() );
break;
}
*/
/*
cv::Mat display;
cv::cvtColor(grey,display,CV_GRAY2RGB);
for(int i=0; i<MarkerCanditates.size(); i++)
{
int index = i;
cv::circle(display,MarkerCanditates[index][0],3,cv::Scalar(255,0,0),-1); //Blue
cv::circle(display,MarkerCanditates[index][1],3,cv::Scalar(0,255,0),-1); //Green
cv::circle(display,MarkerCanditates[index][2],3,cv::Scalar(0,0,255),-1); //Red
cv::circle(display,MarkerCanditates[index][3],3,cv::Scalar(255,255,0),-1);//
cv::imshow("display",display);
cv::waitKey(1);
}
*/
///identify the markers
/// _candidates.clear();
//_markerWarpSize = 112;
for ( unsigned int i=0;i<MarkerCanditates.size();++i )
{
//Find proyective homography
Mat canonicalMarker;
std::string message;
bool resW=false;
if (_enableCylinderWarp)
resW=warp_cylinder( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
else resW=warp ( grey,canonicalMarker,Size ( _markerWarpSize,_markerWarpSize ),MarkerCanditates[i] );
if (resW) {
int nRotations;
int id= ( *qr_markerIdDetector_ptrfunc ) ( canonicalMarker, message, nRotations );
if ( id!=-1 )
{
if(_cornerMethod==LINES) refineCandidateLines( MarkerCanditates[i] ); // make LINES refinement before lose contour points
detectedMarkers.push_back ( MarkerCanditates[i] );
detectedMarkers.back().id=id;
detectedMarkers.back().message = message;
//sort the points so that they are always in the same order no matter the camera orientation
std::rotate ( detectedMarkers.back().begin(),detectedMarkers.back().begin() +4-nRotations,detectedMarkers.back().end() );
}
else _candidates.push_back ( MarkerCanditates[i] );
}
}
// for(unsigned int i=0; i<MarkerCanditates.size(); i++)
// {
// int index = i;//Marker_pair[i].second;
// Marker maker_;
// maker_.push_back(cv::Point2f(MarkerCanditates[index][0].x, MarkerCanditates[index][0].y));
// maker_.push_back(cv::Point2f(MarkerCanditates[index][1].x, MarkerCanditates[index][1].y));
// maker_.push_back(cv::Point2f(MarkerCanditates[index][2].x, MarkerCanditates[index][2].y));
// maker_.push_back(cv::Point2f(MarkerCanditates[index][3].x, MarkerCanditates[index][3].y));
// detectedMarkers.push_back(maker_);
// }
//_cornerMethod = SUBPIX;
///refine the corner location if desired
if ( detectedMarkers.size() >0 && _cornerMethod!=NONE && _cornerMethod!=LINES )
{
vector<Point2f> Corners;
for ( unsigned int i=0;i<detectedMarkers.size();++i )
for ( int c=0;c<4;c++ )
{
Corners.push_back ( detectedMarkers[i][c] );
}
if ( _cornerMethod==HARRIS )
findBestCornerInRegion_harris ( grey, Corners,7 );
else if ( _cornerMethod==SUBPIX )
cornerSubPix ( grey, Corners,cvSize ( 5,5 ), cvSize ( -1,-1 ) ,cvTermCriteria ( CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,3,0.05 ) );
for ( unsigned int i=0;i<detectedMarkers.size();++i )
{
for ( int c=0;c<4;c++ )
{
detectedMarkers[i][c]=Corners[i*12+c];
}
}
}
///detect the position of detected markers if desired
if ( camMatrix.rows!=0 && markerSizeMeters>0 )
{
for ( unsigned int i=0;i<detectedMarkers.size();i++ )
detectedMarkers[i].calculateExtrinsics ( markerSizeMeters,camMatrix,distCoeff,setYPerpendicular );
}
}
detectRectangles_qr只改了一句。
cv::findContours ( thres2 , contours2, hierarchy2,CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE );
void MarkerDetector::detectRectangles_qr(const cv::Mat &thresImg,vector<MarkerCandidate> & OutMarkerCanditates)
{
vector<MarkerCandidate> MarkerCanditates;
//calcualte the min_max contour sizes
unsigned int minSize=_minSize*std::max(thresImg.cols,thresImg.rows)*4;
unsigned int maxSize=_maxSize*std::max(thresImg.cols,thresImg.rows)*4;
std::vector<std::vector<cv::Point> > contours2;
std::vector<cv::Vec4i> hierarchy2;
thresImg.copyTo ( thres2 );
cv::findContours ( thres2 , contours2, hierarchy2,CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE );
vector<Point> approxCurve;
///for each contour, analyze if it is a paralelepiped likely to be the marker
for ( unsigned int i=0;i<contours2.size();i++ )
{
if ( minSize< contours2[i].size() &&contours2[i].size()<maxSize )
{
//@peak.ding add by peak.ding
/*
if(hierarchy2[i][2] == -1
|| hierarchy2[hierarchy2[i][2]][2] == -1
|| hierarchy2[hierarchy2[hierarchy2[i][2]][2]][2] == -1)
{
continue;
}
*/
//check it is a possible element by first checking is has enough points
//approximate to a poligon
approxPolyDP ( contours2[i] ,approxCurve , double ( contours2[i].size() ) *0.05 , true );
// drawApproxCurve(copy,approxCurve,Scalar(0,0,255));
//check that the poligon has 4 points
if ( approxCurve.size() ==4 )
{
//and is convex
if ( isContourConvex ( Mat ( approxCurve ) ) )
{
// drawApproxCurve(input,approxCurve,Scalar(255,0,255));
// //ensure that the distace between consecutive points is large enough
float minDist=1e10;
for ( int j=0;j<4;j++ )
{
float d= std::sqrt ( ( float ) ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) * ( approxCurve[j].x-approxCurve[ ( j+1 ) %4].x ) +
( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) * ( approxCurve[j].y-approxCurve[ ( j+1 ) %4].y ) );
// norm(Mat(approxCurve[i]),Mat(approxCurve[(i+1)%4]));
if ( d<minDist ) minDist=d;
}
//check that distance is not very small
if ( minDist>10 )
{
//add the points
// cout<<"ADDED"<<endl;
MarkerCanditates.push_back ( MarkerCandidate() );
MarkerCanditates.back().idx=i;
for ( int j=0;j<4;j++ )
{
MarkerCanditates.back().push_back ( Point2f ( approxCurve[j].x,approxCurve[j].y ) );
}
}
}
}
}
}
///sort the points in anti-clockwise order
valarray<bool> swapped(false,MarkerCanditates.size());//used later
for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
{
//trace a line between the first and second point.
//if the thrid point is at the right side, then the points are anti-clockwise
double dx1 = MarkerCanditates[i][1].x - MarkerCanditates[i][0].x;
double dy1 = MarkerCanditates[i][1].y - MarkerCanditates[i][0].y;
double dx2 = MarkerCanditates[i][2].x - MarkerCanditates[i][0].x;
double dy2 = MarkerCanditates[i][2].y - MarkerCanditates[i][0].y;
double o = ( dx1*dy2 )- ( dy1*dx2 );
if ( o < 0.0 ) //if the third point is in the left side, then sort in anti-clockwise order
{
swap ( MarkerCanditates[i][1],MarkerCanditates[i][3] );
swapped[i]=true;
//sort the contour points
// reverse(MarkerCanditates[i].contour.begin(),MarkerCanditates[i].contour.end());//????
}
}
/// remove these elements whise corners are too close to each other
//first detect candidates
vector<pair<int,int> > TooNearCandidates;
for ( unsigned int i=0;i<MarkerCanditates.size();i++ )
{
// cout<<"Marker i="<<i<<MarkerCanditates[i]<<endl;
//calculate the average distance of each corner to the nearest corner of the other marker candidate
for ( unsigned int j=i+1;j<MarkerCanditates.size();j++ )
{
float dist=0;
for ( int c=0;c<4;c++ )
dist+= sqrt ( ( MarkerCanditates[i][c].x-MarkerCanditates[j][c].x ) * ( MarkerCanditates[i][c].x-MarkerCanditates[j][c].x ) + ( MarkerCanditates[i][c].y-MarkerCanditates[j][c].y ) * ( MarkerCanditates[i][c].y-MarkerCanditates[j][c].y ) );
dist/=4;
//if distance is too small
if ( dist< 10 )
{
TooNearCandidates.push_back ( pair<int,int> ( i,j ) );
}
}
}
//mark for removal the element of the pair with smaller perimeter
valarray<bool> toRemove ( false,MarkerCanditates.size() );
for ( unsigned int i=0;i<TooNearCandidates.size();i++ )
{
if ( perimeter ( MarkerCanditates[TooNearCandidates[i].first ] ) >perimeter ( MarkerCanditates[ TooNearCandidates[i].second] ) )
toRemove[TooNearCandidates[i].second]=true;
else toRemove[TooNearCandidates[i].first]=true;
}
//remove the invalid ones
// removeElements ( MarkerCanditates,toRemove );
//finally, assign to the remaining candidates the contour
OutMarkerCanditates.reserve(MarkerCanditates.size());
for (size_t i=0;i<MarkerCanditates.size();i++) {
if (!toRemove[i]) {
OutMarkerCanditates.push_back(MarkerCanditates[i]);
OutMarkerCanditates.back().contour=contours2[ MarkerCanditates[i].idx];
if (swapped[i] && _enableCylinderWarp )//if the corners where swapped, it is required to reverse here the points so that they are in the same order
reverse(OutMarkerCanditates.back().contour.begin(),OutMarkerCanditates.back().contour.end());//????
}
}
}
最后是4个点顺训调整
int FiducidalMarkers::detect_qr(const cv::Mat &in,std::string &message, int &nRotations)
{
zbar::ImageScanner scanner_; //@peak.ding add
scanner_.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1); //@peak.ding
assert(in.rows==in.cols);
Mat grey;
if ( in.type()==CV_8UC1) grey=in;
else cv::cvtColor(in,grey,CV_BGR2GRAY);
size_t width = grey.cols;
size_t height = grey.rows;
zbar::Image img(width, height, "Y800", grey.data, width * height);
scanner_.scan(img);
if(img.symbol_begin()==img.symbol_end())
{
cout<<"can't detect QR code！"<<endl;
return -1;
}
//if(img.get_data_length() != 1) return -1;
for(zbar::SymbolIterator symbol = img.symbol_begin();symbol != img.symbol_end(); ++symbol)
{
message = symbol->get_data();
if(symbol->get_location_x(0) > width/2)
{
if(symbol->get_location_y(0) > width/2)
{
nRotations = 3;
}
else
{
nRotations = 0;
}
}
else
{
if(symbol->get_location_y(0) > width/2)
{
nRotations = 2;
}
else
{
nRotations = 1;
}
}
return std::atoi(message.c_str());
}
}
最后是4个点的世界坐标顺序，和原来代码一样没有改
void Marker::calculateExtrinsics(float markerSizeMeters, cv::Mat camMatrix, cv::Mat distCoeff , bool setYPerpendicular)throw(cv::Exception)
{
//if (!isValid()) throw cv::Exception(9004,"!isValid(): invalid marker. It is not possible to calculate extrinsics","calculateExtrinsics",__FILE__,__LINE__);
if (markerSizeMeters<=0)throw cv::Exception(9004,"markerSize<=0: invalid markerSize","calculateExtrinsics",__FILE__,__LINE__);
if ( camMatrix.rows==0 || camMatrix.cols==0) throw cv::Exception(9004,"CameraMatrix is empty","calculateExtrinsics",__FILE__,__LINE__);
double halfSize=markerSizeMeters/2.;
double oneThirdSize = markerSizeMeters/3.;
cv::Mat ObjPoints(4,3,CV_32FC1);
// ObjPoints.at<float>(1,0)=-halfSize;
// ObjPoints.at<float>(1,1)=halfSize;
// ObjPoints.at<float>(1,2)=0;
// ObjPoints.at<float>(2,0)=halfSize;
// ObjPoints.at<float>(2,1)=halfSize;
// ObjPoints.at<float>(2,2)=0;
// ObjPoints.at<float>(3,0)=halfSize;
// ObjPoints.at<float>(3,1)=-halfSize;
// ObjPoints.at<float>(3,2)=0;
// ObjPoints.at<float>(0,0)=-halfSize;
// ObjPoints.at<float>(0,1)=-halfSize;
// ObjPoints.at<float>(0,2)=0;
ObjPoints.at<float>(2,0)=-halfSize;
ObjPoints.at<float>(2,1)=halfSize;
ObjPoints.at<float>(2,2)=0;
ObjPoints.at<float>(3,0)=halfSize;
ObjPoints.at<float>(3,1)=halfSize;
ObjPoints.at<float>(3,2)=0;
ObjPoints.at<float>(0,0)=halfSize;
ObjPoints.at<float>(0,1)=-halfSize;
ObjPoints.at<float>(0,2)=0;
ObjPoints.at<float>(1,0)=-halfSize;
ObjPoints.at<float>(1,1)=-halfSize;
ObjPoints.at<float>(1,2)=0;
cv::Mat ImagePoints(4,2,CV_32FC1);
//Set image points from the marker
for (int c=0;c<4;c++)
{
ImagePoints.at<float>(c,0)=((*this)[c%4].x);
ImagePoints.at<float>(c,1)=((*this)[c%4].y);
}
cv::Mat raux,taux;
cv::solvePnP(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux);
//cv::solveP3P(ObjPoints, ImagePoints, camMatrix, distCoeff,raux,taux,cv::SOLVEPNP_AP3P);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
//rotate the X axis so that Y is perpendicular to the marker plane
if (setYPerpendicular) rotateXAxis(Rvec);
ssize=markerSizeMeters;
}
附图，打完收工
