arucofidmarkers.cpp
Go to the documentation of this file.
00001 
00030 #include "arucofidmarkers.h"
00031 #include <cstdio>
00032 #include <opencv2/imgproc/imgproc.hpp>
00033 using namespace cv;
00034 using namespace std;
00035 namespace aruco {
00036 
00037 /************************************
00038  *
00039  *
00040  *
00041  *
00042  ************************************/
00045 Mat FiducidalMarkers::createMarkerImage(int id, int size, bool addWaterMark, bool locked) throw(cv::Exception) {
00046     Mat marker(size, size, CV_8UC1);
00047     marker.setTo(Scalar(0));
00048     if (0 <= id && id < 1024) {
00049         // for each line, create
00050         int swidth = size / 7;
00051         int ids[4] = {0x10, 0x17, 0x09, 0x0e};
00052         for (int y = 0; y < 5; y++) {
00053             int index = (id >> 2 * (4 - y)) & 0x0003;
00054             int val = ids[index];
00055             for (int x = 0; x < 5; x++) {
00056                 Mat roi = marker(Rect((x + 1) * swidth, (y + 1) * swidth, swidth, swidth));
00057                 if ((val >> (4 - x)) & 0x0001)
00058                     roi.setTo(Scalar(255));
00059                 else
00060                     roi.setTo(Scalar(0));
00061             }
00062         }
00063     } else
00064         throw cv::Exception(9004, "id invalid", "createMarker", __FILE__, __LINE__);
00065 
00066     if (addWaterMark) {
00067         char idcad[30];
00068         sprintf(idcad, "#%d", id);
00069         float ax = float(size) / 100.;
00070         int linew = 1 + (marker.rows / 500);
00071         cv::putText(marker, idcad, cv::Point(0, marker.rows - marker.rows / 40), cv::FONT_HERSHEY_COMPLEX, ax * 0.15f, cv::Scalar::all(30), linew, CV_AA);
00072     }
00073 
00074 
00075     if (locked) {
00076         // add a locking
00077         int sqSize = float(size) * 0.25;
00078 
00079         cv::Mat lock_marker(cv::Size(size + sqSize * 2, size + sqSize * 2), marker.type());
00080         // cerr<<lock_marker.size()<<endl;
00081         lock_marker.setTo(cv::Scalar::all(255));
00082         // write the squares
00083         cv::Mat sub = lock_marker(cv::Range(0, sqSize), cv::Range(0, sqSize));
00084         sub.setTo(cv::Scalar::all(0));
00085 
00086         sub = lock_marker(cv::Range(lock_marker.rows - sqSize, lock_marker.rows), cv::Range(0, sqSize));
00087         sub.setTo(cv::Scalar::all(0));
00088 
00089         sub = lock_marker(cv::Range(lock_marker.rows - sqSize, lock_marker.rows), cv::Range(lock_marker.cols - sqSize, lock_marker.cols));
00090         sub.setTo(cv::Scalar::all(0));
00091 
00092         sub = lock_marker(cv::Range(0, sqSize), cv::Range(lock_marker.cols - sqSize, lock_marker.cols));
00093         sub.setTo(cv::Scalar::all(0));
00094 
00095         sub = lock_marker(cv::Range(sqSize, marker.rows + sqSize), cv::Range(sqSize, marker.cols + sqSize));
00096         marker.copyTo(sub);
00097         marker = lock_marker;
00098     }
00099     return marker;
00100 }
00104 cv::Mat FiducidalMarkers::getMarkerMat(int id) throw(cv::Exception) {
00105     Mat marker(5, 5, CV_8UC1);
00106     marker.setTo(Scalar(0));
00107     if (0 <= id && id < 1024) {
00108         // for each line, create
00109         int ids[4] = {0x10, 0x17, 0x09, 0x0e};
00110         for (int y = 0; y < 5; y++) {
00111             int index = (id >> 2 * (4 - y)) & 0x0003;
00112             int val = ids[index];
00113             for (int x = 0; x < 5; x++) {
00114                 if ((val >> (4 - x)) & 0x0001)
00115                     marker.at< uchar >(y, x) = 1;
00116                 else
00117                     marker.at< uchar >(y, x) = 0;
00118             }
00119         }
00120     } else
00121         throw cv::Exception(9189, "Invalid marker id", "aruco::fiducidal::createMarkerMat", __FILE__, __LINE__);
00122     return marker;
00123 }
00124 /************************************
00125  *
00126  *
00127  *
00128  *
00129  ************************************/
00130 
00131 cv::Mat FiducidalMarkers::createBoardImage(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo,
00132                                            vector< int > *excludedIds) throw(cv::Exception) {
00133 
00134 
00135 
00136     srand(cv::getTickCount());
00137     int nMarkers = gridSize.height * gridSize.width;
00138     TInfo.resize(nMarkers);
00139     vector< int > ids = getListOfValidMarkersIds_random(nMarkers, excludedIds);
00140     for (int i = 0; i < nMarkers; i++)
00141         TInfo[i].id = ids[i];
00142 
00143     int sizeY = gridSize.height * MarkerSize + (gridSize.height - 1) * MarkerDistance;
00144     int sizeX = gridSize.width * MarkerSize + (gridSize.width - 1) * MarkerDistance;
00145     // find the center so that the ref systeem is in it
00146     int centerX = sizeX / 2;
00147     int centerY = sizeY / 2;
00148 
00149     // indicate the data is expressed in pixels
00150     TInfo.mInfoType = BoardConfiguration::PIX;
00151     Mat tableImage(sizeY, sizeX, CV_8UC1);
00152     tableImage.setTo(Scalar(255));
00153     int idp = 0;
00154     for (int y = 0; y < gridSize.height; y++)
00155         for (int x = 0; x < gridSize.width; x++, idp++) {
00156             Mat subrect(tableImage, Rect(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize), MarkerSize, MarkerSize));
00157             Mat marker = createMarkerImage(TInfo[idp].id, MarkerSize);
00158             // set the location of the corners
00159             TInfo[idp].resize(4);
00160             TInfo[idp][0] = cv::Point3f(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize), 0);
00161             TInfo[idp][1] = cv::Point3f(x * (MarkerDistance + MarkerSize) + MarkerSize, y * (MarkerDistance + MarkerSize), 0);
00162             TInfo[idp][2] = cv::Point3f(x * (MarkerDistance + MarkerSize) + MarkerSize, y * (MarkerDistance + MarkerSize) + MarkerSize, 0);
00163             TInfo[idp][3] = cv::Point3f(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize) + MarkerSize, 0);
00164             for (int i = 0; i < 4; i++)
00165                 TInfo[idp][i] -= cv::Point3f(centerX, centerY, 0);
00166             marker.copyTo(subrect);
00167         }
00168 
00169     return tableImage;
00170 }
00171 
00172 /************************************
00173  *
00174  *
00175  *
00176  *
00177  ************************************/
00178 cv::Mat FiducidalMarkers::createBoardImage_ChessBoard(Size gridSize, int MarkerSize, BoardConfiguration &TInfo, bool centerData,
00179                                                       vector< int > *excludedIds) throw(cv::Exception) {
00180 
00181 
00182     srand(cv::getTickCount());
00183 
00184     // determine the total number of markers required
00185     int nMarkers = 3 * (gridSize.width * gridSize.height) / 4; // overdetermine  the number of marker read
00186     vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds);
00187 
00188 
00189     int sizeY = gridSize.height * MarkerSize;
00190     int sizeX = gridSize.width * MarkerSize;
00191     // find the center so that the ref systeem is in it
00192     int centerX = sizeX / 2;
00193     int centerY = sizeY / 2;
00194 
00195     Mat tableImage(sizeY, sizeX, CV_8UC1);
00196     tableImage.setTo(Scalar(255));
00197     TInfo.mInfoType = BoardConfiguration::PIX;
00198     int CurMarkerIdx = 0;
00199     for (int y = 0; y < gridSize.height; y++) {
00200 
00201         bool toWrite;
00202         if (y % 2 == 0)
00203             toWrite = false;
00204         else
00205             toWrite = true;
00206         for (int x = 0; x < gridSize.width; x++) {
00207             toWrite = !toWrite;
00208             if (toWrite) {
00209                 if (CurMarkerIdx >= idsVector.size())
00210                     throw cv::Exception(999, " FiducidalMarkers::createBoardImage_ChessBoard", "INTERNAL ERROR. REWRITE THIS!!", __FILE__, __LINE__);
00211                 TInfo.push_back(MarkerInfo(idsVector[CurMarkerIdx++]));
00212 
00213                 Mat subrect(tableImage, Rect(x * MarkerSize, y * MarkerSize, MarkerSize, MarkerSize));
00214                 Mat marker = createMarkerImage(TInfo.back().id, MarkerSize);
00215                 // set the location of the corners
00216                 TInfo.back().resize(4);
00217                 TInfo.back()[0] = cv::Point3f(x * (MarkerSize), y * (MarkerSize), 0);
00218                 TInfo.back()[1] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize), 0);
00219                 TInfo.back()[2] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize)+MarkerSize, 0);
00220                 TInfo.back()[3] = cv::Point3f(x * (MarkerSize), y * (MarkerSize)+MarkerSize, 0);
00221                 if (centerData) {
00222                     for (int i = 0; i < 4; i++)
00223                         TInfo.back()[i] -= cv::Point3f(centerX, centerY, 0);
00224                 }
00225                 marker.copyTo(subrect);
00226             }
00227         }
00228     }
00229 
00230     return tableImage;
00231 }
00232 
00233 
00234 
00235 /************************************
00236  *
00237  *
00238  *
00239  *
00240  ************************************/
00241 cv::Mat FiducidalMarkers::createBoardImage_Frame(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, bool centerData,
00242                                                  vector< int > *excludedIds) throw(cv::Exception) {
00243 
00244 
00245 
00246     srand(cv::getTickCount());
00247     int nMarkers = 2 * gridSize.height * 2 * gridSize.width;
00248     vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds);
00249 
00250     int sizeY = gridSize.height * MarkerSize + MarkerDistance * (gridSize.height - 1);
00251     int sizeX = gridSize.width * MarkerSize + MarkerDistance * (gridSize.width - 1);
00252     // find the center so that the ref systeem is in it
00253     int centerX = sizeX / 2;
00254     int centerY = sizeY / 2;
00255 
00256     Mat tableImage(sizeY, sizeX, CV_8UC1);
00257     tableImage.setTo(Scalar(255));
00258     TInfo.mInfoType = BoardConfiguration::PIX;
00259     int CurMarkerIdx = 0;
00260     int mSize = MarkerSize + MarkerDistance;
00261     for (int y = 0; y < gridSize.height; y++) {
00262         for (int x = 0; x < gridSize.width; x++) {
00263             if (y == 0 || y == gridSize.height - 1 || x == 0 || x == gridSize.width - 1) {
00264                 TInfo.push_back(MarkerInfo(idsVector[CurMarkerIdx++]));
00265                 Mat subrect(tableImage, Rect(x * mSize, y * mSize, MarkerSize, MarkerSize));
00266                 Mat marker = createMarkerImage(TInfo.back().id, MarkerSize);
00267                 marker.copyTo(subrect);
00268                 // set the location of the corners
00269                 TInfo.back().resize(4);
00270                 TInfo.back()[0] = cv::Point3f(x * (mSize), y * (mSize), 0);
00271                 TInfo.back()[1] = cv::Point3f(x * (mSize)+MarkerSize, y * (mSize), 0);
00272                 TInfo.back()[2] = cv::Point3f(x * (mSize)+MarkerSize, y * (mSize)+MarkerSize, 0);
00273                 TInfo.back()[3] = cv::Point3f(x * (mSize), y * (mSize)+MarkerSize, 0);
00274                 if (centerData) {
00275                     for (int i = 0; i < 4; i++)
00276                         TInfo.back()[i] -= cv::Point3f(centerX, centerY, 0);
00277                 }
00278             }
00279         }
00280     }
00281 
00282     return tableImage;
00283 }
00284 /************************************
00285  *
00286  *
00287  *
00288  *
00289  ************************************/
00290 Mat FiducidalMarkers::rotate(const Mat &in) {
00291     Mat out;
00292     in.copyTo(out);
00293     for (int i = 0; i < in.rows; i++) {
00294         for (int j = 0; j < in.cols; j++) {
00295             out.at< uchar >(i, j) = in.at< uchar >(in.cols - j - 1, i);
00296         }
00297     }
00298     return out;
00299 }
00300 
00301 
00302 /************************************
00303  *
00304  *
00305  *
00306  *
00307  ************************************/
00308 int FiducidalMarkers::hammDistMarker(Mat bits) {
00309     int ids[4][5] = {{1, 0, 0, 0, 0}, {1, 0, 1, 1, 1}, {0, 1, 0, 0, 1}, {0, 1, 1, 1, 0}};
00310     int dist = 0;
00311 
00312     for (int y = 0; y < 5; y++) {
00313         int minSum = 1e5;
00314         // hamming distance to each possible word
00315         for (int p = 0; p < 4; p++) {
00316             int sum = 0;
00317             // now, count
00318             for (int x = 0; x < 5; x++)
00319                 sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1;
00320             if (minSum > sum)
00321                 minSum = sum;
00322         }
00323         // do the and
00324         dist += minSum;
00325     }
00326 
00327     return dist;
00328 }
00329 
00330 /************************************
00331  *
00332  *
00333  *
00334  *
00335  ************************************/
00336 int FiducidalMarkers::analyzeMarkerImage(Mat &grey, int &nRotations) {
00337 
00338     // Markers  are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
00339     // the external border shoould be entirely black
00340 
00341     int swidth = grey.rows / 7;
00342     for (int y = 0; y < 7; y++) {
00343         int inc = 6;
00344         if (y == 0 || y == 6)
00345             inc = 1; // for first and last row, check the whole border
00346         for (int x = 0; x < 7; x += inc) {
00347             int Xstart = (x) * (swidth);
00348             int Ystart = (y) * (swidth);
00349             Mat square = grey(Rect(Xstart, Ystart, swidth, swidth));
00350             int nZ = countNonZero(square);
00351             if (nZ > (swidth * swidth) / 2) {
00352                 //              cout<<"neb"<<endl;
00353                 return -1; // can not be a marker because the border element is not black!
00354             }
00355         }
00356     }
00357 
00358     // now,
00359     vector< int > markerInfo(5);
00360     Mat _bits = Mat::zeros(5, 5, CV_8UC1);
00361     // get information(for each inner square, determine if it is  black or white)
00362 
00363     for (int y = 0; y < 5; y++) {
00364 
00365         for (int x = 0; x < 5; x++) {
00366             int Xstart = (x + 1) * (swidth);
00367             int Ystart = (y + 1) * (swidth);
00368             Mat square = grey(Rect(Xstart, Ystart, swidth, swidth));
00369             int nZ = countNonZero(square);
00370             if (nZ > (swidth * swidth) / 2)
00371                 _bits.at< uchar >(y, x) = 1;
00372         }
00373     }
00374     //          printMat<uchar>( _bits,"or mat");
00375 
00376     // checkl all possible rotations
00377     Mat _bitsFlip;
00378     Mat Rotations[4];
00379     Rotations[0] = _bits;
00380     int dists[4];
00381     dists[0] = hammDistMarker(Rotations[0]);
00382     pair< int, int > minDist(dists[0], 0);
00383     for (int i = 1; i < 4; i++) {
00384         // rotate
00385         Rotations[i] = rotate(Rotations[i - 1]);
00386         // get the hamming distance to the nearest possible word
00387         dists[i] = hammDistMarker(Rotations[i]);
00388         if (dists[i] < minDist.first) {
00389             minDist.first = dists[i];
00390             minDist.second = i;
00391         }
00392     }
00393     //                  printMat<uchar>( Rotations [ minDist.second]);
00394     //                  cout<<"MinDist="<<minDist.first<<" "<<minDist.second<<endl;
00395 
00396     nRotations = minDist.second;
00397     if (minDist.first != 0) // FUTURE WORK: correct if any error
00398         return -1;
00399     else { // Get id of the marker
00400         int MatID = 0;
00401         cv::Mat bits = Rotations[minDist.second];
00402         for (int y = 0; y < 5; y++) {
00403             MatID <<= 1;
00404             if (bits.at< uchar >(y, 1))
00405                 MatID |= 1;
00406             MatID <<= 1;
00407             if (bits.at< uchar >(y, 3))
00408                 MatID |= 1;
00409         }
00410         return MatID;
00411     }
00412 }
00413 
00414 
00415 /************************************
00416  *
00417  *
00418  *
00419  *
00420  ************************************/
00421 bool FiducidalMarkers::correctHammMarker(Mat &bits) {
00422     // detect this lines with errors
00423     bool errors[4];
00424     int ids[4][5] = {{0, 0, 0, 0, 0}, {0, 0, 1, 1, 1}, {1, 1, 0, 0, 1}, {1, 1, 1, 1, 0}};
00425 
00426     for (int y = 0; y < 5; y++) {
00427         int minSum = 1e5;
00428         // hamming distance to each possible word
00429         for (int p = 0; p < 4; p++) {
00430             int sum = 0;
00431             // now, count
00432             for (int x = 0; x < 5; x++)
00433                 sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1;
00434             if (minSum > sum)
00435                 minSum = sum;
00436         }
00437         if (minSum != 0)
00438             errors[y] = true;
00439         else
00440             errors[y] = false;
00441     }
00442 
00443     return true;
00444 }
00445 
00446 /************************************
00447  *
00448  *
00449  *
00450  *
00451  ************************************/
00452 int FiducidalMarkers::detect(const Mat &in, int &nRotations) {
00453     assert(in.rows == in.cols);
00454     Mat grey;
00455     if (in.type() == CV_8UC1)
00456         grey = in;
00457     else
00458         cv::cvtColor(in, grey, CV_BGR2GRAY);
00459     // threshold image
00460     threshold(grey, grey, 125, 255, THRESH_BINARY | THRESH_OTSU);
00461 
00462     // now, analyze the interior in order to get the id
00463     // try first with the big ones
00464 
00465     return analyzeMarkerImage(grey, nRotations);
00466     ;
00467     // too many false positives
00468     /*    int id=analyzeMarkerImage(grey,nRotations);
00469         if (id!=-1) return id;
00470         id=analyzeMarkerImage_type2(grey,nRotations);
00471         if (id!=-1) return id;
00472         return -1;*/
00473 }
00474 
00475 vector< int > FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers, vector< int > *excluded) throw(cv::Exception) {
00476 
00477     if (excluded != NULL)
00478         if (nMarkers + excluded->size() > 1024)
00479             throw cv::Exception(8888, "FiducidalMarkers::getListOfValidMarkersIds_random", "Number of possible markers is exceeded", __FILE__, __LINE__);
00480 
00481     vector< int > listOfMarkers(1024);
00482     // set a list with all ids
00483     for (int i = 0; i < 1024; i++)
00484         listOfMarkers[i] = i;
00485 
00486     if (excluded != NULL) // set excluded to -1
00487         for (size_t i = 0; i < excluded->size(); i++)
00488             listOfMarkers[excluded->at(i)] = -1;
00489     // random shuffle
00490     random_shuffle(listOfMarkers.begin(), listOfMarkers.end());
00491     // now, take the first  nMarkers elements with value !=-1
00492     int i = 0;
00493     vector< int > retList;
00494     while (retList.size() < nMarkers) {
00495         if (listOfMarkers[i] != -1)
00496             retList.push_back(listOfMarkers[i]);
00497         i++;
00498     }
00499     return retList;
00500 }
00501 }


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Mei├čner Pascal, Qattan Mohamad
autogenerated on Thu Jun 6 2019 21:14:12