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