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