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
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
float fieldOfView_yaw
Information about the.
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
TMatchingOptions()
Intrinsic parameters of the stereo rig.
CImage makeShallowCopy() const
Returns a shallow copy of the original image.
Definition: img/CImage.h:484
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:75
uint32_t nrows
Definition: TCamera.h:40
float rCC_TH
Maximum Ratio Between the two highest CC values.
#define MRPT_START
Definition: exceptions.h:241
uint64_t TFeatureID
Definition of a feature ID.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
double maxORB_dist
Maximun distance between ORB descriptors.
void resize(size_t row, size_t col)
float range
The sensed landmark distance, in meters.
TInternalFeatList::iterator iterator
Definition: CFeature.h:331
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
bool estimateDepth
Whether or not estimate the 3D position of the real features for the matches (only with parallelOptic...
auto col(int colIdx)
Definition: MatrixBase.h:89
Template for column vectors of dynamic size, compatible with Eigen.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
bool useXRestriction
Whether or not employ the x-coord restriction for finding correspondences (bumblebee camera...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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
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.
void setRightMaxID(const TFeatureID &rightID)
Definition: CFeature.h:526
bool chol(Derived &U) const
Cholesky M=UT * U decomposition for symmetric matrix (upper-half of the matrix is actually ignored...
double SAD_RATIO
Boundary Ratio between the two highest SAD.
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.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void fill(const Scalar &val)
size_t size() const
Definition: CFeature.h:352
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.
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
TImageChannels getChannelCount() const
Returns the number of channels, typically 1 (GRAY) or 3 (RGB)
Definition: CImage.cpp:884
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:855
mrpt::vision::TStereoCalibParams params
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
float epipolar_TH
Epipolar constraint (rows of pixels)
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
float EDSD_RATIO
Boundary Ratio between the two lowest SURF EDSD.
TKeyPointMethod get_type() const
The type of the first feature in the list.
Definition: CFeature.h:285
bool useEpipolarRestriction
Whether or not take into account the epipolar restriction for finding correspondences.
Matching by Hamming distance between ORB descriptors.
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...
void getSize(TImageSize &s) const
Return the size of the image.
Definition: CImage.cpp:807
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
float stdPixel
Standard deviation of the error in feature detection.
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Matching by Euclidean distance between SIFT descriptors.
bool parallelOpticalAxis
Whether or not the stereo rig has the optical axes parallel.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:818
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:173
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
CVectorDynamic< double > CVectorDouble
TKeyPointf keypoint
Definition: CFeature.h:64
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
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.
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1206
Parameters associated to a stereo system.
bool addMatches
Whether or not to add the matches found into the input matched list (if false the input list will be ...
auto row(int rowIdx)
Definition: MatrixBase.h:100
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Matching by Euclidean distance between SURF descriptors.
double x
Translation in x,y,z.
Definition: TPose3DQuat.h:27
A list of TMatchingPair.
Definition: TMatchingPair.h:70
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
double distance(const TPoint2D &point) const
Distance from a given point.
Definition: TLine2D.cpp:43
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
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
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
A generic 2D feature from an image, extracted with CFeatureExtraction Each feature may have one or mo...
Definition: CFeature.h:53
Uncertainty propagation through the Unscented Transformation.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
double maxDepthThreshold
The maximum allowed depth for the matching.
double computeMsd(const mrpt::tfest::TMatchingPairList &list, const poses::CPose3D &Rt)
Computes the mean squared distance between a set of 3D correspondences ...
iterator erase(const iterator &it)
Definition: CFeature.h:345
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...
size_type rows() const
Number of rows in the matrix.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:275
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
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
TStereoSystemParams()
Initilization of default parameters.
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:332
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...
Matching by sum of absolute differences of the image patches.
double maxSAD_TH
Minimum Euclidean Distance Between Sum of Absolute Differences.
return_t square(const num_t x)
Inline function for the square of a number.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
const int FEAT_FREE
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
std::array< double, 3 > coefs
Line coefficients, stored as an array: .
Definition: TLine2D.h:52
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
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)
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:865
const float R
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) ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:50
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
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.
#define MRPT_END
Definition: exceptions.h:245
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
A structure containing options for the matching.
Matching by cross correlation of the image patches.
void setLeftMaxID(const TFeatureID &leftID)
Explicitly set the max IDs values to certain values.
Definition: CFeature.h:525
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.
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
bool hasFundamentalMatrix
Whether or not there is a fundamental matrix.
std::vector< mrpt::vision::CFeature > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
double computeSAD(const mrpt::img::CImage &patch1, const mrpt::img::CImage &patch2)
Calculates the Sum of Absolutes Differences (range [0,1]) between two patches.
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::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:31
float maxEDSD_TH
Maximum Euclidean Distance Between SURF Descriptors.
float minCC_TH
Minimum Value of the Cross Correlation.
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
internal::TSequenceLandmarks::const_iterator const_iterator
T norm() const
Point norm: |v| = sqrt(x^2+y^2+z^2)
Definition: TPoint3D.h:150
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
A RGB color - 8bit.
Definition: TColor.h:25
void resize(std::size_t N, bool zeroNewElements=false)
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
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 ...
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...
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
TMatchingMethod matching_method
Matching method.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
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.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
Functions for estimating the optimal transformation between two frames of references given measuremen...
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
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
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:
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
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...
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
pixel_coords_t pt
Coordinates in the image.
Definition: TKeyPoint.h:36
Uncertainty propagation through the Scaled Unscented Transformation.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
float stdDisp
Standard deviation of the error in disparity computation.
The central class from which images can be analyzed in search of different kinds of interest points a...
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
TMeasurementList sensedData
The list of observed ranges:
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
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
2D line without bounds, represented by its equation .
Definition: TLine2D.h:19
mrpt::math::CMatrixDouble33 buildIntrinsicParamsMatrix(const double focalLengthX, const double focalLengthY, const double centerX, const double centerY)
Builds the intrinsic parameters matrix A from parameters:
A list of features.
Definition: CFeature.h:494
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.
mrpt::math::CMatrixDouble33 covariance
The covariance matrix of the landmark, with variable indices [0,1,2] being [range,yaw,pitch].
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020