chromaticmask.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 "chromaticmask.h"
30 #include <set>
31 // #include <omp.h>
32 
33 
36 EMClassifier::EMClassifier(unsigned int nelements) {
37 #ifdef OPENCV_VERSION_3
38  _classifier = cv::ml::EM::create();
39  _classifier->setTermCriteria(cv::TermCriteria(cv::TermCriteria::COUNT, 4, FLT_EPSILON));
40  _classifier->setClustersNumber(2);
41  _classifier->setCovarianceMatrixType(cv::ml::EM::COV_MAT_DIAGONAL);
42 #else
43  _classifier = cv::EM(2, cv::EM::COV_MAT_DIAGONAL, cv::TermCriteria(cv::TermCriteria::COUNT , 4, FLT_EPSILON));
44 #endif
45  _nelem = nelements;
46  _threshProb = 0.0001;
47  for (unsigned int i = 0; i < 256; i++)
48  _prob[i] = 0.5;
49 }
50 
51 
52 
56 
57  // fill histogram
58 
59  for (unsigned int i = 0; i < 256; i++)
60  _histogram[i] = 0;
61 
62  for (unsigned int i = 0; i < _samples.size(); i++) {
63  uchar val = _samples[i];
64  _histogram[val] += 3;
65  if (val > 0)
66  _histogram[val - 1] += 2;
67  if (val < 255)
68  _histogram[val + 1] += 2;
69  if (val > 1)
70  _histogram[val - 2] += 1;
71  if (val < 254)
72  _histogram[val + 2] += 1;
73  }
74 
75  double sum = 0.;
76  for (unsigned int i = 0; i < 256; i++)
77  sum += _histogram[i];
78  for (unsigned int i = 0; i < 256; i++)
79  _histogram[i] /= sum;
80 
81 
82  // discretize histogram to speed up training
83  unsigned int histCount[256];
84  int n = 0;
85  for (unsigned int i = 0; i < 256; i++)
86  n += (histCount[i] = (unsigned int)(_nelem * _histogram[i]));
87 
88  if (n < 10)
89  return;
90 
91  cv::Mat samples(n, 1, CV_64FC1);
92  unsigned int idx = 0;
93  for (unsigned int i = 0; i < 256; i++) {
94  for (unsigned int j = 0; j < histCount[i]; j++) {
95  samples.ptr< double >(0)[idx] = i;
96  idx++;
97  }
98  }
99 
100 #ifdef OPENCV_VERSION_3
101  _classifier->trainEM(samples);
102 #else
103  _classifier.train(samples);
104 #endif
105 
106  cv::Mat sampleAux(1, 1, CV_64FC1);
107  for (unsigned int i = 0; i < 256; i++) {
108  sampleAux.ptr< double >(0)[0] = i;
109  cv::Mat probs;
110 #ifdef OPENCV_VERSION_3
111  cv::Vec2f r = _classifier->predict2(sampleAux, probs);
112 #else
113  cv::Vec2d r = _classifier.predict(sampleAux);
114 #endif
115  _prob[i] = exp(r[0]);
116  if (_prob[i] > _threshProb)
117  _inside[i] = true;
118  else
119  _inside[i] = false;
120  }
121 }
122 
123 
124 void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, float markersize) {
125  if (BC.mInfoType!=aruco::BoardConfiguration::METERS && markersize == -1) {
126  std::cerr << "Invalid markersize in ChromaticMask::setParams" << std::endl;
127  return;
128  }
129 
130  if (BC.size() == 0) {
131  std::cerr << "Invalid BoardConfiguration size in ChromaticMask::setParams" << std::endl;
132  return;
133  }
134 
136  markersize = cv::norm(BC[0][0]-BC[0][1]);
137  }
138 
139  // calculate corners from min and max in BC
140  cv::Point3f min, max;
141  min = max = BC[0][0];
142  for (unsigned int i = 0; i < BC.size(); i++) {
143  for (unsigned int j = 0; j < 4; j++) {
144  if (BC[i][j].x <= min.x && BC[i][j].y <= min.y)
145  min = BC[i][j];
146  if (BC[i][j].x >= max.x && BC[i][j].y >= max.y)
147  max = BC[i][j];
148  }
149  }
150  double pixSize = fabs(markersize / (BC[0][1].x - BC[0][0].x));
151  min.x *= pixSize;
152  min.y *= pixSize;
153  max.x *= pixSize;
154  max.y *= pixSize;
155 
156  // calculate border corners coordinates
157  vector< cv::Point3f > corners;
158  corners.push_back(min);
159  corners.push_back(cv::Point3f(min.x, max.y, 0));
160  corners.push_back(max);
161  corners.push_back(cv::Point3f(max.x, min.y, 0));
162 
163  setParams(mc, nc, threshProb, CP, BC, corners);
164 }
165 
166 
167 
170 void ChromaticMask::setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC,
171  vector< cv::Point3f > corners) {
172  _classifiers.resize(mc * nc);
173  for (unsigned int i = 0; i < _classifiers.size(); i++)
174  _classifiers[i].setProb(threshProb);
175  _mc = mc;
176  _nc = nc;
177  _threshProb = threshProb;
178  _cell_neighbours.resize(mc * nc);
179  int idx_ = 0;
180  // SGJ: revisar, no engo claro sepa bien que es i y j y los puedo estar confundiendo!!!
181  for (unsigned int j = 0; j < nc; j++)
182  for (unsigned int i = 0; i < mc; i++, idx_++) {
183  _centers.push_back(cv::Point2f(i + 0.5, j + 0.5));
184  for (unsigned int nj = max(j - 1, (unsigned int)0); nj < min(mc, j + 1); nj++) {
185  for (unsigned int ni = max(i - 1, (unsigned int)0); ni < min(mc, i + 1); ni++) {
186  size_t tidx = nj * mc + ni;
187  _cell_neighbours[idx_].push_back(tidx);
188  }
189  }
190  }
191 
192  // deterimne the idx of the neighbours
193  _BD.getMarkerDetector().setThresholdParams(35, 7);
194  _BD.getMarkerDetector().enableErosion(false);
195  _BD.getMarkerDetector().setCornerRefinementMethod(aruco::MarkerDetector::LINES);
196  _BD.setYPerpendicular(false);
197 
198  _BD.setParams(BC, CP);
199 
200  _objCornerPoints = corners;
201  _CP = CP;
202 
203  _pixelsVector.clear();
204  for (unsigned int i = 0; i < CP.CamSize.height / 2; i++)
205  for (unsigned int j = 0; j < CP.CamSize.width / 2; j++)
206  _pixelsVector.push_back(cv::Point2f(2 * j, 2 * i));
207 
208  resetMask();
209  _cellMap = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC1, cv::Scalar::all(0));
210  _canonicalPos = cv::Mat(CP.CamSize.height, CP.CamSize.width, CV_8UC2);
211 
212  _cellCenters.resize(_classifiers.size());
213  for (unsigned int i = 0; i < mc; i++) {
214  for (unsigned int j = 0; j < nc; j++) {
215  _cellCenters[i * mc + j].x = ((2 * i + 1) * _cellSize) / 2.;
216  _cellCenters[i * mc + j].y = ((2 * j + 1) * _cellSize) / 2.;
217  }
218  }
219 }
220 
221 pair< double, double > AvrgTime(0, 0);
222 
226 
227  // project corner points to image
228  cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints);
229 
230  // obtain the perspective transform
231  cv::Point2f pointsRes[4], pointsIn[4];
232  for (int i = 0; i < 4; i++)
233  pointsIn[i] = _imgCornerPoints[i];
234  pointsRes[0] = (cv::Point2f(0, 0));
235  pointsRes[1] = cv::Point2f(_cellSize * _mc - 1, 0);
236  pointsRes[2] = cv::Point2f(_cellSize * _mc - 1, _cellSize * _nc - 1);
237  pointsRes[3] = cv::Point2f(0, _cellSize * _nc - 1);
238  _perpTrans = cv::getPerspectiveTransform(pointsIn, pointsRes);
239 
240  double tick = (double)cv::getTickCount();
241 
242  vector< cv::Point2f > transformedPixels;
243  cv::perspectiveTransform(_pixelsVector, transformedPixels, _perpTrans);
244 
245  AvrgTime.first += ((double)cv::getTickCount() - tick) / cv::getTickFrequency();
246  AvrgTime.second++;
247  cout << "Time cc detection=" << 1000 * AvrgTime.first / AvrgTime.second << " milliseconds" << endl;
248 
249 
250 
251  cv::Rect cellRect(0, 0, _mc, _nc);
252  for (unsigned int i = 0; i < transformedPixels.size(); i++) {
253  //_canonicalPos.at<cv::Vec2b>(_pixelsVector[i].y, _pixelsVector[i].x) = cv::Vec2b(transformedPixels[i].x, transformedPixels[i].y);
254  transformedPixels[i].x /= _cellSize;
255  transformedPixels[i].y /= _cellSize;
256  if (!transformedPixels[i].inside(cellRect)) {
257  _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x) = 0;
258  _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x) = 0;
259  _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x + 1) = 0;
260  _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x + 1) = 0;
261  } else {
262  uchar cellNum = (unsigned int)transformedPixels[i].y * _nc + (unsigned int)transformedPixels[i].x;
263  _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x) = 1 + cellNum;
264  _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x) = 1 + cellNum;
265  _cellMap.at< uchar >(_pixelsVector[i].y, _pixelsVector[i].x + 1) = 1 + cellNum;
266  _cellMap.at< uchar >(_pixelsVector[i].y + 1, _pixelsVector[i].x + 1) = 1 + cellNum;
267  }
268  }
269 }
270 
271 
272 
275 void ChromaticMask::train(const cv::Mat &in, const aruco::Board &board) {
276  calculateGridImage(board);
277 
278  for (unsigned int i = 0; i < _classifiers.size(); i++)
279  _classifiers[i].clearSamples();
280 
281  for (unsigned int i = 0; i < in.rows; i++) {
282  for (unsigned int j = 0; j < in.cols; j++) {
283  uchar idx = _cellMap.at< uchar >(i, j);
284  if (idx != 0)
285  _classifiers[idx - 1].addSample(in.at< uchar >(i, j));
286  }
287  }
288 
289  for (unsigned int i = 0; i < _classifiers.size(); i++)
290  _classifiers[i].train();
291 
292 
293  // for(uint i=0; i<_mc; i++) {
294  // for(uint j=0; j<_nc; j++) {
295  // vector<uint> neighbors;
296  // for(int k=-1; k<=1; k++) {
297  // if((i+k)<0 || (i+k)>=_mc) continue;
298  // for(int l=-1; l<=1; l++) {
299  // if((j+l)<0 || (j+l)>=_nc) continue;
300  // neighbors.push_back((i+k)*_mc + (j+l));
301  // }
302  // }
303  //
304  //
305  // for(uint l=0; l<256; l++) {
306  // _classifiers[i*_mc+j].probConj[l] = 0.;
307  // for(uint k=0; k<neighbors.size(); k++) {
308  // _classifiers[i*_mc+j].probConj[l] += _classifiers[k].getProb(l);
309  // }
310  // _classifiers[i*_mc+j].probConj[l] += _classifiers[i*_mc+j].getProb(l); // center prob x2
311  // _classifiers[i*_mc+j].probConj[l] / (float)(neighbors.size()+1);
312  // }
313  //
314  // }
315  // }
316 
317  _isValid = true;
318 }
319 
320 
323 void ChromaticMask::classify(const cv::Mat &in, const aruco::Board &board) {
324  calculateGridImage(board);
325 
326  resetMask();
327 
328  for (unsigned int i = 0; i < in.rows; i++) {
329  const uchar *in_ptr = in.ptr< uchar >(i);
330  const uchar *_cellMap_ptr = _cellMap.ptr< uchar >(i);
331  for (unsigned int j = 0; j < in.cols; j++) {
332  uchar idx = _cellMap_ptr[j];
333  if (idx != 0) {
334 
335  // considering neighbours
336  // float prob=0.0;
337  // float totalW = 0.0;
338  // cv::Point2d pix(i,j);
339  // for(uint k=0; k<_classifiers.size()-17; k++) {
340  // float w = 1./(1.+getDistance(pix,k));
341  // totalW += w;
342  // prob += w*_classifiers[k].getProb(in_ptr[j] );
343  // }
344  // prob /= totalW;
345  // if(prob > _threshProb) _mask.at<uchar>(i,j)=1;
346 
347  // not considering neighbours
348  if (_classifiers[idx - 1].classify(in.at< uchar >(i, j)))
349  _mask.at< uchar >(i, j) = 1;
350  // if(_classifiers[idx-1].probConj[ in.at<uchar>(i,j)]>_threshProb ) _mask.at<uchar>(i,j)=1;
351  }
352  }
353  }
354 
355  // apply closing to mask
356  cv::Mat maskClose;
357  cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
358  cv::morphologyEx(_mask, maskClose, CV_MOP_CLOSE, element);
359  _mask = maskClose;
360 }
361 
362 cv::Rect fitRectToSize(cv::Rect r, cv::Size size) {
363 
364  r.x = max(r.x, 0);
365  r.y = max(r.y, 0);
366  int endx = r.x + r.width;
367  int endy = r.y + r.height;
368  endx = min(endx, size.width);
369  endy = min(endy, size.height);
370  r.width = endx - r.x;
371  r.height = endy - r.y;
372  return r;
373 }
374 
377 void ChromaticMask::classify2(const cv::Mat &in, const aruco::Board &board) {
378 
379  _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1);
380  _maskAux.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1);
381  _maskAux.setTo(cv::Scalar::all(0));
382 
383  cv::projectPoints(_objCornerPoints, board.Rvec, board.Tvec, _CP.CameraMatrix, _CP.Distorsion, _imgCornerPoints);
384  // obtain the perspective transform
385  cv::Point2f pointsRes[4], pointsIn[4];
386  for (int i = 0; i < 4; i++)
387  pointsIn[i] = _imgCornerPoints[i];
388 
389  pointsRes[0] = (cv::Point2f(0, 0));
390  pointsRes[1] = cv::Point2f(_mc - 1, 0);
391  pointsRes[2] = cv::Point2f(_mc - 1, _nc - 1);
392  pointsRes[3] = cv::Point2f(0, _nc - 1);
393  _perpTrans = cv::getPerspectiveTransform(pointsIn, pointsRes);
394 
395  cv::Mat pT_32;
396  _perpTrans.convertTo(pT_32, CV_32F); // RMS: CAMBIA A FLOAT
397 
398  // cout << _perpTrans << endl;
399 
400  cv::Rect r = cv::boundingRect(_imgCornerPoints);
401  r = fitRectToSize(r, in.size()); // fit rectangle to image limits
402  float *H = pT_32.ptr< float >(0);
403  // #pragma omp parallel for
404  int ny = 0;
405  for (unsigned int y = r.y; y < r.y + r.height; y += 2, ny++) {
406  const uchar *in_ptr = in.ptr< uchar >(y);
407  uchar *_mask_ptr = _maskAux.ptr< uchar >(y);
408  int startx = r.x + ny % 2; // alternate starting point
409  for (unsigned int x = startx; x < r.x + r.width; x += 2) {
410  cv::Point2f point;
411  float _inv_pointz = 1. / (x * H[6] + y * H[7] + H[8]);
412  point.x = _inv_pointz * (x * H[0] + y * H[1] + H[2]);
413  point.y = _inv_pointz * (x * H[3] + y * H[4] + H[5]);
414  cv::Point2i c;
415  c.x = int(point.x + 0.5);
416  c.y = int(point.y + 0.5);
417  if (c.x < 0 || c.x > _mc - 1 || c.y < 0 || c.y > _nc - 1)
418  continue;
419  size_t cell_idx = c.y * _mc + c.x; // SGJ: revisar si esto esta bien!!
420  float prob = 0.0, totalW = 0.0, dist, w;
421 
422  for (unsigned int k = 0; k < _cell_neighbours[cell_idx].size(); k++) {
423  dist = fabs(point.x - _centers[k].x) + fabs(point.y - _centers[k].y);
424  w = (2 - dist);
425  w *= w;
426  // w=1/(1- sqrt( (point.x-_centers[k].x)*(point.x-_centers[k].x) + (point.y-_centers[k].y)*(point.y-_centers[k].y));
427  totalW += w;
428  prob += w * _classifiers[_cell_neighbours[cell_idx][k]].getProb(in_ptr[x]);
429  }
430  prob /= totalW;
431  // prob/=float(_classifiers.size()-17);
432  if (prob > _threshProb) {
433  _mask_ptr[x] = 1;
434  }
435  }
436  }
437  // // apply closing to mask
438  // _mask=_maskAux;
439  cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
440  cv::morphologyEx(_maskAux, _mask, CV_MOP_CLOSE, element);
441 }
442 
443 
444 
445 void ChromaticMask::update(const cv::Mat &in) {
446  cv::Mat maskCells;
447  maskCells = _cellMap.mul(_mask);
448 
449  for (unsigned int i = 0; i < _classifiers.size(); i++)
450  _classifiers[i].clearSamples();
451 
452  for (unsigned int i = 0; i < maskCells.rows; i++) {
453  uchar *maskCells_ptr = maskCells.ptr< uchar >(i);
454  const uchar *in_ptr = in.ptr< uchar >(i);
455  for (unsigned int j = 0; j < maskCells.cols; j++) {
456  if (maskCells_ptr[j] != 0)
457  _classifiers[maskCells_ptr[j] - 1].addSample(in_ptr[j]);
458  }
459  }
460 
461  for (unsigned int i = 0; i < _classifiers.size(); i++)
462  if (_classifiers[i].numsamples() > 50) {
463  _classifiers[i].train();
464  }
465 }
466 
467 
468 
470 
471  _mask.create(_CP.CamSize.height, _CP.CamSize.width, CV_8UC1);
472  _mask.setTo(cv::Scalar::all(0));
473 }
cv::Mat Tvec
Definition: board.h:133
pair< double, double > AvrgTime(0, 0)
double _prob[256]
Definition: chromaticmask.h:65
void classify(const cv::Mat &in, const aruco::Board &board)
void train(const cv::Mat &in, const aruco::Board &board)
bool _inside[256]
Definition: chromaticmask.h:64
double _histogram[256]
Definition: chromaticmask.h:66
void setParams(unsigned int mc, unsigned int nc, double threshProb, aruco::CameraParameters CP, aruco::BoardConfiguration BC, vector< cv::Point3f > corners)
void update(const cv::Mat &in)
void classify2(const cv::Mat &in, const aruco::Board &board)
EMClassifier(unsigned int nelements=200)
void calculateGridImage(const aruco::Board &board)
unsigned int numsamples()
Definition: chromaticmask.h:52
cv::Rect fitRectToSize(cv::Rect r, cv::Size size)
bool classify(uchar s)
Definition: chromaticmask.h:50
cv::Mat Rvec
Definition: board.h:133
double _threshProb
Definition: chromaticmask.h:68
Parameters of the camera.
void clearSamples()
Definition: chromaticmask.h:48
vector< uchar > _samples
Definition: chromaticmask.h:63
This class defines a board with several markers. A Board contains several markers so that they are mo...
Definition: board.h:69
int min(int a, int b)
void setProb(double p)
Definition: chromaticmask.h:53
unsigned int _nelem
Definition: chromaticmask.h:67
cv::EM _classifier
Definition: chromaticmask.h:61


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