MRPT  1.9.9
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-2019, 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/vision/CFeature.h>
14 #include <mrpt/vision/pinhole.h>
15 #include <mrpt/vision/utils.h>
16 
18 #include <mrpt/math/geometry.h>
20 #include <mrpt/math/ops_vectors.h>
21 #include <mrpt/math/utils.h>
25 #include <mrpt/poses/CPoint3D.h>
26 #include <mrpt/system/CTicTac.h>
27 #include <mrpt/system/filesystem.h>
28 
29 // Universal include for all versions of OpenCV
30 #include <mrpt/otherlibs/do_opencv_includes.h>
31 
32 using namespace mrpt;
33 using namespace mrpt::vision;
34 using namespace mrpt::img;
35 using namespace mrpt::config;
36 using namespace mrpt::maps;
37 using namespace mrpt::tfest;
38 using namespace mrpt::math;
39 using namespace mrpt::system;
40 using namespace mrpt::poses;
41 using namespace mrpt::obs;
42 using namespace std;
43 const int FEAT_FREE = -1;
44 // const int NOT_ASIG = 0;
45 // const int ASG_FEAT = 1;
46 // const int AMB_FEAT = 2;
47 
48 /*-------------------------------------------------------------
49  openCV_cross_correlation
50 -------------------------------------------------------------*/
52  const CImage& img, const CImage& patch_img, size_t& x_max, size_t& y_max,
53  double& max_val, int x_search_ini, int y_search_ini, int x_search_size,
54  int y_search_size)
55 {
57 
58 #if MRPT_HAS_OPENCV
59  bool entireImg =
60  (x_search_ini < 0 || y_search_ini < 0 || x_search_size < 0 ||
61  y_search_size < 0);
62 
63  CImage im, patch_im;
64 
65  if (img.isColor() && patch_img.isColor())
66  {
67  img.grayscale(im);
68  patch_img.grayscale(patch_im);
69  }
70  else
71  {
72  ASSERT_(!img.isColor() && !patch_img.isColor());
73  im = img.makeShallowCopy();
74  patch_im = patch_img.makeShallowCopy();
75  }
76 
77  const int im_w = im.getWidth();
78  const int im_h = im.getHeight();
79  const int patch_w = patch_im.getWidth();
80  const int patch_h = patch_im.getHeight();
81 
82  if (entireImg)
83  {
84  x_search_size = im_w - patch_w;
85  y_search_size = im_h - patch_h;
86  }
87 
88  // JLBC: Perhaps is better to raise the exception always??
89  if ((x_search_ini + x_search_size + patch_w) > im_w)
90  x_search_size -= (x_search_ini + x_search_size + patch_w) - im_w;
91 
92  if ((y_search_ini + y_search_size + patch_h) > im_h)
93  y_search_size -= (y_search_ini + y_search_size + patch_h) - im_h;
94 
95  ASSERT_((x_search_ini + x_search_size + patch_w) <= im_w);
96  ASSERT_((y_search_ini + y_search_size + patch_h) <= im_h);
97  CImage img_region_to_search;
98 
99  if (entireImg)
100  {
101  img_region_to_search = im.makeShallowCopy();
102  }
103  else
104  {
105  im.extract_patch(
106  img_region_to_search,
107  x_search_ini, // start corner
108  y_search_ini,
109  patch_w + x_search_size, // sub-image size
110  patch_h + y_search_size);
111  }
112 
113  cv::Mat result(cvSize(x_search_size + 1, y_search_size + 1), CV_32FC1);
114 
115  // Compute cross correlation:
116  cv::matchTemplate(
117  img_region_to_search.asCvMat<cv::Mat>(SHALLOW_COPY),
118  patch_im.asCvMat<cv::Mat>(SHALLOW_COPY), result, CV_TM_CCORR_NORMED);
119 
120  // Find the max point:
121  double mini;
122  cv::Point min_point, max_point;
123 
124  cv::minMaxLoc(
125  result, &mini, &max_val, &min_point, &max_point, cv::noArray());
126  x_max = max_point.x + x_search_ini + (mrpt::round(patch_w - 1) >> 1);
127  y_max = max_point.y + y_search_ini + (mrpt::round(patch_h - 1) >> 1);
128 
129 #else
130  THROW_EXCEPTION("The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
131 #endif
132 
133  MRPT_END
134 }
135 
136 /*-------------------------------------------------------------
137  pixelTo3D
138 -------------------------------------------------------------*/
140 {
141  TPoint3D res;
142 
143  // Build the vector:
144  res.x = xy.x - A.get_unsafe(0, 2);
145  res.y = xy.y - A.get_unsafe(1, 2);
146  res.z = A.get_unsafe(0, 0);
147 
148  // Normalize:
149  const double u = res.norm();
150  ASSERT_(u != 0);
151  res *= 1.0 / u;
152 
153  return res;
154 }
155 
156 /*-------------------------------------------------------------
157  buildIntrinsicParamsMatrix
158 -------------------------------------------------------------*/
160  const double focalLengthX, const double focalLengthY, const double centerX,
161  const double centerY)
162 {
164 
165  A(0, 0) = focalLengthX;
166  A(1, 1) = focalLengthY;
167  A(2, 2) = 1;
168 
169  A(0, 2) = centerX;
170  A(1, 2) = centerY;
171 
172  return A;
173 }
174 
175 /*-------------------------------------------------------------
176  defaultIntrinsicParamsMatrix
177 -------------------------------------------------------------*/
179  unsigned int camIndex, unsigned int resX, unsigned int resY)
180 {
181  float fx, fy, cx, cy;
182 
183  switch (camIndex)
184  {
185  case 0:
186  // Bumblebee:
187  fx = 0.79345f;
188  fy = 1.05793f;
189  cx = 0.55662f;
190  cy = 0.52692f;
191  break;
192 
193  case 1:
194  // Sony:
195  fx = 0.95666094f;
196  fy = 1.3983423f;
197  cx = 0.54626328f;
198  cy = 0.4939191f;
199  break;
200 
201  default:
202  {
204  "Unknown camera index!! for 'camIndex'=%u", camIndex);
205  }
206  }
207 
209  resX * fx, resY * fy, resX * cx, resY * cy);
210 }
211 
212 /*-------------------------------------------------------------
213  deleteRepeatedFeats
214 -------------------------------------------------------------*/
216 {
217  CFeatureList::iterator itList1, itList2;
218  float lx = 0.0, ly = 0.0;
219 
220  // Look for repeated features in the feat_list of features
221  for (itList1 = feat_list.begin(); itList1 != feat_list.end(); itList1++)
222  {
223  lx = (*itList1)->x;
224  ly = (*itList1)->y;
225  for (itList2 = itList1; itList2 != feat_list.end(); itList2++)
226  {
227  if ((lx == (*itList2)->x && ly == (*itList2)->y) &&
228  ((*itList2)->x > 0.0f && (*itList2)->y > 0.0f))
229  {
230  (*itList2)->x = -1.0f;
231  (*itList2)->y = -1.0f;
232  } // end if
233  } // end for
234  } // end for
235 
236  // Delete the repeated features
237  for (itList1 = feat_list.begin(); itList1 != feat_list.end();)
238  {
239  if ((*itList1)->x == -1.0f && (*itList1)->y == -1.0f)
240  itList1 = feat_list.erase(itList1);
241  else
242  itList1++;
243  } // end for
244 } // end deleteRepeatedFeats
245 
246 /*------------------------------------------------------------
247  rowChecking
248 -------------------------------------------------------------*/
250  CFeatureList& leftList, CFeatureList& rightList, float threshold)
251 {
252  ASSERT_(leftList.size() == rightList.size());
253 
254  // By now: row checking -> Delete bad correspondences
255  std::cout << std::endl
256  << "[ROW CHECKING]------------------------------------------"
257  << std::endl;
258 
259  CFeatureList::iterator itLeft, itRight;
260  for (itLeft = leftList.begin(), itRight = rightList.begin();
261  itLeft != leftList.end();)
262  {
263  if ((*itLeft)->x < 0 || (*itLeft)->y < 0 || (*itRight)->x < 0 ||
264  (*itRight)->y < 0 || fabs((*itLeft)->y - (*itRight)->y) > threshold)
265  {
266  std::cout << "[Erased Feature] Row Dif: "
267  << fabs((*itLeft)->y - (*itRight)->y) << std::endl;
268  itLeft = leftList.erase(itLeft);
269  itRight = rightList.erase(itRight);
270  } // end if
271  else
272  {
273  itLeft++;
274  itRight++;
275  }
276  } // end for
277 
278  std::cout << "------------------------------------------" << std::endl;
279 
280  std::cout << "Tracked features: " << leftList.size() << " and "
281  << rightList.size() << std::endl;
282  ASSERT_(leftList.size() == rightList.size());
283 
284 } // end rowChecking
285 
286 /*------------------------------------------------------------
287  getDispersion
288 -------------------------------------------------------------*/
291 {
292  std.assign(2, 0);
293  mean.assign(2, 0);
294 
296  double varx = 0, vary = 0;
297 
298  for (it = list.begin(); it != list.end(); it++)
299  {
300  mean[0] += (*it)->x;
301  mean[1] += (*it)->y;
302  }
303  mean[0] /= list.size();
304  mean[1] /= list.size();
305 
306  for (it = list.begin(); it != list.end(); it++)
307  {
308  varx += square((*it)->x - mean[0]);
309  vary += square((*it)->y - mean[1]);
310  }
311  varx /= list.size();
312  vary /= list.size();
313 
314  std[0] = sqrt(varx);
315  std[1] = sqrt(vary);
316 } // end getDispersion
317 
318 /*-------------------------------------------------------------
319  computeMsd
320 -------------------------------------------------------------*/
322  const TMatchingPairList& feat_list, const poses::CPose3D& Rt)
323 {
324  CMatrixDouble44 mat;
325  Rt.getHomogeneousMatrix(mat);
326  double acum = 0.0;
327 
329  TPoint3D err;
330  for (it = feat_list.begin(); it != feat_list.end(); it++)
331  {
332  err.x = it->other_x - (it->this_x * mat.get_unsafe(0, 0) +
333  it->this_y * mat.get_unsafe(0, 1) +
334  it->this_z * mat.get_unsafe(0, 2) + Rt.x());
335  err.y = it->other_y - (it->this_x * mat.get_unsafe(1, 0) +
336  it->this_y * mat.get_unsafe(1, 1) +
337  it->this_z * mat.get_unsafe(1, 2) + Rt.y());
338  err.z = it->other_z - (it->this_x * mat.get_unsafe(2, 0) +
339  it->this_y * mat.get_unsafe(2, 1) +
340  it->this_z * mat.get_unsafe(2, 2) + Rt.z());
341 
342  acum += err.norm();
343 
344  } // end for
345  return (acum / feat_list.size());
346 } // end msd
347 
348 /*-------------------------------------------------------------
349  cloudsToMatchedList
350 -------------------------------------------------------------*/
352  const CObservationVisualLandmarks& cloud1,
353  const CObservationVisualLandmarks& cloud2, TMatchingPairList& outList)
354 {
356  TMatchingPair pair;
357 
358  for (itLand1 = cloud1.landmarks.landmarks.begin();
359  itLand1 != cloud1.landmarks.landmarks.end(); itLand1++)
360  for (itLand2 = cloud2.landmarks.landmarks.begin();
361  itLand2 != cloud2.landmarks.landmarks.end(); itLand2++)
362  if (itLand1->ID == itLand2->ID)
363  {
364  // Match found!
365  pair.this_idx = pair.other_idx = (unsigned int)itLand1->ID;
366 
367  pair.this_x = itLand1->pose_mean.x;
368  pair.this_y = itLand1->pose_mean.y;
369  pair.this_z = itLand1->pose_mean.z;
370 
371  pair.other_x = itLand2->pose_mean.x;
372  pair.other_y = itLand2->pose_mean.y;
373  pair.other_z = itLand2->pose_mean.z;
374 
375  outList.push_back(pair);
376  } // end if
377 } // end-cloudsToMatchedList
378 
379 /*-------------------------------------------------------------
380  computeMainOrientation
381 -------------------------------------------------------------*/
383  const CImage& image, unsigned int x, unsigned int y)
384 {
385  MRPT_START
386  float orientation = 0;
387  if ((int(x) - 1 >= 0) && (int(y) - 1 >= 0) && (x + 1 < image.getWidth()) &&
388  (y + 1 < image.getHeight()))
389  orientation = (float)atan2(
390  (double)*image(x, y + 1) - (double)*image(x, y - 1),
391  (double)*image(x + 1, y) - (double)*image(x - 1, y));
392 
393  // Convert from [-pi,pi] to [0,2pi]
394 
395  return orientation;
396 
397  MRPT_END
398 } // end vision::computeMainOrientation
399 
400 /*-------------------------------------------------------------
401  normalizeImage
402 -------------------------------------------------------------*/
404 {
405  ASSERT_(image.getChannelCount() == 1);
406  nimage.resize(image.getWidth(), image.getHeight(), image.getChannelCount());
407 
408  CMatrixFloat im, nim;
409  nim.resize(image.getHeight(), image.getWidth());
410 
411  image.getAsMatrix(im);
412 
413  double m, s;
414  im.meanAndStdAll(m, s);
415 
416  for (int k1 = 0; k1 < (int)nim.cols(); ++k1)
417  for (int k2 = 0; k2 < (int)nim.rows(); ++k2)
418  nim.set_unsafe(k2, k1, (im.get_unsafe(k2, k1) - m) / s);
419 
420  nimage.setFromMatrix(nim);
421 } // end normalizeImage
422 
423 /*-------------------------------------------------------------
424  matchFeatures
425 -------------------------------------------------------------*/
427  const CFeatureList& list1, const CFeatureList& list2,
428  CMatchedFeatureList& matches, const TMatchingOptions& options,
430 {
431  // Clear the output structure
432  MRPT_START
433  // matches.clear();
434 
435  // Preliminary comprobations
436  size_t sz1 = list1.size(), sz2 = list2.size();
437 
438  ASSERT_((sz1 > 0) && (sz2 > 0)); // Both lists have features within it
439  ASSERT_(
440  list1.get_type() ==
441  list2.get_type()); // Both lists must be of the same type
442 
443  CFeatureList::const_iterator itList1, itList2; // Iterators for the lists
444 
445  // For SIFT & SURF
446  float distDesc; // EDD or EDSD
447  float minDist1; // Minimum EDD or EDSD
448  float minDist2; // Second minimum EDD or EDSD
449 
450  // For Harris
451  double maxCC1; // Maximum CC
452  double maxCC2; // Second maximum CC
453 
454  // For SAD
455  double minSAD1, minSAD2;
456 
457  vector<int> idxLeftList, idxRightList;
458  idxLeftList.resize(sz1, FEAT_FREE);
459  idxRightList.resize(sz2, FEAT_FREE);
460  vector<double> distCorrs(sz1);
461  int lFeat, rFeat;
462  int minLeftIdx = 0, minRightIdx;
463  int nMatches = 0;
464 
465  // For each feature in list1 ...
466  for (lFeat = 0, itList1 = list1.begin(); itList1 != list1.end();
467  ++itList1, ++lFeat)
468  {
469  // For SIFT & SURF
470  minDist1 = 1e5;
471  minDist2 = 1e5;
472 
473  // For Harris
474  maxCC1 = 0;
475  maxCC2 = 0;
476 
477  // For SAD
478  minSAD1 = 1e5;
479  minSAD2 = 1e5;
480 
481  // For all the cases
482  minRightIdx = 0;
483 
484  for (rFeat = 0, itList2 = list2.begin(); itList2 != list2.end();
485  ++itList2, ++rFeat) // ... compare with all the features in list2.
486  {
487  // Filter out by epipolar constraint
488  double d = 0.0; // Distance to the epipolar line
489  if (options.useEpipolarRestriction)
490  {
491  if (options.parallelOpticalAxis)
492  d = (*itList1)->y - (*itList2)->y;
493  else
494  {
495  ASSERT_(options.hasFundamentalMatrix);
496 
497  // Compute epipolar line Ax + By + C = 0
498  TLine2D epiLine;
499  TPoint2D oPoint((*itList2)->x, (*itList2)->y);
500 
501  CMatrixDouble31 l, p;
502  p(0, 0) = (*itList1)->x;
503  p(1, 0) = (*itList1)->y;
504  p(2, 0) = 1;
505 
506  l = params.F * p;
507 
508  epiLine.coefs[0] = l(0, 0);
509  epiLine.coefs[1] = l(1, 0);
510  epiLine.coefs[2] = l(2, 0);
511 
512  d = epiLine.distance(oPoint);
513  } // end else
514  } // end if
515 
516  bool c1 = options.useEpipolarRestriction
517  ? fabs(d) < options.epipolar_TH
518  : true; // Use epipolar restriction
519  bool c2 = options.useXRestriction
520  ? ((*itList1)->x - (*itList2)->x) > 0
521  : true; // Use x-coord restriction
522 
523  if (c1 && c2)
524  {
525  switch (options.matching_method)
526  {
528  {
529  // Ensure that both features have SIFT descriptors
530  ASSERT_(
531  (*itList1)->descriptors.hasDescriptorSIFT() &&
532  (*itList2)->descriptors.hasDescriptorSIFT());
533 
534  // Compute the Euclidean distance between descriptors
535  distDesc =
536  (*itList1)->descriptorSIFTDistanceTo(*(*itList2));
537 
538  // Search for the two minimum values
539  if (distDesc < minDist1)
540  {
541  minDist2 = minDist1;
542  minDist1 = distDesc;
543  minLeftIdx = lFeat;
544  minRightIdx = rFeat;
545  }
546  else if (distDesc < minDist2)
547  minDist2 = distDesc;
548 
549  break;
550  } // end mmDescriptorSIFT
551 
553  {
554  size_t u, v; // Coordinates of the peak
555  double res; // Value of the peak
556 
557  // Ensure that both features have patches
558  ASSERT_(
559  (*itList1)->patchSize > 0 &&
560  (*itList2)->patchSize > 0);
562  (*itList1)->patch, (*itList2)->patch, u, v, res);
563 
564  // Search for the two maximum values
565  if (res > maxCC1)
566  {
567  maxCC2 = maxCC1;
568  maxCC1 = res;
569  minLeftIdx = lFeat;
570  minRightIdx = rFeat;
571  }
572  else if (res > maxCC2)
573  maxCC2 = res;
574 
575  break;
576  } // end mmCorrelation
577 
579  {
580  // Ensure that both features have SURF descriptors
581  ASSERT_(
582  (*itList1)->descriptors.hasDescriptorSURF() &&
583  (*itList2)->descriptors.hasDescriptorSURF());
584 
585  // Compute the Euclidean distance between descriptors
586  distDesc =
587  (*itList1)->descriptorSURFDistanceTo(*(*itList2));
588 
589  // Search for the two minimum values
590  if (distDesc < minDist1)
591  {
592  minDist2 = minDist1;
593  minDist1 = distDesc;
594  minLeftIdx = lFeat;
595  minRightIdx = rFeat;
596  }
597  else if (distDesc < minDist2)
598  minDist2 = distDesc;
599 
600  break; // end case featSURF
601  } // end mmDescriptorSURF
602 
604  {
605  // Ensure that both features have SURF descriptors
606  ASSERT_(
607  (*itList1)->descriptors.hasDescriptorORB() &&
608  (*itList2)->descriptors.hasDescriptorORB());
609  distDesc =
610  (*itList1)->descriptorORBDistanceTo(*(*itList2));
611 
612  // Search for the two minimum values
613  if (distDesc < minDist1)
614  {
615  minDist2 = minDist1;
616  minDist1 = distDesc;
617  minLeftIdx = lFeat;
618  minRightIdx = rFeat;
619  }
620  else if (distDesc < minDist2)
621  minDist2 = distDesc;
622 
623  break;
624  } // end mmDescriptorORB
625 
627  {
628  // Ensure that both features have patches
629  ASSERT_(
630  (*itList1)->patchSize > 0 &&
631  (*itList2)->patchSize == (*itList1)->patchSize);
632 #if !MRPT_HAS_OPENCV
634  "MRPT has been compiled without OpenCV");
635 #else
636  const CImage aux1(
637  (*itList1)->patch, FAST_REF_OR_CONVERT_TO_GRAY);
638  const CImage aux2(
639  (*itList2)->patch, FAST_REF_OR_CONVERT_TO_GRAY);
640  const auto h = aux1.getHeight(), w = aux1.getWidth();
641 
642  double res = 0;
643  for (unsigned int ii = 0; ii < h; ++ii)
644  for (unsigned int jj = 0; jj < w; ++jj)
645  res += std::abs(
646  static_cast<double>(
647  aux1.at<uint8_t>(jj, ii)) -
648  static_cast<double>(
649  aux2.at<uint8_t>(jj, ii)));
650  res = res / (255.0 * w * h);
651 
652  if (res < minSAD1)
653  {
654  minSAD2 = minSAD1;
655  minSAD1 = res;
656  minLeftIdx = lFeat;
657  minRightIdx = rFeat;
658  }
659  else if (res < minSAD2)
660  minSAD2 = res;
661 #endif
662  break;
663  } // end mmSAD
664  } // end switch
665  } // end if
666  } // end for 'list2' (right features)
667 
668  bool cond1 = false, cond2 = false;
669  double minVal = 1.0;
670  switch (options.matching_method)
671  {
673  cond1 = minDist1 < options.maxEDD_TH; // Maximum Euclidean
674  // Distance between SIFT
675  // descriptors (EDD)
676  cond2 = (minDist1 / minDist2) <
677  options.EDD_RATIO; // Ratio between the two lowest EDSD
678  minVal = minDist1;
679  break;
681  cond1 = maxCC1 >
682  options.minCC_TH; // Minimum cross correlation value
683  cond2 = (maxCC2 / maxCC1) <
684  options.rCC_TH; // Ratio between the two highest cross
685  // correlation values
686  minVal = 1 - maxCC1;
687  break;
689  cond1 = minDist1 < options.maxEDSD_TH; // Maximum Euclidean
690  // Distance between SURF
691  // descriptors (EDSD)
692  cond2 =
693  (minDist1 / minDist2) <
694  options.EDSD_RATIO; // Ratio between the two lowest EDSD
695  minVal = minDist1;
696  break;
698  cond1 = minSAD1 < options.maxSAD_TH;
699  cond2 = (minSAD1 / minSAD2) < options.SAD_RATIO;
700  minVal = minSAD1;
701  break;
703  cond1 = minDist1 < options.maxORB_dist;
704  cond2 = true;
705  minVal = minDist1;
706  break;
707  default:
708  THROW_EXCEPTION("Invalid value of 'matching_method'");
709  }
710 
711  // PROCESS THE RESULTS
712  if (cond1 && cond2) // The minimum distance must be below a threshold
713  {
714  int auxIdx = idxRightList[minRightIdx];
715  if (auxIdx != FEAT_FREE)
716  {
717  if (distCorrs[auxIdx] > minVal)
718  {
719  // We've found a better match
720  distCorrs[minLeftIdx] = minVal;
721  idxLeftList[minLeftIdx] = minRightIdx;
722  idxRightList[minRightIdx] = minLeftIdx;
723 
724  distCorrs[auxIdx] = 1.0;
725  idxLeftList[auxIdx] = FEAT_FREE;
726  } // end-if
727  } // end-if
728  else
729  {
730  idxRightList[minRightIdx] = minLeftIdx;
731  idxLeftList[minLeftIdx] = minRightIdx;
732  distCorrs[minLeftIdx] = minVal;
733  nMatches++;
734  }
735  } // end if
736  } // end for 'list1' (left features)
737 
738  if (!options.addMatches) matches.clear();
739 
740  TFeatureID idLeft = 0, idRight = 0;
741  if (!matches.empty()) matches.getMaxID(bothLists, idLeft, idRight);
742 
743  for (int vCnt = 0; vCnt < (int)idxLeftList.size(); ++vCnt)
744  {
745  if (idxLeftList[vCnt] != FEAT_FREE)
746  {
747  std::pair<CFeature::Ptr, CFeature::Ptr> thisMatch;
748 
749  bool isGood = true;
750  double dp1 = -1.0, dp2 = -1.0;
751  TPoint3D p3D = TPoint3D();
752  if (options.estimateDepth && options.parallelOpticalAxis)
753  {
755  list1[vCnt], list2[idxLeftList[vCnt]], p3D, params);
756  dp1 = sqrt(p3D.x * p3D.x + p3D.y * p3D.y + p3D.z * p3D.z);
757  dp2 = sqrt(
758  (p3D.x - params.baseline) * (p3D.x - params.baseline) +
759  p3D.y * p3D.y + p3D.z * p3D.z);
760 
761  if (dp1 > options.maxDepthThreshold ||
762  dp2 > options.maxDepthThreshold)
763  isGood = false;
764  } // end-if
765 
766  if (isGood)
767  {
768  // Set the features
769  thisMatch.first = list1[vCnt];
770  thisMatch.second = list2[idxLeftList[vCnt]];
771 
772  // Update the max ID value
773  if (matches.empty())
774  {
775  idLeft = thisMatch.first->ID;
776  idRight = thisMatch.second->ID;
777  }
778  else
779  {
780  keep_max(idLeft, thisMatch.first->ID);
781  matches.setLeftMaxID(idLeft);
782 
783  keep_max(idRight, thisMatch.second->ID);
784  matches.setRightMaxID(idRight);
785  }
786 
787  // Set the depth and the 3D position of the feature
788  if (options.estimateDepth && options.parallelOpticalAxis)
789  {
790  thisMatch.first->initialDepth = dp1;
791  thisMatch.first->p3D = p3D;
792 
793  thisMatch.second->initialDepth = dp2;
794  thisMatch.second->p3D =
795  TPoint3D(p3D.x - params.baseline, p3D.y, p3D.z);
796  } // end-if
797 
798  // Insert the match into the matched list
799  matches.push_back(thisMatch);
800  } // end-if-isGood
801  } // end-if
802  } // end-for-matches
803  return matches.size();
804 
805  MRPT_END
806 }
807 
808 /*-------------------------------------------------------------
809  generateMask
810 -------------------------------------------------------------*/
811 // Insert zeros around the points in mList according to wSize
813  const CMatchedFeatureList& mList, CMatrixBool& mask1, CMatrixBool& mask2,
814  int wSize)
815 {
816  ASSERT_(mList.size() > 0);
817 
818  // cv::Mat *mask1 = static_cast<cv::Mat*>(_mask1);
819  // cv::Mat *mask2 = static_cast<cv::Mat*>(_mask2);
820 
821  int hwsize = (int)(0.5 * wSize);
822  int mx = mask1.cols(), my = mask1.rows();
823 
824  int idx, idy;
826  for (it = mList.begin(); it != mList.end(); ++it)
827  {
828  for (int ii = -hwsize; ii < hwsize; ++ii)
829  for (int jj = -hwsize; jj < hwsize; ++jj)
830  {
831  idx = (int)(it->first->x) + ii;
832  idy = (int)(it->first->y) + jj;
833  if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
834  mask1.set_unsafe(idy, idx, false);
835  }
836 
837  for (int ii = -hwsize; ii < hwsize; ++ii)
838  for (int jj = -hwsize; jj < hwsize; ++jj)
839  {
840  idx = (int)(it->second->x) + ii;
841  idy = (int)(it->second->y) + jj;
842  if (idx >= 0 && idy >= 0 && idx < mx && idy < my)
843  mask2.set_unsafe(idy, idx, false);
844  }
845  } // end-for
846 } // end generateMask
847 
848 double vision::computeSAD(const CImage& p1, const CImage& p2)
849 {
850  MRPT_START
851 #if MRPT_HAS_OPENCV
852  ASSERT_(p1.getSize() == p2.getSize());
853  const auto w = p1.getWidth(), h = p1.getHeight();
854  double res = 0.0;
855  for (unsigned int ii = 0; ii < h; ++ii)
856  for (unsigned int jj = 0; jj < w; ++jj)
857  res += std::abs(
858  static_cast<double>(p1.at<uint8_t>(jj, ii)) -
859  static_cast<double>(p2.at<uint8_t>(jj, ii)));
860 
861  return res / (255.0 * w * h);
862 #else
864  "MRPT compiled without OpenCV, can't compute SAD of images!");
865 #endif
866  MRPT_END
867 }
868 
870  const CImage& inImg, const CFeatureList& theList, CImage& outImg)
871 {
872  outImg = inImg; // Create a copy of the input image
873  for (const auto& it : theList)
874  outImg.rectangle(
875  it->x - 5, it->y - 5, it->x + 5, it->y + 5, TColor(255, 0, 0));
876 }
877 
878 /*-------------------------------------------------------------
879  projectMatchedFeatures
880 -------------------------------------------------------------*/
882  const CMatchedFeatureList& matches,
883  const mrpt::img::TStereoCamera& stereo_camera, vector<TPoint3D>& out_points)
884 {
885  out_points.clear();
886  out_points.reserve(matches.size());
887  for (const auto& matche : matches)
888  {
889  const double disp = matche.first->x - matche.second->x;
890  if (disp < 1) continue;
891 
892  const double b_d = stereo_camera.rightCameraPose.x / disp;
893  out_points.emplace_back(
894  (matche.first->x - stereo_camera.leftCamera.cx()) * b_d,
895  (matche.first->y - stereo_camera.leftCamera.cy()) * b_d,
896  stereo_camera.leftCamera.fx() * b_d);
897  } // end-for
898 }
899 /*-------------------------------------------------------------
900  projectMatchedFeatures
901 -------------------------------------------------------------*/
903  const CFeatureList& leftList, const CFeatureList& rightList,
904  vector<TPoint3D>& vP3D, const TStereoSystemParams& params)
905 {
906  vP3D.reserve(leftList.size());
908  for (it1 = leftList.begin(), it2 = rightList.begin(); it1 != leftList.end();
909  ++it1, ++it2)
910  {
911  TPoint3D p3D;
912  projectMatchedFeature(*it1, *it2, p3D, params);
913  if (p3D.z < params.maxZ && p3D.z > params.minZ && p3D.y < params.maxY)
914  vP3D.push_back(p3D);
915  }
916 } // end projectMatchedFeatures
917 
918 /*-------------------------------------------------------------
919  projectMatchedFeatures
920 -------------------------------------------------------------*/
922  const CFeature::Ptr& leftFeat, const CFeature::Ptr& rightFeat,
923  TPoint3D& p3D, const TStereoSystemParams& params)
924 {
925  // double disp2 = leftFeat->x-rightFeat->x;
926  // double aux2 = params.baseline/disp2;
927  // p3D.x = (leftFeat->x-params.K(0,2))*aux2;
928  // p3D.y = (leftFeat->y-params.K(1,2))*aux2;
929  // p3D.z = params.K(0,0)*aux2;
930  // cout << "Params: BS: " << params.baseline << " CX: " << params.K(0,2) <<
931  //" CY: " << params.K(1,2) << " FX: " << params.K(0,0) << endl;
932  // cout << "Input: " << leftFeat->x << "," << leftFeat->y << endl;
933  // cout << "Disp: " << disp2 << endl;
934  // cout << p3D << endl;
935  // return;
936 
937  const double f0 = 600;
938  double nfx1 = leftFeat->x, nfy1 = leftFeat->y,
939  nfx2 = rightFeat->x; // nfy2 = rightFeat->y;
940 
941  const double x = leftFeat->x * f0; // x = (x / f0) * f0 x = x
942  const double y = leftFeat->y * f0; // y = (y / f0) * f0 y = y
943  const double xd = rightFeat->x * f0; // x' = (x' / f0) * f0 x' = x'
944  const double yd = rightFeat->y * f0; // y' = (y' / f0) * f0 y' = y'
945 
946  const double f2 = f0 * f0;
947  const double p9 = f2 * params.F.get_unsafe(2, 2);
948  const double Q00 =
949  f2 * (params.F.get_unsafe(0, 2) * params.F.get_unsafe(0, 2) +
950  params.F.get_unsafe(1, 2) * params.F.get_unsafe(1, 2) +
951  params.F.get_unsafe(2, 0) * params.F.get_unsafe(2, 0) +
952  params.F.get_unsafe(2, 1) * params.F.get_unsafe(2, 1));
953 
954  double Jh = (std::numeric_limits<double>::max)(); // J hat = ‡
955  double xh = x; // x hat = x
956  double yh = y; // y hat = y
957  double xhd = xd; // x hat dash = x'
958  double yhd = yd; // y hat dash = y'
959  double
960  xt = 0,
961  yt = 0, xtd = 0,
962  ytd =
963  0; // x tilde = 0, y tilde = 0, x tilde dash = 0, y tilde dash = 0
964  for (;;)
965  {
966  const double p1 =
967  (xh * xhd + xhd * xt + xh * xtd) * params.F.get_unsafe(0, 0);
968  const double p2 =
969  (xh * yhd + yhd * xt + xh * ytd) * params.F.get_unsafe(0, 1);
970  const double p3 = (f0 * (xh + xt)) * params.F.get_unsafe(0, 2);
971  const double p4 =
972  (yh * xhd + xhd * yt + yh * xtd) * params.F.get_unsafe(1, 0);
973  const double p5 =
974  (yh * yhd + yhd * yt + yh * ytd) * params.F.get_unsafe(1, 1);
975  const double p6 = (f0 * (yh + yt)) * params.F.get_unsafe(1, 2);
976  const double p7 = (f0 * (xhd + xtd)) * params.F.get_unsafe(2, 0);
977  const double p8 = (f0 * (yhd + ytd)) * params.F.get_unsafe(2, 1);
978 
979  const double udotxi = p1 + p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;
980 
981  const double Q11 = (xh * xh + xhd * xhd) * params.F.get_unsafe(0, 0) *
982  params.F.get_unsafe(0, 0);
983  const double Q22 = (xh * xh + yhd * yhd) * params.F.get_unsafe(0, 1) *
984  params.F.get_unsafe(0, 1);
985  const double Q44 = (yh * yh + xhd * xhd) * params.F.get_unsafe(1, 0) *
986  params.F.get_unsafe(1, 0);
987  const double Q55 = (yh * yh + yhd * yhd) * params.F.get_unsafe(1, 1) *
988  params.F.get_unsafe(1, 1);
989  const double Q12 =
990  xhd * yhd * params.F.get_unsafe(0, 0) * params.F.get_unsafe(0, 1);
991  const double Q13 =
992  f0 * xhd * params.F.get_unsafe(0, 0) * params.F.get_unsafe(0, 2);
993  const double Q14 =
994  xh * yh * params.F.get_unsafe(0, 0) * params.F.get_unsafe(1, 0);
995  const double Q17 =
996  f0 * xh * params.F.get_unsafe(0, 0) * params.F.get_unsafe(2, 0);
997  const double Q23 =
998  f0 * yhd * params.F.get_unsafe(0, 1) * params.F.get_unsafe(0, 2);
999  const double Q25 =
1000  xh * yh * params.F.get_unsafe(0, 1) * params.F.get_unsafe(1, 1);
1001  const double Q28 =
1002  f0 * xh * params.F.get_unsafe(0, 1) * params.F.get_unsafe(2, 1);
1003  const double Q45 =
1004  xhd * yhd * params.F.get_unsafe(1, 0) * params.F.get_unsafe(1, 1);
1005  const double Q46 =
1006  f0 * xhd * params.F.get_unsafe(1, 0) * params.F.get_unsafe(1, 2);
1007  const double Q47 =
1008  f0 * yh * params.F.get_unsafe(1, 0) * params.F.get_unsafe(2, 0);
1009  const double Q56 =
1010  f0 * yhd * params.F.get_unsafe(1, 1) * params.F.get_unsafe(1, 2);
1011  const double Q58 =
1012  f0 * yh * params.F.get_unsafe(1, 1) * params.F.get_unsafe(2, 1);
1013 
1014  const double udotV0xiu = Q00 + Q11 + Q22 + Q44 + Q55 +
1015  2.0 * (Q12 + Q13 + Q14 + Q17 + Q23 + Q25 +
1016  Q28 + Q45 + Q46 + Q47 + Q56 + Q58);
1017 
1018  ASSERT_(fabs(udotV0xiu) > 1e-5);
1019 
1020  const double C = udotxi / udotV0xiu;
1021 
1022  xt = C *
1023  (params.F.get_unsafe(0, 0) * xhd +
1024  params.F.get_unsafe(0, 1) * yhd + f0 * params.F.get_unsafe(0, 2));
1025  yt = C *
1026  (params.F.get_unsafe(1, 0) * xhd +
1027  params.F.get_unsafe(1, 1) * yhd + f0 * params.F.get_unsafe(1, 2));
1028  xtd = C *
1029  (params.F.get_unsafe(0, 0) * xh + params.F.get_unsafe(1, 0) * yh +
1030  f0 * params.F.get_unsafe(2, 0));
1031  ytd = C *
1032  (params.F.get_unsafe(0, 1) * xh + params.F.get_unsafe(1, 1) * yh +
1033  f0 * params.F.get_unsafe(2, 1));
1034 
1035  const double Jt = xt * xt + yt * yt + xtd * xtd + ytd * ytd;
1036  // cout << "Jt:" << Jt << " and Jh: " << Jh << endl;
1037  if ((std::abs)(Jt - Jh) <= 1e-5)
1038  {
1039  nfx1 = xh / f0;
1040  nfy1 = yh / f0;
1041  nfx2 = xhd / f0;
1042  // nfy2 = yhd / f0;
1043 
1044  break;
1045  }
1046  else
1047  {
1048  Jh = Jt; // J hat = J tilde
1049  xh = x - xt; // x hat = x - x tilde
1050  yh = y - yt; // y hat = y - y tilde
1051  xhd = xd - xtd; // x hat dash = x' - x tilde dash
1052  yhd = yd - ytd; // y hat dash = y' - y tilde dash
1053  }
1054  } // end for
1055 
1056  double disp = nfx1 - nfx2;
1057  double aux = params.baseline / disp;
1058  p3D.x = (nfx1 - params.K(0, 2)) * aux;
1059  p3D.y = (nfy1 - params.K(1, 2)) * aux;
1060  p3D.z = params.K(0, 0) * aux;
1061 } // end projectMatchedFeature
1062 
1063 /*-------------------------------------------------------------
1064  projectMatchedFeatures
1065 -------------------------------------------------------------*/
1068  CLandmarksMap& landmarks)
1069 {
1070  MRPT_START
1071 
1072  landmarks.clear(); // Assert that the output CLandmarksMap is clear
1073 
1075  float stdPixel2 = square(param.stdPixel);
1076  float stdDisp2 = square(param.stdDisp);
1077 
1078  // Main loop
1079  for (itList = mfList.begin(); itList != mfList.end();)
1080  {
1081  float disp = (itList->first->x - itList->second->x); // Disparity
1082  if (disp < 1e-9) // Filter out too far points
1083  itList = mfList.erase(
1084  itList); // Erase the match : itList = mfList.erase( itList );
1085 
1086  else // This
1087  {
1088  // Too much distant features are not taken into account
1089  float x3D =
1090  (itList->first->x - param.K(0, 2)) * ((param.baseline)) / disp;
1091  float y3D =
1092  (itList->first->y - param.K(1, 2)) * ((param.baseline)) / disp;
1093  float z3D = (param.K(0, 0)) * ((param.baseline)) / disp;
1094 
1095  // Filter out bad points
1096  if ((z3D < param.minZ) || (z3D > param.maxZ))
1097  itList = mfList.erase(itList); // Erase the match : itList =
1098  // mfList.erase( itList );
1099  else
1100  {
1101  TPoint3D p3D(x3D, y3D, z3D);
1102 
1103  // STORE THE OBTAINED LANDMARK
1104  CLandmark lm;
1105 
1106  TPoint3D norm3D = p3D;
1107  norm3D *= -1 / norm3D.norm();
1108 
1109  lm.normal = norm3D;
1110  lm.pose_mean = p3D;
1111  lm.ID = itList->first->ID;
1112 
1113  // If the matched landmarks has a (SIFT or SURF) descriptor,
1114  // asign the left one to the landmark.
1115  // TO DO: Assign the mean value of the descriptor (between the
1116  // matches)
1117  lm.features.resize(1);
1118  lm.features[0] = (*itList).first;
1119 
1120  // Compute the covariance matrix for the landmark
1121  switch (param.uncPropagation)
1122  {
1124  {
1125  float foc2 = square(param.K(0, 0));
1126  float c0 = param.K(0, 2);
1127  float r0 = param.K(1, 2);
1128  float base2 = square(param.baseline);
1129  float disp2 =
1130  square(itList->first->x - itList->second->x);
1131 
1132  lm.pose_cov_11 = stdPixel2 * base2 / disp2 +
1133  stdDisp2 * base2 *
1134  square(itList->first->x - c0) /
1135  square(disp2);
1136  lm.pose_cov_12 =
1137  stdDisp2 * base2 * (itList->first->x - c0) *
1138  (itList->first->y - r0) / square(disp2);
1139  lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
1140  (itList->first->x - c0) /
1141  square(disp2);
1142  lm.pose_cov_22 = stdPixel2 * base2 / disp2 +
1143  stdDisp2 * base2 *
1144  square(itList->first->y - r0) /
1145  square(disp2);
1146  lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
1147  (itList->first->y - r0) /
1148  square(disp2);
1149  lm.pose_cov_33 =
1150  stdDisp2 * foc2 * base2 / square(disp2);
1151  } // end case 'Prop_Linear'
1152  break;
1153 
1155  {
1156  // Parameters
1157  unsigned int Na = 3;
1158  unsigned int i;
1159 
1160  float k = param.factor_k;
1161 
1162  float w0 = k / (Na + k);
1163  float w1 = 1 / (2 * (Na + k));
1164 
1165  CMatrix Pa(3, 3);
1166  CMatrix L(3, 3);
1167 
1168  Pa.fill(0);
1169  Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
1170  Pa(2, 2) = (Na + k) * square(param.stdDisp);
1171 
1172  // Cholesky decomposition
1173  Pa.chol(L); // math::chol(Pa,L);
1174 
1175  vector<TPoint3D> B; // B group
1176  TPoint3D meanB; // Mean value of the B group
1177  CMatrix Pb; // Covariance of the B group
1178 
1179  B.resize(2 * Na + 1); // Set of output values
1180  Pb.fill(0); // Reset the output covariance
1181 
1182  CVectorFloat vAux, myPoint; // Auxiliar vectors
1183  CVectorFloat meanA; // Mean value of the A group
1184 
1185  vAux.resize(3); // Set the variables size
1186  meanA.resize(3);
1187  myPoint.resize(3);
1188 
1189  // Mean input value: (c,r,d)
1190  meanA[0] = itList->first->x;
1191  meanA[1] = itList->first->y;
1192  meanA[2] = disp;
1193 
1194  // Output mean
1195  meanB.x = w0 * x3D;
1196  meanB.y = w0 * y3D;
1197  meanB.z = w0 * z3D; // Add to the mean
1198  B[0].x = x3D;
1199  B[0].y = y3D;
1200  B[0].z = z3D; // Insert into B
1201 
1202  for (i = 1; i <= 2 * Na; i++)
1203  {
1204  // Form the Ai value
1205  if (i <= Na)
1206  {
1207  L.extractRowAsCol(
1208  i - 1, vAux); // Extract the proper row
1209  myPoint[0] = meanA[0] + vAux[0];
1210  myPoint[1] = meanA[1] + vAux[1];
1211  myPoint[2] = meanA[2] + vAux[2];
1212  }
1213  else
1214  {
1215  L.extractRowAsCol(
1216  (i - Na) - 1,
1217  vAux); // Extract the proper row
1218  myPoint[0] = meanA[0] - vAux[0];
1219  myPoint[1] = meanA[1] - vAux[1];
1220  myPoint[2] = meanA[2] - vAux[2];
1221  }
1222 
1223  // Pass the Ai through the functions:
1224  x3D = (myPoint[0] - param.K(0, 2)) *
1225  ((param.baseline)) / myPoint[2];
1226  y3D = (myPoint[1] - param.K(1, 2)) *
1227  ((param.baseline)) / myPoint[2];
1228  z3D = (param.K(0, 0)) * ((param.baseline)) /
1229  myPoint[2];
1230 
1231  // Add to the B mean computation and the B vector
1232  meanB.x = meanB.x + w1 * x3D;
1233  meanB.y = meanB.y + w1 * y3D;
1234  meanB.z = meanB.z + w1 * z3D;
1235 
1236  B[i].x = x3D;
1237  B[i].y = y3D;
1238  B[i].z = z3D;
1239 
1240  } // end for 'i'
1241 
1242  // Output covariance
1243  for (i = 0; i <= 2 * Na; i++)
1244  {
1245  float weight = w1;
1246  CMatrix v(3, 1);
1247 
1248  if (i ==
1249  0) // The weight for the mean value of A is w0
1250  weight = w0;
1251 
1252  v(0, 0) = B[i].x - meanB.x;
1253  v(1, 0) = B[i].y - meanB.y;
1254  v(2, 0) = B[i].z - meanB.z;
1255 
1256  Pb = Pb + weight * (v * v.transpose());
1257  } // end for 'i'
1258 
1259  // Store it in the landmark
1260  lm.pose_cov_11 = Pb(0, 0);
1261  lm.pose_cov_12 = Pb(0, 1);
1262  lm.pose_cov_13 = Pb(0, 2);
1263  lm.pose_cov_22 = Pb(1, 1);
1264  lm.pose_cov_23 = Pb(1, 2);
1265  lm.pose_cov_33 = Pb(2, 2);
1266  } // end case 'Prop_UT'
1267  break;
1268 
1270  {
1271  // Parameters
1272  unsigned int Na = 3;
1273  unsigned int i;
1274 
1275  float a = param.factor_a;
1276  float b = param.factor_b;
1277  float k = param.factor_k;
1278 
1279  float lambda = square(a) * (Na + k) - Na;
1280 
1281  float w0_m = lambda / (Na + lambda);
1282  float w0_c = w0_m + (1 - square(a) + b);
1283  float w1 = 1 / (2 * (Na + lambda));
1284 
1285  CMatrix Pa(3, 3);
1286  CMatrix L(3, 3);
1287 
1288  Pa.fill(0);
1289  Pa(0, 0) = Pa(1, 1) =
1290  (Na + lambda) * square(param.stdPixel);
1291  Pa(2, 2) = (Na + lambda) * square(param.stdDisp);
1292 
1293  // Cholesky decomposition
1294  Pa.chol(L); // math::chol(Pa,L);
1295 
1296  vector<TPoint3D> B; // B group
1297  TPoint3D meanB; // Mean value of the B group
1298  CMatrix Pb; // Covariance of the B group
1299 
1300  B.resize(2 * Na + 1); // Set of output values
1301  Pb.fill(0); // Reset the output covariance
1302 
1303  CVectorFloat vAux, myPoint; // Auxiliar vectors
1304  CVectorFloat meanA; // Mean value of the A group
1305 
1306  vAux.resize(3); // Set the variables size
1307  meanA.resize(3);
1308  myPoint.resize(3);
1309 
1310  // Mean input value: (c,r,d)
1311  meanA[0] = itList->first->x;
1312  meanA[1] = itList->first->y;
1313  meanA[2] = disp;
1314 
1315  // Output mean
1316  meanB.x = w0_m * x3D;
1317  meanB.y = w0_m * y3D;
1318  meanB.z = w0_m * z3D; // Add to the mean
1319  B[0].x = x3D;
1320  B[0].y = y3D;
1321  B[0].z = z3D; // Insert into B
1322 
1323  for (i = 1; i <= 2 * Na; i++)
1324  {
1325  // Form the Ai value
1326  if (i <= Na)
1327  {
1328  L.extractRowAsCol(
1329  i - 1, vAux); // Extract the proper row
1330  myPoint = meanA + vAux;
1331  // myPoint[0] = meanA[0] + vAux[0];
1332  // myPoint[1] = meanA[1] + vAux[1];
1333  // myPoint[2] = meanA[2] + vAux[2];
1334  }
1335  else
1336  {
1337  L.extractRowAsCol(
1338  (i - Na) - 1,
1339  vAux); // Extract the proper row
1340  myPoint = meanA - vAux;
1341  // myPoint[0] = meanA[0] - vAux[0];
1342  // myPoint[1] = meanA[1] - vAux[1];
1343  // myPoint[2] = meanA[2] - vAux[2];
1344  }
1345 
1346  // Pass the Ai through the functions:
1347  x3D = (myPoint[0] - param.K(0, 2)) *
1348  ((param.baseline)) / myPoint[2];
1349  y3D = (myPoint[1] - param.K(1, 2)) *
1350  ((param.baseline)) / myPoint[2];
1351  z3D = (param.K(0, 0)) * ((param.baseline)) /
1352  myPoint[2];
1353 
1354  // Add to the B mean computation and the B vector
1355  meanB.x = meanB.x + w1 * x3D;
1356  meanB.y = meanB.y + w1 * y3D;
1357  meanB.z = meanB.z + w1 * z3D;
1358 
1359  B[i].x = x3D;
1360  B[i].y = y3D;
1361  B[i].z = z3D;
1362 
1363  } // end for 'i'
1364 
1365  // Output covariance
1366  for (i = 0; i <= 2 * Na; i++)
1367  {
1368  float weight = w1;
1369  CMatrix v(3, 1);
1370 
1371  if (i ==
1372  0) // The weight for the mean value of A is w0
1373  weight = w0_c;
1374 
1375  v(0, 0) = B[i].x - meanB.x;
1376  v(1, 0) = B[i].y - meanB.y;
1377  v(2, 0) = B[i].z - meanB.z;
1378 
1379  Pb = Pb + weight * (v * v.transpose());
1380  } // end for 'i'
1381 
1382  // Store it in the landmark
1383  lm.pose_cov_11 = Pb(0, 0);
1384  lm.pose_cov_12 = Pb(0, 1);
1385  lm.pose_cov_13 = Pb(0, 2);
1386  lm.pose_cov_22 = Pb(1, 1);
1387  lm.pose_cov_23 = Pb(1, 2);
1388  lm.pose_cov_33 = Pb(2, 2);
1389  } // end case 'Prop_SUT'
1390  break;
1391 
1392  } // end switch
1393  landmarks.landmarks.push_back(lm);
1394  itList++;
1395  } // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
1396  } // end else
1397  } // end for 'i'
1398 
1399  MRPT_END
1400 } // end of projectMatchedFeatures
1401 
1402 /*-------------------------------------------------------------
1403  projectMatchedFeatures
1404 -------------------------------------------------------------*/
1406  CFeatureList& leftList, CFeatureList& rightList,
1408  mrpt::maps::CLandmarksMap& landmarks)
1409 {
1410  MRPT_START
1411  ASSERT_(leftList.size() == rightList.size());
1412 
1413  landmarks.clear(); // Assert that the output CLandmarksMap is clear
1414 
1415  CFeatureList::iterator itListL, itListR;
1416  float stdPixel2 = square(param.stdPixel);
1417  float stdDisp2 = square(param.stdDisp);
1418 
1419  // Main loop
1420  for (itListL = leftList.begin(), itListR = rightList.begin();
1421  itListL != leftList.end();)
1422  {
1423  float disp = ((*itListL)->x - (*itListR)->x); // Disparity
1424  if (disp < 1e-9) // Filter out too far points
1425  {
1426  itListL = leftList.erase(itListL); // Erase the match : itListL =
1427  // leftList.erase( itListL );
1428  itListR = rightList.erase(itListR); // Erase the match : itListR =
1429  // rightList.erase( itListR );
1430  }
1431  else // This
1432  {
1433  // Too much distant features are not taken into account
1434  float x3D =
1435  ((*itListL)->x - param.K(0, 2)) * ((param.baseline)) / disp;
1436  float y3D =
1437  ((*itListL)->y - param.K(1, 2)) * ((param.baseline)) / disp;
1438  float z3D = (param.K(0, 0)) * ((param.baseline)) / disp;
1439 
1440  // Filter out bad points
1441  if ((z3D < param.minZ) || (z3D > param.maxZ))
1442  {
1443  itListL =
1444  leftList.erase(itListL); // Erase the match : (*itListL) =
1445  // leftList.erase( (*itListL) );
1446  itListR =
1447  rightList.erase(itListR); // Erase the match : (*itListR) =
1448  // rightList.erase( (*itListR) );
1449  }
1450  else
1451  {
1452  TPoint3D p3D(x3D, y3D, z3D);
1453 
1454  // STORE THE OBTAINED LANDMARK
1455  CLandmark lm;
1456 
1457  TPoint3D norm3D = p3D;
1458  norm3D *= -1 / norm3D.norm();
1459 
1460  lm.normal = norm3D;
1461  lm.pose_mean = p3D;
1462  lm.ID = (*itListL)->ID;
1463 
1464  // If the matched landmarks has a (SIFT or SURF) descriptor,
1465  // asign the left one to the landmark.
1466  // TO DO: Assign the mean value of the descriptor (between the
1467  // matches)
1468  lm.features.resize(2);
1469  lm.features[0] = *itListL;
1470  lm.features[1] = *itListR;
1471 
1472  // Compute the covariance matrix for the landmark
1473  switch (param.uncPropagation)
1474  {
1476  {
1477  float foc2 = square(param.K(0, 0));
1478  float c0 = param.K(0, 2);
1479  float r0 = param.K(1, 2);
1480  float base2 = square(param.baseline);
1481  float disp2 = square((*itListL)->x - (*itListR)->x);
1482 
1483  lm.pose_cov_11 = stdPixel2 * base2 / disp2 +
1484  stdDisp2 * base2 *
1485  square((*itListL)->x - c0) /
1486  square(disp2);
1487  lm.pose_cov_12 = stdDisp2 * base2 *
1488  ((*itListL)->x - c0) *
1489  ((*itListL)->y - r0) / square(disp2);
1490  lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) *
1491  ((*itListL)->x - c0) / square(disp2);
1492  lm.pose_cov_22 = stdPixel2 * base2 / disp2 +
1493  stdDisp2 * base2 *
1494  square((*itListL)->y - r0) /
1495  square(disp2);
1496  lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) *
1497  ((*itListL)->y - r0) / square(disp2);
1498  lm.pose_cov_33 =
1499  stdDisp2 * foc2 * base2 / square(disp2);
1500  } // end case 'Prop_Linear'
1501  break;
1502 
1504  {
1505  // Parameters
1506  unsigned int Na = 3;
1507  unsigned int i;
1508 
1509  float k = param.factor_k;
1510 
1511  float w0 = k / (Na + k);
1512  float w1 = 1 / (2 * (Na + k));
1513 
1514  CMatrix Pa(3, 3);
1515  CMatrix L(3, 3);
1516 
1517  Pa.fill(0);
1518  Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
1519  Pa(2, 2) = (Na + k) * square(param.stdDisp);
1520 
1521  // Cholesky decomposition
1522  Pa.chol(L); // math::chol(Pa,L);
1523 
1524  vector<TPoint3D> B; // B group
1525  TPoint3D meanB; // Mean value of the B group
1526  CMatrix Pb; // Covariance of the B group
1527 
1528  B.resize(2 * Na + 1); // Set of output values
1529  Pb.fill(0); // Reset the output covariance
1530 
1531  CVectorFloat vAux, myPoint; // Auxiliar vectors
1532  CVectorFloat meanA; // Mean value of the A group
1533 
1534  vAux.resize(3); // Set the variables size
1535  meanA.resize(3);
1536  myPoint.resize(3);
1537 
1538  // Mean input value: (c,r,d)
1539  meanA[0] = (*itListL)->x;
1540  meanA[1] = (*itListL)->y;
1541  meanA[2] = disp;
1542 
1543  // Output mean
1544  meanB.x = w0 * x3D;
1545  meanB.y = w0 * y3D;
1546  meanB.z = w0 * z3D; // Add to the mean
1547  B[0].x = x3D;
1548  B[0].y = y3D;
1549  B[0].z = z3D; // Insert into B
1550 
1551  for (i = 1; i <= 2 * Na; i++)
1552  {
1553  // Form the Ai value
1554  if (i <= Na)
1555  {
1556  L.extractRowAsCol(
1557  i - 1, vAux); // Extract the proper row
1558  myPoint[0] = meanA[0] + vAux[0];
1559  myPoint[1] = meanA[1] + vAux[1];
1560  myPoint[2] = meanA[2] + vAux[2];
1561  }
1562  else
1563  {
1564  L.extractRowAsCol(
1565  (i - Na) - 1,
1566  vAux); // Extract the proper row
1567  myPoint[0] = meanA[0] - vAux[0];
1568  myPoint[1] = meanA[1] - vAux[1];
1569  myPoint[2] = meanA[2] - vAux[2];
1570  }
1571 
1572  // Pass the Ai through the functions:
1573  x3D = (myPoint[0] - param.K(0, 2)) *
1574  ((param.baseline)) / myPoint[2];
1575  y3D = (myPoint[1] - param.K(1, 2)) *
1576  ((param.baseline)) / myPoint[2];
1577  z3D = (param.K(0, 0)) * ((param.baseline)) /
1578  myPoint[2];
1579 
1580  // Add to the B mean computation and the B vector
1581  meanB.x = meanB.x + w1 * x3D;
1582  meanB.y = meanB.y + w1 * y3D;
1583  meanB.z = meanB.z + w1 * z3D;
1584 
1585  B[i].x = x3D;
1586  B[i].y = y3D;
1587  B[i].z = z3D;
1588 
1589  } // end for 'i'
1590 
1591  // Output covariance
1592  for (i = 0; i <= 2 * Na; i++)
1593  {
1594  float weight = w1;
1595  CMatrix v(3, 1);
1596 
1597  if (i ==
1598  0) // The weight for the mean value of A is w0
1599  weight = w0;
1600 
1601  v(0, 0) = B[i].x - meanB.x;
1602  v(1, 0) = B[i].y - meanB.y;
1603  v(2, 0) = B[i].z - meanB.z;
1604 
1605  Pb = Pb + weight * (v * v.transpose());
1606  } // end for 'i'
1607 
1608  // Store it in the landmark
1609  lm.pose_cov_11 = Pb(0, 0);
1610  lm.pose_cov_12 = Pb(0, 1);
1611  lm.pose_cov_13 = Pb(0, 2);
1612  lm.pose_cov_22 = Pb(1, 1);
1613  lm.pose_cov_23 = Pb(1, 2);
1614  lm.pose_cov_33 = Pb(2, 2);
1615  } // end case 'Prop_UT'
1616  break;
1617 
1619  {
1620  // Parameters
1621  unsigned int Na = 3;
1622  unsigned int i;
1623 
1624  float a = param.factor_a;
1625  float b = param.factor_b;
1626  float k = param.factor_k;
1627 
1628  float lambda = square(a) * (Na + k) - Na;
1629 
1630  float w0_m = lambda / (Na + lambda);
1631  float w0_c = w0_m + (1 - square(a) + b);
1632  float w1 = 1 / (2 * (Na + lambda));
1633 
1634  CMatrix Pa(3, 3);
1635  CMatrix L(3, 3);
1636 
1637  Pa.fill(0);
1638  Pa(0, 0) = Pa(1, 1) =
1639  (Na + lambda) * square(param.stdPixel);
1640  Pa(2, 2) = (Na + lambda) * square(param.stdDisp);
1641 
1642  // Cholesky decomposition
1643  Pa.chol(L); // math::chol(Pa,L);
1644 
1645  vector<TPoint3D> B; // B group
1646  TPoint3D meanB; // Mean value of the B group
1647  CMatrix Pb; // Covariance of the B group
1648 
1649  B.resize(2 * Na + 1); // Set of output values
1650  Pb.fill(0); // Reset the output covariance
1651 
1652  CVectorFloat vAux, myPoint; // Auxiliar vectors
1653  CVectorFloat meanA; // Mean value of the A group
1654 
1655  vAux.resize(3); // Set the variables size
1656  meanA.resize(3);
1657  myPoint.resize(3);
1658 
1659  // Mean input value: (c,r,d)
1660  meanA[0] = (*itListL)->x;
1661  meanA[1] = (*itListL)->y;
1662  meanA[2] = disp;
1663 
1664  // Output mean
1665  meanB.x = w0_m * x3D;
1666  meanB.y = w0_m * y3D;
1667  meanB.z = w0_m * z3D; // Add to the mean
1668  B[0].x = x3D;
1669  B[0].y = y3D;
1670  B[0].z = z3D; // Insert into B
1671 
1672  for (i = 1; i <= 2 * Na; i++)
1673  {
1674  // Form the Ai value
1675  if (i <= Na)
1676  {
1677  L.extractRowAsCol(
1678  i - 1, vAux); // Extract the proper row
1679  myPoint = meanA + vAux;
1680  // myPoint[0] = meanA[0] + vAux[0];
1681  // myPoint[1] = meanA[1] + vAux[1];
1682  // myPoint[2] = meanA[2] + vAux[2];
1683  }
1684  else
1685  {
1686  L.extractRowAsCol(
1687  (i - Na) - 1,
1688  vAux); // Extract the proper row
1689  myPoint = meanA - vAux;
1690  // myPoint[0] = meanA[0] - vAux[0];
1691  // myPoint[1] = meanA[1] - vAux[1];
1692  // myPoint[2] = meanA[2] - vAux[2];
1693  }
1694 
1695  // Pass the Ai through the functions:
1696  x3D = (myPoint[0] - param.K(0, 2)) *
1697  ((param.baseline)) / myPoint[2];
1698  y3D = (myPoint[1] - param.K(1, 2)) *
1699  ((param.baseline)) / myPoint[2];
1700  z3D = (param.K(0, 0)) * ((param.baseline)) /
1701  myPoint[2];
1702 
1703  // Add to the B mean computation and the B vector
1704  meanB.x = meanB.x + w1 * x3D;
1705  meanB.y = meanB.y + w1 * y3D;
1706  meanB.z = meanB.z + w1 * z3D;
1707 
1708  B[i].x = x3D;
1709  B[i].y = y3D;
1710  B[i].z = z3D;
1711 
1712  } // end for 'i'
1713 
1714  // Output covariance
1715  for (i = 0; i <= 2 * Na; i++)
1716  {
1717  float weight = w1;
1718  CMatrix v(3, 1);
1719 
1720  if (i ==
1721  0) // The weight for the mean value of A is w0
1722  weight = w0_c;
1723 
1724  v(0, 0) = B[i].x - meanB.x;
1725  v(1, 0) = B[i].y - meanB.y;
1726  v(2, 0) = B[i].z - meanB.z;
1727 
1728  Pb = Pb + weight * (v * v.transpose());
1729  } // end for 'i'
1730 
1731  // Store it in the landmark
1732  lm.pose_cov_11 = Pb(0, 0);
1733  lm.pose_cov_12 = Pb(0, 1);
1734  lm.pose_cov_13 = Pb(0, 2);
1735  lm.pose_cov_22 = Pb(1, 1);
1736  lm.pose_cov_23 = Pb(1, 2);
1737  lm.pose_cov_33 = Pb(2, 2);
1738  } // end case 'Prop_SUT'
1739  break;
1740 
1741  } // end switch
1742  landmarks.landmarks.push_back(lm);
1743  itListL++;
1744  itListR++;
1745  } // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
1746  } // end else
1747  } // end for 'i'
1748 
1749  MRPT_END
1750 }
1751 
1752 /* -------------------------------------------------------
1753  StereoObs2RBObs #1
1754  ------------------------------------------------------- */
1756  const CMatchedFeatureList& inMatches,
1757  const CMatrixDouble33& intrinsicParams, const double& baseline,
1758  const CPose3D& sensorPose, const vector<double>& sg,
1759  CObservationBearingRange& outObs)
1760 {
1761  // Compute the range and bearing
1762  double f = intrinsicParams(0, 0); // Focal length in pixels
1763  double x0 = intrinsicParams(0, 2); // Principal point column
1764  double y0 = intrinsicParams(1, 2); // Principal point row
1765  double b = baseline; // Stereo camera baseline
1766  double sg_c2 = square(sg[0]); // Sigma of the column variable
1767  double sg_r2 = square(sg[1]); // Sigma of the row variable
1768  double sg_d2 = square(sg[2]); // Sigma of the disparity
1769 
1770  for (const auto& inMatche : inMatches)
1771  {
1772  double x = inMatche.first->x; // Column of the feature
1773  double y = inMatche.first->y; // Row of the feature
1774 
1775  double d = inMatche.first->x - inMatche.second->x; // Disparity
1776  double d2 = square(d);
1777  double k = square(b / d);
1778 
1779  // Projection equations according to a standard camera coordinate axis
1780  // (+Z forward & +Y downwards)
1781  // -------------------------------------------------------------------------------------------------------
1782  double X = (x - x0) * b / d;
1783  double Y = (y - y0) * b / d;
1784  double Z = f * b / d;
1785 
1786  // Projection equations according to a standard coordinate axis (+X
1787  // forward & +Z upwards)
1788  // -------------------------------------------------------------------------------------------------------
1789  // double X = f * b / d;
1790  // double Y = ( x0 - x ) * b / d;
1791  // double Z = ( y0 - y ) * b / d;
1792 
1794  m.range = sqrt(square(X) + square(Y) + square(Z));
1795  m.yaw = atan2(Y, X);
1796  m.pitch = -asin(Z / m.range);
1797  m.landmarkID = inMatche.first->ID;
1798 
1799  // Compute the covariance
1800  // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
1801  // \---------------------------------------/
1802  // aux
1803 
1804  CMatrixDouble33 aux;
1805 
1806  // Jacobian equations according to a standard CAMERA coordinate axis (+Z
1807  // forward & +Y downwards)
1808  // -------------------------------------------------------------------------------------------------------
1809  aux.get_unsafe(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
1810  aux.get_unsafe(0, 1) = aux.get_unsafe(1, 0) =
1811  k * (sg_d2 * (x - x0) * (y - y0) / d2);
1812  aux.get_unsafe(0, 2) = aux.get_unsafe(2, 0) =
1813  k * (sg_d2 * (x - x0) * f / d2);
1814 
1815  aux.get_unsafe(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
1816  aux.get_unsafe(1, 2) = aux.get_unsafe(2, 1) =
1817  k * (sg_d2 * (y - y0) * f / d2);
1818 
1819  aux.get_unsafe(2, 2) = k * (sg_d2 * square(f) / d2);
1820 
1821  // Jacobian equations according to a standard coordinate axis (+X
1822  // forward & +Z upwards)
1823  // -------------------------------------------------------------------------------------------------------
1824  // aux.get_unsafe( 0, 0 ) = k*(sg_d2*square(f)/d2);
1825  // aux.get_unsafe( 0, 1 ) = aux.get_unsafe( 1, 0 ) =
1826  // k*sg_d2*(x0-x)*f/d2;
1827  // aux.get_unsafe( 0, 2 ) = aux.get_unsafe( 2, 0 ) =
1828  // k*sg_d2*(y0-y)*f/d2;
1829 
1830  // aux.get_unsafe( 1, 1 ) = k*(sg_c2 + sg_d2*square(x0-x)/d2);
1831  // aux.get_unsafe( 1, 2 ) = aux.get_unsafe( 2, 1 ) =
1832  // k*sg_d2*(x0-x)*(y0-y)/d2;
1833 
1834  // aux.get_unsafe( 2, 2 ) = k*(sg_r2 + sg_d2*square(y0-y)/d2);
1835 
1836  // CMatrixDouble33 JF;
1837  // JF.set_unsafe(0,0) = JF.set_unsafe(1,1) = JF.set_unsafe(2,0) =
1838  // JF.set_unsafe(2,1) = 0.0f;
1839  // JF.set_unsafe(0,1) = JF.set_unsafe(1,0) = b/d;
1840  // JF.set_unsafe(0,2) = -X/d;
1841  // JF.set_unsafe(1,2) = -Y/d;
1842  // JF.set_unsafe(2,2) = -Z/d;
1843 
1844  CMatrixDouble33 JG;
1845  JG.set_unsafe(0, 0, X / m.range);
1846  JG.set_unsafe(0, 1, Y / m.range);
1847  JG.set_unsafe(0, 2, Z / m.range);
1848 
1849  JG.set_unsafe(1, 0, -Y / (square(X) + square(Y)));
1850  JG.set_unsafe(1, 1, X / (square(X) + square(Y)));
1851  JG.set_unsafe(1, 2, 0);
1852 
1853  JG.set_unsafe(
1854  2, 0, Z * X / (square(m.range) * sqrt(square(X) + square(Y))));
1855  JG.set_unsafe(
1856  2, 1, Z * Y / (square(m.range) * sqrt(square(X) + square(Y))));
1857  JG.set_unsafe(2, 2, -sqrt(square(X) + square(Y)) / square(m.range));
1858 
1859  // CMatrixDouble33 aux;
1860  // CMatrixDouble33 diag;
1861  // diag.zeros();
1862  // diag.set_unsafe(0,0) = square( sg_r );
1863  // diag.set_unsafe(1,1) = square( sg_c );
1864  // diag.set_unsafe(2,2) = square( sg_d );
1865 
1866  // JF.multiply_HCHt( diag, aux );
1867  JG.multiply_HCHt(aux, m.covariance);
1868  // DEBUG:
1869  // m.covariance = aux; // error covariance in 3D
1870  // m.landmarkID = itMatchList->first->id;
1871  outObs.sensedData.push_back(m);
1872 
1873  } // end for
1874 
1875  // Indicate that the covariances have been calculated (for compatibility
1876  // with earlier versions)
1877  outObs.validCovariances = true;
1878  outObs.setSensorPose(sensorPose);
1879 } // end-StereoObs2BRObs
1880 
1881 /* -------------------------------------------------------
1882  StereoObs2RBObs #2
1883  ------------------------------------------------------- */
1885  const CObservationStereoImages& inObs, const vector<double>& sg,
1886  CObservationBearingRange& outObs)
1887 {
1888  // Local variables
1889  CFeatureExtraction fExt;
1890  CFeatureList leftList, rightList;
1891  CMatchedFeatureList matchList;
1892  unsigned int id = 0;
1893 
1894  // Extract features
1895  fExt.detectFeatures(inObs.imageLeft, leftList);
1896  fExt.detectFeatures(inObs.imageRight, rightList);
1897 
1898  // DEBUG:
1899  // CDisplayWindow win1, win2;
1900  // win1.showImageAndPoints( inObs.imageLeft, leftList );
1901  // win2.showImageAndPoints( inObs.imageRight, rightList );
1902 
1903  // Match features
1904  size_t nMatches = vision::matchFeatures(leftList, rightList, matchList);
1905  MRPT_UNUSED_PARAM(nMatches);
1906 
1907  // Compute the range and bearing
1908  double f = inObs.leftCamera.fx(); // Focal length in pixels
1909  double x0 = inObs.leftCamera.cx(); // Principal point column
1910  double y0 = inObs.leftCamera.cy(); // Principal point row
1911  double b = inObs.rightCameraPose.x(); // Stereo camera baseline
1912  double sg_c2 = square(sg[0]); // Sigma of the column variable
1913  double sg_r2 = square(sg[1]); // Sigma of the row variable
1914  double sg_d2 = square(sg[2]); // Sigma of the disparity
1915 
1916  for (auto itMatchList = matchList.begin(); itMatchList != matchList.end();
1917  itMatchList++, id++)
1918  {
1919  double x = itMatchList->first->x; // Column of the feature
1920  double y = itMatchList->first->y; // Row of the feature
1921 
1922  double d = itMatchList->first->x - itMatchList->second->x; // Disparity
1923  double d2 = square(d);
1924  double k = square(b / d);
1925 
1926  // Projection equations according to a standard camera coordinate axis
1927  // (+Z forward & +Y downwards)
1928  // -------------------------------------------------------------------------------------------------------
1929  double X = (x - x0) * b / d;
1930  double Y = (y - y0) * b / d;
1931  double Z = f * b / d;
1932 
1933  // Projection equations according to a standard coordinate axis (+X
1934  // forward & +Z upwards)
1935  // -------------------------------------------------------------------------------------------------------
1936  // double X = f * b / d;
1937  // double Y = ( x0 - x ) * b / d;
1938  // double Z = ( y0 - y ) * b / d;
1939 
1941  m.range = sqrt(square(X) + square(Y) + square(Z));
1942  // m.yaw = atan2( Y,X );
1943  // m.pitch = -asin( Z/m.range );
1944  m.yaw = atan2(X, Z);
1945  m.pitch = atan2(Y, Z);
1946 
1947  // Compute the covariance
1948  // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
1949  // \---------------------------------------/
1950  // aux
1951 
1952  CMatrixDouble33 aux;
1953 
1954  // Jacobian equations according to a standard CAMERA coordinate axis (+Z
1955  // forward & +Y downwards)
1956  // -------------------------------------------------------------------------------------------------------
1957  aux.get_unsafe(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
1958  aux.get_unsafe(0, 1) = aux.get_unsafe(1, 0) =
1959  k * (sg_d2 * (x - x0) * (y - y0) / d2);
1960  aux.get_unsafe(0, 2) = aux.get_unsafe(2, 0) =
1961  k * (sg_d2 * (x - x0) * f / d2);
1962 
1963  aux.get_unsafe(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
1964  aux.get_unsafe(1, 2) = aux.get_unsafe(2, 1) =
1965  k * (sg_d2 * (y - y0) * f / d2);
1966 
1967  aux.get_unsafe(2, 2) = k * (sg_d2 * square(f) / d2);
1968 
1969  // Jacobian equations according to a standard coordinate axis (+X
1970  // forward & +Z upwards)
1971  // -------------------------------------------------------------------------------------------------------
1972  // aux.get_unsafe( 0, 0 ) = k*(sg_d2*square(f)/d2);
1973  // aux.get_unsafe( 0, 1 ) = aux.get_unsafe( 1, 0 ) =
1974  // k*sg_d2*(x0-x)*f/d2;
1975  // aux.get_unsafe( 0, 2 ) = aux.get_unsafe( 2, 0 ) =
1976  // k*sg_d2*(y0-y)*f/d2;
1977 
1978  // aux.get_unsafe( 1, 1 ) = k*(sg_c2 + sg_d2*square(x0-x)/d2);
1979  // aux.get_unsafe( 1, 2 ) = aux.get_unsafe( 2, 1 ) =
1980  // k*sg_d2*(x0-x)*(y0-y)/d2;
1981 
1982  // aux.get_unsafe( 2, 2 ) = k*(sg_r2 + sg_d2*square(y0-y)/d2);
1983 
1984  // CMatrixDouble33 JF;
1985  // JF.set_unsafe(0,0) = JF.set_unsafe(1,1) = JF.set_unsafe(2,0) =
1986  // JF.set_unsafe(2,1) = 0.0f;
1987  // JF.set_unsafe(0,1) = JF.set_unsafe(1,0) = b/d;
1988  // JF.set_unsafe(0,2) = -X/d;
1989  // JF.set_unsafe(1,2) = -Y/d;
1990  // JF.set_unsafe(2,2) = -Z/d;
1991 
1992  CMatrixDouble33 JG;
1993  JG.set_unsafe(0, 0, X / m.range);
1994  JG.set_unsafe(0, 1, Y / m.range);
1995  JG.set_unsafe(0, 2, Z / m.range);
1996 
1997  JG.set_unsafe(1, 0, -Y / (square(X) + square(Y)));
1998  JG.set_unsafe(1, 1, X / (square(X) + square(Y)));
1999  JG.set_unsafe(1, 2, 0);
2000 
2001  JG.set_unsafe(
2002  2, 0, Z * X / (square(m.range) * sqrt(square(X) + square(Y))));
2003  JG.set_unsafe(
2004  2, 1, Z * Y / (square(m.range) * sqrt(square(X) + square(Y))));
2005  JG.set_unsafe(2, 2, -sqrt(square(X) + square(Y)) / square(m.range));
2006 
2007  // CMatrixDouble33 aux;
2008  // CMatrixDouble33 diag;
2009  // diag.zeros();
2010  // diag.set_unsafe(0,0) = square( sg_r );
2011  // diag.set_unsafe(1,1) = square( sg_c );
2012  // diag.set_unsafe(2,2) = square( sg_d );
2013 
2014  // JF.multiply_HCHt( diag, aux );
2015  JG.multiply_HCHt(aux, m.covariance);
2016  // DEBUG:
2017  // m.covariance = aux; // error covariance in 3D
2018  m.landmarkID = id;
2019  outObs.sensedData.push_back(m);
2020  outObs.fieldOfView_yaw = 2 * fabs(atan2(-x0, f));
2021  outObs.fieldOfView_pitch = 2 * fabs(atan2(-y0, f));
2022 
2023  } // end for
2024 
2025  // Indicate that the covariances have been calculated (for compatibility
2026  // with earlier versions)
2027  outObs.validCovariances = true;
2029 
2030 } // end StereoObs2BRObs
2031 
2032 /* -------------------------------------------------------
2033  StereoObs2RBObs #3
2034  ------------------------------------------------------- */
2037 {
2038  // For each of the 3D landmarks [X,Y,Z] we compute their range and bearing
2039  // representation.
2040  // The reference system is assumed to be that typical of cameras: +Z forward
2041  // and +X to the right.
2043  for (itCloud = inObs.landmarks.landmarks.begin();
2044  itCloud != inObs.landmarks.landmarks.end(); ++itCloud)
2045  {
2047  m.range = sqrt(
2048  square(itCloud->pose_mean.x) + square(itCloud->pose_mean.y) +
2049  square(itCloud->pose_mean.z));
2050  // m.yaw = atan2( itCloud->pose_mean.x, itCloud->pose_mean.z
2051  // );
2052  // m.pitch = atan2( itCloud->pose_mean.y, itCloud->pose_mean.z );
2053  // The reference system is assumed to be that typical robot operation:
2054  // +X forward and +Z upwards.
2055  m.yaw = atan2(itCloud->pose_mean.y, itCloud->pose_mean.x);
2056  m.pitch = -sin(itCloud->pose_mean.z / m.range);
2057  m.landmarkID = itCloud->ID;
2058 
2059  outObs.sensedData.push_back(m);
2060  } // end for
2061 } // end StereoObs2BRObs
2062 
2063 /* -------------------------------------------------------
2064  computeStereoRectificationMaps
2065  ------------------------------------------------------- */
2067  const TCamera& cam1, const TCamera& cam2,
2068  const poses::CPose3D& rightCameraPose, void* outMap1x, void* outMap1y,
2069  void* outMap2x, void* outMap2y)
2070 {
2071  ASSERT_(cam1.ncols == cam2.ncols && cam1.nrows == cam2.nrows);
2072 
2073 #if MRPT_HAS_OPENCV
2074 
2075  cv::Mat *mapx1, *mapy1, *mapx2, *mapy2;
2076  mapx1 = static_cast<cv::Mat*>(outMap1x);
2077  mapy1 = static_cast<cv::Mat*>(outMap1y);
2078  mapx2 = static_cast<cv::Mat*>(outMap2x);
2079  mapy2 = static_cast<cv::Mat*>(outMap2y);
2080 
2081  const int resX = cam1.ncols;
2082  const int resY = cam1.nrows;
2083 
2084  CMatrixDouble44 hMatrix;
2085  rightCameraPose.getHomogeneousMatrix(hMatrix);
2086 
2087  double rcTrans[3];
2088  rcTrans[0] = hMatrix(0, 3);
2089  rcTrans[1] = hMatrix(1, 3);
2090  rcTrans[2] = hMatrix(2, 3);
2091 
2092  double m1[3][3];
2093  for (unsigned int i = 0; i < 3; ++i)
2094  for (unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);
2095 
2096  double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
2097  for (unsigned int i = 0; i < 3; ++i)
2098  for (unsigned int j = 0; j < 3; ++j)
2099  {
2100  ipl[i][j] = cam1.intrinsicParams(i, j);
2101  ipr[i][j] = cam2.intrinsicParams(i, j);
2102  }
2103 
2104  for (unsigned int i = 0; i < 5; ++i)
2105  {
2106  dpl[i] = cam1.dist[i];
2107  dpr[i] = cam2.dist[i];
2108  }
2109 
2110  cv::Mat R(3, 3, CV_64F, &m1);
2111  cv::Mat T(3, 1, CV_64F, &rcTrans);
2112 
2113  cv::Mat K1(3, 3, CV_64F, ipl);
2114  cv::Mat K2(3, 3, CV_64F, ipr);
2115  cv::Mat D1(1, 5, CV_64F, dpl);
2116  cv::Mat D2(1, 5, CV_64F, dpr);
2117 
2118  double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
2119  cv::Mat R1(3, 3, CV_64F, _R1);
2120  cv::Mat R2(3, 3, CV_64F, _R2);
2121  cv::Mat P1(3, 4, CV_64F, _P1);
2122  cv::Mat P2(3, 4, CV_64F, _P2);
2123  cv::Mat Q(4, 4, CV_64F, _Q);
2124 
2125  cv::Size nSize(resX, resY);
2126  double alpha = 0.0; // alpha value: 0.0 = zoom and crop the image so that
2127  // there's not black areas
2128 
2129  // OpenCV 2.3+ has this signature:
2130  cv::stereoRectify(
2131  K1, D1, K2, D2, nSize, R, T, R1, R2, P1, P2, Q,
2132  cv::CALIB_ZERO_DISPARITY, alpha);
2133  // Rest of arguments -> default
2134 
2135  cv::Size sz1, sz2;
2136  cv::initUndistortRectifyMap(
2137  K1, D1, R1, P1, cv::Size(resX, resY), CV_32FC1, *mapx1, *mapy1);
2138  cv::initUndistortRectifyMap(
2139  K2, D2, R2, P2, cv::Size(resX, resY), CV_32FC1, *mapx2, *mapy2);
2140 /**/
2141 #else
2142  THROW_EXCEPTION("The MRPT has been compiled without OPENCV!");
2143 #endif
2144 } // end computeStereoRectificationMaps
2145 
2146 /*-------------------------------------------------------------
2147  TROI Constructors
2148 -------------------------------------------------------------*/
2149 vision::TROI::TROI() = default;
2150 vision::TROI::TROI(float x1, float x2, float y1, float y2, float z1, float z2)
2151  : xMin(x1), xMax(x2), yMin(y1), yMax(y2), zMin(z1), zMax(z2)
2152 {
2153 }
2154 
2155 /*-------------------------------------------------------------
2156  TImageROI Constructors
2157 -------------------------------------------------------------*/
2158 vision::TImageROI::TImageROI() = default;
2159 vision::TImageROI::TImageROI(float x1, float x2, float y1, float y2)
2160  : xMin(x1), xMax(x2), yMin(y1), yMax(y2)
2161 {
2162 }
2163 
2164 /*-------------------------------------------------------------
2165  TStereoSystemParams: constructor
2166 -------------------------------------------------------------*/
2168 
2169 {
2170  K = defaultIntrinsicParamsMatrix(0, 640, 480);
2171  F.zeros();
2172  F.set_unsafe(1, 2, -1);
2173  F.set_unsafe(2, 1, 1);
2174 }
2175 
2176 /*-------------------------------------------------------------
2177  TStereoSystemParams: loadFromConfigFile
2178 -------------------------------------------------------------*/
2180  const CConfigFileBase& iniFile, const string& section)
2181 {
2182  int unc;
2183  unc = iniFile.read_int(section.c_str(), "uncPropagation", uncPropagation);
2184  switch (unc)
2185  {
2186  case 0:
2188  break;
2189  case 1:
2191  break;
2192  case 2:
2194  break;
2195  } // end switch
2196 
2197  CVectorDouble k_vec(9);
2198  iniFile.read_vector(
2199  section.c_str(), "k_vec", CVectorDouble(), k_vec, false);
2200  for (unsigned int ii = 0; ii < 3; ++ii)
2201  for (unsigned int jj = 0; jj < 3; ++jj) K(ii, jj) = k_vec[ii * 3 + jj];
2202 
2203  CVectorDouble f_vec(9);
2204  iniFile.read_vector(
2205  section.c_str(), "f_vec", CVectorDouble(), f_vec, false);
2206  for (unsigned int ii = 0; ii < 3; ++ii)
2207  for (unsigned int jj = 0; jj < 3; ++jj) F(ii, jj) = f_vec[ii * 3 + jj];
2208 
2209  baseline = iniFile.read_float(section.c_str(), "baseline", baseline);
2210  stdPixel = iniFile.read_float(section.c_str(), "stdPixel", stdPixel);
2211  stdDisp = iniFile.read_float(section.c_str(), "stdDisp", stdDisp);
2212  maxZ = iniFile.read_float(section.c_str(), "maxZ", maxZ);
2213  minZ = iniFile.read_float(section.c_str(), "minZ", minZ);
2214  maxY = iniFile.read_float(section.c_str(), "maxY", maxY);
2215  factor_k = iniFile.read_float(section.c_str(), "factor_k", factor_k);
2216  factor_a = iniFile.read_float(section.c_str(), "factor_a", factor_a);
2217  factor_b = iniFile.read_float(section.c_str(), "factor_b", factor_b);
2218 } // end of loadFromConfigFile
2219 
2220 /*---------------------------------------------------------------
2221  TStereoSystemParams: dumpToTextStream
2222  ---------------------------------------------------------------*/
2223 void TStereoSystemParams::dumpToTextStream(std::ostream& out) const
2224 {
2225  out << mrpt::format(
2226  "\n----------- [vision::TStereoSystemParams] ------------ \n");
2227  out << mrpt::format("Method for 3D Uncert. \t= ");
2228  switch (uncPropagation)
2229  {
2230  case Prop_Linear:
2231  out << mrpt::format("Linear propagation\n");
2232  break;
2233  case Prop_UT:
2234  out << mrpt::format("Unscented Transform\n");
2235  break;
2236  case Prop_SUT:
2237  out << mrpt::format("Scaled Unscented Transform\n");
2238  break;
2239  } // end switch
2240 
2241  out << mrpt::format("K\t\t\t= [%f\t%f\t%f]\n", K(0, 0), K(0, 1), K(0, 2));
2242  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", K(1, 0), K(1, 1), K(1, 2));
2243  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", K(2, 0), K(2, 1), K(2, 2));
2244 
2245  out << mrpt::format("F\t\t\t= [%f\t%f\t%f]\n", F(0, 0), F(0, 1), F(0, 2));
2246  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", F(1, 0), F(1, 1), F(1, 2));
2247  out << mrpt::format(" \t\t\t [%f\t%f\t%f]\n", F(2, 0), F(2, 1), F(2, 2));
2248 
2249  out << mrpt::format("Baseline \t\t= %f\n", baseline);
2250  out << mrpt::format("Pixel std \t\t= %f\n", stdPixel);
2251  out << mrpt::format("Disparity std\t\t= %f\n", stdDisp);
2252  out << mrpt::format("Z maximum\t\t= %f\n", maxZ);
2253  out << mrpt::format("Z minimum\t\t= %f\n", minZ);
2254  out << mrpt::format("Y maximum\t\t= %f\n", maxY);
2255 
2256  out << mrpt::format("k Factor [UT]\t\t= %f\n", factor_k);
2257  out << mrpt::format("a Factor [UT]\t\t= %f\n", factor_a);
2258  out << mrpt::format("b Factor [UT]\t\t= %f\n", factor_b);
2259  out << mrpt::format(
2260  "-------------------------------------------------------- \n");
2261 }
2262 
2263 /*-------------------------------------------------------------
2264  TMatchingOptions: constructor
2265 -------------------------------------------------------------*/
2267 
2268  = default; // end constructor TMatchingOptions
2269 
2270 /*-------------------------------------------------------------
2271  TMatchingOptions: loadFromConfigFile
2272 -------------------------------------------------------------*/
2274  const CConfigFileBase& iniFile, const string& section)
2275 {
2276  int mm =
2277  iniFile.read_int(section.c_str(), "matching_method", matching_method);
2278  switch (mm)
2279  {
2280  case 0:
2282  break;
2283  case 1:
2285  break;
2286  case 2:
2288  break;
2289  case 3:
2291  break;
2292  case 4:
2294  break;
2295  } // end switch
2296 
2297  useEpipolarRestriction = iniFile.read_bool(
2298  section.c_str(), "useEpipolarRestriction", useEpipolarRestriction);
2299  hasFundamentalMatrix = iniFile.read_bool(
2300  section.c_str(), "hasFundamentalMatrix", hasFundamentalMatrix);
2301  parallelOpticalAxis = iniFile.read_bool(
2302  section.c_str(), "parallelOpticalAxis", parallelOpticalAxis);
2303  useXRestriction =
2304  iniFile.read_bool(section.c_str(), "useXRestriction", useXRestriction);
2305  addMatches = iniFile.read_bool(section.c_str(), "addMatches", addMatches);
2306  useDisparityLimits = iniFile.read_bool(
2307  section.c_str(), "useDisparityLimits", useDisparityLimits);
2308 
2309  min_disp = iniFile.read_float(section.c_str(), "min_disp", min_disp);
2310  max_disp = iniFile.read_float(section.c_str(), "max_disp", max_disp);
2311 
2312  epipolar_TH =
2313  iniFile.read_float(section.c_str(), "epipolar_TH", epipolar_TH);
2314  maxEDD_TH = iniFile.read_float(section.c_str(), "maxEDD_TH", maxEDD_TH);
2315  EDD_RATIO = iniFile.read_float(section.c_str(), "minDIF_TH", EDD_RATIO);
2316  minCC_TH = iniFile.read_float(section.c_str(), "minCC_TH", minCC_TH);
2317  minDCC_TH = iniFile.read_float(section.c_str(), "minDCC_TH", minDCC_TH);
2318  rCC_TH = iniFile.read_float(section.c_str(), "rCC_TH", rCC_TH);
2319  maxEDSD_TH = iniFile.read_float(section.c_str(), "maxEDSD_TH", maxEDSD_TH);
2320  EDSD_RATIO = iniFile.read_float(section.c_str(), "EDSD_RATIO", EDSD_RATIO);
2321  maxSAD_TH = iniFile.read_float(section.c_str(), "maxSAD_TH", maxSAD_TH);
2322  SAD_RATIO = iniFile.read_float(section.c_str(), "SAD_RATIO", SAD_RATIO);
2323  maxORB_dist =
2324  iniFile.read_float(section.c_str(), "maxORB_dist", maxORB_dist);
2325 
2326  estimateDepth =
2327  iniFile.read_bool(section.c_str(), "estimateDepth", estimateDepth);
2328  maxDepthThreshold = iniFile.read_float(
2329  section.c_str(), "maxDepthThreshold", maxDepthThreshold);
2330  // fx = iniFile.read_float(section.c_str(),"fx",fx);
2331  // cx = iniFile.read_float(section.c_str(),"cx",cx);
2332  // cy = iniFile.read_float(section.c_str(),"cy",cy);
2333  // baseline =
2334  // iniFile.read_float(section.c_str(),"baseline",baseline);
2335 } // end TMatchingOptions::loadFromConfigFile
2336 
2337 /*---------------------------------------------------------------
2338  TMatchingOptions: dumpToTextStream
2339  ---------------------------------------------------------------*/
2340 void TMatchingOptions::dumpToTextStream(std::ostream& out) const
2341 {
2342  out << mrpt::format(
2343  "\n----------- [vision::TMatchingOptions] ------------ \n");
2344  out << mrpt::format("Matching method: ");
2345  switch (matching_method)
2346  {
2347  case mmCorrelation:
2348  out << mrpt::format("Cross Correlation\n");
2349  out << mrpt::format(
2350  "· Min. CC. Threshold: %f\n", minCC_TH);
2351  out << mrpt::format(
2352  "· Min. Dif. CC Threshold: %f\n", minDCC_TH);
2353  out << mrpt::format("· Max. Ratio CC Threshold: %f\n", rCC_TH);
2354  break;
2355  case mmDescriptorSIFT:
2356  out << mrpt::format("SIFT descriptor\n");
2357  out << mrpt::format(
2358  "· Max. EDD Threshold: %f\n", maxEDD_TH);
2359  out << mrpt::format(
2360  "· EDD Ratio: %f\n", EDD_RATIO);
2361  break;
2362  case mmDescriptorSURF:
2363  out << mrpt::format("SURF descriptor\n");
2364  out << mrpt::format(
2365  "· EDD Ratio: %f\n", maxEDSD_TH);
2366  out << mrpt::format(
2367  "· Min. CC Threshold: %f\n", EDSD_RATIO);
2368  break;
2369  case mmSAD:
2370  out << mrpt::format("SAD\n");
2371  out << mrpt::format(
2372  "· Max. Dif. SAD Threshold: %f\n", maxSAD_TH);
2373  out << mrpt::format(
2374  "· Ratio SAD Threshold: %f\n", SAD_RATIO);
2375  break;
2376  case mmDescriptorORB:
2377  out << mrpt::format("ORB\n");
2378  out << mrpt::format(
2379  "· Max. distance between desc: %f\n", maxORB_dist);
2380  break;
2381  } // end switch
2382  out << mrpt::format(
2383  "Epipolar Thres: %.2f px\n", epipolar_TH);
2384  out << mrpt::format("Using epipolar restriction?: ");
2385  out << mrpt::format(useEpipolarRestriction ? "Yes\n" : "No\n");
2386  out << mrpt::format("Has Fundamental Matrix?: ");
2387  out << mrpt::format(hasFundamentalMatrix ? "Yes\n" : "No\n");
2388  out << mrpt::format("Are camera axis parallel?: ");
2389  out << mrpt::format(parallelOpticalAxis ? "Yes\n" : "No\n");
2390  out << mrpt::format("Use X-coord restriction?: ");
2391  out << mrpt::format(useXRestriction ? "Yes\n" : "No\n");
2392  out << mrpt::format("Use disparity limits?: ");
2393  out << mrpt::format(useDisparityLimits ? "Yes\n" : "No\n");
2394  if (useDisparityLimits)
2395  out << mrpt::format(
2396  "· Min/max disp limits: %.2f/%.2f px\n", min_disp,
2397  max_disp);
2398  out << mrpt::format("Estimate depth?: ");
2399  out << mrpt::format(estimateDepth ? "Yes\n" : "No\n");
2400  if (estimateDepth)
2401  {
2402  // out << mrpt::format("· Focal length: %f px\n",
2403  // fx);
2404  // out << mrpt::format("· Principal Point (cx): %f px\n",
2405  // cx);
2406  // out << mrpt::format("· Principal Point (cy): %f px\n",
2407  // cy);
2408  // out << mrpt::format("· Baseline: %f m\n",
2409  // baseline);
2410  out << mrpt::format(
2411  "· Maximum depth allowed: %f m\n", maxDepthThreshold);
2412  }
2413  out << mrpt::format("Add matches to list?: ");
2414  out << mrpt::format(addMatches ? "Yes\n" : "No\n");
2415  out << mrpt::format(
2416  "-------------------------------------------------------- \n");
2417 } // end TMatchingOptions::dumpToTextStream
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
float fieldOfView_yaw
Information about the.
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
TMatchingOptions()
Intrinsic parameters of the stereo rig.
Scalar * iterator
Definition: eigen_plugins.h:26
CImage makeShallowCopy() const
Returns a shallow copy of the original image.
Definition: img/CImage.h:498
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
Shallow copy: the copied object is a reference to the original one.
Definition: img/CImage.h:74
uint32_t nrows
Definition: TCamera.h:39
float rCC_TH
Maximum Ratio Between the two highest CC values.
mrpt::math::CMatrixDouble33 F
Stereo Fundamental matrix.
#define MRPT_START
Definition: exceptions.h:282
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.
Declares a matrix of booleans (non serializable).
std::vector< mrpt::vision::CFeature::Ptr > features
The set of features from which the landmark comes.
Definition: CLandmark.h:43
float range
The sensed landmark distance, in meters.
TInternalFeatList::iterator iterator
Definition: CFeature.h:374
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...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:163
bool useXRestriction
Whether or not employ the x-coord restriction for finding correspondences (bumblebee camera...
void setFromMatrix(const Eigen::MatrixBase< Derived > &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
void setRightMaxID(const TFeatureID &rightID)
Definition: CFeature.h:545
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.
size_t size() const
Definition: CFeature.h:395
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...
float min_disp
Disparity limits, see also &#39;useDisparityLimits&#39;.
double norm() const
Point norm.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:45
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:878
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3555
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:581
float EDSD_RATIO
Boundary Ratio between the two lowest SURF EDSD.
bool useEpipolarRestriction
Whether or not take into account the epipolar restriction for finding correspondences.
Matching by Hamming distance between ORB descriptors.
GLdouble s
Definition: glext.h:3682
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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:836
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:223
bool useDisparityLimits
Whether or not use limits (min,max) for the disparity, see also &#39;min_disp, max_disp&#39;.
float stdPixel
Standard deviation of the error in feature detection.
unsigned char uint8_t
Definition: rptypes.h:44
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
T square(const T x)
Inline function for the square of a number.
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:42
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:161
Matching by Euclidean distance between SIFT descriptors.
bool parallelOpticalAxis
Whether or not the stereo rig has the optical axes parallel.
float minDCC_TH
Minimum Difference Between the Maximum Cross Correlation Values.
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:847
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...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:161
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
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:253
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:1337
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 ...
GLint GLvoid * img
Definition: glext.h:3769
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.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
size_t rows() const
Number of rows in the matrix.
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:171
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"))
GLubyte GLubyte b
Definition: glext.h:6372
double coefs[3]
Line coefficients, stored as an array: .
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:17
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:27
double x
X,Y,Z coordinates.
Uncertainty propagation through the Unscented Transformation.
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:388
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_t cols() const
Number of columns in the matrix.
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:313
void rowChecking(CFeatureList &leftList, CFeatureList &rightList, float threshold=1.0)
Search for correspondences which are not in the same row and deletes them.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:953
TStereoSystemParams()
Initilization of default parameters.
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:375
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:45
Matching by sum of absolute differences of the image patches.
double maxSAD_TH
Minimum Euclidean Distance Between Sum of Absolute Differences.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:159
const int FEAT_FREE
const GLdouble * v
Definition: glext.h:3684
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:34
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.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:888
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:82
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:49
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
GLuint id
Definition: glext.h:3920
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:286
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:544
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.
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:1184
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.
GLenum GLint GLint y
Definition: glext.h:3542
float minCC_TH
Minimum Value of the Cross Correlation.
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
internal::TSequenceLandmarks::const_iterator const_iterator
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:47
void deleteRepeatedFeats(CFeatureList &list)
Explore the feature list and removes features which are in the same coordinates.
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
GLuint res
Definition: glext.h:7385
A RGB color - 8bit.
Definition: TColor.h:20
void getDispersion(const CFeatureList &list, mrpt::math::CVectorFloat &std, mrpt::math::CVectorFloat &mean)
Computes the dispersion of the features in the image.
GLenum GLint x
Definition: glext.h:3542
TFeatureType get_type() const
The type of the first feature in the list.
Definition: CFeature.h:323
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:46
Lightweight 3D point.
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...
double distance(const TPoint2D &point) const
Distance from a given point.
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:22
TMatchingMethod matching_method
Matching method.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:110
Lightweight 2D point.
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.
GLfloat param
Definition: glext.h:3838
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
void projectMatchedFeature(const CFeature::Ptr &leftFeat, const CFeature::Ptr &rightFeat, mrpt::math::TPoint3D &p3D, const TStereoSystemParams &params=TStereoSystemParams())
Computes the 3D position of a particular matched feature.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:218
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Uncertainty propagation through the Scaled Unscented Transformation.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
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:74
TMeasurementList sensedData
The list of observed ranges:
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5645
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
2D line without bounds, represented by its equation .
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:512
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at mié abr 24 13:10:13 CEST 2019