highlyreliablemarkers.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 
29 #include "highlyreliablemarkers.h"
30 #include <iostream>
31 using namespace std;
32 
33 namespace aruco {
34 
35 // static variables from HighlyReliableMarkers. Need to be here to avoid linking errors
36 Dictionary HighlyReliableMarkers::_D;
37 HighlyReliableMarkers::BalancedBinaryTree HighlyReliableMarkers::_binaryTree;
38 unsigned int HighlyReliableMarkers::_n, HighlyReliableMarkers::_ncellsBorder, HighlyReliableMarkers::_correctionDistance;
39 int HighlyReliableMarkers::_swidth;
40 
41 
44 MarkerCode::MarkerCode(unsigned int n) {
45  // resize bits vectors and initialize to 0
46  for (unsigned int i = 0; i < 4; i++) {
47  _bits[i].resize(n * n);
48  for (unsigned int j = 0; j < _bits[i].size(); j++)
49  _bits[i][j] = 0;
50  _ids[i] = 0; // ids are also 0
51  }
52  _n = n;
53 };
54 
55 
58 MarkerCode::MarkerCode(const MarkerCode &MC) {
59  for (unsigned int i = 0; i < 4; i++) {
60  _bits[i] = MC._bits[i];
61  _ids[i] = MC._ids[i];
62  }
63  _n = MC._n;
64 }
65 
66 
67 
70 void MarkerCode::set(unsigned int pos, bool val, bool updateIds) {
71  // if not the same value
72  if (get(pos) != val) {
73  for (unsigned int i = 0; i < 4; i++) { // calculate bit coordinates for each rotation
74  unsigned int y = pos / n(), x = pos % n(); // if rotation 0, dont do anything
75  // else calculate bit position in that rotation
76  if (i == 1) {
77  unsigned int aux = y;
78  y = x;
79  x = n() - aux - 1;
80  } else if (i == 2) {
81  y = n() - y - 1;
82  x = n() - x - 1;
83  } else if (i == 3) {
84  unsigned int aux = y;
85  y = n() - x - 1;
86  x = aux;
87  }
88  unsigned int rotPos = y * n() + x; // calculate position in the unidimensional string
89  _bits[i][rotPos] = val; // modify value
90  // update identifier in that rotation
91  if(updateIds) {
92  if (val == true)
93  _ids[i] += (unsigned int)pow(float(2), float(rotPos)); // if 1, add 2^pos
94  else
95  _ids[i] -= (unsigned int)pow(float(2), float(rotPos)); // if 0, substract 2^pos
96  }
97  }
98  }
99 }
100 
101 
104 unsigned int MarkerCode::selfDistance(unsigned int &minRot) const {
105  unsigned int res = _bits[0].size(); // init to n*n (max value)
106  for (unsigned int i = 1; i < 4; i++) { // self distance is not calculated for rotation 0
107  unsigned int hammdist = hammingDistance(_bits[0], _bits[i]);
108  if (hammdist < res) {
109  minRot = i;
110  res = hammdist;
111  }
112  }
113  return res;
114 }
115 
116 
119 unsigned int MarkerCode::distance(const MarkerCode &m, unsigned int &minRot) const {
120  unsigned int res = _bits[0].size(); // init to n*n (max value)
121  for (unsigned int i = 0; i < 4; i++) {
122  unsigned int hammdist = hammingDistance(_bits[0], m.getRotation(i));
123  if (hammdist < res) {
124  minRot = i;
125  res = hammdist;
126  }
127  }
128  return res;
129 };
130 
131 
134 void MarkerCode::fromString(std::string s) {
135  for (unsigned int i = 0; i < s.length(); i++) {
136  if (s[i] == '0')
137  set(i, false);
138  else
139  set(i, true);
140  }
141 }
142 
145 std::string MarkerCode::toString() const {
146  std::string s;
147  s.resize(size());
148  for (unsigned int i = 0; i < size(); i++) {
149  if (get(i))
150  s[i] = '1';
151  else
152  s[i] = '0';
153  }
154  return s;
155 }
156 
157 
160 cv::Mat MarkerCode::getImg(unsigned int pixSize) const {
161  const unsigned int borderSize = 1;
162  unsigned int nrows = n() + 2 * borderSize;
163  if (pixSize % nrows != 0)
164  pixSize = pixSize + nrows - pixSize % nrows;
165  unsigned int cellSize = pixSize / nrows;
166  cv::Mat img(pixSize, pixSize, CV_8U, cv::Scalar::all(0)); // create black image (init image to 0s)
167  // double for to go over all the cells
168  for (unsigned int i = 0; i < n(); i++) {
169  for (unsigned int j = 0; j < n(); j++) {
170  if (_bits[0][i * n() + j] != 0) { // just draw if it is 1, since the image has been init to 0
171  // double for to go over all the pixels in the cell
172  for (unsigned int k = 0; k < cellSize; k++) {
173  for (unsigned int l = 0; l < cellSize; l++) {
174  img.at< uchar >((i + borderSize) * cellSize + k, (j + borderSize) * cellSize + l) = 255;
175  }
176  }
177  }
178  }
179  }
180  return img;
181 }
182 
183 
186 unsigned int MarkerCode::hammingDistance(const std::vector< bool > &m1, const std::vector< bool > &m2) const {
187  unsigned int res = 0;
188  for (unsigned int i = 0; i < m1.size(); i++)
189  if (m1[i] != m2[i])
190  res++;
191  return res;
192 }
193 
194 
195 
196 
199 bool Dictionary::fromFile(std::string filename) {
200  cv::FileStorage fs(filename, cv::FileStorage::READ);
201  int nmarkers, markersize;
202 
203  // read number of markers
204  fs["nmarkers"] >> nmarkers; // cardinal of D
205  fs["markersize"] >> markersize; // n
206  fs["tau0"] >> tau0;
207 
208  // read each marker info
209  for (int i = 0; i < nmarkers; i++) {
210  std::string s;
211  fs["marker_" + toStr(i)] >> s;
212  MarkerCode m(markersize);
213  m.fromString(s);
214  push_back(m);
215  }
216  fs.release();
217  return true;
218 };
219 
222 bool Dictionary::toFile(std::string filename) {
223  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
224  // save number of markers
225  fs << "nmarkers" << (int)size(); // cardinal of D
226  fs << "markersize" << (int)((*this)[0].n()); // n
227  fs << "tau0" << (int)(this->tau0); // n
228  // save each marker code
229  for (unsigned int i = 0; i < size(); i++) {
230  std::string s = ((*this)[i]).toString();
231  fs << "marker_" + toStr(i) << s;
232  }
233  fs.release();
234  return true;
235 };
236 
239 unsigned int Dictionary::distance(const MarkerCode &m, unsigned int &minMarker, unsigned int &minRot) {
240  unsigned int res = m.size();
241  for (unsigned int i = 0; i < size(); i++) {
242  unsigned int minRotAux;
243  unsigned int distance = (*this)[i].distance(m, minRotAux);
244  if (distance < res) {
245  minMarker = i;
246  minRot = minRotAux;
247  res = distance;
248  }
249  }
250  return res;
251 }
252 
253 
256 unsigned int Dictionary::minimunDistance() {
257  if (size() == 0)
258  return 0;
259  unsigned int minDist = (*this)[0].size();
260  // for each marker in D
261  for (unsigned int i = 0; i < size(); i++) {
262  // calculate self distance of the marker
263  minDist = std::min(minDist, (*this)[i].selfDistance());
264 
265  // calculate distance to all the following markers
266  for (unsigned int j = i + 1; j < size(); j++) {
267  minDist = std::min(minDist, (*this)[i].distance((*this)[j]));
268  }
269  }
270  return minDist;
271 }
272 
273 
274 
275 
278 bool HighlyReliableMarkers::loadDictionary(Dictionary D, float correctionDistanceRate) {
279  if (D.size() == 0)
280  return false;
281  _D = D;
282  _n = _D[0].n();
283  _ncellsBorder = (_D[0].n() + 2);
284  _correctionDistance = correctionDistanceRate * ((D.tau0-1)/2);
285  cerr << "aruco :: _correctionDistance = " << _correctionDistance << endl;
286  _binaryTree.loadDictionary(&D);
287  return true;
288 }
289 
290 bool HighlyReliableMarkers::loadDictionary(std::string filename, float correctionDistance) {
291  Dictionary D;
292  D.fromFile(filename);
293  return loadDictionary(D, correctionDistance);
294 }
295 
296 
299 int HighlyReliableMarkers::detect(const cv::Mat &in, int &nRotations) {
300 
301  assert(in.rows == in.cols);
302  cv::Mat grey;
303  if (in.type() == CV_8UC1)
304  grey = in;
305  else
306  cv::cvtColor(in, grey, CV_BGR2GRAY);
307  // threshold image
308  cv::threshold(grey, grey, 125, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);
309  _swidth = grey.rows / _ncellsBorder;
310 
311  // check borders, even not necesary for the highly reliable markers
312  // if(!checkBorders(grey)) return -1;
313 
314  // obtain inner code
315  MarkerCode candidate = getMarkerCode(grey);
316 
317  // search each marker id in the balanced binary tree
318  unsigned int orgPos;
319  for (unsigned int i = 0; i < 4; i++) {
320  if (_binaryTree.findId(candidate.getId(i), orgPos)) {
321  nRotations = i;
322  // return candidate.getId(i);
323  return orgPos;
324  }
325  }
326 
327  // alternative version without using the balanced binary tree (less eficient)
328  // for(uint i=0; i<_D.size(); i++) {
329  // for(uint j=0; j<4; j++) {
330  // if(_D[i].getId() == candidate.getId(j)) {
331  // nRotations = j;
332  // //return candidate.getId(j);
333  // return i;
334  // }
335  // }
336  // }
337 
338  // correct errors
339  unsigned int minMarker, minRot;
340  if (_D.distance(candidate, minMarker, minRot) <= _correctionDistance) {
341  nRotations = minRot;
342  return minMarker;
343  // return _D[minMarker].getId();
344  }
345 
346  return -1;
347 }
348 
349 
352 bool HighlyReliableMarkers::checkBorders(cv::Mat grey) {
353  for (int y = 0; y < _ncellsBorder; y++) {
354  int inc = _ncellsBorder - 1;
355  if (y == 0 || y == _ncellsBorder - 1)
356  inc = 1; // for first and last row, check the whole border
357  for (int x = 0; x < _ncellsBorder; x += inc) {
358  int Xstart = (x) * (_swidth);
359  int Ystart = (y) * (_swidth);
360  cv::Mat square = grey(cv::Rect(Xstart, Ystart, _swidth, _swidth));
361  int nZ = cv::countNonZero(square);
362  if (nZ > (_swidth * _swidth) / 2) {
363  return false; // can not be a marker because the border element is not black!
364  }
365  }
366  }
367  return true;
368 }
369 
372 MarkerCode HighlyReliableMarkers::getMarkerCode(const cv::Mat &grey) {
373  MarkerCode candidate(_n);
374  for (int y = 0; y < _n; y++) {
375  for (int x = 0; x < _n; x++) {
376  int Xstart = (x + 1) * (_swidth);
377  int Ystart = (y + 1) * (_swidth);
378  cv::Mat square = grey(cv::Rect(Xstart, Ystart, _swidth, _swidth));
379  int nZ = countNonZero(square);
380  if (nZ > (_swidth * _swidth) / 2)
381  candidate.set(y * _n + x, 1);
382  }
383  }
384  return candidate;
385 }
386 
387 
388 
391 void HighlyReliableMarkers::BalancedBinaryTree::loadDictionary(Dictionary *D) {
392  // create _orderD wich is a sorted version of D
393  _orderD.clear();
394  for (unsigned int i = 0; i < D->size(); i++) {
395  _orderD.push_back(std::pair< unsigned int, unsigned int >((*D)[i].getId(), i));
396  }
397  std::sort(_orderD.begin(), _orderD.end());
398 
399  // calculate the number of levels of the tree
400  unsigned int levels = 0;
401  while (pow(float(2), float(levels)) <= _orderD.size())
402  levels++;
403  // levels-=1; // only count full levels
404 
405  // auxiliar vector to know which elements are already in the tree
406  std::vector< bool > visited;
407  visited.resize(_orderD.size(), false);
408 
409  // calculate position of the root element
410  unsigned int rootIdx = _orderD.size() / 2;
411  visited[rootIdx] = true; // mark it as visited
412  _root = rootIdx;
413 
414  // for(int i=0; i<visited.size(); i++) std::cout << visited[i] << std::endl;
415 
416  // auxiliar vector to store the ids intervals (max and min) during the creation of the tree
417  std::vector< std::pair< unsigned int, unsigned int > > intervals;
418  // first, add the two intervals at each side of root element
419  intervals.push_back(std::pair< unsigned int, unsigned int >(0, rootIdx));
420  intervals.push_back(std::pair< unsigned int, unsigned int >(rootIdx, _orderD.size()));
421 
422  // init the tree
423  _binaryTree.clear();
424  _binaryTree.resize(_orderD.size());
425 
426  // add root information to the tree (make sure child do not coincide with self root for small sizes of D)
427  if (!visited[(0 + rootIdx) / 2])
428  _binaryTree[rootIdx].first = (0 + rootIdx) / 2;
429  else
430  _binaryTree[rootIdx].first = -1;
431  if (!visited[(rootIdx + _orderD.size()) / 2])
432  _binaryTree[rootIdx].second = (rootIdx + _orderD.size()) / 2;
433  else
434  _binaryTree[rootIdx].second = -1;
435 
436  // for each tree level
437  for (unsigned int i = 1; i < levels; i++) {
438  unsigned int nintervals = intervals.size(); // count number of intervals and process them
439  for (unsigned int j = 0; j < nintervals; j++) {
440  // store interval information and delete it
441  unsigned int lowerBound, higherBound;
442  lowerBound = intervals.back().first;
443  higherBound = intervals.back().second;
444  intervals.pop_back();
445 
446  // center of the interval
447  unsigned int center = (higherBound + lowerBound) / 2;
448 
449  // if center not visited, continue
450  if (!visited[center])
451  visited[center] = true;
452  else
453  continue;
454 
455  // calculate centers of the child intervals
456  unsigned int lowerChild = (lowerBound + center) / 2;
457  unsigned int higherChild = (center + higherBound) / 2;
458 
459  // if not visited (lower child)
460  if (!visited[lowerChild]) {
461  intervals.insert(intervals.begin(), std::pair< unsigned int, unsigned int >(lowerBound, center)); // add the interval to analyze later
462  _binaryTree[center].first = lowerChild; // add as a child in the tree
463  } else
464  _binaryTree[center].first = -1; // if not, mark as no child
465 
466  // (higher child, same as lower child)
467  if (!visited[higherChild]) {
468  intervals.insert(intervals.begin(), std::pair< unsigned int, unsigned int >(center, higherBound));
469  _binaryTree[center].second = higherChild;
470  } else
471  _binaryTree[center].second = -1;
472  }
473  }
474 
475  // print tree
476  // for(uint i=0; i<_binaryTree.size(); i++) std::cout << _binaryTree[i].first << " " << _binaryTree[i].second << std::endl;
477  // std::cout << std::endl;
478 }
479 
480 
481 int count = 0;
482 int idc = 11;
483 
486 bool HighlyReliableMarkers::BalancedBinaryTree::findId(unsigned int id, unsigned int &orgPos) {
487  int pos = _root; // first position is root
488  while (pos != -1) { // while having a valid position
489  unsigned int posId = _orderD[pos].first; // calculate id of the node
490  if (posId == id) {
491  orgPos = _orderD[pos].second;
492  return true; // if is the desire id, return true
493  } else if (posId < id)
494  pos = _binaryTree[pos].second; // if desired id is higher, look in higher child
495  else
496  pos = _binaryTree[pos].first; // if it is lower, look in lower child
497  }
498  count++;
499  return false; // if nothing found, return false
500 }
501 }
unsigned int size() const
void fromString(std::string s)
XmlRpcServer s
bool fromFile(std::string filename)
std::vector< bool > _bits[4]
void set(unsigned int pos, bool val, bool updateIds=true)
unsigned int getId(unsigned int rot=0) const
const std::vector< bool > & getRotation(unsigned int rot) const


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