Esempio n. 1
0
float BoardDetector::detect(const vector< Marker > &detectedMarkers, const BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix, Mat distCoeff,
                            float markerSizeMeters) throw(cv::Exception) {
    if (BConf.size() == 0)
        throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty", __FILE__, __LINE__);
    if (BConf[0].size() < 2)
        throw cv::Exception(8881, "BoardDetector::detect", "Invalid BoardConfig that is empty 2", __FILE__, __LINE__);
    // compute the size of the markers in meters, which is used for some routines(mostly drawing)
    float ssize;
    if (BConf.mInfoType == BoardConfiguration::PIX && markerSizeMeters > 0)
        ssize = markerSizeMeters;
    else if (BConf.mInfoType == BoardConfiguration::METERS) {
        ssize = cv::norm(BConf[0][0] - BConf[0][1]);
    }

    // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
    Bdetected.clear();
    /// find among detected markers these that belong to the board configuration
    for (unsigned int i = 0; i < detectedMarkers.size(); i++) {
        int idx = BConf.getIndexOfMarkerId(detectedMarkers[i].id);
        if (idx != -1) {
            Bdetected.push_back(detectedMarkers[i]);
            Bdetected.back().ssize = ssize;
        }
    }
    // copy configuration
    Bdetected.conf = BConf;
    //

    bool hasEnoughInfoForRTvecCalculation = false;
    if (Bdetected.size() >= 1) {
        if (camMatrix.rows != 0) {
            if (markerSizeMeters > 0 && BConf.mInfoType == BoardConfiguration::PIX)
                hasEnoughInfoForRTvecCalculation = true;
            else if (BConf.mInfoType == BoardConfiguration::METERS)
                hasEnoughInfoForRTvecCalculation = true;
        }
    }

    // calculate extrinsic if there is information for that
    if (hasEnoughInfoForRTvecCalculation) {

        // calculate the size of the markers in meters if expressed in pixels
        double marker_meter_per_pix = 0;
        if (BConf.mInfoType == BoardConfiguration::PIX)
            marker_meter_per_pix = markerSizeMeters / cv::norm(BConf[0][0] - BConf[0][1]);
        else
            marker_meter_per_pix = 1; // to avoind interferring the process below

        // now, create the matrices for finding the extrinsics
        vector< cv::Point3f > objPoints;
        vector< cv::Point2f > imagePoints;
        for (size_t i = 0; i < Bdetected.size(); i++) {
            int idx = Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
            assert(idx != -1);
            for (int p = 0; p < 4; p++) {
                imagePoints.push_back(Bdetected[i][p]);
                const aruco::MarkerInfo &Minfo = Bdetected.conf.getMarkerInfo(Bdetected[i].id);
                objPoints.push_back(Minfo[p] * marker_meter_per_pix);
                //  		cout<<objPoints.back()<<endl;
            }
        }
        if (distCoeff.total() == 0)
            distCoeff = cv::Mat::zeros(1, 4, CV_32FC1);

        // 	    for(size_t i=0;i< imagePoints.size();i++){
        // 		cout<<objPoints[i]<<" "<<imagePoints[i]<<endl;
        // 	    }
        // 	    cout<<"cam="<<camMatrix<<" "<<distCoeff<<endl;
        cv::Mat rvec, tvec;
        cv::solvePnP(objPoints, imagePoints, camMatrix, distCoeff, rvec, tvec);
        rvec.convertTo(Bdetected.Rvec, CV_32FC1);
        tvec.convertTo(Bdetected.Tvec, CV_32FC1);
        //             cout<<rvec<< " "<<tvec<<" _setYPerpendicular="<<_setYPerpendicular<<endl;

        {
            vector< cv::Point2f > reprojected;
            cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);
            double errSum = 0;
            // check now the reprojection error and
            for (size_t i = 0; i < reprojected.size(); i++) {
                errSum += cv::norm(reprojected[i] - imagePoints[i]);
            }
            //                  cout<<"AAA RE="<<errSum/double ( reprojected.size() ) <<endl;
        }
        // now, do a refinement and remove points whose reprojection error is above a threshold, then repeat calculation with the rest
        if (repj_err_thres > 0) {
            vector< cv::Point2f > reprojected;
            cv::projectPoints(objPoints, rvec, tvec, camMatrix, distCoeff, reprojected);

            vector< int > pointsThatPassTest; // indices
            // check now the reprojection error and
            for (size_t i = 0; i < reprojected.size(); i++) {
                float err = cv::norm(reprojected[i] - imagePoints[i]);
                if (err < repj_err_thres)
                    pointsThatPassTest.push_back(i);
            }
            // cerr<<"Number of points after reprjection test "<<pointsThatPassTest.size() <<"/"<<objPoints.size() <<endl;
            // copy these data to another vectors and repeat
            vector< cv::Point3f > objPoints_filtered;
            vector< cv::Point2f > imagePoints_filtered;
            for (size_t i = 0; i < pointsThatPassTest.size(); i++) {
                objPoints_filtered.push_back(objPoints[pointsThatPassTest[i]]);
                imagePoints_filtered.push_back(imagePoints[pointsThatPassTest[i]]);
            }

            cv::solvePnP(objPoints_filtered, imagePoints_filtered, camMatrix, distCoeff, rvec, tvec);
            rvec.convertTo(Bdetected.Rvec, CV_32FC1);
            tvec.convertTo(Bdetected.Tvec, CV_32FC1);
        }


        // now, rotate 90 deg in X so that Y axis points up
        if (_setYPerpendicular)
            rotateXAxis(Bdetected.Rvec);
        //         cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
        //         cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
    }

    float prob = float(Bdetected.size()) / double(Bdetected.conf.size());
    return prob;
}
float BoardDetector::detect ( const vector<Marker> &detectedMarkers,const  BoardConfiguration &BConf, Board &Bdetected, Mat camMatrix,Mat distCoeff,float markerSizeMeters ) throw ( cv::Exception )
{
    if (BConf.size()==0) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty",__FILE__,__LINE__);
    if (BConf[0].size()<2) throw cv::Exception(8881,"BoardDetector::detect","Invalid BoardConfig that is empty 2",__FILE__,__LINE__);
    //compute the size of the markers in meters, which is used for some routines(mostly drawing)
    float ssize;
    if ( BConf.mInfoType==BoardConfiguration::PIX && markerSizeMeters>0 ) ssize=markerSizeMeters;
    else if ( BConf.mInfoType==BoardConfiguration::METERS )
    {
        ssize=cv::norm ( BConf[0][0]-BConf[0][1] );
    }

    // cout<<"markerSizeMeters="<<markerSizeMeters<<endl;
    Bdetected.clear();
    ///find among detected markers these that belong to the board configuration
    for ( unsigned int i=0;i<detectedMarkers.size();i++ )
    {
        int idx=BConf.getIndexOfMarkerId(detectedMarkers[i].id);
        if (idx!=-1) {
            Bdetected.push_back ( detectedMarkers[i] );
            Bdetected.back().ssize=ssize;
        }
    }
    //copy configuration
    Bdetected.conf=BConf;
//

    bool hasEnoughInfoForRTvecCalculation=false;
    if ( Bdetected.size() >=1 )
    {
        if ( camMatrix.rows!=0 )
        {
            if ( markerSizeMeters>0 && BConf.mInfoType==BoardConfiguration::PIX ) hasEnoughInfoForRTvecCalculation=true;
            else if ( BConf.mInfoType==BoardConfiguration::METERS ) hasEnoughInfoForRTvecCalculation=true;
        }
    }

//calculate extrinsic if there is information for that
    if ( hasEnoughInfoForRTvecCalculation )
    {

        //calculate the size of the markers in meters if expressed in pixels
        double marker_meter_per_pix=0;
        if ( BConf.mInfoType==BoardConfiguration::PIX ) marker_meter_per_pix=markerSizeMeters /  cv::norm ( BConf[0][0]-BConf[0][1] );
        else marker_meter_per_pix=1;//to avoind interferring the process below

        // now, create the matrices for finding the extrinsics
        Mat objPoints ( 4*Bdetected.size(),3,CV_32FC1 );
        Mat imagePoints ( 4*Bdetected.size(),2,CV_32FC1 );
        //size in meters of inter-marker distance

        for ( size_t i=0;i<Bdetected.size();i++ )
        {
            int idx=Bdetected.conf.getIndexOfMarkerId(Bdetected[i].id);
            assert(idx!=-1);
            for ( int p=0;p<4;p++ )
            {
                imagePoints.at<float> ( ( i*4 ) +p,0 ) =Bdetected[i][p].x;
                imagePoints.at<float> ( ( i*4 ) +p,1 ) =Bdetected[i][p].y;
                const aruco::MarkerInfo &Minfo=Bdetected.conf.getMarkerInfo( Bdetected[i].id);

                objPoints.at<float> ( ( i*4 ) +p,0 ) = Minfo[p].x*marker_meter_per_pix;
                objPoints.at<float> ( ( i*4 ) +p,1 ) = Minfo[p].y*marker_meter_per_pix;
                objPoints.at<float> ( ( i*4 ) +p,2 ) = Minfo[p].z*marker_meter_per_pix;
// 		cout<<objPoints.at<float>( (i*4)+p,0)<<" "<<objPoints.at<float>( (i*4)+p,1)<<" "<<objPoints.at<float>( (i*4)+p,2)<<endl;
            }
        }
        if (distCoeff.total()==0) distCoeff=cv::Mat::zeros(1,4,CV_32FC1 );

        cv::Mat rvec,tvec;
        cv::solvePnP(objPoints,imagePoints,camMatrix,distCoeff,rvec,tvec );
        rvec.convertTo(Bdetected.Rvec,CV_32FC1);
        tvec.convertTo(Bdetected.Tvec,CV_32FC1);
        //now, rotate 90 deg in X so that Y axis points up
        if (_setYPerperdicular)
            rotateXAxis ( Bdetected.Rvec );
//         cout<<Bdetected.Rvec.at<float>(0,0)<<" "<<Bdetected.Rvec.at<float>(1,0)<<" "<<Bdetected.Rvec.at<float>(2,0)<<endl;
//         cout<<Bdetected.Tvec.at<float>(0,0)<<" "<<Bdetected.Tvec.at<float>(1,0)<<" "<<Bdetected.Tvec.at<float>(2,0)<<endl;
    }

    float prob=float( Bdetected.size() ) /double ( Bdetected.conf.size() );
    return prob;
}