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