MRPT  2.0.4
vision_utils.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <mrpt/math/geometry.h>
15 #include <mrpt/math/ops_matrices.h>
16 #include <mrpt/math/ops_vectors.h>
17 #include <mrpt/math/utils.h>
21 #include <mrpt/poses/CPoint3D.h>
22 #include <mrpt/system/CTicTac.h>
23 #include <mrpt/system/filesystem.h>
24 #include <mrpt/vision/CFeature.h>
26 #include <mrpt/vision/pinhole.h>
27 #include <mrpt/vision/utils.h>
28 #include <Eigen/Dense>
29 
30 using namespace mrpt;
31 using namespace mrpt::vision;
32 using namespace mrpt::img;
33 using namespace mrpt::config;
34 using namespace mrpt::maps;
35 using namespace mrpt::tfest;
36 using namespace mrpt::math;
37 using namespace mrpt::system;
38 using namespace mrpt::poses;
39 using namespace mrpt::obs;
40 using namespace std;
41 const int FEAT_FREE = -1;
42 // const int NOT_ASIG = 0;
43 // const int ASG_FEAT = 1;
44 // const int AMB_FEAT = 2;
45 
46 /*-------------------------------------------------------------
47  openCV_cross_correlation
48 -------------------------------------------------------------*/
50  const CImage& img, const CImage& patch_img, size_t& x_max, size_t& y_max,
51  double& max_val, int x_search_ini, int y_search_ini, int x_search_size,
52  int y_search_size)
53 {
55 
56 #if MRPT_HAS_OPENCV
57  bool entireImg =
58  (x_search_ini < 0 || y_search_ini < 0 || x_search_size < 0 ||
59  y_search_size < 0);
60 
61  CImage im, patch_im;
62 
63  if (img.isColor() && patch_img.isColor())
64  {
65  img.grayscale(im);
66  patch_img.grayscale(patch_im);
67  }
68  else
69  {
70  ASSERT_(!img.isColor() && !patch_img.isColor());
71  im = img.makeShallowCopy();
72  patch_im = patch_img.makeShallowCopy();
73  }
74 
75  const int im_w = im.getWidth();
76  const int im_h = im.getHeight();
77  const int patch_w = patch_im.getWidth();
78  const int patch_h = patch_im.getHeight();
79 
80  if (entireImg)
81  {
82  x_search_size = im_w - patch_w;
83  y_search_size = im_h - patch_h;
84  }
85 
86  // JLBC: Perhaps is better to raise the exception always??
87  if ((x_search_ini + x_search_size + patch_w) > im_w)
88  x_search_size -= (x_search_ini + x_search_size + patch_w) - im_w;
89 
90  if ((y_search_ini + y_search_size + patch_h) > im_h)
91  y_search_size -= (y_search_ini + y_search_size + patch_h) - im_h;
92 
93  ASSERT_((x_search_ini + x_search_size + patch_w) <= im_w);
94  ASSERT_((y_search_ini + y_search_size + patch_h) <= im_h);
95  CImage img_region_to_search;
96 
97  if (entireImg)
98  {
99  img_region_to_search = im.makeShallowCopy();
100  }
101  else
102  {
103  im.extract_patch(
104  img_region_to_search,
105  x_search_ini, // start corner
106  y_search_ini,
107  patch_w + x_search_size, // sub-image size
108  patch_h + y_search_size);
109  }
110 
111  cv::Mat result(cvSize(x_search_size + 1, y_search_size + 1), CV_32FC1);
112 
113  // Compute cross correlation:
114  cv::matchTemplate(
115  img_region_to_search.asCvMat<cv::Mat>(SHALLOW_COPY),
116  patch_im.asCvMat<cv::Mat>(SHALLOW_COPY), result, CV_TM_CCORR_NORMED);
117 
118  // Find the max point:
119  double mini;
120  cv::Point min_point, max_point;
121 
122  cv::minMaxLoc(
123  result, &mini, &max_val, &min_point, &max_point, cv::noArray());
124  x_max = max_point.x + x_search_ini + (mrpt::round(patch_w - 1) >> 1);
125  y_max = max_point.y + y_search_ini + (mrpt::round(patch_h - 1) >> 1);
126 
127 #else
128  THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
129 #endif
130 
131  MRPT_END
132 }
133 
134 /*-------------------------------------------------------------
135  pixelTo3D
136 -------------------------------------------------------------*/
138 {
139  TPoint3D res;
140 
141  // Build the vector:
142  res.x = xy.x - A(0, 2);
143  res.y = xy.y - A(1, 2);
144  res.z = A(0, 0);
145 
146  // Normalize:
147  const double u = res.norm();
148  ASSERT_(u != 0);
149  res *= 1.0 / u;
150 
151  return res;
152 }
153 
154 /*-------------------------------------------------------------
155  buildIntrinsicParamsMatrix
156 -------------------------------------------------------------*/
158  const double focalLengthX, const double focalLengthY, const double centerX,
159  const double centerY)
160 {
162 
163  A(0, 0) = focalLengthX;
164  A(1, 1) = focalLengthY;
165  A(2, 2) = 1;
166 
167  A(0, 2) = centerX;
168  A(1, 2) = centerY;
169 
170  return A;
171 }
172 
173 /*-------------------------------------------------------------
174  defaultIntrinsicParamsMatrix
175 -------------------------------------------------------------*/
177  unsigned int camIndex, unsigned int resX, unsigned int resY)
178 {
179  float fx, fy, cx, cy;
180 
181  switch (camIndex)
182  {
183  case 0:
184  // Bumblebee:
185  fx = 0.79345f;
186  fy = 1.05793f;
187  cx = 0.55662f;
188  cy = 0.52692f;
189  break;
190 
191  case 1:
192  // Sony:
193  fx = 0.95666094f;
194  fy = 1.3983423f;
195  cx = 0.54626328f;
196  cy = 0.4939191f;
197  break;
198 
199  default:
200  {
202  "Unknown camera index!! for 'camIndex'=%u", camIndex);
203  }
204  }
205 
207  resX * fx, resY * fy, resX * cx, resY * cy);
208 }
209 
210 /*-------------------------------------------------------------
211  computeMsd
212 -------------------------------------------------------------*/
214  const TMatchingPairList& feat_list, const poses::CPose3D& Rt)
215 {
216  CMatrixDouble44 mat;
217  Rt.getHomogeneousMatrix(mat);
218  double acum = 0.0;
219 
220  TMatchingPairList::const_iterator it;
221  TPoint3D err;
222  for (it = feat_list.begin(); it != feat_list.end(); it++)
223  {
224  err.x = it->other_x - (it->this_x * mat(0, 0) + it->this_y * mat(0, 1) +
225  it->this_z * mat(0, 2) + Rt.x());
226  err.y = it->other_y - (it->this_x * mat(1, 0) + it->this_y * mat(1, 1) +
227  it->this_z * mat(1, 2) + Rt.y());
228  err.z = it->other_z - (it->this_x * mat(2, 0) + it->this_y * mat(2, 1) +
229  it->this_z * mat(2, 2) + Rt.z());
230 
231  acum += err.norm();
232 
233  } // end for
234  return (acum / feat_list.size());
235 } // end msd
236 
237 /*-------------------------------------------------------------
238  cloudsToMatchedList
239 -------------------------------------------------------------*/
241  const CObservationVisualLandmarks& cloud1,
242  const CObservationVisualLandmarks& cloud2, TMatchingPairList& outList)
243 {
245  TMatchingPair pair;
246 
247  for (itLand1 = cloud1.landmarks.landmarks.begin();
248  itLand1 != cloud1.landmarks.landmarks.end(); itLand1++)
249  for (itLand2 = cloud2.landmarks.landmarks.begin();
250  itLand2 != cloud2.landmarks.landmarks.end(); itLand2++)
251  if (itLand1->ID == itLand2->ID)
252  {
253  // Match found!
254  pair.this_idx = pair.other_idx = (unsigned int)itLand1->ID;
255 
256  pair.this_x = itLand1->pose_mean.x;
257  pair.this_y = itLand1->pose_mean.y;
258  pair.this_z = itLand1->pose_mean.z;
259 
260  pair.other_x = itLand2->pose_mean.x;
261  pair.other_y = itLand2->pose_mean.y;
262  pair.other_z = itLand2->pose_mean.z;
263 
264  outList.push_back(pair);
265  } // end if
266 } // end-cloudsToMatchedList
267 
268 /*-------------------------------------------------------------
269  computeMainOrientation
270 -------------------------------------------------------------*/
272  const CImage& image, unsigned int x, unsigned int y)
273 {
274  MRPT_START
275  float orientation = 0;
276  if ((int(x) - 1 >= 0) && (int(y) - 1 >= 0) && (x + 1 < image.getWidth()) &&
277  (y + 1 < image.getHeight()))
278  orientation = (float)atan2(
279  (double)*image(x, y + 1) - (double)*image(x, y - 1),
280  (double)*image(x + 1, y) - (double)*image(x - 1, y));
281 
282  // Convert from [-pi,pi] to [0,2pi]
283 
284  return orientation;
285 
286  MRPT_END
287 } // end vision::computeMainOrientation
288 
289 /*-------------------------------------------------------------
290  normalizeImage
291 -------------------------------------------------------------*/
292 void vision::normalizeImage(const CImage& image, CImage& nimage)
293 {
294  ASSERT_(image.getChannelCount() == 1);
295  nimage.resize(image.getWidth(), image.getHeight(), image.getChannelCount());
296 
297  CMatrixFloat im, nim;
298  nim.resize(image.getHeight(), image.getWidth());
299 
300  image.getAsMatrix(im);
301 
302  double m, s;
303  mrpt::math::meanAndStd(im, m, s);
304 
305  for (int k1 = 0; k1 < (int)nim.cols(); ++k1)
306  for (int k2 = 0; k2 < (int)nim.rows(); ++k2)
307  nim(k2, k1) = (im(k2, k1) - m) / s;
308 
309  nimage.setFromMatrix(nim);
310 }
311 
312 /*-------------------------------------------------------------
313  matchFeatures
314 -------------------------------------------------------------*/
316  const CFeatureList& list1, const CFeatureList& list2,
317  CMatchedFeatureList& matches, const TMatchingOptions& options,
319 {
320  // Clear the output structure
321  MRPT_START
322  // matches.clear();
323 
324  // Preliminary comprobations
325  size_t sz1 = list1.size(), sz2 = list2.size();
326 
327  ASSERT_((sz1 > 0) && (sz2 > 0)); // Both lists have features within it
328  ASSERT_(
329  list1.get_type() ==
330  list2.get_type()); // Both lists must be of the same type
331 
332  CFeatureList::const_iterator itList1, itList2; // Iterators for the lists
333 
334  // For SIFT & SURF
335  float distDesc; // EDD or EDSD
336  float minDist1; // Minimum EDD or EDSD
337  float minDist2; // Second minimum EDD or EDSD
338 
339  // For Harris
340  double maxCC1; // Maximum CC
341  double maxCC2; // Second maximum CC
342 
343  // For SAD
344  double minSAD1, minSAD2;
345 
346  vector<int> idxLeftList, idxRightList;
347  idxLeftList.resize(sz1, FEAT_FREE);
348  idxRightList.resize(sz2, FEAT_FREE);
349  vector<double> distCorrs(sz1);
350  int lFeat, rFeat;
351  int minLeftIdx = 0, minRightIdx;
352  int nMatches = 0;
353 
354  // For each feature in list1 ...
355  for (lFeat = 0, itList1 = list1.begin(); itList1 != list1.end();
356  ++itList1, ++lFeat)
357  {
358  // For SIFT & SURF
359  minDist1 = 1e5;
360  minDist2 = 1e5;
361 
362  // For Harris
363  maxCC1 = 0;
364  maxCC2 = 0;
365 
366  // For SAD
367  minSAD1 = 1e5;
368  minSAD2 = 1e5;
369 
370  // For all the cases
371  minRightIdx = 0;
372 
373  for (rFeat = 0, itList2 = list2.begin(); itList2 != list2.end();
374  ++itList2, ++rFeat) // ... compare with all the features in list2.
375  {
376  // Filter out by epipolar constraint
377  double d = 0.0; // Distance to the epipolar line
378  if (options.useEpipolarRestriction)
379  {
380  if (options.parallelOpticalAxis)
381  d = itList1->keypoint.pt.y - itList2->keypoint.pt.y;
382  else
383  {
384  ASSERT_(options.hasFundamentalMatrix);
385 
386  // Compute epipolar line Ax + By + C = 0
387  TLine2D epiLine;
388  TPoint2D oPoint(
389  itList2->keypoint.pt.x, itList2->keypoint.pt.y);
390 
391  CMatrixDouble31 l, p;
392  p(0, 0) = itList1->keypoint.pt.x;
393  p(1, 0) = itList1->keypoint.pt.y;
394  p(2, 0) = 1;
395 
396  l = params.F * p;
397 
398  epiLine.coefs[0] = l(0, 0);
399  epiLine.coefs[1] = l(1, 0);
400  epiLine.coefs[2] = l(2, 0);
401 
402  d = epiLine.distance(oPoint);
403  } // end else
404  } // end if
405 
406  // Use epipolar restriction
407  bool c1 = options.useEpipolarRestriction
408  ? fabs(d) < options.epipolar_TH
409  : true;
410  // Use x-coord restriction
411  bool c2 = options.useXRestriction
412  ? itList1->keypoint.pt.x - itList2->keypoint.pt.x > 0
413  : true;
414 
415  if (c1 && c2)
416  {
417  switch (options.matching_method)
418  {
420  {
421  // Ensure that both features have SIFT descriptors
422  ASSERT_(
423  itList1->descriptors.hasDescriptorSIFT() &&
424  itList2->descriptors.hasDescriptorSIFT());
425 
426  // Compute the Euclidean distance between descriptors
427  distDesc = itList1->descriptorSIFTDistanceTo(*itList2);
428 
429  // Search for the two minimum values
430  if (distDesc < minDist1)
431  {
432  minDist2 = minDist1;
433  minDist1 = distDesc;
434  minLeftIdx = lFeat;
435  minRightIdx = rFeat;
436  }
437  else if (distDesc < minDist2)
438  minDist2 = distDesc;
439 
440  break;
441  } // end mmDescriptorSIFT
442 
444  {
445  size_t u, v; // Coordinates of the peak
446  double res; // Value of the peak
447 
448  // Ensure that both features have patches
449  ASSERT_(
450  itList1->patchSize > 0 && itList2->patchSize > 0);
452  *itList1->patch, *itList2->patch, u, v, res);
453 
454  // Search for the two maximum values
455  if (res > maxCC1)
456  {
457  maxCC2 = maxCC1;
458  maxCC1 = res;
459  minLeftIdx = lFeat;
460  minRightIdx = rFeat;
461  }
462  else if (res > maxCC2)
463  maxCC2 = res;
464 
465  break;
466  } // end mmCorrelation
467 
469  {
470  // Ensure that both features have SURF descriptors
471  ASSERT_(
472  itList1->descriptors.hasDescriptorSURF() &&
473  itList2->descriptors.hasDescriptorSURF());
474 
475  // Compute the Euclidean distance between descriptors
476  distDesc = itList1->descriptorSURFDistanceTo(*itList2);
477 
478  // Search for the two minimum values
479  if (distDesc < minDist1)
480  {
481  minDist2 = minDist1;
482  minDist1 = distDesc;
483  minLeftIdx = lFeat;
484  minRightIdx = rFeat;
485  }
486  else if (distDesc < minDist2)
487  minDist2 = distDesc;
488 
489  break; // end case featSURF
490  } // end mmDescriptorSURF
491 
493  {
494  // Ensure that both features have SURF descriptors
495  ASSERT_(
496  itList1->descriptors.hasDescriptorORB() &&
497  itList2->descriptors.hasDescriptorORB());
498  distDesc = itList1->descriptorORBDistanceTo(*itList2);
499 
500  // Search for the two minimum values
501  if (distDesc < minDist1)
502  {
503  minDist2 = minDist1;
504  minDist1 = distDesc;
505  minLeftIdx = lFeat;
506  minRightIdx = rFeat;
507  }
508  else if (distDesc < minDist2)
509  minDist2 = distDesc;
510 
511  break;
512  } // end mmDescriptorORB
513 
515  {
516  // Ensure that both features have patches
517  ASSERT_(
518  itList1->patchSize > 0 &&
519  itList2->patchSize == itList1->patchSize);
520 #if !MRPT_HAS_OPENCV
522  "MRPT has been compiled without OpenCV");
523 #else
524  const CImage aux1(
525  *itList1->patch, FAST_REF_OR_CONVERT_TO_GRAY);
526  const CImage aux2(
527  *itList2->patch, FAST_REF_OR_CONVERT_TO_GRAY);
528  const auto h = aux1.getHeight(), w = aux1.getWidth();
529 
530  double res = 0;
531  for (unsigned int ii = 0; ii < h; ++ii)
532  for (unsigned int jj = 0; jj < w; ++jj)
533  res += std::abs(
534  static_cast<double>(
535  aux1.at<uint8_t>(jj, ii)) -
536  static_cast<double>(
537  aux2.at<uint8_t>(jj, ii)));
538  res = res / (255.0 * w * h);
539 
540  if (res < minSAD1)
541  {
542  minSAD2 = minSAD1;
543  minSAD1 = res;
544  minLeftIdx = lFeat;
545  minRightIdx = rFeat;
546  }
547  else if (res < minSAD2)
548  minSAD2 = res;
549 #endif
550  break;
551  } // end mmSAD
552  } // end switch
553  } // end if
554  } // end for 'list2' (right features)
555 
556  bool cond1 = false, cond2 = false;
557  double minVal = 1.0;
558  switch (options.matching_method)
559  {
561  cond1 = minDist1 < options.maxEDD_TH; // Maximum Euclidean
562  // Distance between SIFT
563  // descriptors (EDD)
564  cond2 = (minDist1 / minDist2) <
565  options.EDD_RATIO; // Ratio between the two lowest EDSD
566  minVal = minDist1;
567  break;
569  cond1 = maxCC1 >
570  options.minCC_TH; // Minimum cross correlation value
571  cond2 = (maxCC2 / maxCC1) <
572  options.rCC_TH; // Ratio between the two highest cross
573  // correlation values
574  minVal = 1 - maxCC1;
575  break;
577  cond1 = minDist1 < options.maxEDSD_TH; // Maximum Euclidean
578  // Distance between SURF
579  // descriptors (EDSD)
580  cond2 =
581  (minDist1 / minDist2) <
582  options.EDSD_RATIO; // Ratio between the two lowest EDSD
583  minVal = minDist1;
584  break;
586  cond1 = minSAD1 < options.maxSAD_TH;
587  cond2 = (minSAD1 / minSAD2) < options.SAD_RATIO;
588  minVal = minSAD1;
589  break;
591  cond1 = minDist1 < options.maxORB_dist;
592  cond2 = true;
593  minVal = minDist1;
594  break;
595  default:
596  THROW_EXCEPTION("Invalid value of 'matching_method'");
597  }
598 
599  // PROCESS THE RESULTS
600  if (cond1 && cond2) // The minimum distance must be below a threshold
601  {
602  int auxIdx = idxRightList[minRightIdx];
603  if (auxIdx != FEAT_FREE)
604  {
605  if (distCorrs[auxIdx] > minVal)
606  {
607  // We've found a better match
608  distCorrs[minLeftIdx] = minVal;
609  idxLeftList[minLeftIdx] = minRightIdx;
610  idxRightList[minRightIdx] = minLeftIdx;
611 
612  distCorrs[auxIdx] = 1.0;
613  idxLeftList[auxIdx] = FEAT_FREE;
614  } // end-if
615  } // end-if
616  else
617  {
618  idxRightList[minRightIdx] = minLeftIdx;
619  idxLeftList[minLeftIdx] = minRightIdx;
620  distCorrs[minLeftIdx] = minVal;
621  nMatches++;
622  }
623  } // end if
624  } // end for 'list1' (left features)
625 
626  if (!options.addMatches) matches.clear();
627 
628  TFeatureID idLeft = 0, idRight = 0;
629  if (!matches.empty()) matches.getMaxID(bothLists, idLeft, idRight);
630 
631  for (int vCnt = 0; vCnt < (int)idxLeftList.size(); ++vCnt)
632  {
633  if (idxLeftList[vCnt] != FEAT_FREE)
634  {
635  std::pair<CFeature, CFeature> thisMatch;
636 
637  bool isGood = true;
638  double dp1 = -1.0, dp2 = -1.0;
639  TPoint3D p3D = TPoint3D();
640  if (options.estimateDepth && options.parallelOpticalAxis)
641  {
643  list1[vCnt], list2[idxLeftList[vCnt]], p3D, params);
644  dp1 = sqrt(p3D.x * p3D.x + p3D.y * p3D.y + p3D.z * p3D.z);
645  dp2 = sqrt(
646  (p3D.x - params.baseline) * (p3D.x - params.baseline) +
647  p3D.y * p3D.y + p3D.z * p3D.z);
648 
649  if (dp1 > options.maxDepthThreshold ||
650  dp2 > options.maxDepthThreshold)
651  isGood = false;
652  } // end-if
653 
654  if (isGood)
655  {
656  // Set the features
657  thisMatch.first = list1[vCnt];
658  thisMatch.second = list2[idxLeftList[vCnt]];
659 
660  // Update the max ID value
661  if (matches.empty())
662  {
663  idLeft = thisMatch.first.keypoint.ID;
664  idRight = thisMatch.second.keypoint.ID;
665  }
666  else
667  {
668  keep_max(idLeft, thisMatch.first.keypoint.ID);
669  matches.setLeftMaxID(idLeft);
670 
671  keep_max(idRight, thisMatch.second.keypoint.ID);
672  matches.setRightMaxID(idRight);
673  }
674 
675  // Set the depth and the 3D position of the feature
676  if (options.estimateDepth && options.parallelOpticalAxis)
677  {
678  thisMatch.first.initialDepth = dp1;
679  thisMatch.first.p3D = p3D;
680 
681  thisMatch.second.initialDepth = dp2;
682  thisMatch.second.p3D =
683  TPoint3D(p3D.x - params.baseline, p3D.y, p3D.z);
684  } // end-if
685 
686  // Insert the match into the matched list
687  matches.push_back(thisMatch);
688  } // end-if-isGood
689  } // end-if
690  } // end-for-matches
691  return matches.size();
692 
693  MRPT_END
694 }
695 
696 /*-------------------------------------------------------------
697  generateMask
698 -------------------------------------------------------------*/
699 // Insert zeros around the points in mList according to wSize
701  const CMatchedFeatureList& mList, CMatrixBool& mask1, CMatrixBool& mask2,
702  int wSize)
703 {
704  ASSERT_(mList.size() > 0);
705 
706  // cv::Mat *mask1 = static_cast<cv::Mat*>(_mask1);
707  // cv::Mat *mask2 = static_cast<cv::Mat*>(_mask2);
708 
709  int hwsize = (int)(0.5 * wSize);
710  int mx = mask1.cols(), my = mask1.rows();
711 
712  int idx, idy;
713  CMatchedFeatureList::const_iterator it;
714  for (it = mList.begin(); it != mList.end(); ++it)
715  {
716  for (int ii = -hwsize; ii < hwsize; ++ii)
717  for (int jj = -hwsize; jj < hwsize; ++jj)
718  {
719  idx = (int)(it->first.keypoint.pt.x) + ii;
720  idy = (int)(it->first.keypoint.pt.y) + jj;
721  if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
722  mask1(idy, idx) = false;
723  }
724 
725  for (int ii = -hwsize; ii < hwsize; ++ii)
726  for (int jj = -hwsize; jj < hwsize; ++jj)
727  {
728  idx = (int)(it->second.keypoint.pt.x) + ii;
729  idy = (int)(it->second.keypoint.pt.y) + jj;
730  if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
731  mask2(idy, idx) = false;
732  }
733  } // end-for
734 } // end generateMask
735 
736 double vision::computeSAD(const CImage& p1, const CImage& p2)
737 {
738  MRPT_START
739 #if MRPT_HAS_OPENCV
740  ASSERT_(p1.getSize() == p2.getSize());
741  const auto w = p1.getWidth(), h = p1.getHeight();
742  double res = 0.0;
743  for (unsigned int ii = 0; ii < h; ++ii)
744  for (unsigned int jj = 0; jj < w; ++jj)
745  res += std::abs(
746  static_cast<double>(p1.at<uint8_t>(jj, ii)) -
747  static_cast<double>(p2.at<uint8_t>(jj, ii)));
748 
749  return res / (255.0 * w * h);
750 #else
752  "MRPT compiled without OpenCV, can't compute SAD of images!");
753 #endif
754  MRPT_END
755 }
756 
758  const CImage& inImg, const CFeatureList& theList, CImage& outImg)
759 {
760  outImg = inImg; // Create a copy of the input image
761  for (const auto& it : theList)
762  outImg.rectangle(
763  it.keypoint.pt.x - 5, it.keypoint.pt.y - 5, it.keypoint.pt.x + 5,
764  it.keypoint.pt.y + 5, TColor(255, 0, 0));
765 }
766 
767 /*-------------------------------------------------------------
768  projectMatchedFeatures
769 -------------------------------------------------------------*/
771  const CMatchedFeatureList& matches,
772  const mrpt::img::TStereoCamera& stereo_camera, vector<TPoint3D>& out_points)
773 {
774  out_points.clear();
775  out_points.reserve(matches.size());
776  for (const auto& match : matches)
777  {
778  const auto& pt1 = match.first.keypoint.pt;
779  const auto& pt2 = match.second.keypoint.pt;
780 
781  const double disp = pt1.x - pt2.x;
782  if (disp < 1) continue;
783 
784  const double b_d = stereo_camera.rightCameraPose.x / disp;
785  out_points.emplace_back(
786  (pt1.x - stereo_camera.leftCamera.cx()) * b_d,
787  (pt1.y - stereo_camera.leftCamera.cy()) * b_d,
788  stereo_camera.leftCamera.fx() * b_d);
789  }
790 }
791 /*-------------------------------------------------------------
792  projectMatchedFeatures
793 -------------------------------------------------------------*/
795  const CFeatureList& leftList, const CFeatureList& rightList,
796  vector<TPoint3D>& vP3D, const TStereoSystemParams& params)
797 {
798  vP3D.reserve(leftList.size());
800  for (it1 = leftList.begin(), it2 = rightList.begin(); it1 != leftList.end();
801  ++it1, ++it2)
802  {
803  TPoint3D p3D;
804  projectMatchedFeature(*it1, *it2, p3D, params);
805  if (p3D.z < params.maxZ && p3D.z > params.minZ && p3D.y < params.maxY)
806  vP3D.push_back(p3D);
807  }
808 } // end projectMatchedFeatures
809 
810 /*-------------------------------------------------------------
811  projectMatchedFeatures
812 -------------------------------------------------------------*/
814  const CFeature& leftFeat, const CFeature& rightFeat, TPoint3D& p3D,
816 {
817  const double f0 = 600;
818  double nfx1 = leftFeat.keypoint.pt.x, nfy1 = leftFeat.keypoint.pt.y,
819  nfx2 = rightFeat.keypoint.pt.x, nfy2 = rightFeat.keypoint.pt.y;
820 
821  const double x = nfx1 * f0; // x = (x / f0) * f0 x = x
822  const double y = nfy1 * f0; // y = (y / f0) * f0 y = y
823  const double xd = nfx2 * f0; // x' = (x' / f0) * f0 x' = x'
824  const double yd = nfy2 * f0; // y' = (y' / f0) * f0 y' = y'
825 
826  const double f2 = f0 * f0;
827  const double p9 = f2 * params.F(2, 2);
828  const double Q00 =
829  f2 *
830  (params.F(0, 2) * params.F(0, 2) + params.F(1, 2) * params.F(1, 2) +
831  params.F(2, 0) * params.F(2, 0) + params.F(2, 1) * params.F(2, 1));
832 
833  double Jh = (std::numeric_limits<double>::max)(); // J hat = ‡
834  double xh = x; // x hat = x
835  double yh = y; // y hat = y
836  double xhd = xd; // x hat dash = x'
837  double yhd = yd; // y hat dash = y'
838  double
839  xt = 0,
840  yt = 0, xtd = 0,
841  ytd =
842  0; // x tilde = 0, y tilde = 0, x tilde dash = 0, y tilde dash = 0
843  for (;;)
844  {
845  const double p1 = (xh * xhd + xhd * xt + xh * xtd) * params.F(0, 0);
846  const double p2 = (xh * yhd + yhd * xt + xh * ytd) * params.F(0, 1);
847  const double p3 = (f0 * (xh + xt)) * params.F(0, 2);
848  const double p4 = (yh * xhd + xhd * yt + yh * xtd) * params.F(1, 0);
849  const double p5 = (yh * yhd + yhd * yt + yh * ytd) * params.F(1, 1);
850  const double p6 = (f0 * (yh + yt)) * params.F(1, 2);
851  const double p7 = (f0 * (xhd + xtd)) * params.F(2, 0);
852  const double p8 = (f0 * (yhd + ytd)) * params.F(2, 1);
853 
854  const double udotxi = p1 + p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;
855 
856  const double Q11 =
857  (xh * xh + xhd * xhd) * params.F(0, 0) * params.F(0, 0);
858  const double Q22 =
859  (xh * xh + yhd * yhd) * params.F(0, 1) * params.F(0, 1);
860  const double Q44 =
861  (yh * yh + xhd * xhd) * params.F(1, 0) * params.F(1, 0);
862  const double Q55 =
863  (yh * yh + yhd * yhd) * params.F(1, 1) * params.F(1, 1);
864  const double Q12 = xhd * yhd * params.F(0, 0) * params.F(0, 1);
865  const double Q13 = f0 * xhd * params.F(0, 0) * params.F(0, 2);
866  const double Q14 = xh * yh * params.F(0, 0) * params.F(1, 0);
867  const double Q17 = f0 * xh * params.F(0, 0) * params.F(2, 0);
868  const double Q23 = f0 * yhd * params.F(0, 1) * params.F(0, 2);
869  const double Q25 = xh * yh * params.F(0, 1) * params.F(1, 1);
870  const double Q28 = f0 * xh * params.F(0, 1) * params.F(2, 1);
871  const double Q45 = xhd * yhd * params.F(1, 0) * params.F(1, 1);
872  const double Q46 = f0 * xhd * params.F(1, 0) * params.F(1, 2);
873  const double Q47 = f0 * yh * params.F(1, 0) * params.F(2, 0);
874  const double Q56 = f0 * yhd * params.F(1, 1) * params.F(1, 2);
875  const double Q58 = f0 * yh * params.F(1, 1) * params.F(2, 1);
876 
877  const double udotV0xiu = Q00 + Q11 + Q22 + Q44 + Q55 +
878  2.0 * (Q12 + Q13 + Q14 + Q17 + Q23 + Q25 +
879  Q28 + Q45 + Q46 + Q47 + Q56 + Q58);
880 
881  ASSERT_(fabs(udotV0xiu) > 1e-5);
882 
883  const double C = udotxi / udotV0xiu;
884 
885  xt = C * (params.F(0, 0) * xhd + params.F(0, 1) * yhd +
886  f0 * params.F(0, 2));
887  yt = C * (params.F(1, 0) * xhd + params.F(1, 1) * yhd +
888  f0 * params.F(1, 2));
889  xtd = C *
890  (params.F(0, 0) * xh + params.F(1, 0) * yh + f0 * params.F(2, 0));
891  ytd = C *
892  (params.F(0, 1) * xh + params.F(1, 1) * yh + f0 * params.F(2, 1));
893 
894  const double Jt = xt * xt + yt * yt + xtd * xtd + ytd * ytd;
895  // cout << "Jt:" << Jt << " and Jh: " << Jh << endl;
896  if ((std::abs)(Jt - Jh) <= 1e-5)
897  {
898  nfx1 = xh / f0;
899  nfy1 = yh / f0;
900  nfx2 = xhd / f0;
901  // nfy2 = yhd / f0;
902 
903  break;
904  }
905  else
906  {
907  Jh = Jt; // J hat = J tilde
908  xh = x - xt; // x hat = x - x tilde
909  yh = y - yt; // y hat = y - y tilde
910  xhd = xd - xtd; // x hat dash = x' - x tilde dash
911  yhd = yd - ytd; // y hat dash = y' - y tilde dash
912  }
913  } // end for
914 
915  double disp = nfx1 - nfx2;
916  double aux = params.baseline / disp;
917  p3D.x = (nfx1 - params.K(0, 2)) * aux;
918  p3D.y = (nfy1 - params.K(1, 2)) * aux;
919  p3D.z = params.K(0, 0) * aux;
920 } // end projectMatchedFeature
921 
922 /*-------------------------------------------------------------
923  projectMatchedFeatures
924 -------------------------------------------------------------*/
926  CMatchedFeatureList& mfList, const TStereoSystemParams& param,
927  CLandmarksMap& landmarks)
928 {
929  MRPT_START
930 
931  landmarks.clear(); // Assert that the output CLandmarksMap is clear
932 
933  float stdPixel2 = square(param.stdPixel);
934  float stdDisp2 = square(param.stdDisp);
935 
936  // Main loop
937  for (auto itList = mfList.begin(); itList != mfList.end();)
938  {
939  const auto& pt1 = itList->first.keypoint.pt;
940  const auto& pt2 = itList->second.keypoint.pt;
941 
942  const float disp = pt1.x - pt2.x; // Disparity
943  if (disp < 1e-9)
944  {
945  // Filter out too far points
946  // Erase the match
947  itList = mfList.erase(itList);
948  continue;
949  }
950 
951  float x3D = (pt1.x - param.K(0, 2)) * ((param.baseline)) / disp;
952  float y3D = (pt1.y - param.K(1, 2)) * ((param.baseline)) / disp;
953  float z3D = (param.K(0, 0)) * ((param.baseline)) / disp;
954 
955  // Filter out bad points
956  if ((z3D < param.minZ) || (z3D > param.maxZ))
957  {
958  itList = mfList.erase(itList);
959  }
960  else
961  {
962  TPoint3D p3D(x3D, y3D, z3D);
963 
964  // STORE THE OBTAINED LANDMARK
965  CLandmark lm;
966 
967  TPoint3D norm3D = p3D;
968  norm3D *= -1 / norm3D.norm();
969 
970  lm.normal = norm3D;
971  lm.pose_mean = p3D;
972  lm.ID = itList->first.keypoint.ID;
973 
974  // If the matched landmarks has a (SIFT or SURF) descriptor,
975  // asign the left one to the landmark.
976  // TO DO: Assign the mean value of the descriptor (between the
977  // matches)
978  lm.features.resize(1);
979  lm.features[0] = (*itList).first;
980 
981  // Compute the covariance matrix for the landmark
982  switch (param.uncPropagation)
983  {
985  {
986  float foc2 = square(param.K(0, 0));
987  float c0 = param.K(0, 2);
988  float r0 = param.K(1, 2);
989  float base2 = square(param.baseline);
990  float disp2 = square(disp);
991 
992  lm.pose_cov_11 =
993  stdPixel2 * base2 / disp2 +
994  stdDisp2 * base2 * square(pt1.x - c0) / square(disp2);
995  lm.pose_cov_12 = stdDisp2 * base2 * (pt1.x - c0) *
996  (pt1.y - r0) / square(disp2);
997  lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
998  (pt1.x - c0) / square(disp2);
999  lm.pose_cov_22 =
1000  stdPixel2 * base2 / disp2 +
1001  stdDisp2 * base2 * square(pt1.y - r0) / square(disp2);
1002  lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
1003  (pt1.y - r0) / square(disp2);
1004  lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
1005  } // end case 'Prop_Linear'
1006  break;
1007 
1009  {
1010  // Parameters
1011  unsigned int Na = 3;
1012  unsigned int i;
1013 
1014  float k = param.factor_k;
1015 
1016  float w0 = k / (Na + k);
1017  float w1 = 1 / (2 * (Na + k));
1018 
1019  CMatrixF Pa(3, 3);
1020  CMatrixF L(3, 3);
1021 
1022  Pa.fill(0);
1023  Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
1024  Pa(2, 2) = (Na + k) * square(param.stdDisp);
1025 
1026  // Cholesky decomposition
1027  Pa.chol(L); // math::chol(Pa,L);
1028 
1029  vector<TPoint3D> B; // B group
1030  TPoint3D meanB; // Mean value of the B group
1031  CMatrixF Pb; // Covariance of the B group
1032 
1033  B.resize(2 * Na + 1); // Set of output values
1034  Pb.fill(0); // Reset the output covariance
1035 
1036  CVectorFloat vAux, myPoint; // Auxiliar vectors
1037  CVectorFloat meanA; // Mean value of the A group
1038 
1039  vAux.resize(3); // Set the variables size
1040  meanA.resize(3);
1041  myPoint.resize(3);
1042 
1043  // Mean input value: (c,r,d)
1044  meanA[0] = pt1.x;
1045  meanA[1] = pt1.y;
1046  meanA[2] = disp;
1047 
1048  // Output mean
1049  meanB.x = w0 * x3D;
1050  meanB.y = w0 * y3D;
1051  meanB.z = w0 * z3D; // Add to the mean
1052  B[0].x = x3D;
1053  B[0].y = y3D;
1054  B[0].z = z3D; // Insert into B
1055 
1056  for (i = 1; i <= 2 * Na; i++)
1057  {
1058  // Form the Ai value
1059  if (i <= Na)
1060  {
1061  // Extract the proper row
1062  vAux.asEigen() = L.asEigen().row(i - 1);
1063  myPoint[0] = meanA[0] + vAux[0];
1064  myPoint[1] = meanA[1] + vAux[1];
1065  myPoint[2] = meanA[2] + vAux[2];
1066  }
1067  else
1068  {
1069  vAux.asEigen() = L.asEigen().row((i - Na) - 1);
1070  myPoint[0] = meanA[0] - vAux[0];
1071  myPoint[1] = meanA[1] - vAux[1];
1072  myPoint[2] = meanA[2] - vAux[2];
1073  }
1074 
1075  // Pass the Ai through the functions:
1076  x3D = (myPoint[0] - param.K(0, 2)) *
1077  ((param.baseline)) / myPoint[2];
1078  y3D = (myPoint[1] - param.K(1, 2)) *
1079  ((param.baseline)) / myPoint[2];
1080  z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];
1081 
1082  // Add to the B mean computation and the B vector
1083  meanB.x = meanB.x + w1 * x3D;
1084  meanB.y = meanB.y + w1 * y3D;
1085  meanB.z = meanB.z + w1 * z3D;
1086 
1087  B[i].x = x3D;
1088  B[i].y = y3D;
1089  B[i].z = z3D;
1090 
1091  } // end for 'i'
1092 
1093  // Output covariance
1094  for (i = 0; i <= 2 * Na; i++)
1095  {
1096  float weight = w1;
1097  CMatrixF v(3, 1);
1098 
1099  if (i == 0) // The weight for the mean value of A is w0
1100  weight = w0;
1101 
1102  v(0, 0) = B[i].x - meanB.x;
1103  v(1, 0) = B[i].y - meanB.y;
1104  v(2, 0) = B[i].z - meanB.z;
1105 
1106  Pb.asEigen() +=
1107  (weight * (v.asEigen() * v.transpose())).eval();
1108  } // end for 'i'
1109 
1110  // Store it in the landmark
1111  lm.pose_cov_11 = Pb(0, 0);
1112  lm.pose_cov_12 = Pb(0, 1);
1113  lm.pose_cov_13 = Pb(0, 2);
1114  lm.pose_cov_22 = Pb(1, 1);
1115  lm.pose_cov_23 = Pb(1, 2);
1116  lm.pose_cov_33 = Pb(2, 2);
1117  } // end case 'Prop_UT'
1118  break;
1119 
1121  {
1122  // Parameters
1123  unsigned int Na = 3;
1124  unsigned int i;
1125 
1126  float a = param.factor_a;
1127  float b = param.factor_b;
1128  float k = param.factor_k;
1129 
1130  float lambda = square(a) * (Na + k) - Na;
1131 
1132  float w0_m = lambda / (Na + lambda);
1133  float w0_c = w0_m + (1 - square(a) + b);
1134  float w1 = 1 / (2 * (Na + lambda));
1135 
1136  CMatrixF Pa(3, 3);
1137  CMatrixF L(3, 3);
1138 
1139  Pa.fill(0);
1140  Pa(0, 0) = Pa(1, 1) =
1141  (Na + lambda) * square(param.stdPixel);
1142  Pa(2, 2) = (Na + lambda) * square(param.stdDisp);
1143 
1144  // Cholesky decomposition
1145  Pa.chol(L); // math::chol(Pa,L);
1146 
1147  vector<TPoint3D> B; // B group
1148  TPoint3D meanB; // Mean value of the B group
1149  CMatrixF Pb; // Covariance of the B group
1150 
1151  B.resize(2 * Na + 1); // Set of output values
1152  Pb.fill(0); // Reset the output covariance
1153 
1154  CVectorFloat vAux, myPoint; // Auxiliar vectors
1155  CVectorFloat meanA; // Mean value of the A group
1156 
1157  vAux.resize(3); // Set the variables size
1158  meanA.resize(3);
1159  myPoint.resize(3);
1160 
1161  // Mean input value: (c,r,d)
1162  meanA[0] = pt1.x;
1163  meanA[1] = pt1.y;
1164  meanA[2] = disp;
1165 
1166  // Output mean
1167  meanB.x = w0_m * x3D;
1168  meanB.y = w0_m * y3D;
1169  meanB.z = w0_m * z3D; // Add to the mean
1170  B[0].x = x3D;
1171  B[0].y = y3D;
1172  B[0].z = z3D; // Insert into B
1173 
1174  for (i = 1; i <= 2 * Na; i++)
1175  {
1176  // Form the Ai value
1177  if (i <= Na)
1178  {
1179  // Extract the proper row
1180  vAux.asEigen() = L.row(i - 1);
1181  myPoint = meanA + vAux;
1182  // myPoint[0] = meanA[0] + vAux[0];
1183  // myPoint[1] = meanA[1] + vAux[1];
1184  // myPoint[2] = meanA[2] + vAux[2];
1185  }
1186  else
1187  {
1188  vAux = L.row((i - Na) - 1);
1189  myPoint = meanA - vAux;
1190  // myPoint[0] = meanA[0] - vAux[0];
1191  // myPoint[1] = meanA[1] - vAux[1];
1192  // myPoint[2] = meanA[2] - vAux[2];
1193  }
1194 
1195  // Pass the Ai through the functions:
1196  x3D = (myPoint[0] - param.K(0, 2)) *
1197  ((param.baseline)) / myPoint[2];
1198  y3D = (myPoint[1] - param.K(1, 2)) *
1199  ((param.baseline)) / myPoint[2];
1200  z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];
1201 
1202  // Add to the B mean computation and the B vector
1203  meanB.x = meanB.x + w1 * x3D;
1204  meanB.y = meanB.y + w1 * y3D;
1205  meanB.z = meanB.z + w1 * z3D;
1206 
1207  B[i].x = x3D;
1208  B[i].y = y3D;
1209  B[i].z = z3D;
1210 
1211  } // end for 'i'
1212 
1213  // Output covariance
1214  for (i = 0; i <= 2 * Na; i++)
1215  {
1216  float weight = w1;
1217  CMatrixF v(3, 1);
1218 
1219  if (i == 0) // The weight for the mean value of A is w0
1220  weight = w0_c;
1221 
1222  v(0, 0) = B[i].x - meanB.x;
1223  v(1, 0) = B[i].y - meanB.y;
1224  v(2, 0) = B[i].z - meanB.z;
1225 
1226  Pb.asEigen() +=
1227  (weight * (v.asEigen() * v.transpose())).eval();
1228  } // end for 'i'
1229 
1230  // Store it in the landmark
1231  lm.pose_cov_11 = Pb(0, 0);
1232  lm.pose_cov_12 = Pb(0, 1);
1233  lm.pose_cov_13 = Pb(0, 2);
1234  lm.pose_cov_22 = Pb(1, 1);
1235  lm.pose_cov_23 = Pb(1, 2);
1236  lm.pose_cov_33 = Pb(2, 2);
1237  } // end case 'Prop_SUT'
1238  break;
1239 
1240  } // end switch
1241  landmarks.landmarks.push_back(lm);
1242  itList++;
1243  } // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
1244  } // end for 'i'
1245 
1246  MRPT_END
1247 } // end of projectMatchedFeatures
1248 
1249 /*-------------------------------------------------------------
1250  projectMatchedFeatures
1251 -------------------------------------------------------------*/
1253  CFeatureList& leftList, CFeatureList& rightList,
1254  const vision::TStereoSystemParams& param,
1255  mrpt::maps::CLandmarksMap& landmarks)
1256 {
1257  MRPT_START
1258  ASSERT_(leftList.size() == rightList.size());
1259 
1260  landmarks.clear(); // Assert that the output CLandmarksMap is clear
1261 
1262  CFeatureList::iterator itListL, itListR;
1263  float stdPixel2 = square(param.stdPixel);
1264  float stdDisp2 = square(param.stdDisp);
1265 
1266  // Main loop
1267  for (itListL = leftList.begin(), itListR = rightList.begin();
1268  itListL != leftList.end();)
1269  {
1270  const auto& ptL = itListL->keypoint.pt;
1271  const auto& ptR = itListR->keypoint.pt;
1272 
1273  float disp = ptL.x - ptR.x; // Disparity
1274  if (disp < 1e-9) // Filter out too far points
1275  {
1276  itListL = leftList.erase(itListL);
1277  itListR = rightList.erase(itListR);
1278  continue;
1279  }
1280 
1281  // Too much distant features are not taken into account
1282  float x3D = (ptL.x - param.K(0, 2)) * param.baseline / disp;
1283  float y3D = (ptL.y - param.K(1, 2)) * param.baseline / disp;
1284  float z3D = (param.K(0, 0)) * param.baseline / disp;
1285 
1286  // Filter out bad points
1287  if ((z3D < param.minZ) || (z3D > param.maxZ))
1288  {
1289  itListL = leftList.erase(itListL);
1290  itListR = rightList.erase(itListR);
1291  }
1292  else
1293  {
1294  TPoint3D p3D(x3D, y3D, z3D);
1295 
1296  // STORE THE OBTAINED LANDMARK
1297  CLandmark lm;
1298 
1299  TPoint3D norm3D = p3D;
1300  norm3D *= -1. / norm3D.norm();
1301 
1302  lm.normal = norm3D;
1303  lm.pose_mean = p3D;
1304  lm.ID = itListL->keypoint.ID;
1305 
1306  // If the matched landmarks has a (SIFT or SURF) descriptor,
1307  // asign the left one to the landmark.
1308  // TO DO: Assign the mean value of the descriptor (between the
1309  // matches)
1310  lm.features.resize(2);
1311  lm.features[0] = *itListL;
1312  lm.features[1] = *itListR;
1313 
1314  // Compute the covariance matrix for the landmark
1315  switch (param.uncPropagation)
1316  {
1318  {
1319  float foc2 = square(param.K(0, 0));
1320  float c0 = param.K(0, 2);
1321  float r0 = param.K(1, 2);
1322  float base2 = square(param.baseline);
1323  float disp2 = square(ptL.x - ptR.x);
1324 
1325  lm.pose_cov_11 =
1326  stdPixel2 * base2 / disp2 +
1327  stdDisp2 * base2 * square(ptL.x - c0) / square(disp2);
1328  lm.pose_cov_12 = stdDisp2 * base2 * (ptL.x - c0) *
1329  (ptL.y - r0) / square(disp2);
1330  lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
1331  (ptL.x - c0) / square(disp2);
1332  lm.pose_cov_22 =
1333  stdPixel2 * base2 / disp2 +
1334  stdDisp2 * base2 * square(ptL.y - r0) / square(disp2);
1335  lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
1336  (ptL.y - r0) / square(disp2);
1337  lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
1338  } // end case 'Prop_Linear'
1339  break;
1340 
1342  {
1343  // Parameters
1344  unsigned int Na = 3;
1345  unsigned int i;
1346 
1347  float k = param.factor_k;
1348 
1349  float w0 = k / (Na + k);
1350  float w1 = 1 / (2 * (Na + k));
1351 
1352  CMatrixF Pa(3, 3);
1353  CMatrixF L(3, 3);
1354 
1355  Pa.fill(0);
1356  Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
1357  Pa(2, 2) = (Na + k) * square(param.stdDisp);
1358 
1359  // Cholesky decomposition
1360  Pa.chol(L); // math::chol(Pa,L);
1361 
1362  vector<TPoint3D> B; // B group
1363  TPoint3D meanB; // Mean value of the B group
1364  CMatrixF Pb; // Covariance of the B group
1365 
1366  B.resize(2 * Na + 1); // Set of output values
1367  Pb.fill(0); // Reset the output covariance
1368 
1369  CVectorFloat vAux, myPoint; // Auxiliar vectors
1370  CVectorFloat meanA; // Mean value of the A group
1371 
1372  vAux.resize(3); // Set the variables size
1373  meanA.resize(3);
1374  myPoint.resize(3);
1375 
1376  // Mean input value: (c,r,d)
1377  meanA[0] = ptL.x;
1378  meanA[1] = ptL.y;
1379  meanA[2] = disp;
1380 
1381  // Output mean
1382  meanB.x = w0 * x3D;
1383  meanB.y = w0 * y3D;
1384  meanB.z = w0 * z3D; // Add to the mean
1385  B[0].x = x3D;
1386  B[0].y = y3D;
1387  B[0].z = z3D; // Insert into B
1388 
1389  for (i = 1; i <= 2 * Na; i++)
1390  {
1391  // Form the Ai value
1392  if (i <= Na)
1393  {
1394  vAux.asEigen() = L.col(i - 1);
1395  myPoint[0] = meanA[0] + vAux[0];
1396  myPoint[1] = meanA[1] + vAux[1];
1397  myPoint[2] = meanA[2] + vAux[2];
1398  }
1399  else
1400  {
1401  vAux = L.col((i - Na) - 1);
1402  myPoint[0] = meanA[0] - vAux[0];
1403  myPoint[1] = meanA[1] - vAux[1];
1404  myPoint[2] = meanA[2] - vAux[2];
1405  }
1406 
1407  // Pass the Ai through the functions:
1408  x3D = (myPoint[0] - param.K(0, 2)) *
1409  ((param.baseline)) / myPoint[2];
1410  y3D = (myPoint[1] - param.K(1, 2)) *
1411  ((param.baseline)) / myPoint[2];
1412  z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];
1413 
1414  // Add to the B mean computation and the B vector
1415  meanB.x = meanB.x + w1 * x3D;
1416  meanB.y = meanB.y + w1 * y3D;
1417  meanB.z = meanB.z + w1 * z3D;
1418 
1419  B[i].x = x3D;
1420  B[i].y = y3D;
1421  B[i].z = z3D;
1422 
1423  } // end for 'i'
1424 
1425  // Output covariance
1426  for (i = 0; i <= 2 * Na; i++)
1427  {
1428  float weight = w1;
1429  CMatrixF v(3, 1);
1430 
1431  if (i == 0) // The weight for the mean value of A is w0
1432  weight = w0;
1433 
1434  v(0, 0) = B[i].x - meanB.x;
1435  v(1, 0) = B[i].y - meanB.y;
1436  v(2, 0) = B[i].z - meanB.z;
1437 
1438  Pb.asEigen() +=
1439  (weight * (v.asEigen() * v.transpose())).eval();
1440  } // end for 'i'
1441 
1442  // Store it in the landmark
1443  lm.pose_cov_11 = Pb(0, 0);
1444  lm.pose_cov_12 = Pb(0, 1);
1445  lm.pose_cov_13 = Pb(0, 2);
1446  lm.pose_cov_22 = Pb(1, 1);
1447  lm.pose_cov_23 = Pb(1, 2);
1448  lm.pose_cov_33 = Pb(2, 2);
1449  } // end case 'Prop_UT'
1450  break;
1451 
1453  {
1454  // Parameters
1455  unsigned int Na = 3;
1456  unsigned int i;
1457 
1458  float a = param.factor_a;
1459  float b = param.factor_b;
1460  float k = param.factor_k;
1461 
1462  float lambda = square(a) * (Na + k) - Na;
1463 
1464  float w0_m = lambda / (Na + lambda);
1465  float w0_c = w0_m + (1 - square(a) + b);
1466  float w1 = 1 / (2 * (Na + lambda));
1467 
1468  CMatrixF Pa(3, 3);
1469  CMatrixF L(3, 3);
1470 
1471  Pa.fill(0);
1472  Pa(0, 0) = Pa(1, 1) =
1473  (Na + lambda) * square(param.stdPixel);
1474  Pa(2, 2) = (Na + lambda) * square(param.stdDisp);
1475 
1476  // Cholesky decomposition
1477  Pa.chol(L); // math::chol(Pa,L);
1478 
1479  vector<TPoint3D> B; // B group
1480  TPoint3D meanB; // Mean value of the B group
1481  CMatrixF Pb; // Covariance of the B group
1482 
1483  B.resize(2 * Na + 1); // Set of output values
1484  Pb.fill(0); // Reset the output covariance
1485 
1486  CVectorFloat vAux, myPoint; // Auxiliar vectors
1487  CVectorFloat meanA; // Mean value of the A group
1488 
1489  vAux.resize(3); // Set the variables size
1490  meanA.resize(3);
1491  myPoint.resize(3);
1492 
1493  // Mean input value: (c,r,d)
1494  meanA[0] = ptL.x;
1495  meanA[1] = ptL.y;
1496  meanA[2] = disp;
1497 
1498  // Output mean
1499  meanB.x = w0_m * x3D;
1500  meanB.y = w0_m * y3D;
1501  meanB.z = w0_m * z3D; // Add to the mean
1502  B[0].x = x3D;
1503  B[0].y = y3D;
1504  B[0].z = z3D; // Insert into B
1505 
1506  for (i = 1; i <= 2 * Na; i++)
1507  {
1508  // Form the Ai value
1509  if (i <= Na)
1510  {
1511  vAux = L.row(i - 1);
1512  myPoint = meanA + vAux;
1513  }
1514  else
1515  {
1516  vAux = L.col((i - Na) - 1);
1517  myPoint = meanA - vAux;
1518  }
1519 
1520  // Pass the Ai through the functions:
1521  x3D = (myPoint[0] - param.K(0, 2)) *
1522  ((param.baseline)) / myPoint[2];
1523  y3D = (myPoint[1] - param.K(1, 2)) *
1524  ((param.baseline)) / myPoint[2];
1525  z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];
1526 
1527  // Add to the B mean computation and the B vector
1528  meanB.x = meanB.x + w1 * x3D;
1529  meanB.y = meanB.y + w1 * y3D;
1530  meanB.z = meanB.z + w1 * z3D;
1531 
1532  B[i].x = x3D;
1533  B[i].y = y3D;
1534  B[i].z = z3D;
1535 
1536  } // end for 'i'
1537 
1538  // Output covariance
1539  for (i = 0; i <= 2 * Na; i++)
1540  {
1541  float weight = w1;
1542  CMatrixF v(3, 1);
1543 
1544  if (i == 0) // The weight for the mean value of A is w0
1545  weight = w0_c;
1546 
1547  v(0, 0) = B[i].x - meanB.x;
1548  v(1, 0) = B[i].y - meanB.y;
1549  v(2, 0) = B[i].z - meanB.z;
1550 
1551  Pb.asEigen() +=
1552  (weight * (v.asEigen() * v.transpose())).eval();
1553  } // end for 'i'
1554 
1555  // Store it in the landmark
1556  lm.pose_cov_11 = Pb(0, 0);
1557  lm.pose_cov_12 = Pb(0, 1);
1558  lm.pose_cov_13 = Pb(0, 2);
1559  lm.pose_cov_22 = Pb(1, 1);
1560  lm.pose_cov_23 = Pb(1, 2);
1561  lm.pose_cov_33 = Pb(2, 2);
1562  } // end case 'Prop_SUT'
1563  break;
1564 
1565  } // end switch
1566  landmarks.landmarks.push_back(lm);
1567  itListL++;
1568  itListR++;
1569  } // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
1570  } // end for 'i'
1571 
1572  MRPT_END
1573 }
1574 
1575 /* -------------------------------------------------------
1576  StereoObs2RBObs #1
1577  ------------------------------------------------------- */
1579  const CMatchedFeatureList& inMatches,
1580  const CMatrixDouble33& intrinsicParams, double baseline,
1581  const CPose3D& sensorPose, const vector<double>& sg,
1582  CObservationBearingRange& outObs)
1583 {
1584  // Compute the range and bearing
1585  double f = intrinsicParams(0, 0); // Focal length in pixels
1586  double x0 = intrinsicParams(0, 2); // Principal point column
1587  double y0 = intrinsicParams(1, 2); // Principal point row
1588  double b = baseline; // Stereo camera baseline
1589  double sg_c2 = square(sg[0]); // Sigma of the column variable
1590  double sg_r2 = square(sg[1]); // Sigma of the row variable
1591  double sg_d2 = square(sg[2]); // Sigma of the disparity
1592 
1593  for (const auto& inMatche : inMatches)
1594  {
1595  double x = inMatche.first.keypoint.pt.x; // Column of the feature
1596  double y = inMatche.first.keypoint.pt.y; // Row of the feature
1597 
1598  double d = x - inMatche.second.keypoint.pt.x; // Disparity
1599  double d2 = square(d);
1600  double k = square(b / d);
1601 
1602  // Projection equations according to a standard camera coordinate axis
1603  // (+Z forward & +Y downwards)
1604  // -------------------------------------------------------------------------------------------------------
1605  double X = (x - x0) * b / d;
1606  double Y = (y - y0) * b / d;
1607  double Z = f * b / d;
1608 
1609  // Projection equations according to a standard coordinate axis (+X
1610  // forward & +Z upwards)
1611  // -------------------------------------------------------------------------------------------------------
1612  // double X = f * b / d;
1613  // double Y = ( x0 - x ) * b / d;
1614  // double Z = ( y0 - y ) * b / d;
1615 
1617  m.range = sqrt(square(X) + square(Y) + square(Z));
1618  m.yaw = atan2(Y, X);
1619  m.pitch = -asin(Z / m.range);
1620  m.landmarkID = inMatche.first.keypoint.ID;
1621 
1622  // Compute the covariance
1623  // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
1624  // \---------------------------------------/
1625  // aux
1626 
1627  CMatrixDouble33 aux;
1628 
1629  // Jacobian equations according to a standard CAMERA coordinate axis (+Z
1630  // forward & +Y downwards)
1631  // -------------------------------------------------------------------------------------------------------
1632  aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
1633  aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
1634  aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);
1635 
1636  aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
1637  aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);
1638 
1639  aux(2, 2) = k * (sg_d2 * square(f) / d2);
1640 
1641  // Jacobian equations according to a standard coordinate axis (+X
1642  // forward & +Z upwards)
1643  // -------------------------------------------------------------------------------------------------------
1644  CMatrixDouble33 JG;
1645  JG(0, 0) = X / m.range;
1646  JG(0, 1) = Y / m.range;
1647  JG(0, 2) = Z / m.range;
1648 
1649  JG(1, 0) = -Y / (square(X) + square(Y));
1650  JG(1, 1) = X / (square(X) + square(Y));
1651  JG(1, 2) = 0;
1652 
1653  JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
1654  JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
1655  JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);
1656 
1658 
1659  outObs.sensedData.push_back(m);
1660 
1661  } // end for
1662 
1663  // Indicate that the covariances have been calculated (for compatibility
1664  // with earlier versions)
1665  outObs.validCovariances = true;
1666  outObs.setSensorPose(sensorPose);
1667 } // end-StereoObs2BRObs
1668 
1669 /* -------------------------------------------------------
1670  StereoObs2RBObs #2
1671  ------------------------------------------------------- */
1673  const CObservationStereoImages& inObs, const vector<double>& sg,
1674  CObservationBearingRange& outObs)
1675 {
1676  // Local variables
1677  CFeatureExtraction fExt;
1678  CFeatureList leftList, rightList;
1679  CMatchedFeatureList matchList;
1680  unsigned int id = 0;
1681 
1682  // Extract features
1683  fExt.detectFeatures(inObs.imageLeft, leftList);
1684  fExt.detectFeatures(inObs.imageRight, rightList);
1685 
1686  // DEBUG:
1687  // CDisplayWindow win1, win2;
1688  // win1.showImageAndPoints( inObs.imageLeft, leftList );
1689  // win2.showImageAndPoints( inObs.imageRight, rightList );
1690 
1691  // Match features
1692  // size_t nMatches =
1693  vision::matchFeatures(leftList, rightList, matchList);
1694 
1695  // Compute the range and bearing
1696  double f = inObs.leftCamera.fx(); // Focal length in pixels
1697  double x0 = inObs.leftCamera.cx(); // Principal point column
1698  double y0 = inObs.leftCamera.cy(); // Principal point row
1699  double b = inObs.rightCameraPose.x(); // Stereo camera baseline
1700  double sg_c2 = square(sg[0]); // Sigma of the column variable
1701  double sg_r2 = square(sg[1]); // Sigma of the row variable
1702  double sg_d2 = square(sg[2]); // Sigma of the disparity
1703 
1704  for (auto itMatchList = matchList.begin(); itMatchList != matchList.end();
1705  itMatchList++, id++)
1706  {
1707  double x = itMatchList->first.keypoint.pt.x; // Column of the feature
1708  double y = itMatchList->first.keypoint.pt.y; // Row of the feature
1709 
1710  double d = x - itMatchList->second.keypoint.pt.x; // Disparity
1711  double d2 = square(d);
1712  double k = square(b / d);
1713 
1714  // Projection equations according to a standard camera coordinate axis
1715  // (+Z forward & +Y downwards)
1716  // -------------------------------------------------------------------------------------------------------
1717  double X = (x - x0) * b / d;
1718  double Y = (y - y0) * b / d;
1719  double Z = f * b / d;
1720 
1721  // Projection equations according to a standard coordinate axis (+X
1722  // forward & +Z upwards)
1723  // -------------------------------------------------------------------------------------------------------
1724  // double X = f * b / d;
1725  // double Y = ( x0 - x ) * b / d;
1726  // double Z = ( y0 - y ) * b / d;
1727 
1729  m.range = sqrt(square(X) + square(Y) + square(Z));
1730  // m.yaw = atan2( Y,X );
1731  // m.pitch = -asin( Z/m.range );
1732  m.yaw = atan2(X, Z);
1733  m.pitch = atan2(Y, Z);
1734 
1735  // Compute the covariance
1736  // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
1737  // \---------------------------------------/
1738  // aux
1739 
1740  CMatrixDouble33 aux;
1741 
1742  // Jacobian equations according to a standard CAMERA coordinate axis (+Z
1743  // forward & +Y downwards)
1744  // -------------------------------------------------------------------------------------------------------
1745  aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
1746  aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
1747  aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);
1748 
1749  aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
1750  aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);
1751 
1752  aux(2, 2) = k * (sg_d2 * square(f) / d2);
1753 
1754  // Jacobian equations according to a standard coordinate axis (+X
1755  // forward & +Z upwards)
1756  // -------------------------------------------------------------------------------------------------------
1757  CMatrixDouble33 JG;
1758  JG(0, 0) = X / m.range;
1759  JG(0, 1) = Y / m.range;
1760  JG(0, 2) = Z / m.range;
1761 
1762  JG(1, 0) = -Y / (square(X) + square(Y));
1763  JG(1, 1) = X / (square(X) + square(Y));
1764  JG(1, 2) = 0;
1765 
1766  JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
1767  JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
1768  JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);
1769 
1770  // JF.multiply_HCHt( diag, aux );
1772 
1773  m.landmarkID = id;
1774  outObs.sensedData.push_back(m);
1775  outObs.fieldOfView_yaw = 2 * fabs(atan2(-x0, f));
1776  outObs.fieldOfView_pitch = 2 * fabs(atan2(-y0, f));
1777 
1778  } // end for
1779 
1780  // Indicate that the covariances have been calculated (for compatibility
1781  // with earlier versions)
1782  outObs.validCovariances = true;
1784 
1785 } // end StereoObs2BRObs
1786 
1787 /* -------------------------------------------------------
1788  StereoObs2RBObs #3
1789  ------------------------------------------------------- */
1792 {
1793  // For each of the 3D landmarks [X,Y,Z] we compute their range and bearing
1794  // representation.
1795  // The reference system is assumed to be that typical of cameras: +Z forward
1796  // and +X to the right.
1798  for (itCloud = inObs.landmarks.landmarks.begin();
1799  itCloud != inObs.landmarks.landmarks.end(); ++itCloud)
1800  {
1802  m.range = sqrt(
1803  square(itCloud->pose_mean.x) + square(itCloud->pose_mean.y) +
1804  square(itCloud->pose_mean.z));
1805 
1806  // The reference system is assumed to be that typical robot operation:
1807  // +X forward and +Z upwards.
1808  m.yaw = atan2(itCloud->pose_mean.y, itCloud->pose_mean.x);
1809  m.pitch = -sin(itCloud->pose_mean.z / m.range);
1810  m.landmarkID = itCloud->ID;
1811 
1812  outObs.sensedData.push_back(m);
1813  } // end for
1814 } // end StereoObs2BRObs
1815 
1816 /* -------------------------------------------------------
1817  computeStereoRectificationMaps
1818  ------------------------------------------------------- */
1820  const TCamera& cam1, const TCamera& cam2,
1821  const poses::CPose3D& rightCameraPose, void* outMap1x, void* outMap1y,
1822  void* outMap2x, void* outMap2y)
1823 {
1824  ASSERT_(cam1.ncols == cam2.ncols && cam1.nrows == cam2.nrows);
1825 
1826 #if MRPT_HAS_OPENCV
1827 
1828  cv::Mat *mapx1, *mapy1, *mapx2, *mapy2;
1829  mapx1 = static_cast<cv::Mat*>(outMap1x);
1830  mapy1 = static_cast<cv::Mat*>(outMap1y);
1831  mapx2 = static_cast<cv::Mat*>(outMap2x);
1832  mapy2 = static_cast<cv::Mat*>(outMap2y);
1833 
1834  const int resX = cam1.ncols;
1835  const int resY = cam1.nrows;
1836 
1837  CMatrixDouble44 hMatrix;
1838  rightCameraPose.getHomogeneousMatrix(hMatrix);
1839 
1840  double rcTrans[3];
1841  rcTrans[0] = hMatrix(0, 3);
1842  rcTrans[1] = hMatrix(1, 3);
1843  rcTrans[2] = hMatrix(2, 3);
1844 
1845  double m1[3][3];
1846  for (unsigned int i = 0; i < 3; ++i)
1847  for (unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
1848 
1849  double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
1850  for (unsigned int i = 0; i < 3; ++i)
1851  for (unsigned int j = 0; j < 3; ++j)
1852  {
1853  ipl[i][j] = cam1.intrinsicParams(i, j);
1854  ipr[i][j] = cam2.intrinsicParams(i, j);
1855  }
1856 
1857  for (unsigned int i = 0; i < 5; ++i)
1858  {
1859  dpl[i] = cam1.dist[i];
1860  dpr[i] = cam2.dist[i];
1861  }
1862 
1863  cv::Mat R(3, 3, CV_64F, &m1);
1864  cv::Mat T(3, 1, CV_64F, &rcTrans);
1865 
1866  cv::Mat K1(3, 3, CV_64F, ipl);
1867  cv::Mat K2(3, 3, CV_64F, ipr);
1868  cv::Mat D1(1, 5, CV_64F, dpl);
1869  cv::Mat D2(1, 5, CV_64F, dpr);
1870 
1871  double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
1872  cv::Mat R1(3, 3, CV_64F, _R1);
1873  cv::Mat R2(3, 3, CV_64F, _R2);
1874  cv::Mat P1(3, 4, CV_64F, _P1);
1875  cv::Mat P2(3, 4, CV_64F, _P2);
1876  cv::Mat Q(4, 4, CV_64F, _Q);
1877 
1878  cv::Size nSize(resX, resY);
1879  double alpha = 0.0; // alpha value: 0.0 = zoom and crop the image so that
1880  // there's not black areas
1881 
1882  // OpenCV 2.3+ has this signature:
1883  cv::stereoRectify(
1884  K1, D1, K2, D2, nSize, R, T, R1, R2, P1, P2, Q,
1885  cv::CALIB_ZERO_DISPARITY, alpha);
1886  // Rest of arguments -> default
1887 
1888  cv::Size sz1, sz2;
1889  cv::initUndistortRectifyMap(
1890  K1, D1, R1, P1, cv::Size(resX, resY), CV_32FC1, *mapx1, *mapy1);
1891  cv::initUndistortRectifyMap(
1892  K2, D2, R2, P2, cv::Size(resX, resY), CV_32FC1, *mapx2, *mapy2);
1893 /**/
1894 #else
1895  THROW_EXCEPTION("The MRPT has been compiled without OPENCV!");
1896 #endif
1897 } // end computeStereoRectificationMaps
1898 
1900 
1901 {
1902  K = defaultIntrinsicParamsMatrix(0, 640, 480);
1903  F.setZero();
1904  F(1, 2) = -1;
1905  F(2, 1) = 1;
1906 }
1907 
1908 /*-------------------------------------------------------------
1909  TStereoSystemParams: loadFromConfigFile
1910 -------------------------------------------------------------*/
1912  const CConfigFileBase& iniFile, const string& section)
1913 {
1914  int unc;
1915  unc = iniFile.read_int(section.c_str(), "uncPropagation", uncPropagation);
1916  switch (unc)
1917  {
1918  case 0:
1919  uncPropagation = Prop_Linear;
1920  break;
1921  case 1:
1922  uncPropagation = Prop_UT;
1923  break;
1924  case 2:
1925  uncPropagation = Prop_SUT;
1926  break;
1927  } // end switch
1928 
1929  CVectorDouble k_vec(9);
1930  iniFile.read_vector(
1931  section.c_str(), "k_vec", CVectorDouble(), k_vec, false);
1932  for (unsigned int ii = 0; ii < 3; ++ii)
1933  for (unsigned int jj = 0; jj < 3; ++jj) K(ii, jj) = k_vec[ii * 3 + jj];
1934 
1935  CVectorDouble f_vec(9);
1936  iniFile.read_vector(
1937  section.c_str(), "f_vec", CVectorDouble(), f_vec, false);
1938  for (unsigned int ii = 0; ii < 3; ++ii)
1939  for (unsigned int jj = 0; jj < 3; ++jj) F(ii, jj) = f_vec[ii * 3 + jj];
1940 
1941  baseline = iniFile.read_float(section.c_str(), "baseline", baseline);
1942  stdPixel = iniFile.read_float(section.c_str(), "stdPixel", stdPixel);
1943  stdDisp = iniFile.read_float(section.c_str(), "stdDisp", stdDisp);
1944  maxZ = iniFile.read_float(section.c_str(), "maxZ", maxZ);
1945  minZ = iniFile.read_float(section.c_str(), "minZ", minZ);
1946  maxY = iniFile.read_float(section.c_str(), "maxY", maxY);
1947  factor_k = iniFile.read_float(section.c_str(), "factor_k", factor_k);
1948  factor_a = iniFile.read_float(section.c_str(), "factor_a", factor_a);
1949  factor_b = iniFile.read_float(section.c_str(), "factor_b", factor_b);
1950 } // end of loadFromConfigFile
1951 
1952 /*---------------------------------------------------------------
1953  TStereoSystemParams: dumpToTextStream
1954  ---------------------------------------------------------------*/
1956 {
1957  out << "\n----------- [vision::TStereoSystemParams] ------------ \n";
1958  out << "Method for 3D Uncert. \t= ";
1959  switch (uncPropagation)
1960  {
1961  case Prop_Linear:
1962  out << "Linear propagation\n";
1963  break;
1964  case Prop_UT:
1965  out << "Unscented Transform\n";
1966  break;
1967  case Prop_SUT:
1968  out << "Scaled Unscented Transform\n";
1969  break;
1970  } // end switch
1971 
1972  out << mrpt::format("K\t\t\t= [%f\t%f\t%f]\n", K(0, 0), K(0, 1), K(0, 2));
1973  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", K(1, 0), K(1, 1), K(1, 2));
1974  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", K(2, 0), K(2, 1), K(2, 2));
1975 
1976  out << mrpt::format("F\t\t\t= [%f\t%f\t%f]\n", F(0, 0), F(0, 1), F(0, 2));
1977  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", F(1, 0), F(1, 1), F(1, 2));
1978  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", F(2, 0), F(2, 1), F(2, 2));
1979 
1980  out << mrpt::format("Baseline \t\t= %f\n", baseline);
1981  out << mrpt::format("Pixel std \t\t= %f\n", stdPixel);
1982  out << mrpt::format("Disparity std\t\t= %f\n", stdDisp);
1983  out << mrpt::format("Z maximum\t\t= %f\n", maxZ);
1984  out << mrpt::format("Z minimum\t\t= %f\n", minZ);
1985  out << mrpt::format("Y maximum\t\t= %f\n", maxY);
1986 
1987  out << mrpt::format("k Factor [UT]\t\t= %f\n", factor_k);
1988  out << mrpt::format("a Factor [UT]\t\t= %f\n", factor_a);
1989  out << mrpt::format("b Factor [UT]\t\t= %f\n", factor_b);
1990  out << "-------------------------------------------------------- \n";
1991 }
1992 
1993 /*-------------------------------------------------------------
1994  TMatchingOptions: constructor
1995 -------------------------------------------------------------*/
1997 
1998  = default; // end constructor TMatchingOptions
1999 
2000 /*-------------------------------------------------------------
2001  TMatchingOptions: loadFromConfigFile
2002 -------------------------------------------------------------*/
2004  const CConfigFileBase& iniFile, const string& section)
2005 {
2006  int mm =
2007  iniFile.read_int(section.c_str(), "matching_method", matching_method);
2008  switch (mm)
2009  {
2010  case 0:
2011  matching_method = mmCorrelation;
2012  break;
2013  case 1:
2014  matching_method = mmDescriptorSIFT;
2015  break;
2016  case 2:
2017  matching_method = mmDescriptorSURF;
2018  break;
2019  case 3:
2020  matching_method = mmSAD;
2021  break;
2022  case 4:
2023  matching_method = mmDescriptorORB;
2024  break;
2025  } // end switch
2026 
2027  useEpipolarRestriction = iniFile.read_bool(
2028  section.c_str(), "useEpipolarRestriction", useEpipolarRestriction);
2029  hasFundamentalMatrix = iniFile.read_bool(
2030  section.c_str(), "hasFundamentalMatrix", hasFundamentalMatrix);
2031  parallelOpticalAxis = iniFile.read_bool(
2032  section.c_str(), "parallelOpticalAxis", parallelOpticalAxis);
2033  useXRestriction =
2034  iniFile.read_bool(section.c_str(), "useXRestriction", useXRestriction);
2035  addMatches = iniFile.read_bool(section.c_str(), "addMatches", addMatches);
2036  useDisparityLimits = iniFile.read_bool(
2037  section.c_str(), "useDisparityLimits", useDisparityLimits);
2038 
2039  min_disp = iniFile.read_float(section.c_str(), "min_disp", min_disp);
2040  max_disp = iniFile.read_float(section.c_str(), "max_disp", max_disp);
2041 
2042  epipolar_TH =
2043  iniFile.read_float(section.c_str(), "epipolar_TH", epipolar_TH);
2044  maxEDD_TH = iniFile.read_float(section.c_str(), "maxEDD_TH", maxEDD_TH);
2045  EDD_RATIO = iniFile.read_float(section.c_str(), "minDIF_TH", EDD_RATIO);
2046  minCC_TH = iniFile.read_float(section.c_str(), "minCC_TH", minCC_TH);
2047  minDCC_TH = iniFile.read_float(section.c_str(), "minDCC_TH", minDCC_TH);
2048  rCC_TH = iniFile.read_float(section.c_str(), "rCC_TH", rCC_TH);
2049  maxEDSD_TH = iniFile.read_float(section.c_str(), "maxEDSD_TH", maxEDSD_TH);
2050  EDSD_RATIO = iniFile.read_float(section.c_str(), "EDSD_RATIO", EDSD_RATIO);
2051  maxSAD_TH = iniFile.read_float(section.c_str(), "maxSAD_TH", maxSAD_TH);
2052  SAD_RATIO = iniFile.read_float(section.c_str(), "SAD_RATIO", SAD_RATIO);
2053  maxORB_dist =
2054  iniFile.read_float(section.c_str(), "maxORB_dist", maxORB_dist);
2055 
2056  estimateDepth =
2057  iniFile.read_bool(section.c_str(), "estimateDepth", estimateDepth);
2058  maxDepthThreshold = iniFile.read_float(
2059  section.c_str(), "maxDepthThreshold", maxDepthThreshold);
2060  // fx = iniFile.read_float(section.c_str(),"fx",fx);
2061  // cx = iniFile.read_float(section.c_str(),"cx",cx);
2062  // cy = iniFile.read_float(section.c_str(),"cy",cy);
2063  // baseline =
2064  // iniFile.read_float(section.c_str(),"baseline",baseline);
2065 } // end TMatchingOptions::loadFromConfigFile
2066 
2067 /*---------------------------------------------------------------
2068  TMatchingOptions: dumpToTextStream
2069  ---------------------------------------------------------------*/
2070 void TMatchingOptions::dumpToTextStream(std::ostream& out) const
2071 {
2072  out << "\n----------- [vision::TMatchingOptions] ------------ \n";
2073  out << "Matching method: ";
2074  switch (matching_method)
2075  {
2076  case mmCorrelation:
2077  out << "Cross Correlation\n";
2078  out << mrpt::format(
2079  "· Min. CC. Threshold: %f\n", minCC_TH);
2080  out << mrpt::format(
2081  "· Min. Dif. CC Threshold: %f\n", minDCC_TH);
2082  out << mrpt::format("· Max. Ratio CC Threshold: %f\n", rCC_TH);
2083  break;
2084  case mmDescriptorSIFT:
2085  out << "SIFT descriptor\n";
2086  out << mrpt::format(
2087  "· Max. EDD Threshold: %f\n", maxEDD_TH);
2088  out << mrpt::format(
2089  "· EDD Ratio: %f\n", EDD_RATIO);
2090  break;
2091  case mmDescriptorSURF:
2092  out << "SURF descriptor\n";
2093  out << mrpt::format(
2094  "· EDD Ratio: %f\n", maxEDSD_TH);
2095  out << mrpt::format(
2096  "· Min. CC Threshold: %f\n", EDSD_RATIO);
2097  break;
2098  case mmSAD:
2099  out << "SAD\n";
2100  out << mrpt::format(
2101  "· Max. Dif. SAD Threshold: %f\n", maxSAD_TH);
2102  out << mrpt::format(
2103  "· Ratio SAD Threshold: %f\n", SAD_RATIO);
2104  break;
2105  case mmDescriptorORB:
2106  out << "ORB\n";
2107  out << mrpt::format(
2108  "· Max. distance between desc: %f\n", maxORB_dist);
2109  break;
2110  } // end switch
2111  out << mrpt::format(
2112  "Epipolar Thres: %.2f px\n", epipolar_TH);
2113  out << "Using epipolar restriction?: "
2114  << (useEpipolarRestriction ? "Yes\n" : "No\n");
2115  out << "Has Fundamental Matrix?: "
2116  << (hasFundamentalMatrix ? "Yes\n" : "No\n");
2117  out << "Are camera axis parallel?: "
2118  << (parallelOpticalAxis ? "Yes\n" : "No\n");
2119  out << "Use X-coord restriction?: "
2120  << (useXRestriction ? "Yes\n" : "No\n");
2121  out << "Use disparity limits?: "
2122  << (useDisparityLimits ? "Yes\n" : "No\n");
2123  if (useDisparityLimits)
2124  out << mrpt::format(
2125  "· Min/max disp limits: %.2f/%.2f px\n", min_disp,
2126  max_disp);
2127  out << "Estimate depth?: "
2128  << (estimateDepth ? "Yes\n" : "No\n");
2129  if (estimateDepth)
2130  {
2131  out << mrpt::format(
2132  "· Maximum depth allowed: %f m\n", maxDepthThreshold);
2133  }
2134  out << "Add matches to list?: ";
2135  out << (addMatches ? "Yes\n" : "No\n");
2136  out << "-------------------------------------------------------- \n";
2137 } // end TMatchingOptions::dumpToTextStream
mrpt::img::CImage::resize
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content,...
Definition: CImage.cpp:250
mrpt::vision::CFeatureList::size
size_t size() const
Definition: CFeature.h:352
mrpt::vision::TMatchingOptions::maxEDSD_TH
float maxEDSD_TH
Maximum Euclidean Distance Between SURF Descriptors.
Definition: vision/include/mrpt/vision/types.h:415
mrpt::img::TPixelCoordf
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
mrpt::config
Definition: config/CConfigFile.h:14
mrpt::maps::CLandmark::ID
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CLandmark.h:75
filesystem.h
mrpt::math::MatrixBase::col
auto col(int colIdx)
Definition: MatrixBase.h:89
mrpt::img::FAST_REF_OR_CONVERT_TO_GRAY
@ FAST_REF_OR_CONVERT_TO_GRAY
Definition: img/CImage.h:68
mrpt::vision::CFeatureExtraction
The central class from which images can be analyzed in search of different kinds of interest points a...
Definition: CFeatureExtraction.h:71
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::end
iterator end()
Definition: CLandmarksMap.h:161
mrpt::vision::TMatchingOptions::maxEDD_TH
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
Definition: vision/include/mrpt/vision/types.h:401
mrpt::img::TCamera::intrinsicParams
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates):
Definition: TCamera.h:50
mrpt::vision::TMatchingOptions::useXRestriction
bool useXRestriction
Whether or not employ the x-coord restriction for finding correspondences (bumblebee camera,...
Definition: vision/include/mrpt/vision/types.h:376
mrpt::vision::TMatchingOptions::maxORB_dist
double maxORB_dist
Maximun distance between ORB descriptors.
Definition: vision/include/mrpt/vision/types.h:427
mrpt::vision::bothLists
@ bothLists
Definition: CFeature.h:30
mrpt::img::CImage::getWidth
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
mrpt::vision::CFeatureList::get_type
TKeyPointMethod get_type() const
The type of the first feature in the list.
Definition: CFeature.h:285
mrpt::vision::TStereoSystemParams::factor_b
float factor_b
Beta factor for the SUT.
Definition: vision/include/mrpt/vision/types.h:295
mrpt::img::CImage::getChannelCount
TImageChannels getChannelCount() const
Returns the number of channels, typically 1 (GRAY) or 3 (RGB)
Definition: CImage.cpp:884
mrpt::math::TPoint3D
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
mrpt::img::CImage::getSize
void getSize(TImageSize &s) const
Return the size of the image.
Definition: CImage.cpp:807
mrpt::img::CImage::asCvMat
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
Definition: CImage.cpp:220
geometry.h
mrpt::math::TPoint3D_< double >
mrpt::vision::TMatchingOptions::rCC_TH
float rCC_TH
Maximum Ratio Between the two highest CC values.
Definition: vision/include/mrpt/vision/types.h:411
mrpt::img::TCamera::fx
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
mrpt::vision::matchFeatures
size_t matchFeatures(const CFeatureList &list1, const CFeatureList &list2, CMatchedFeatureList &matches, const TMatchingOptions &options=TMatchingOptions(), const TStereoSystemParams &params=TStereoSystemParams())
Find the matches between two lists of features which must be of the same type.
Definition: vision_utils.cpp:315
mrpt::vision::computeStereoRectificationMaps
void computeStereoRectificationMaps(const mrpt::img::TCamera &cam1, const mrpt::img::TCamera &cam2, const mrpt::poses::CPose3D &rightCameraPose, void *outMap1x, void *outMap1y, void *outMap2x, void *outMap2y)
Computes a pair of x-and-y maps for stereo rectification from a pair of cameras and the relative pose...
Definition: vision_utils.cpp:1819
mrpt::obs::CObservationBearingRange::TMeasurement
Each one of the measurements:
Definition: CObservationBearingRange.h:51
mrpt::vision::TMatchingOptions::SAD_RATIO
double SAD_RATIO
Boundary Ratio between the two highest SAD.
Definition: vision/include/mrpt/vision/types.h:423
mrpt::math::TPose3DQuat::x
double x
Translation in x,y,z.
Definition: TPose3DQuat.h:27
mrpt::obs::CObservationVisualLandmarks
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
Definition: CObservationVisualLandmarks.h:25
mrpt::vision::CFeatureList::const_iterator
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:332
mrpt::vision::TStereoSystemParams::factor_a
float factor_a
Alpha factor for SUT.
Definition: vision/include/mrpt/vision/types.h:292
mrpt::vision::TStereoSystemParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: vision_utils.cpp:1911
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:53
mrpt::math::TPoint2D_< double >
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::vision
Copyright (C) 2010 Hauke Strasdat Imperial College London Copyright (c) 2005-2020,...
Definition: bundle_adjustment.h:35
mrpt::vision::TStereoSystemParams::stdDisp
float stdDisp
Standard deviation of the error in disparity computation.
Definition: vision/include/mrpt/vision/types.h:277
mrpt::math::MatrixVectorBase::fill
void fill(const Scalar &val)
Definition: MatrixVectorBase.h:70
mrpt::img::TCamera::dist
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:51
mrpt::math::meanAndStd
void meanAndStd(const VECTORLIKE &v, double &out_mean, double &out_std, bool unbiased=true)
Computes the standard deviation of a vector (or all elements of a matrix)
Definition: ops_containers.h:329
CObservationVisualLandmarks.h
mrpt::obs::CObservationVisualLandmarks::landmarks
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
Definition: CObservationVisualLandmarks.h:37
mrpt::vision::TMatchingOptions::epipolar_TH
float epipolar_TH
Epipolar constraint (rows of pixels)
Definition: vision/include/mrpt/vision/types.h:397
mrpt::vision::TStereoSystemParams::factor_k
float factor_k
K factor for the UT.
Definition: vision/include/mrpt/vision/types.h:289
mrpt::img::TCamera::ncols
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
mrpt::vision::TMatchingOptions::EDSD_RATIO
float EDSD_RATIO
Boundary Ratio between the two lowest SURF EDSD.
Definition: vision/include/mrpt/vision/types.h:417
mrpt::vision::buildIntrinsicParamsMatrix
mrpt::math::CMatrixDouble33 buildIntrinsicParamsMatrix(const double focalLengthX, const double focalLengthY, const double centerX, const double centerY)
Builds the intrinsic parameters matrix A from parameters:
Definition: vision_utils.cpp:157
mrpt::vision::projectMatchedFeature
void projectMatchedFeature(const CFeature &leftFeat, const CFeature &rightFeat, mrpt::math::TPoint3D &p3D, const TStereoSystemParams &params=TStereoSystemParams())
Computes the 3D position of a particular matched feature.
Definition: vision_utils.cpp:813
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:331
mrpt::vision::CMatchedFeatureList::getMaxID
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1206
mrpt::img::CImage::grayscale
CImage grayscale() const
Returns a grayscale version of the image, or a shallow copy of itself if it is already a grayscale im...
Definition: CImage.cpp:939
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::vision::StereoObs2BRObs
void StereoObs2BRObs(const mrpt::obs::CObservationStereoImages &inObs, const std::vector< double > &sg, mrpt::obs::CObservationBearingRange &outObs)
Converts a stereo images observation into a bearing and range observation.
Definition: vision_utils.cpp:1672
mrpt::vision::computeMainOrientation
float computeMainOrientation(const mrpt::img::CImage &image, unsigned int x, unsigned int y)
Computes the main orientation of a set of points with an image (for using in SIFT-based algorithms)
Definition: vision_utils.cpp:271
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::vision::CFeatureList::begin
iterator begin()
Definition: CFeature.h:337
R
const float R
Definition: CKinematicChain.cpp:137
mrpt::vision::TMatchingOptions::mmDescriptorSIFT
@ mmDescriptorSIFT
Matching by Euclidean distance between SIFT descriptors.
Definition: vision/include/mrpt/vision/types.h:354
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::vision::TMatchingOptions::mmDescriptorORB
@ mmDescriptorORB
Matching by Hamming distance between ORB descriptors.
Definition: vision/include/mrpt/vision/types.h:363
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs::CObservationStereoImages::imageRight
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Definition: CObservationStereoImages.h:56
mrpt::vision::TMatchingOptions::useEpipolarRestriction
bool useEpipolarRestriction
Whether or not take into account the epipolar restriction for finding correspondences.
Definition: vision/include/mrpt/vision/types.h:369
mrpt::obs::CObservationBearingRange::TMeasurement::pitch
float pitch
Definition: CObservationBearingRange.h:61
vision-precomp.h
mrpt::img::CImage::isColor
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:865
mrpt::math::CVectorDynamic::resize
void resize(std::size_t N, bool zeroNewElements=false)
Definition: CVectorDynamic.h:151
mrpt::obs::CObservationBearingRange::sensedData
TMeasurementList sensedData
The list of observed ranges:
Definition: CObservationBearingRange.h:75
mrpt::maps::CLandmark::pose_cov_12
float pose_cov_12
Definition: CLandmark.h:51
mrpt::maps::CLandmark::pose_cov_13
float pose_cov_13
Definition: CLandmark.h:52
mrpt::img::TCamera::cy
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
mrpt::maps::CLandmark::pose_cov_22
float pose_cov_22
Definition: CLandmark.h:51
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::vision::TMatchingOptions::parallelOpticalAxis
bool parallelOpticalAxis
Whether or not the stereo rig has the optical axes parallel.
Definition: vision/include/mrpt/vision/types.h:373
mrpt::img::CCanvas::rectangle
void rectangle(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1)
Draws a rectangle (an empty rectangle, without filling)
Definition: CCanvas.cpp:161
mrpt::vision::openCV_cross_correlation
void openCV_cross_correlation(const mrpt::img::CImage &img, const mrpt::img::CImage &patch_img, size_t &x_max, size_t &y_max, double &max_val, int x_search_ini=-1, int y_search_ini=-1, int x_search_size=-1, int y_search_size=-1)
Computes the correlation between this image and another one, encapsulating the openCV function cvMatc...
Definition: vision_utils.cpp:49
mrpt::vision::cloudsToMatchedList
void cloudsToMatchedList(const mrpt::obs::CObservationVisualLandmarks &cloud1, const mrpt::obs::CObservationVisualLandmarks &cloud2, mrpt::tfest::TMatchingPairList &outList)
Transform two clouds of 3D points into a matched list of points ...
Definition: vision_utils.cpp:240
mrpt::vision::TKeyPoint_templ::pt
pixel_coords_t pt
Coordinates in the image.
Definition: TKeyPoint.h:36
CObservationStereoImages.h
mrpt::vision::CFeatureExtraction::detectFeatures
void detectFeatures(const mrpt::img::CImage &img, CFeatureList &feats, const unsigned int init_ID=0, const unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI())
Extract features from the image based on the method defined in TOptions.
Definition: CFeatureExtraction_common.cpp:37
mrpt::vision::TStereoSystemParams::K
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
Definition: vision/include/mrpt/vision/types.h:266
mrpt::vision::TFeatureID
uint64_t TFeatureID
Definition of a feature ID.
Definition: vision/include/mrpt/vision/types.h:24
mrpt::vision::TMatchingOptions::mmDescriptorSURF
@ mmDescriptorSURF
Matching by Euclidean distance between SURF descriptors.
Definition: vision/include/mrpt/vision/types.h:357
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers,...
Definition: CFeature.h:275
mrpt::math::CMatrixF
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
mrpt::math::TPoint3D_::norm
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
mrpt::vision::TStereoSystemParams::Prop_SUT
@ Prop_SUT
Uncertainty propagation through the Scaled Unscented Transformation.
Definition: vision/include/mrpt/vision/types.h:256
mrpt::img::CImage::setFromMatrix
void setFromMatrix(const MAT &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0,...
Definition: img/CImage.h:844
mrpt::obs::CObservationBearingRange::validCovariances
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
Definition: CObservationBearingRange.h:82
mrpt::img::CImage::getAsMatrix
void getAsMatrix(mrpt::math::CMatrixFloat &outMatrix, bool doResize=true, int x_min=0, int y_min=0, int x_max=-1, int y_max=-1, bool normalize_01=true) const
Returns the image as a matrix with pixel grayscale values in the range [0,1].
Definition: CImage.cpp:1244
mrpt::obs::CObservationBearingRange::setSensorPose
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
Definition: CObservationBearingRange.h:102
ops_vectors.h
mrpt::vision::TStereoSystemParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: vision_utils.cpp:1955
mrpt::img
Definition: CCanvas.h:16
mrpt::img::CImage::makeShallowCopy
CImage makeShallowCopy() const
Returns a shallow copy of the original image.
Definition: img/CImage.h:484
mrpt::vision::CMatchedFeatureList
A list of features.
Definition: CFeature.h:494
mrpt::vision::TStereoSystemParams::Prop_Linear
@ Prop_Linear
Linear propagation of the uncertainty.
Definition: vision/include/mrpt/vision/types.h:250
CLandmarksMap.h
mrpt::math::CMatrixFixed< double, 3, 3 >
CObservationBearingRange.h
mrpt::obs::CObservationBearingRange::TMeasurement::yaw
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
Definition: CObservationBearingRange.h:61
mrpt::obs::CObservationStereoImages::leftCamera
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras.
Definition: CObservationStereoImages.h:77
mrpt::poses::CPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:781
FEAT_FREE
const int FEAT_FREE
Definition: vision_utils.cpp:41
mrpt::obs::CObservationStereoImages::imageLeft
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
Definition: CObservationStereoImages.h:52
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:52
mrpt::img::TCamera::cx
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:50
mrpt::maps::CLandmark
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::vision::TStereoSystemParams::Prop_UT
@ Prop_UT
Uncertainty propagation through the Unscented Transformation.
Definition: vision/include/mrpt/vision/types.h:253
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::img::TStereoCamera
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::maps::CLandmark::features
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
mrpt::obs::CObservationBearingRange::TMeasurement::landmarkID
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
Definition: CObservationBearingRange.h:65
mrpt::math::MatrixBase::chol
bool chol(Derived &U) const
Cholesky M=UT * U decomposition for symmetric matrix (upper-half of the matrix is actually ignored.
Definition: MatrixBase_impl.h:165
mrpt::img::TCamera::nrows
uint32_t nrows
Definition: TCamera.h:40
mrpt::math::multiply_HCHt
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::push_back
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Definition: CLandmarksMap.cpp:1806
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:53
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TMatchingOptions::maxSAD_TH
double maxSAD_TH
Minimum Euclidean Distance Between Sum of Absolute Differences.
Definition: vision/include/mrpt/vision/types.h:421
ops_matrices.h
mrpt::img::TStereoCamera::rightCameraPose
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:31
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
pinhole.h
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixDynamic.h:540
mrpt::math::TLine2D::coefs
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:52
mrpt::vision::pixelTo3D
mrpt::math::TPoint3D pixelTo3D(const mrpt::img::TPixelCoordf &xy, const mrpt::math::CMatrixDouble33 &A)
Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates,...
Definition: vision_utils.cpp:137
mrpt::maps::CLandmarksMap::landmarks
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
CFeatureExtraction.h
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::obs::CObservationBearingRange::TMeasurement::covariance
mrpt::math::CMatrixDouble33 covariance
The covariance matrix of the landmark, with variable indices [0,1,2] being [range,...
Definition: CObservationBearingRange.h:69
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::begin
iterator begin()
Definition: CLandmarksMap.h:160
mrpt::vision::TStereoSystemParams::stdPixel
float stdPixel
Standard deviation of the error in feature detection.
Definition: vision/include/mrpt/vision/types.h:273
A
Definition: is_defined_unittest.cpp:13
mrpt::obs::CObservationBearingRange
This observation represents a number of range-bearing value pairs, each one for a detected landmark,...
Definition: CObservationBearingRange.h:28
mrpt::img::TCamera
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
mrpt::vision::CFeatureList::erase
iterator erase(const iterator &it)
Definition: CFeature.h:345
mrpt::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::maps::CLandmark::pose_mean
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::vision::TMatchingOptions::addMatches
bool addMatches
Whether or not to add the matches found into the input matched list (if false the input list will be ...
Definition: vision/include/mrpt/vision/types.h:380
utils.h
mrpt::math::MatrixVectorBase::transpose
auto transpose()
Definition: MatrixVectorBase.h:159
mrpt::maps::CLandmark::normal
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:50
mrpt::vision::TMatchingOptions::mmSAD
@ mmSAD
Matching by sum of absolute differences of the image patches.
Definition: vision/include/mrpt/vision/types.h:360
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:52
mrpt::math::CVectorDynamic
Template for column vectors of dynamic size, compatible with Eigen.
Definition: CVectorDynamic.h:31
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::vision::TMatchingOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: vision_utils.cpp:2003
mrpt::vision::TMatchingOptions::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: vision_utils.cpp:2070
mrpt::img::CImage::at
const T & at(unsigned int col, unsigned int row, unsigned int channel=0) const
Access to pixels without checking boundaries, and doing a reinterpret_cast<> of the data as the given...
Definition: img/CImage.h:567
mrpt::vision::TMatchingOptions::minCC_TH
float minCC_TH
Minimum Value of the Cross Correlation.
Definition: vision/include/mrpt/vision/types.h:407
CPoint3D.h
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::const_iterator
internal::TSequenceLandmarks::const_iterator const_iterator
Definition: CLandmarksMap.h:164
mrpt::vision::CFeatureList::end
iterator end()
Definition: CFeature.h:338
mrpt::vision::TStereoSystemParams::TStereoSystemParams
TStereoSystemParams()
Initilization of default parameters.
Definition: vision_utils.cpp:1899
mrpt::vision::TStereoSystemParams::baseline
float baseline
Baseline.
Definition: vision/include/mrpt/vision/types.h:269
mrpt::obs::CObservationBearingRange::fieldOfView_pitch
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch ).
Definition: CObservationBearingRange.h:43
mrpt::vision::CMatchedFeatureList::setLeftMaxID
void setLeftMaxID(const TFeatureID &leftID)
Explicitly set the max IDs values to certain values.
Definition: CFeature.h:527
mrpt::math::CMatrixDynamic::resize
void resize(size_t row, size_t col)
Definition: CMatrixDynamic.h:356
mrpt::math::MatrixBase::row
auto row(int rowIdx)
Definition: MatrixBase.h:100
mrpt::vision::TMatchingOptions::maxDepthThreshold
double maxDepthThreshold
The maximum allowed depth for the matching.
Definition: vision/include/mrpt/vision/types.h:435
mrpt::img::CImage::getHeight
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:855
mrpt::math::TLine2D::distance
double distance(const TPoint2D &point) const
Distance from a given point.
Definition: TLine2D.cpp:43
mrpt::maps::CLandmark::pose_cov_33
float pose_cov_33
Definition: CLandmark.h:51
CTicTac.h
mrpt::vision::projectMatchedFeatures
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
Definition: vision_utils.cpp:770
mrpt::img::SHALLOW_COPY
@ SHALLOW_COPY
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
mrpt::obs::CObservationStereoImages::cameraPose
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
Definition: CObservationStereoImages.h:80
mrpt::vision::generateMask
void generateMask(const CMatchedFeatureList &mList, mrpt::math::CMatrixBool &mask1, mrpt::math::CMatrixBool &mask2, int wSize=10)
Calculates the Sum of Absolutes Differences (range [0,1]) between two patches.
Definition: vision_utils.cpp:700
mrpt::vision::TMatchingOptions::mmCorrelation
@ mmCorrelation
Matching by cross correlation of the image patches.
Definition: vision/include/mrpt/vision/types.h:351
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:52
mrpt::vision::TMatchingOptions::TMatchingOptions
TMatchingOptions()
Intrinsic parameters of the stereo rig.
mrpt::vision::TStereoSystemParams::maxZ
float maxZ
Maximum allowed distance.
Definition: vision/include/mrpt/vision/types.h:280
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::vision::TStereoSystemParams
Parameters associated to a stereo system.
Definition: vision/include/mrpt/vision/types.h:233
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::obs::CObservationBearingRange::fieldOfView_yaw
float fieldOfView_yaw
Information about the.
Definition: CObservationBearingRange.h:40
mrpt::img::TPixelCoordf::y
float y
Definition: TPixelCoord.h:23
mrpt::img::TPixelCoordf::x
float x
Definition: TPixelCoord.h:23
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::vision::TMatchingOptions::hasFundamentalMatrix
bool hasFundamentalMatrix
Whether or not there is a fundamental matrix.
Definition: vision/include/mrpt/vision/types.h:371
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
CFeature.h
mrpt::vision::TMatchingOptions::estimateDepth
bool estimateDepth
Whether or not estimate the 3D position of the real features for the matches (only with parallelOptic...
Definition: vision/include/mrpt/vision/types.h:432
mrpt::obs::CObservationBearingRange::TMeasurement::range
float range
The sensed landmark distance, in meters.
Definition: CObservationBearingRange.h:54
mrpt::img::TStereoCamera::leftCamera
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
mrpt::vision::TMatchingOptions::matching_method
TMatchingMethod matching_method
Matching method.
Definition: vision/include/mrpt/vision/types.h:395
mrpt::vision::TStereoSystemParams::minZ
float minZ
Maximum allowed distance.
Definition: vision/include/mrpt/vision/types.h:283
mrpt::vision::addFeaturesToImage
void addFeaturesToImage(const mrpt::img::CImage &inImg, const CFeatureList &theList, mrpt::img::CImage &outImg)
Draw rectangles around each of the features on a copy of the input image.
Definition: vision_utils.cpp:757
mrpt::maps::CLandmark::pose_cov_11
float pose_cov_11
Definition: CLandmark.h:51
mrpt::vision::computeMsd
double computeMsd(const mrpt::tfest::TMatchingPairList &list, const poses::CPose3D &Rt)
Computes the mean squared distance between a set of 3D correspondences ...
Definition: vision_utils.cpp:213
mrpt::vision::defaultIntrinsicParamsMatrix
mrpt::math::CMatrixDouble33 defaultIntrinsicParamsMatrix(unsigned int camIndex=0, unsigned int resolutionX=320, unsigned int resolutionY=240)
Returns the stored, default intrinsic params matrix for a given camera:
Definition: vision_utils.cpp:176
mrpt::math::CVectorDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CVectorDynamic.h:284
mrpt::vision::TStereoSystemParams::uncPropagation
TUnc_Prop_Method uncPropagation
Definition: vision/include/mrpt/vision/types.h:259
mrpt::maps::CLandmark::pose_cov_23
float pose_cov_23
Definition: CLandmark.h:52
mrpt::img::CImage::extract_patch
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten).
Definition: CImage.cpp:1172
mrpt::math::CMatrixDynamic
This template class provides the basic functionality for a general 2D any-size, resizable container o...
Definition: CMatrixDynamic.h:39
mrpt::obs::CObservationStereoImages::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
Definition: CObservationStereoImages.h:88
mrpt::vision::TMatchingOptions::EDD_RATIO
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
Definition: vision/include/mrpt/vision/types.h:403
mrpt::vision::CFeature
A generic 2D feature from an image, extracted with CFeatureExtraction Each feature may have one or mo...
Definition: CFeature.h:53
mrpt::vision::CFeature::keypoint
TKeyPointf keypoint
Definition: CFeature.h:66
mrpt::vision::computeSAD
double computeSAD(const mrpt::img::CImage &patch1, const mrpt::img::CImage &patch2)
Calculates the Sum of Absolutes Differences (range [0,1]) between two patches.
Definition: vision_utils.cpp:736
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
Definition: CObservationStereoImages.h:38
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:53
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system
Definition: backtrace.h:14
mrpt::vision::CMatchedFeatureList::setRightMaxID
void setRightMaxID(const TFeatureID &rightID)
Definition: CFeature.h:528
mrpt::vision::normalizeImage
void normalizeImage(const mrpt::img::CImage &image, mrpt::img::CImage &nimage)
Normalizes the brigthness and contrast of an image by setting its mean value to zero and its standard...
Definition: vision_utils.cpp:292
utils.h
mrpt::vision::TMatchingOptions
A structure containing options for the matching.
Definition: vision/include/mrpt/vision/types.h:342



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020