fractalposetracker.cpp
Go to the documentation of this file.
1 #include "fractallabelers/fractalposetracker.h"
2 #include "levmarq.h"
3 #include "ippe.h"
4 #include "aruco_export.h"
5 #include "timers.h"
6 
7 
8 #include "opencv2/calib3d/calib3d.hpp"
9 #include "aruco_cvversioning.h"
10 
11 namespace aruco
12 {
13 
14 /*
15  * KeyPoint cornersubpixel
16  */
17 void kcornerSubPix(const cv::Mat image, std::vector<cv::KeyPoint> &kpoints)
18 {
19  std::vector<cv::Point2f> points;
20  cv::KeyPoint::convert(kpoints, points);
21 
22  cv::Size winSize = cv::Size(4, 4);
23  cv::Size zeroZone = cv::Size(-1, -1);
24  cv::TermCriteria criteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 12,
25  0.005); // cv::cornerSubPix(image, points, winSize, zeroZone, criteria);
26  cornerSubPix(image, points, winSize, zeroZone, criteria);
27 
28  // Update kpoints
29  uint32_t i = 0;
30  for (auto &k : kpoints)
31  {
32  k.pt = points[i];
33  i++;
34  }
35 }
36 
37 /*
38  * KeyPoints Filter. Delete kpoints with low response and duplicated.
39  */
40 void kfilter(std::vector<cv::KeyPoint> &kpoints)
41 {
42  float minResp = kpoints[0].response;
43  float maxResp = kpoints[0].response;
44  for (auto &p : kpoints)
45  {
46  p.size = 40;
47  if (p.response < minResp)
48  minResp = p.response;
49  if (p.response > maxResp)
50  maxResp = p.response;
51  }
52  float thresoldResp = (maxResp - minResp) * 0.20f + minResp;
53 
54  for (uint32_t xi = 0; xi < kpoints.size(); xi++)
55  {
56  // Erase keypoints with low response (20%)
57  if (kpoints[xi].response < thresoldResp)
58  {
59  kpoints[xi].size = -1;
60  continue;
61  }
62 
63  // Duplicated keypoints (closer)
64  for (uint32_t xj = xi + 1; xj < kpoints.size(); xj++)
65  {
66  if (pow(kpoints[xi].pt.x - kpoints[xj].pt.x, 2) +
67  pow(kpoints[xi].pt.y - kpoints[xj].pt.y, 2) <
68  200)
69  {
70  if (kpoints[xj].response > kpoints[xi].response)
71  kpoints[xi] = kpoints[xj];
72 
73  kpoints[xj].size = -1;
74  }
75  }
76  }
77  kpoints.erase(std::remove_if(kpoints.begin(), kpoints.end(),
78  [](const cv::KeyPoint &kpt) { return kpt.size == -1; }),
79  kpoints.end());
80 }
81 
82 void assignClass(const cv::Mat &im, std::vector<cv::KeyPoint> &kpoints, float sizeNorm, int wsize)
83 {
84  if (im.type() != CV_8UC1)
85  throw std::runtime_error("assignClass Input image must be 8UC1");
86  int wsizeFull = wsize * 2 + 1;
87 
88  cv::Mat labels = cv::Mat::zeros(wsize * 2 + 1, wsize * 2 + 1, CV_8UC1);
89  cv::Mat thresIm = cv::Mat(wsize * 2 + 1, wsize * 2 + 1, CV_8UC1);
90 
91  for (auto &kp : kpoints)
92  {
93  float x = kp.pt.x;
94  float y = kp.pt.y;
95 
96  // Convert point range from norm (-size/2, size/2) to (0,imageSize)
97  if (sizeNorm > 0)
98  {
99  // x = im.cols * (x/_fractalMarker.getFractalSize() + 0.5f);
100  // y = im.rows * (-y/_fractalMarker.getFractalSize() + 0.5f);
101  x = im.cols * (x / sizeNorm + 0.5f);
102  y = im.rows * (-y / sizeNorm + 0.5f);
103  }
104 
105  x = int(x + 0.5f);
106  y = int(y + 0.5f);
107 
108  cv::Rect r = cv::Rect(x - wsize, y - wsize, wsize * 2 + 1, wsize * 2 + 1);
109  // Check boundaries
110  if (r.x < 0 || r.x + r.width > im.cols || r.y < 0 || r.y + r.height > im.rows)
111  continue;
112 
113  int endX = r.x + r.width;
114  int endY = r.y + r.height;
115  uchar minV = 255, maxV = 0;
116  for (int y = r.y; y < endY; y++)
117  {
118  const uchar *ptr = im.ptr<uchar>(y);
119  for (int x = r.x; x < endX; x++)
120  {
121  if (minV > ptr[x])
122  minV = ptr[x];
123  if (maxV < ptr[x])
124  maxV = ptr[x];
125  }
126  }
127 
128  if ((maxV - minV) < 25)
129  {
130  kp.class_id = 0;
131  continue;
132  }
133 
134  double thres = (maxV + minV) / 2.0;
135 
136  unsigned int nZ = 0;
137  // count non zero considering the threshold
138  for (int y = 0; y < wsizeFull; y++)
139  {
140  const uchar *ptr = im.ptr<uchar>(r.y + y) + r.x;
141  uchar *thresPtr = thresIm.ptr<uchar>(y);
142  for (int x = 0; x < wsizeFull; x++)
143  {
144  if (ptr[x] > thres)
145  {
146  nZ++;
147  thresPtr[x] = 255;
148  }
149  else
150  thresPtr[x] = 0;
151  }
152  }
153  // set all to zero labels.setTo(cv::Scalar::all(0));
154  for (int y = 0; y < thresIm.rows; y++)
155  {
156  uchar *labelsPtr = labels.ptr<uchar>(y);
157  for (int x = 0; x < thresIm.cols; x++)
158  labelsPtr[x] = 0;
159  }
160 
161  uchar newLab = 1;
162  std::map<uchar, uchar> unions;
163  for (int y = 0; y < thresIm.rows; y++)
164  {
165  uchar *thresPtr = thresIm.ptr<uchar>(y);
166  uchar *labelsPtr = labels.ptr<uchar>(y);
167  for (int x = 0; x < thresIm.cols; x++)
168  {
169  uchar reg = thresPtr[x];
170  uchar lleft_px = 0;
171  uchar ltop_px = 0;
172 
173  if (x - 1 > -1)
174  {
175  if (reg == thresPtr[x - 1])
176  lleft_px = labelsPtr[x - 1];
177  }
178 
179  if (y - 1 > -1)
180  {
181  if (reg == thresIm.ptr<uchar>(y - 1)[x]) // thresIm.at<uchar>(y-1, x)
182  ltop_px = labels.at<uchar>(y - 1, x);
183  }
184 
185  if (lleft_px == 0 && ltop_px == 0)
186  labelsPtr[x] = newLab++;
187 
188  else if (lleft_px != 0 && ltop_px != 0)
189  {
190  if (lleft_px < ltop_px)
191  {
192  labelsPtr[x] = lleft_px;
193  unions[ltop_px] = lleft_px;
194  }
195  else if (lleft_px > ltop_px)
196  {
197  labelsPtr[x] = ltop_px;
198  unions[lleft_px] = ltop_px;
199  }
200  else
201  { // IGuales
202  labelsPtr[x] = ltop_px;
203  }
204  }
205  else
206  {
207  if (lleft_px != 0)
208  labelsPtr[x] = lleft_px;
209  else
210  labelsPtr[x] = ltop_px;
211  }
212  }
213  }
214 
215  int nc = newLab - 1 - unions.size();
216  if (nc == 2)
217  {
218  if (nZ > thresIm.total() - nZ)
219  kp.class_id = 0;
220  else
221  kp.class_id = 1;
222  }
223  else if (nc > 2)
224  {
225  kp.class_id = 2;
226  }
227  }
228 }
229 
230 FractalPoseTracker::FractalPoseTracker()
231 {
232 }
233 
234 void FractalPoseTracker::setParams(const CameraParameters &cam_params,
235  const FractalMarkerSet &msconf, const float markerSize)
236 {
237  _cam_params = cam_params;
238  _fractalMarker = msconf;
239 
240  if (!cam_params.isValid())
241  throw cv::Exception(9001, "Invalid camera parameters",
242  "FractalPoseTracker::setParams", __FILE__, __LINE__);
243  if (_fractalMarker.mInfoType == aruco::FractalMarkerSet::NONE)
244  throw cv::Exception(9001, "Invalid FractalMarker", "FractalPoseTracker::setParams",
245  __FILE__, __LINE__);
246  if ((_fractalMarker.mInfoType == aruco::FractalMarkerSet::PIX ||
247  _fractalMarker.mInfoType == aruco::FractalMarkerSet::NORM) &&
248  markerSize <= 0)
249  throw cv::Exception(9001, "You should indicate the markersize since the Fractal Marker is in pixels or normalized",
250  "FractalPoseTracker::setParams", __FILE__, __LINE__);
251  if (_fractalMarker.mInfoType == aruco::FractalMarkerSet::PIX ||
252  _fractalMarker.mInfoType == aruco::FractalMarkerSet::NORM)
253  _fractalMarker = _fractalMarker.convertToMeters(markerSize);
254 
255  for (auto id_innerMarker : _fractalMarker.fractalMarkerCollection)
256  {
257  int markerId = id_innerMarker.first;
258  FractalMarker innerMarker = id_innerMarker.second;
259 
260  // Inner corners
261  _id_innerp3d[markerId] = innerMarker.getInnerCorners();
262  for (auto pt : _id_innerp3d[markerId])
263  {
264  _innerp3d.push_back(pt);
265  _innerkpoints.push_back(cv::KeyPoint(cv::Point2f(pt.x, pt.y), 20, -1, 0, markerId));
266  }
267 
268  // radius search by marker
269  float radiusF = 0.25;
270  float ratio = innerMarker.getMarkerSize() / _fractalMarker.getFractalSize();
271  float NBits = float(sqrt(_fractalMarker.nBits()) + 2) * ratio;
272  _id_radius[markerId] =
273  ((2 * NBits) / ((sqrt(innerMarker.nBits()) + 2) * (sqrt(_fractalMarker.nBits()) + 2))) *
274  radiusF * _fractalMarker.getFractalSize() / 2;
275  }
276 
277  // Get synthetic image (pixsize 6 in order to get inner corners classification)
278  cv::Mat imageGray = _fractalMarker.getFractalMarkerImage(6) * 255;
279  assignClass(imageGray, _innerkpoints, _fractalMarker.getFractalSize());
280  _kdtree.build(_innerkpoints);
281 
282  // #define _fractal_debug_classification
283 #ifdef _fractal_debug_classification
284  drawKeyPoints(imageGray, _innerkpoints, false, true);
285 #endif
286 }
287 
288 bool FractalPoseTracker::fractalRefinement(const cv::Ptr<MarkerDetector> markerDetector,
289  int markerWarpPix)
290 {
291  // ScopedTimerEvents Timer("fractal-refinement");
292 
293  std::vector<cv::Mat> imagePyramid = markerDetector->getImagePyramid();
294  std::vector<cv::Point3f> _ref_inner3d;
295  std::vector<cv::Point2f> _ref_inner2d;
296 
297  cv::Mat _p_rvec;
298  _rvec.copyTo(_p_rvec);
299  cv::Mat _p_tvec;
300  _tvec.copyTo(_p_tvec);
301 
302  cv::Mat rot;
303  cv::Rodrigues(_rvec, rot);
304 
305  for (auto id_marker : _fractalMarker.fractalMarkerCollection)
306  {
307  // Check z value for 4 external corners
308  std::vector<cv::Point3f> marker3d;
309  for (auto pt : id_marker.second.points)
310  {
311  cv::Mat_<double> src(3, 1, rot.type());
312  src(0, 0) = pt.x;
313  src(1, 0) = pt.y;
314  src(2, 0) = pt.z;
315 
316  cv::Mat cam_image_point = rot * src + _tvec;
317  cam_image_point = cam_image_point / cv::norm(cam_image_point);
318 
319  if (cam_image_point.at<double>(2, 0) > 0.85)
320  marker3d.push_back(pt);
321  else
322  break;
323  }
324 
325  std::vector<cv::Point3f> _inners3d;
326  std::vector<cv::Point2f> _inners2d;
327  float area = 0;
328  if (marker3d.size() < 4)
329  {
330  if (_id_area.find(id_marker.first) != _id_area.end())
331  area = _id_area[id_marker.first];
332  else
333  return false;
334 
335  for (auto pt : _id_innerp3d[id_marker.first])
336  {
337  cv::Mat_<double> src(3, 1, _rvec.type());
338  src(0, 0) = pt.x;
339  src(1, 0) = pt.y;
340  src(2, 0) = pt.z;
341 
342  cv::Mat cam_image_point = rot * src + _tvec;
343  cam_image_point = cam_image_point / cv::norm(cam_image_point);
344 
345  if (cam_image_point.at<double>(2, 0) > 0.85)
346  _inners3d.push_back(pt);
347  }
348 
349  if (_inners3d.size() == 0)
350  return false;
351 
352  cv::projectPoints(_inners3d, _rvec, _tvec, _cam_params.CameraMatrix,
353  _cam_params.Distorsion, _inners2d);
354  }
355  else
356  {
357  std::vector<cv::Point2f> marker2d;
358  cv::projectPoints(marker3d, _rvec, _tvec, _cam_params.CameraMatrix,
359  _cam_params.Distorsion, marker2d);
360 
361  cv::Point2f v01 = marker2d[1] - marker2d[0];
362  cv::Point2f v03 = marker2d[3] - marker2d[0];
363  float area1 = fabs(v01.x * v03.y - v01.y * v03.x);
364  cv::Point2f v21 = marker2d[1] - marker2d[2];
365  cv::Point2f v23 = marker2d[3] - marker2d[2];
366  float area2 = fabs(v21.x * v23.y - v21.y * v23.x);
367 
368  area = (area2 + area1) / 2.f;
369  _id_area[id_marker.first] = area;
370 
371  _inners3d = _id_innerp3d[id_marker.first];
372  cv::projectPoints(_inners3d, _rvec, _tvec, _cam_params.CameraMatrix,
373  _cam_params.Distorsion, _inners2d);
374  }
375 
376  size_t imgPyrIdx = 0;
377  auto markerWarpSize = (sqrt(id_marker.second.nBits()) + 2) * markerWarpPix;
378  float desiredarea = std::pow(static_cast<float>(markerWarpSize), 2.f);
379  for (size_t p = 1; p < imagePyramid.size(); p++)
380  {
381  if (area / pow(4, p) >= desiredarea)
382  imgPyrIdx = p;
383  else
384  break;
385  }
386 
387  float ratio = float(imagePyramid[imgPyrIdx].cols) / float(imagePyramid[0].cols);
388 
389  // std::cout << "REFINE["<< id_marker.first <<"], imgPyrId:"<<imgPyrIdx << ", ratio:"<< ratio<<std::endl;
390 
391  std::vector<double> _inners2d_error;
392  if (ratio == 1 && area >= desiredarea)
393  {
394  int halfwsize =
395  4 * float(imagePyramid[imgPyrIdx].cols) / float(imagePyramid[imgPyrIdx].cols) + 0.5;
396 
397  std::vector<cv::Point2f> _inners2d_copy;
398  for (auto pt : _inners2d)
399  {
400  _inners2d_copy.push_back(pt);
401  }
402  cornerSubPix(
403  imagePyramid[imgPyrIdx], _inners2d, cv::Size(halfwsize, halfwsize), cv::Size(-1, -1),
404  cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 12, 0.005));
405  int idx = 0;
406  for (auto pt : _inners2d)
407  {
408  _inners2d_error.push_back(sqrt(pow(_inners2d_copy[idx].x - pt.x, 2) +
409  pow(_inners2d_copy[idx].y - pt.y, 2)));
410  idx++;
411  }
412  }
413  else if (ratio != 1 && area >= desiredarea)
414  {
415  std::vector<cv::Point2f> _inners2d_copy;
416  for (auto &pt : _inners2d)
417  {
418  _inners2d_copy.push_back(pt);
419  pt *= ratio;
420  }
421 
422  std::vector<std::vector<cv::Point2f>> vpnts;
423  vpnts.push_back(_inners2d);
424  markerDetector->cornerUpsample(vpnts, imagePyramid[imgPyrIdx].size());
425 
426  int idx = 0;
427  for (auto pt : vpnts[0])
428  {
429  _inners2d_error.push_back(sqrt(pow(_inners2d_copy[idx].x - pt.x, 2) +
430  pow(_inners2d_copy[idx].y - pt.y, 2)));
431  _inners2d[idx++] = pt;
432  }
433  }
434 
435  // Elimina puntos que no son esquinas
436  if (area >= desiredarea)
437  {
438  // We discard outliers. Points above limit Q3+3*(Q3-1)
439  std::vector<double> _inners2d_error_copy;
440  for (auto err : _inners2d_error)
441  _inners2d_error_copy.push_back(err);
442  sort(_inners2d_error_copy.begin(), _inners2d_error_copy.end());
443  int q1 = (_inners2d_error_copy.size() + 1) / 4;
444  int q3 = 3 * (_inners2d_error_copy.size() + 1) / 4;
445 
446  double limit = _inners2d_error_copy[q3] +
447  3 * (_inners2d_error_copy[q3] - _inners2d_error_copy[q1]);
448 
449  int wsize = 10;
450  for (int idx = 0; idx < _inners2d.size(); idx++)
451  {
452  if (_inners2d_error[idx] > limit)
453  continue;
454 
455  float x = int(_inners2d[idx].x + 0.5f);
456  float y = int(_inners2d[idx].y + 0.5f);
457 
458  cv::Rect r = cv::Rect(x - wsize, y - wsize, wsize * 2 + 1, wsize * 2 + 1);
459  // Check boundaries
460  if (r.x < 0 || r.x + r.width > imagePyramid[0].cols || r.y < 0 ||
461  r.y + r.height > imagePyramid[0].rows)
462  continue;
463 
464  int endX = r.x + r.width;
465  int endY = r.y + r.height;
466  uchar minV = 255, maxV = 0;
467  for (int y = r.y; y < endY; y++)
468  {
469  const uchar *ptr = imagePyramid[0].ptr<uchar>(y);
470  for (int x = r.x; x < endX; x++)
471  {
472  if (minV > ptr[x])
473  minV = ptr[x];
474  if (maxV < ptr[x])
475  maxV = ptr[x];
476  }
477  }
478 
479  if ((maxV - minV) < 25)
480  continue;
481 
482  _ref_inner3d.push_back(_inners3d[idx]);
483  _ref_inner2d.push_back(_inners2d[idx]);
484  }
485  }
486  // Timer.add("refine-"+std::to_string(id_marker.first));
487  }
488 
489  // Solve
490  if (_ref_inner3d.size() > 4)
491  {
492  aruco::solvePnP(_ref_inner3d, _ref_inner2d, _cam_params.CameraMatrix,
493  _cam_params.Distorsion, _rvec, _tvec);
494 
495 //#define _fractal_debug_inners
496 #ifdef _fractal_debug_inners
497  cv::Mat InImageCopy;
498  cv::cvtColor(imagePyramid[0], InImageCopy, CV_GRAY2RGB);
499 
500  // Show first position (estimation with detected markers)
501  std::vector<cv::Point2f> preinnersPrj;
502  for (auto id_marker : _fractalMarker.fractalMarkerCollection)
503  {
504  cv::projectPoints(_id_innerp3d[id_marker.first], _p_rvec, _p_tvec,
505  _cam_params.CameraMatrix, _cam_params.Distorsion, preinnersPrj);
506  for (auto pt : preinnersPrj)
507  cv::circle(InImageCopy, pt, 5, cv::Scalar(0, 0, 255), CV_FILLED);
508  }
509 
510  // Show first position with refinement
511  for (auto p : _ref_inner2d)
512  cv::circle(InImageCopy, p, 5, cv::Scalar(255, 0, 0), CV_FILLED);
513 
514  _rvec.convertTo(_rvec, CV_32F);
515  _tvec.convertTo(_tvec, CV_32F);
516  cv::Rodrigues(_rvec, rot);
517 
518  std::vector<cv::Point3f> _inners;
519  for (auto pt : _inner_corners_3d)
520  {
521  cv::Mat_<float> src(3, 1, rot.type());
522  src(0, 0) = pt.x;
523  src(1, 0) = pt.y;
524  src(2, 0) = pt.z;
525 
526  cv::Mat cam_image_point = rot * src + _tvec;
527  cam_image_point = cam_image_point / cv::norm(cam_image_point);
528 
529  if (cam_image_point.at<float>(2, 0) > 0.85f)
530  _inners.push_back(pt);
531  }
532  std::vector<cv::Point2f> _inners_prj;
533  cv::projectPoints(_inners, _rvec, _tvec, _cam_params.CameraMatrix,
534  _cam_params.Distorsion, _inners_prj);
535 
536  // Show new projection using all inner points
537  for (auto pt : _inners_prj)
538  cv::circle(InImageCopy, pt, 5, cv::Scalar(0, 255, 0), CV_FILLED);
539 
540  cv::namedWindow("AA", cv::WINDOW_NORMAL);
541  imshow("AA", InImageCopy);
542  cv::waitKey();
543 #endif
544  // Timer.add("solve");
545 
546  return true;
547  }
548  else
549  return false;
550 }
551 
552 bool FractalPoseTracker::fractalInnerPose(const cv::Ptr<aruco::MarkerDetector> markerDetector,
553  const std::vector<aruco::Marker> &vmarkers,
554  bool refinement)
555 {
556  if (vmarkers.size() > 0)
557  {
558  // ScopedTimerEvents Timer("pnp");
559  // std::cout << "[Case 1]"<< std::endl;
560  std::vector<cv::Point2f> p2d;
561  std::vector<cv::Point3f> p3d;
562  for (auto marker : vmarkers)
563  {
564  if (_fractalMarker.fractalMarkerCollection.find(marker.id) !=
565  _fractalMarker.fractalMarkerCollection.end())
566  {
567  for (auto p : marker)
568  p2d.push_back(p);
569 
570  for (auto p : _fractalMarker.fractalMarkerCollection[marker.id].points)
571  p3d.push_back(p);
572  }
573  }
574 
575  // Initial pose estimation
576  aruco::solvePnP(p3d, p2d, _cam_params.CameraMatrix, _cam_params.Distorsion, _rvec, _tvec);
577 
578  // Timer.add("solve");
579 
580  // REFINE
581  if (refinement)
582  {
583  fractalRefinement(markerDetector);
584  // Timer.add("refine-solution");
585  }
586 
587  return true;
588  }
589  else
590  {
591  if (!_rvec.empty())
592  {
593  // std::cout << "[Case 2]"<< std::endl;
594  // ScopedTimerEvents Timer("ransac");
595  // Timer.add("detect");
596 
597  std::vector<cv::Point2f> innerPoints2d;
598  std::vector<cv::Point3f> innerPoints3d;
599 
600  float radius = 0;
601  cv::Mat rot;
602  cv::Rodrigues(_rvec, rot);
603 
604  // Getting the keypoints search radius
605  for (auto id_marker : _fractalMarker.fractalMarkerCollection)
606  {
607  std::vector<cv::Point2f> marker2d;
608  std::vector<cv::Point3f> marker3d;
609  for (auto pt : _fractalMarker.fractalMarkerCollection[id_marker.first].points)
610  {
611  cv::Mat_<double> src(3, 1, rot.type());
612  src(0, 0) = pt.x;
613  src(1, 0) = pt.y;
614  src(2, 0) = pt.z;
615  cv::Mat cam_image_point = rot * src + _tvec;
616  cam_image_point = cam_image_point / cv::norm(cam_image_point);
617 
618  if (cam_image_point.at<double>(2, 0) > 0.85)
619  marker3d.push_back(pt);
620  else
621  break;
622  }
623 
624  if (marker3d.size() == 4)
625  {
626  cv::projectPoints(marker3d, _rvec, _tvec, _cam_params.CameraMatrix,
627  _cam_params.Distorsion, marker2d);
628 
629  // Find marker area
630  cv::Point2f v01 = marker2d[1] - marker2d[0];
631  cv::Point2f v03 = marker2d[3] - marker2d[0];
632  float area1 = fabs(v01.x * v03.y - v01.y * v03.x);
633  cv::Point2f v21 = marker2d[1] - marker2d[2];
634  cv::Point2f v23 = marker2d[3] - marker2d[2];
635  float area2 = fabs(v21.x * v23.y - v21.y * v23.x);
636  double area = (area2 + area1) / 2.f;
637 
638  auto markerWarpSize =
639  (sqrt(_fractalMarker.fractalMarkerCollection[id_marker.first].nBits()) + 2) * 10;
640  float desiredarea = std::pow(static_cast<float>(markerWarpSize), 2.f);
641  if (area >= desiredarea)
642  {
643  for (auto pt : _id_innerp3d[id_marker.first])
644  innerPoints3d.push_back(pt);
645  }
646 
647  if (radius == 0.f)
648  radius = sqrt(area) / (sqrt(id_marker.second.nBits()) + 2.f);
649  }
650  else
651  {
652  for (auto pt : _id_innerp3d[id_marker.first])
653  innerPoints3d.push_back(pt);
654  }
655  }
656 
657  if (radius == 0)
658  radius = _preRadius;
659  _preRadius = radius;
660 
661  if (innerPoints3d.size() > 0 && radius > 0)
662  {
663  cv::projectPoints(innerPoints3d, _rvec, _tvec, _cam_params.CameraMatrix,
664  _cam_params.Distorsion, innerPoints2d);
665 
666  cv::Mat region;
667  cv::Point2f offset;
668  float ratio;
669  if (!ROI(markerDetector->getImagePyramid(), region, innerPoints2d, offset, ratio))
670  return false;
671  radius = radius * ratio;
672  // Timer.add("roi");
673 
674 
675  std::cout << "radius: " << radius << std::endl;
676 
677 //#define _fractal_debug_region
678 #ifdef _fractal_debug_region
679  cv::Mat out;
680  cv::cvtColor(region, out, CV_GRAY2RGB);
681  for (uint32_t i = 0; i < innerPoints2d.size(); i++)
682  circle(out, innerPoints2d[i], radius, cv::Scalar(0, 0, 255), 2);
683  cv::imshow("REGION ", out);
684  cv::waitKey();
685 #endif
686 
687  // FAST
688  std::vector<cv::KeyPoint> kpoints;
689  cv::Ptr<cv::FastFeatureDetector> fd = cv::FastFeatureDetector::create();
690  fd->detect(region, kpoints);
691  // Timer.add("fast");
692 
693 
694  if (kpoints.size() > 0)
695  {
696  // Filter kpoints (low response) and removing duplicated.
697  kfilter(kpoints);
698  // Timer.add("fast-filter");
699  // std::cout << "fast-filter" << std::endl;
700 
701  // Assign class to keypoints
702  assignClass(region, kpoints);
703 // Timer.add("fast-class");
704 
705 //#define _fractal_debug_classification
706 #ifdef _fractal_debug_classification
707  drawKeyPoints(region, kpoints);
708  cv::waitKey();
709 #endif
710  // Get keypoints with better response in a radius
711  picoflann::KdTreeIndex<2, PicoFlann_KeyPointAdapter> kdtreeImg;
712  kdtreeImg.build(kpoints);
713  std::vector<std::pair<uint, std::vector<uint>>> inner_candidates;
714  for (uint idx = 0; idx < innerPoints2d.size(); idx++)
715  {
716  std::vector<std::pair<uint32_t, double>> res =
717  kdtreeImg.radiusSearch(kpoints, innerPoints2d[idx], radius);
718 
719  std::vector<uint> candidates;
720  for (auto r : res)
721  {
722  if (kpoints[r.first].class_id == _innerkpoints[idx].class_id)
723  candidates.push_back(r.first);
724  }
725  if (candidates.size() > 0)
726  inner_candidates.push_back(make_pair(idx, candidates));
727  }
728 
729  // Timer.add("candidates");
730  cv::Mat bestModel =
731  fractal_solve_ransac(innerPoints2d.size(), inner_candidates, kpoints);
732  // Timer.add("find-solution");
733 
734  if (!bestModel.empty())
735  {
736  std::vector<cv::Point3f> p3d;
737  std::vector<cv::Point2f> p2d;
738 
739  std::vector<cv::Point2f> pnts, pntsDst;
740  cv::KeyPoint::convert(kpoints, pnts);
741  perspectiveTransform(pnts, pntsDst, bestModel);
742  for (uint32_t id = 0; id < pntsDst.size(); id++)
743  {
744  std::vector<std::pair<uint32_t, double>> res =
745  _kdtree.radiusSearch(_innerkpoints, pntsDst[id], _id_radius[0]);
746 
747  int i = 0;
748  for (auto r : res)
749  {
750  uint32_t innerId = r.first;
751  double dist = sqrt(r.second);
752 
753  uint32_t fmarkerId = _innerkpoints[innerId].octave;
754  if (dist > _id_radius[fmarkerId])
755  res.erase(res.begin() + i);
756  else
757  i++;
758  }
759  if (res.size() > 0)
760  {
761  p3d.push_back(_innerp3d[res[0].first]);
762  p2d.push_back(
763  cv::Point2f(kpoints[id].pt.x + offset.x, kpoints[id].pt.y + offset.y) / ratio);
764  }
765  }
766 
767  if (p3d.size() >= 4)
768  {
769  // std::cout << "solves" << std::endl;
770  aruco::solvePnP(p3d, p2d, _cam_params.CameraMatrix, _cam_params.Distorsion,
771  _rvec, _tvec);
772  // Timer.add("solves");
773 
774  if (refinement)
775  {
776  // std::cout << "refine-solution" << std::endl;
777  fractalRefinement(markerDetector);
778  // Timer.add("refine-solution");
779  }
780 
781  return true;
782  }
783  }
784  }
785  }
786  }
787  _rvec = cv::Mat();
788  _tvec = cv::Mat();
789 
790  return false;
791  }
792 }
793 
794 bool FractalPoseTracker::ROI(const std::vector<cv::Mat> imagePyramid, cv::Mat &img,
795  std::vector<cv::Point2f> &innerPoints2d, cv::Point2f &offset,
796  float &ratio)
797 {
798  cv::Mat rot;
799  cv::Rodrigues(_rvec, rot);
800 
801  // Biggest marker projection
802  std::vector<cv::Point2f> biggest_p2d;
803  std::vector<cv::Point3f> biggest_p3d;
804  for (int idx = 0;
805  idx < _fractalMarker.fractalMarkerCollection.size() & biggest_p3d.size() < 4; idx++)
806  {
807  biggest_p3d.empty();
808  for (auto pt : _fractalMarker.fractalMarkerCollection[idx].points)
809  {
810  cv::Mat_<double> src(3, 1, rot.type());
811  src(0, 0) = pt.x;
812  src(1, 0) = pt.y;
813  src(2, 0) = pt.z;
814  cv::Mat cam_image_point = rot * src + _tvec;
815  cam_image_point = cam_image_point / cv::norm(cam_image_point);
816 
817  if (cam_image_point.at<double>(2, 0) > 0.85)
818  biggest_p3d.push_back(pt);
819  else
820  break;
821  }
822  }
823 
824  if (!biggest_p3d.empty())
825  {
826  cv::projectPoints(biggest_p3d, _rvec, _tvec, _cam_params.CameraMatrix,
827  _cam_params.Distorsion, biggest_p2d);
828 
829  // Smallest marker projection
830  std::vector<cv::Point2f> smallest_p2d;
831  std::vector<cv::Point3f> smallest_p3d;
832  auto marker_smallest = (--_fractalMarker.fractalMarkerCollection.end())->second;
833  for (auto pt : marker_smallest.points)
834  smallest_p3d.push_back(pt);
835  cv::projectPoints(smallest_p3d, _rvec, _tvec, _cam_params.CameraMatrix,
836  _cam_params.Distorsion, smallest_p2d);
837 
838  // Smallest marker area
839  cv::Point2f v01 = smallest_p2d[1] - smallest_p2d[0];
840  cv::Point2f v03 = smallest_p2d[3] - smallest_p2d[0];
841  float area1 = fabs(v01.x * v03.y - v01.y * v03.x);
842  cv::Point2f v21 = smallest_p2d[1] - smallest_p2d[2];
843  cv::Point2f v23 = smallest_p2d[3] - smallest_p2d[2];
844  float area2 = fabs(v21.x * v23.y - v21.y * v23.x);
845  double area = (area2 + area1) / 2.f;
846 
847 
848  // Compute boundaries region
849  float border = sqrt(area) / sqrt(marker_smallest.nBits()) + 2;
850  int minX = imagePyramid[0].cols, minY = imagePyramid[0].rows, maxX = 0, maxY = 0;
851  for (auto p : biggest_p2d)
852  {
853  if (p.x < minX)
854  minX = p.x - border;
855  if (p.x > maxX)
856  maxX = p.x + border;
857  if (p.y < minY)
858  minY = p.y - border;
859  if (p.y > maxY)
860  maxY = p.y + border;
861  }
862  if (minX < 0)
863  minX = 0;
864  if (minY < 0)
865  minY = 0;
866  if (maxX > imagePyramid[0].cols)
867  maxX = imagePyramid[0].cols;
868  if (maxY > imagePyramid[0].rows)
869  maxY = imagePyramid[0].rows;
870 
871 
872  // Select imagePyramid
873  size_t imgPyrIdx = 0;
874  auto markerWarpSize = (sqrt(marker_smallest.nBits()) + 2) * 10;
875  float desiredarea = std::pow(static_cast<float>(markerWarpSize), 2.f);
876  for (size_t p = 1; p < imagePyramid.size(); p++)
877  {
878  if (area / pow(4, p) >= desiredarea)
879  imgPyrIdx = p;
880  else
881  break;
882  }
883 
884  ratio = float(imagePyramid[imgPyrIdx].cols) / float(imagePyramid[0].cols);
885  offset = cv::Point2i(minX, minY) * ratio;
886  cv::Rect region = cv::Rect(cv::Point2i(minX, minY) * ratio, cv::Point2i(maxX, maxY) * ratio);
887  img = imagePyramid[imgPyrIdx](region);
888 
889  for (auto &pt : innerPoints2d)
890  {
891  pt.x = pt.x * ratio - region.x;
892  pt.y = pt.y * ratio - region.y;
893  }
894  return true;
895  }
896  else
897  return false;
898 }
899 
900 cv::Mat FractalPoseTracker::fractal_solve_ransac(
901  int ninners, std::vector<std::pair<uint, std::vector<uint>>> inner_kpnt,
902  std::vector<cv::KeyPoint> kpnts, uint32_t maxIter, float _minInliers, float _thresInliers)
903 {
904  std::vector<cv::Point2f> pnts;
905  cv::KeyPoint::convert(kpnts, pnts);
906 
907  // Number randomly values selected
908  uint32_t numInliers = 4;
909  // Number of inliers to consider it good model, stop iterating!!!
910  uint32_t thresInliers = uint32_t(ninners * _thresInliers);
911  // Enough number of inliers to consider the model as valid
912  uint32_t minInliers = uint32_t(ninners * _minInliers);
913 
914  // Number of inliers
915  uint32_t bestInliers = 0;
916  // Best model
917  cv::Mat bestH = cv::Mat();
918 
919  // struct timespec ts;
920  // clock_gettime(CLOCK_MONOTONIC, &ts);
921  // srand((time_t)ts.tv_nsec);
922 
923  uint32_t count = 0;
924  for (auto ik : inner_kpnt)
925  if (ik.second.size() > 0)
926  count++;
927  if (count < minInliers)
928  return cv::Mat();
929 
930  uint32_t iter = 0;
931  do
932  {
933  // New stop condition to avoid infinite loop, when it tries to find the initial inliers
934  // For instance, for innerkpts group: 23{2} 18{3}, 14{3}, 3{7,6,5}, 2{3},
935  // when the initial group of inliers is selected:
936  // 23{2},18{3},3{6} there's no way to find the fourth ...
937  uint32_t iter2 = 0;
938  std::vector<std::pair<uint, uint>> inliers;
939  while (inliers.size() < numInliers && iter2++ < maxIter)
940  {
941  uint idx = rand() % inner_kpnt.size();
942  uint id_dst = inner_kpnt[idx].first;
943 
944  uint idxc = rand() % inner_kpnt[idx].second.size();
945  uint id_src = inner_kpnt[idx].second[idxc];
946 
947  // avoid duplicate observations
948  bool exist = false;
949  for (auto in : inliers)
950  {
951  if ((in.first != id_dst) && (in.second != id_src))
952  continue;
953  else
954  {
955  exist = true;
956  break;
957  }
958  }
959 
960  // if it is a new observation, add it!
961  if (!exist)
962  inliers.push_back(std::make_pair(id_dst, id_src));
963  }
964 
965  if (iter2 >= maxIter)
966  return cv::Mat();
967 
968  std::vector<cv::Point2f> srcInliers;
969  std::vector<cv::Point2f> dstInliers;
970  for (auto in : inliers)
971  {
972  dstInliers.push_back(_innerkpoints[in.first].pt);
973  srcInliers.push_back(kpnts[in.second].pt);
974  }
975  // Fit model with initial random inliers
976  cv::Mat H = findHomography(srcInliers, dstInliers);
977 
978  if (!H.empty())
979  {
980  std::vector<std::pair<uint, uint>> newInliers;
981  std::vector<std::pair<uint, uint>> newInliers2;
982 
983  std::vector<cv::Point2f> dstNewInliers;
984  std::vector<cv::Point2f> srcNewInliers;
985  std::vector<cv::Point2f> pntsTranf;
986  perspectiveTransform(pnts, pntsTranf, H);
987 
988  for (uint idP = 0; idP < pntsTranf.size(); idP++)
989  {
990  std::vector<std::pair<uint32_t, double>> res =
991  _kdtree.radiusSearch(_innerkpoints, pntsTranf[idP], _id_radius[0]);
992 
993  int i = 0;
994  for (auto r : res)
995  {
996  uint32_t innerId = r.first;
997  double dist = sqrt(r.second);
998 
999  uint32_t fmarkerId = _innerkpoints[innerId].octave;
1000  if (dist > _id_radius[fmarkerId])
1001  res.erase(res.begin() + i);
1002  else
1003  i++;
1004  }
1005 
1006  if (res.size() > 0)
1007  {
1008  // avoid duplicate observations
1009  bool exist = false;
1010  for (auto in : newInliers)
1011  {
1012  if (in.first != res[0].first)
1013  continue;
1014  else
1015  {
1016  exist = true;
1017  break;
1018  }
1019  }
1020  // if it is a new observation and these belong to the same class, add it!
1021  if (!exist)
1022  {
1023  if (_innerkpoints[res[0].first].class_id == kpnts[idP].class_id)
1024  newInliers.push_back(std::make_pair(res[0].first, idP));
1025  }
1026  }
1027  }
1028  if (newInliers.size() > bestInliers)
1029  {
1030  bestInliers = newInliers.size();
1031  bestH = H;
1032  }
1033  }
1034  iter++;
1035  } while (iter < maxIter && bestInliers < thresInliers);
1036 
1037  // std::cout << "[RANSAC] minInliers: "<< minInliers ;
1038  // std::cout << " ,bestInliers: "<< bestInliers;
1039  // std::cout << " ,iterations: "<< iter << std::endl;
1040 
1041  if (bestInliers < minInliers)
1042  bestH = cv::Mat();
1043 
1044  return bestH;
1045 }
1046 
1047 void FractalPoseTracker::drawKeyPoints(const cv::Mat image, std::vector<cv::KeyPoint> kpoints,
1048  bool text, bool transf)
1049 {
1050  if (transf)
1051  {
1052  // Convert point range from norm (-size/2, size/2) to (0,imageSize)
1053  for (auto &k : kpoints)
1054  {
1055  k.pt.x = image.cols * (k.pt.x / _fractalMarker.getFractalSize() + 0.5f);
1056  k.pt.y = image.rows * (-k.pt.y / _fractalMarker.getFractalSize() + 0.5f);
1057  }
1058  }
1059 
1060  cv::Mat out;
1061  cv::cvtColor(image, out, CV_GRAY2BGR);
1062  // drawKeypoints(image, kpoints, out, cv::Scalar(0,0,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
1063 
1064  cv::Scalar color;
1065  for (auto kp : kpoints)
1066  {
1067  if (kp.class_id == -1)
1068  color = cv::Scalar(255, 0, 255);
1069  if (kp.class_id == 0)
1070  color = cv::Scalar(0, 0, 255);
1071  if (kp.class_id == 1)
1072  color = cv::Scalar(0, 255, 0);
1073  if (kp.class_id == 2)
1074  color = cv::Scalar(255, 0, 0);
1075  circle(out, kp.pt, 2, color, -1);
1076  }
1077 
1078  if (text)
1079  {
1080  int nkm = 0;
1081  for (auto kp : kpoints)
1082  {
1083  putText(out, std::to_string(nkm++), kp.pt, CV_FONT_HERSHEY_COMPLEX, 1,
1084  cv::Scalar(0, 0, 255), 2, 8);
1085  }
1086  }
1087 
1088  //#ifdef _DEBUG
1089  // imshow("KPoints", out);
1090  // cv::waitKey();
1091  //#endif
1092 }
1093 } // namespace aruco
aruco::FractalPoseTracker::_id_innerp3d
std::map< int, std::vector< cv::Point3f > > _id_innerp3d
Definition: fractalposetracker.h:139
aruco::FractalPoseTracker::fractalInnerPose
bool fractalInnerPose(const cv::Ptr< MarkerDetector > markerDetector, const std::vector< aruco::Marker > &markers, bool refinement=true)
fractalInnerPose
Definition: fractalposetracker.cpp:552
ippe.h
aruco::CameraParameters
Parameters of the camera.
Definition: cameraparameters.h:29
aruco::FractalPoseTracker::fractalRefinement
bool fractalRefinement(const cv::Ptr< MarkerDetector > markerDetector, int markerWarpPix=10)
fractalRefinement
Definition: fractalposetracker.cpp:288
aruco::FractalMarkerSet::mInfoType
int mInfoType
Definition: fractalmarkerset.h:132
picoflann::KdTreeIndex::radiusSearch
std::vector< std::pair< uint32_t, double > > radiusSearch(const Container &container, const Type &val, double dist, bool sorted=true, int maxNN=-1) const
Definition: picoflann.h:197
aruco::FractalPoseTracker::_tvec
cv::Mat _tvec
Definition: fractalposetracker.h:138
aruco::kfilter
void kfilter(std::vector< cv::KeyPoint > &kpoints)
Definition: fractalposetracker.cpp:40
aruco::CameraParameters::isValid
bool isValid() const
Definition: cameraparameters.h:63
aruco::FractalPoseTracker::FractalPoseTracker
FractalPoseTracker()
Definition: fractalposetracker.cpp:230
aruco::FractalPoseTracker::_innerkpoints
std::vector< cv::KeyPoint > _innerkpoints
Definition: fractalposetracker.h:141
aruco::FractalPoseTracker::_rvec
cv::Mat _rvec
Definition: fractalposetracker.h:138
f
f
picoflann::KdTreeIndex::build
void build(const Container &container)
Definition: picoflann.h:156
aruco::FractalPoseTracker::_cam_params
aruco::CameraParameters _cam_params
Definition: fractalposetracker.h:137
aruco::FractalMarker::nBits
int nBits()
Definition: fractalmarker.h:37
aruco::FractalPoseTracker::drawKeyPoints
void drawKeyPoints(const cv::Mat image, std::vector< cv::KeyPoint > kpoints, bool text=false, bool transf=false)
image
Definition: fractalposetracker.cpp:1047
fractalposetracker.h
aruco::FractalPoseTracker::_id_radius
std::map< int, double > _id_radius
Definition: fractalposetracker.h:143
aruco::CameraParameters::CameraMatrix
cv::Mat CameraMatrix
Definition: cameraparameters.h:33
aruco_export.h
aruco::FractalPoseTracker::setParams
void setParams(const CameraParameters &cam_params, const FractalMarkerSet &msconf, const float markerSize=-1)
setParams
Definition: fractalposetracker.cpp:234
aruco::FractalMarkerSet::NORM
@ NORM
Definition: fractalmarkerset.h:79
aruco::FractalMarkerSet::getFractalSize
float getFractalSize() const
Definition: fractalmarkerset.h:110
aruco::FractalMarkerSet::nBits
int nBits() const
Definition: fractalmarkerset.h:117
aruco::FractalMarker::getInnerCorners
std::vector< cv::Point3f > getInnerCorners()
Definition: fractalmarker.h:49
aruco::FractalMarkerSet::convertToMeters
FractalMarkerSet convertToMeters(float fractalSize_meters)
Definition: fractalmarkerset.cpp:725
levmarq.h
aruco::assignClass
void assignClass(const cv::Mat &im, std::vector< cv::KeyPoint > &kpoints, float sizeNorm=0.f, int wsize=5)
assignClass
Definition: fractalposetracker.cpp:82
aruco::FractalPoseTracker::_innerp3d
std::vector< cv::Point3f > _innerp3d
Definition: fractalposetracker.h:140
aruco::FractalMarkerSet::PIX
@ PIX
Definition: fractalmarkerset.h:77
timers.h
aruco::FractalMarkerSet::getFractalMarkerImage
cv::Mat getFractalMarkerImage(int pixSize, bool border=false)
Definition: fractalmarkerset.cpp:786
aruco::Marker3DInfo::getMarkerSize
float getMarkerSize() const
Definition: markermap.h:47
aruco::FractalMarkerSet::fractalMarkerCollection
std::map< int, FractalMarker > fractalMarkerCollection
Definition: fractalmarkerset.h:70
aruco::FractalPoseTracker::_fractalMarker
FractalMarkerSet _fractalMarker
Definition: fractalposetracker.h:136
aruco::FractalMarkerSet::NONE
@ NONE
Definition: fractalmarkerset.h:76
picoflann::KdTreeIndex
Definition: picoflann.h:149
aruco::FractalPoseTracker::ROI
bool ROI(const std::vector< cv::Mat > imagePyramid, cv::Mat &img, std::vector< cv::Point2f > &innerPoints2d, cv::Point2f &offset, float &ratio)
ROI.
Definition: fractalposetracker.cpp:794
aruco
Definition: cameraparameters.h:24
aruco_cvversioning.h
aruco::FractalMarkerSet
Definition: fractalmarkerset.h:10
aruco::FractalMarker
Definition: fractalmarker.h:11
aruco::kcornerSubPix
void kcornerSubPix(const cv::Mat image, std::vector< cv::KeyPoint > &kpoints)
Definition: fractalposetracker.cpp:17
aruco::FractalPoseTracker::_id_area
std::map< int, float > _id_area
Definition: fractalposetracker.h:144
aruco::FractalPoseTracker::_preRadius
float _preRadius
Definition: fractalposetracker.h:145
aruco::FractalPoseTracker::_kdtree
picoflann::KdTreeIndex< 2, PicoFlann_KeyPointAdapter > _kdtree
Definition: fractalposetracker.h:142
aruco::CameraParameters::Distorsion
cv::Mat Distorsion
Definition: cameraparameters.h:35
aruco::FractalPoseTracker::fractal_solve_ransac
cv::Mat fractal_solve_ransac(int ninners, std::vector< std::pair< uint, std::vector< uint >>> inner_kpnt, std::vector< cv::KeyPoint > kpnts, uint32_t maxIter=500, float _minInliers=0.2f, float _thresInliers=0.7f)
fractal_solve_ransac
Definition: fractalposetracker.cpp:900
aruco::solvePnP
std::vector< cv::Mat > solvePnP(const std::vector< cv::Point3f > &objPoints, const std::vector< cv::Point2f > &imgPoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs)
Definition: ippe.cpp:88

aruco
Author(s): Rafael Muñoz Salinas , Bence Magyar
autogenerated on Sat Sep 23 2023 02:26:45