基于Aruco与zbar对QR进行3D定位

参考文档参考 opencv aruco 实现对二维码(QR码)的检测与定位
使用usb相机需要进行相机内参的标定
标定代码参考ROS下单目相机标定过程
然后保存即可。
在usb_cam的launch文件中给camera_info_url赋值

    <param name="camera_info_url" value="file:///home/ding/.ros/camera_info/s_yue_camera.yaml"/>

后面改了mark的尺寸，发现有问题，待修正。
原理还是以上那篇博客
增加detect_qr函数

  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);


    //     cv::cvtColor(grey,_ssImC ,CV_GRAY2BGR); //DELETE

    //clear input data
    detectedMarkers.clear();
    zbar::Image::SymbolIterator symbol = img.symbol_begin();
    if(img.symbol_begin()==img.symbol_end())
    {
        cout<<"can't detect QR code！"<<endl;
        return;
    }
    //std::vector<int> ids;
    //std::vector<std::vector<cv::Point2f> > corners;
    //std::vector<cv::Vec3d> rvecs, tvecs;
    for(int i = 0;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 maker_;
        maker_.id = i;

        maker_.push_back(cv::Point2f(symbol->get_location_x(0),symbol->get_location_y(0)));
        maker_.push_back(cv::Point2f(symbol->get_location_x(3),symbol->get_location_y(3)));
        maker_.push_back(cv::Point2f(symbol->get_location_x(2),symbol->get_location_y(2)));
        maker_.push_back(cv::Point2f(symbol->get_location_x(1),symbol->get_location_y(1)));
        //corners.push_back(corner);
        //ids.push_back(i);
        detectedMarkers.push_back(maker_);
        i++;
    }

    ///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 ) );

      //copy back
      for ( unsigned int i=0;i<detectedMarkers.size();++i )
        for ( int c=0;c<4;c++ )     detectedMarkers[i][c]=Corners[i*4+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 );
    }

  }

然后在aruco_ros里面调用detect_qr即可，大功告成。附图