基于aruco源码定位QR码的三个回字型用于定位

aruco是使用4个角点进行空间定位，所以想用QR码的三个回字形来替代，实现过程先是找到三个回字形，使用的是aruco的源码，然后使用zbar找到QR码的4个角点（这4个角点的顺序是不变的），所以将3个回字形的12个点与QR的3个角点进行距离比较就可以固定三个回字形的顺序，然后继续进行距离比较，确定每个回字形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;

    size_t width = grey.cols;
    size_t height = grey.rows;

    zbar::Image img(width, height, "Y800", grey.data, width * height);
    scanner_.scan(img);

    //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(MarkerCanditates.size() < 3) return;

    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(auto symbol = img.symbol_begin();symbol != img.symbol_end(); ++symbol)
    {
        //        cout<<"type："<<endl<<symbol->get_type_name()<<endl<<endl;
        //        cout<<"data："<<endl<<symbol->get_data()<<endl<<endl;
        //        cout<<"data_length："<<endl<<symbol->get_data_length()<<endl<<endl;
        //        cout<<"location_size："<<endl<<symbol->get_location_size()<<endl<<endl;
        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)));


        //        Marker maker_;
        maker_.message = symbol->get_data();
        maker_.id = std::atoi(maker_.message.c_str());
        //        maker_.push_back(cv::Point2f(symbol->get_location_x(0),symbol->get_location_y(0)));
        //        maker_.push_back(cv::Point2f(symbol->get_location_x(1),symbol->get_location_y(1)));
        //        maker_.push_back(cv::Point2f(symbol->get_location_x(2),symbol->get_location_y(2)));
        //        maker_.push_back(cv::Point2f(symbol->get_location_x(3),symbol->get_location_y(3)));
        //        detectedMarkers.push_back(maker_);
        break;
    }

    ///pair the markers
    vector<pair<int,int>  > Marker_pair;

    for ( unsigned int i=0;i<Marker_zbar.size(); i++ )
    {
        int index = 0;
        int min_distance = std::numeric_limits<int>::max();
        for(unsigned int j=0; j<MarkerCanditates.size(); j++)
        {
            int dist = sqrt((MarkerCanditates[j][0].x - Marker_zbar[i].x) * (MarkerCanditates[j][0].x - Marker_zbar[i].x)
                    + (MarkerCanditates[j][0].y - Marker_zbar[i].y) * (MarkerCanditates[j][0].y - Marker_zbar[i].y));
            if(dist < min_distance)
            {
                index = j;
                min_distance = dist;
            }

        }

        Marker_pair.push_back ( pair<int,int> ( i,index ) );
    }

    ///sort the markers
    for ( unsigned int i=0;i<Marker_pair.size();i++ )
    {
        int index = 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() );
    }

    if(Marker_pair.size() != 3) return;

    for(unsigned int i=0; i<Marker_pair.size(); i++)
    {
        int index = Marker_pair[i].second;

        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 )
            //@peak.ding here default is below
            //for ( int c=0;c<4;c++ )
            for ( int c=0;c<12;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 )
        {
            //@peak.ding here default is below
            //for ( int c=0;c<4;c++ )     detectedMarkers[i][c]=Corners[i*4+c];
            for ( int c=0;c<12;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 );
    }
}