arucofidmarkers.cpp
Go to the documentation of this file.
1 
30 #include "arucofidmarkers.h"
31 #include <cstdio>
32 #include <opencv2/imgproc/imgproc.hpp>
33 using namespace cv;
34 using namespace std;
35 namespace aruco {
36 
37 /************************************
38  *
39  *
40  *
41  *
42  ************************************/
45 Mat FiducidalMarkers::createMarkerImage(int id, int size, bool addWaterMark, bool locked) throw(cv::Exception) {
46  Mat marker(size, size, CV_8UC1);
47  marker.setTo(Scalar(0));
48  if (0 <= id && id < 1024) {
49  // for each line, create
50  int swidth = size / 7;
51  int ids[4] = {0x10, 0x17, 0x09, 0x0e};
52  for (int y = 0; y < 5; y++) {
53  int index = (id >> 2 * (4 - y)) & 0x0003;
54  int val = ids[index];
55  for (int x = 0; x < 5; x++) {
56  Mat roi = marker(Rect((x + 1) * swidth, (y + 1) * swidth, swidth, swidth));
57  if ((val >> (4 - x)) & 0x0001)
58  roi.setTo(Scalar(255));
59  else
60  roi.setTo(Scalar(0));
61  }
62  }
63  } else
64  throw cv::Exception(9004, "id invalid", "createMarker", __FILE__, __LINE__);
65 
66  if (addWaterMark) {
67  char idcad[30];
68  sprintf(idcad, "#%d", id);
69  float ax = float(size) / 100.;
70  int linew = 1 + (marker.rows / 500);
71  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);
72  }
73 
74 
75  if (locked) {
76  // add a locking
77  int sqSize = float(size) * 0.25;
78 
79  cv::Mat lock_marker(cv::Size(size + sqSize * 2, size + sqSize * 2), marker.type());
80  // cerr<<lock_marker.size()<<endl;
81  lock_marker.setTo(cv::Scalar::all(255));
82  // write the squares
83  cv::Mat sub = lock_marker(cv::Range(0, sqSize), cv::Range(0, sqSize));
84  sub.setTo(cv::Scalar::all(0));
85 
86  sub = lock_marker(cv::Range(lock_marker.rows - sqSize, lock_marker.rows), cv::Range(0, sqSize));
87  sub.setTo(cv::Scalar::all(0));
88 
89  sub = lock_marker(cv::Range(lock_marker.rows - sqSize, lock_marker.rows), cv::Range(lock_marker.cols - sqSize, lock_marker.cols));
90  sub.setTo(cv::Scalar::all(0));
91 
92  sub = lock_marker(cv::Range(0, sqSize), cv::Range(lock_marker.cols - sqSize, lock_marker.cols));
93  sub.setTo(cv::Scalar::all(0));
94 
95  sub = lock_marker(cv::Range(sqSize, marker.rows + sqSize), cv::Range(sqSize, marker.cols + sqSize));
96  marker.copyTo(sub);
97  marker = lock_marker;
98  }
99  return marker;
100 }
104 cv::Mat FiducidalMarkers::getMarkerMat(int id) throw(cv::Exception) {
105  Mat marker(5, 5, CV_8UC1);
106  marker.setTo(Scalar(0));
107  if (0 <= id && id < 1024) {
108  // for each line, create
109  int ids[4] = {0x10, 0x17, 0x09, 0x0e};
110  for (int y = 0; y < 5; y++) {
111  int index = (id >> 2 * (4 - y)) & 0x0003;
112  int val = ids[index];
113  for (int x = 0; x < 5; x++) {
114  if ((val >> (4 - x)) & 0x0001)
115  marker.at< uchar >(y, x) = 1;
116  else
117  marker.at< uchar >(y, x) = 0;
118  }
119  }
120  } else
121  throw cv::Exception(9189, "Invalid marker id", "aruco::fiducidal::createMarkerMat", __FILE__, __LINE__);
122  return marker;
123 }
124 /************************************
125  *
126  *
127  *
128  *
129  ************************************/
130 
131 cv::Mat FiducidalMarkers::createBoardImage(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo,
132  vector< int > *excludedIds) throw(cv::Exception) {
133 
134 
135 
136  srand(cv::getTickCount());
137  int nMarkers = gridSize.height * gridSize.width;
138  TInfo.resize(nMarkers);
139  vector< int > ids = getListOfValidMarkersIds_random(nMarkers, excludedIds);
140  for (int i = 0; i < nMarkers; i++)
141  TInfo[i].id = ids[i];
142 
143  int sizeY = gridSize.height * MarkerSize + (gridSize.height - 1) * MarkerDistance;
144  int sizeX = gridSize.width * MarkerSize + (gridSize.width - 1) * MarkerDistance;
145  // find the center so that the ref systeem is in it
146  int centerX = sizeX / 2;
147  int centerY = sizeY / 2;
148 
149  // indicate the data is expressed in pixels
150  TInfo.mInfoType = BoardConfiguration::PIX;
151  Mat tableImage(sizeY, sizeX, CV_8UC1);
152  tableImage.setTo(Scalar(255));
153  int idp = 0;
154  for (int y = 0; y < gridSize.height; y++)
155  for (int x = 0; x < gridSize.width; x++, idp++) {
156  Mat subrect(tableImage, Rect(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize), MarkerSize, MarkerSize));
157  Mat marker = createMarkerImage(TInfo[idp].id, MarkerSize);
158  // set the location of the corners
159  TInfo[idp].resize(4);
160  TInfo[idp][0] = cv::Point3f(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize), 0);
161  TInfo[idp][1] = cv::Point3f(x * (MarkerDistance + MarkerSize) + MarkerSize, y * (MarkerDistance + MarkerSize), 0);
162  TInfo[idp][2] = cv::Point3f(x * (MarkerDistance + MarkerSize) + MarkerSize, y * (MarkerDistance + MarkerSize) + MarkerSize, 0);
163  TInfo[idp][3] = cv::Point3f(x * (MarkerDistance + MarkerSize), y * (MarkerDistance + MarkerSize) + MarkerSize, 0);
164  for (int i = 0; i < 4; i++)
165  TInfo[idp][i] -= cv::Point3f(centerX, centerY, 0);
166  marker.copyTo(subrect);
167  }
168 
169  return tableImage;
170 }
171 
172 /************************************
173  *
174  *
175  *
176  *
177  ************************************/
178 cv::Mat FiducidalMarkers::createBoardImage_ChessBoard(Size gridSize, int MarkerSize, BoardConfiguration &TInfo, bool centerData,
179  vector< int > *excludedIds) throw(cv::Exception) {
180 
181 
182  srand(cv::getTickCount());
183 
184  // determine the total number of markers required
185  int nMarkers = 3 * (gridSize.width * gridSize.height) / 4; // overdetermine the number of marker read
186  vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds);
187 
188 
189  int sizeY = gridSize.height * MarkerSize;
190  int sizeX = gridSize.width * MarkerSize;
191  // find the center so that the ref systeem is in it
192  int centerX = sizeX / 2;
193  int centerY = sizeY / 2;
194 
195  Mat tableImage(sizeY, sizeX, CV_8UC1);
196  tableImage.setTo(Scalar(255));
197  TInfo.mInfoType = BoardConfiguration::PIX;
198  int CurMarkerIdx = 0;
199  for (int y = 0; y < gridSize.height; y++) {
200 
201  bool toWrite;
202  if (y % 2 == 0)
203  toWrite = false;
204  else
205  toWrite = true;
206  for (int x = 0; x < gridSize.width; x++) {
207  toWrite = !toWrite;
208  if (toWrite) {
209  if (CurMarkerIdx >= idsVector.size())
210  throw cv::Exception(999, " FiducidalMarkers::createBoardImage_ChessBoard", "INTERNAL ERROR. REWRITE THIS!!", __FILE__, __LINE__);
211  TInfo.push_back(MarkerInfo(idsVector[CurMarkerIdx++]));
212 
213  Mat subrect(tableImage, Rect(x * MarkerSize, y * MarkerSize, MarkerSize, MarkerSize));
214  Mat marker = createMarkerImage(TInfo.back().id, MarkerSize);
215  // set the location of the corners
216  TInfo.back().resize(4);
217  TInfo.back()[0] = cv::Point3f(x * (MarkerSize), y * (MarkerSize), 0);
218  TInfo.back()[1] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize), 0);
219  TInfo.back()[2] = cv::Point3f(x * (MarkerSize)+MarkerSize, y * (MarkerSize)+MarkerSize, 0);
220  TInfo.back()[3] = cv::Point3f(x * (MarkerSize), y * (MarkerSize)+MarkerSize, 0);
221  if (centerData) {
222  for (int i = 0; i < 4; i++)
223  TInfo.back()[i] -= cv::Point3f(centerX, centerY, 0);
224  }
225  marker.copyTo(subrect);
226  }
227  }
228  }
229 
230  return tableImage;
231 }
232 
233 
234 
235 /************************************
236  *
237  *
238  *
239  *
240  ************************************/
241 cv::Mat FiducidalMarkers::createBoardImage_Frame(Size gridSize, int MarkerSize, int MarkerDistance, BoardConfiguration &TInfo, bool centerData,
242  vector< int > *excludedIds) throw(cv::Exception) {
243 
244 
245 
246  srand(cv::getTickCount());
247  int nMarkers = 2 * gridSize.height * 2 * gridSize.width;
248  vector< int > idsVector = getListOfValidMarkersIds_random(nMarkers, excludedIds);
249 
250  int sizeY = gridSize.height * MarkerSize + MarkerDistance * (gridSize.height - 1);
251  int sizeX = gridSize.width * MarkerSize + MarkerDistance * (gridSize.width - 1);
252  // find the center so that the ref systeem is in it
253  int centerX = sizeX / 2;
254  int centerY = sizeY / 2;
255 
256  Mat tableImage(sizeY, sizeX, CV_8UC1);
257  tableImage.setTo(Scalar(255));
258  TInfo.mInfoType = BoardConfiguration::PIX;
259  int CurMarkerIdx = 0;
260  int mSize = MarkerSize + MarkerDistance;
261  for (int y = 0; y < gridSize.height; y++) {
262  for (int x = 0; x < gridSize.width; x++) {
263  if (y == 0 || y == gridSize.height - 1 || x == 0 || x == gridSize.width - 1) {
264  TInfo.push_back(MarkerInfo(idsVector[CurMarkerIdx++]));
265  Mat subrect(tableImage, Rect(x * mSize, y * mSize, MarkerSize, MarkerSize));
266  Mat marker = createMarkerImage(TInfo.back().id, MarkerSize);
267  marker.copyTo(subrect);
268  // set the location of the corners
269  TInfo.back().resize(4);
270  TInfo.back()[0] = cv::Point3f(x * (mSize), y * (mSize), 0);
271  TInfo.back()[1] = cv::Point3f(x * (mSize)+MarkerSize, y * (mSize), 0);
272  TInfo.back()[2] = cv::Point3f(x * (mSize)+MarkerSize, y * (mSize)+MarkerSize, 0);
273  TInfo.back()[3] = cv::Point3f(x * (mSize), y * (mSize)+MarkerSize, 0);
274  if (centerData) {
275  for (int i = 0; i < 4; i++)
276  TInfo.back()[i] -= cv::Point3f(centerX, centerY, 0);
277  }
278  }
279  }
280  }
281 
282  return tableImage;
283 }
284 /************************************
285  *
286  *
287  *
288  *
289  ************************************/
290 Mat FiducidalMarkers::rotate(const Mat &in) {
291  Mat out;
292  in.copyTo(out);
293  for (int i = 0; i < in.rows; i++) {
294  for (int j = 0; j < in.cols; j++) {
295  out.at< uchar >(i, j) = in.at< uchar >(in.cols - j - 1, i);
296  }
297  }
298  return out;
299 }
300 
301 
302 /************************************
303  *
304  *
305  *
306  *
307  ************************************/
308 int FiducidalMarkers::hammDistMarker(Mat bits) {
309  int ids[4][5] = {{1, 0, 0, 0, 0}, {1, 0, 1, 1, 1}, {0, 1, 0, 0, 1}, {0, 1, 1, 1, 0}};
310  int dist = 0;
311 
312  for (int y = 0; y < 5; y++) {
313  int minSum = 1e5;
314  // hamming distance to each possible word
315  for (int p = 0; p < 4; p++) {
316  int sum = 0;
317  // now, count
318  for (int x = 0; x < 5; x++)
319  sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1;
320  if (minSum > sum)
321  minSum = sum;
322  }
323  // do the and
324  dist += minSum;
325  }
326 
327  return dist;
328 }
329 
330 /************************************
331  *
332  *
333  *
334  *
335  ************************************/
336 int FiducidalMarkers::analyzeMarkerImage(Mat &grey, int &nRotations) {
337 
338  // Markers are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
339  // the external border shoould be entirely black
340 
341  int swidth = grey.rows / 7;
342  for (int y = 0; y < 7; y++) {
343  int inc = 6;
344  if (y == 0 || y == 6)
345  inc = 1; // for first and last row, check the whole border
346  for (int x = 0; x < 7; x += inc) {
347  int Xstart = (x) * (swidth);
348  int Ystart = (y) * (swidth);
349  Mat square = grey(Rect(Xstart, Ystart, swidth, swidth));
350  int nZ = countNonZero(square);
351  if (nZ > (swidth * swidth) / 2) {
352  // cout<<"neb"<<endl;
353  return -1; // can not be a marker because the border element is not black!
354  }
355  }
356  }
357 
358  // now,
359  vector< int > markerInfo(5);
360  Mat _bits = Mat::zeros(5, 5, CV_8UC1);
361  // get information(for each inner square, determine if it is black or white)
362 
363  for (int y = 0; y < 5; y++) {
364 
365  for (int x = 0; x < 5; x++) {
366  int Xstart = (x + 1) * (swidth);
367  int Ystart = (y + 1) * (swidth);
368  Mat square = grey(Rect(Xstart, Ystart, swidth, swidth));
369  int nZ = countNonZero(square);
370  if (nZ > (swidth * swidth) / 2)
371  _bits.at< uchar >(y, x) = 1;
372  }
373  }
374  // printMat<uchar>( _bits,"or mat");
375 
376  // checkl all possible rotations
377  Mat _bitsFlip;
378  Mat Rotations[4];
379  Rotations[0] = _bits;
380  int dists[4];
381  dists[0] = hammDistMarker(Rotations[0]);
382  pair< int, int > minDist(dists[0], 0);
383  for (int i = 1; i < 4; i++) {
384  // rotate
385  Rotations[i] = rotate(Rotations[i - 1]);
386  // get the hamming distance to the nearest possible word
387  dists[i] = hammDistMarker(Rotations[i]);
388  if (dists[i] < minDist.first) {
389  minDist.first = dists[i];
390  minDist.second = i;
391  }
392  }
393  // printMat<uchar>( Rotations [ minDist.second]);
394  // cout<<"MinDist="<<minDist.first<<" "<<minDist.second<<endl;
395 
396  nRotations = minDist.second;
397  if (minDist.first != 0) // FUTURE WORK: correct if any error
398  return -1;
399  else { // Get id of the marker
400  int MatID = 0;
401  cv::Mat bits = Rotations[minDist.second];
402  for (int y = 0; y < 5; y++) {
403  MatID <<= 1;
404  if (bits.at< uchar >(y, 1))
405  MatID |= 1;
406  MatID <<= 1;
407  if (bits.at< uchar >(y, 3))
408  MatID |= 1;
409  }
410  return MatID;
411  }
412 }
413 
414 
415 /************************************
416  *
417  *
418  *
419  *
420  ************************************/
421 bool FiducidalMarkers::correctHammMarker(Mat &bits) {
422  // detect this lines with errors
423  bool errors[4];
424  int ids[4][5] = {{0, 0, 0, 0, 0}, {0, 0, 1, 1, 1}, {1, 1, 0, 0, 1}, {1, 1, 1, 1, 0}};
425 
426  for (int y = 0; y < 5; y++) {
427  int minSum = 1e5;
428  // hamming distance to each possible word
429  for (int p = 0; p < 4; p++) {
430  int sum = 0;
431  // now, count
432  for (int x = 0; x < 5; x++)
433  sum += bits.at< uchar >(y, x) == ids[p][x] ? 0 : 1;
434  if (minSum > sum)
435  minSum = sum;
436  }
437  if (minSum != 0)
438  errors[y] = true;
439  else
440  errors[y] = false;
441  }
442 
443  return true;
444 }
445 
446 /************************************
447  *
448  *
449  *
450  *
451  ************************************/
452 int FiducidalMarkers::detect(const Mat &in, int &nRotations) {
453  assert(in.rows == in.cols);
454  Mat grey;
455  if (in.type() == CV_8UC1)
456  grey = in;
457  else
458  cv::cvtColor(in, grey, CV_BGR2GRAY);
459  // threshold image
460  threshold(grey, grey, 125, 255, THRESH_BINARY | THRESH_OTSU);
461 
462  // now, analyze the interior in order to get the id
463  // try first with the big ones
464 
465  return analyzeMarkerImage(grey, nRotations);
466  ;
467  // too many false positives
468  /* int id=analyzeMarkerImage(grey,nRotations);
469  if (id!=-1) return id;
470  id=analyzeMarkerImage_type2(grey,nRotations);
471  if (id!=-1) return id;
472  return -1;*/
473 }
474 
475 vector< int > FiducidalMarkers::getListOfValidMarkersIds_random(int nMarkers, vector< int > *excluded) throw(cv::Exception) {
476 
477  if (excluded != NULL)
478  if (nMarkers + excluded->size() > 1024)
479  throw cv::Exception(8888, "FiducidalMarkers::getListOfValidMarkersIds_random", "Number of possible markers is exceeded", __FILE__, __LINE__);
480 
481  vector< int > listOfMarkers(1024);
482  // set a list with all ids
483  for (int i = 0; i < 1024; i++)
484  listOfMarkers[i] = i;
485 
486  if (excluded != NULL) // set excluded to -1
487  for (size_t i = 0; i < excluded->size(); i++)
488  listOfMarkers[excluded->at(i)] = -1;
489  // random shuffle
490  random_shuffle(listOfMarkers.begin(), listOfMarkers.end());
491  // now, take the first nMarkers elements with value !=-1
492  int i = 0;
493  vector< int > retList;
494  while (retList.size() < nMarkers) {
495  if (listOfMarkers[i] != -1)
496  retList.push_back(listOfMarkers[i]);
497  i++;
498  }
499  return retList;
500 }
501 }
This class defines a board with several markers. A Board contains several markers so that they are mo...
Definition: board.h:69


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Feb 28 2022 21:41:30