Main MRPT website > C++ reference for 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>
15 #include <mrpt/vision/CFeature.h>
16 
17 #include <mrpt/poses/CPoint3D.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>
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 -------------------------------------------------------------*/
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 {
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());
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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  {
233  "Unknown camera index!! for 'camIndex'=%u", camIndex);
234  }
235  }
236 
238  resX * fx, resY * fy, resX * cx, resY * cy);
239 }
240 
241 /*-------------------------------------------------------------
242  deleteRepeatedFeats
243 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
320 {
321  std.assign(2, 0);
322  mean.assign(2, 0);
323 
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 -------------------------------------------------------------*/
351  const TMatchingPairList& feat_list, const poses::CPose3D& Rt)
352 {
353  CMatrixDouble44 mat;
354  Rt.getHomogeneousMatrix(mat);
355  double acum = 0.0;
356 
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 -------------------------------------------------------------*/
381  const CObservationVisualLandmarks& cloud1,
382  const CObservationVisualLandmarks& cloud2, TMatchingPairList& outList)
383 {
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
457  const CFeatureList& list1, const CFeatureList& list2,
458  CMatchedFeatureList& matches, const TMatchingOptions& options,
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  {
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 
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);
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 
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 
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 
657  {
658  // Ensure that both features have patches
659  ASSERT_(
660  (*itList1)->patchSize > 0 &&
661  (*itList2)->patchSize == (*itList1)->patchSize);
662 #if !MRPT_HAS_OPENCV
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  {
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;
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;
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;
774  cond1 = minSAD1 < options.maxSAD_TH;
775  cond2 = (minSAD1 / minSAD2) < options.SAD_RATIO;
776  minVal = minSAD1;
777  break;
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  {
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
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;
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
966  "MRPT compiled without OpenCV, can't compute SAD of images!")
967 #endif
968  MRPT_END
969 } // end computeSAD
970 
971 /*-------------------------------------------------------------
972  addFeaturesToImage
973 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
1012  const CFeatureList& leftList, const CFeatureList& rightList,
1013  vector<TPoint3D>& vP3D, const TStereoSystemParams& params)
1014 {
1015  vP3D.reserve(leftList.size());
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
1177  CLandmarksMap& landmarks)
1178 {
1179  MRPT_START
1180 
1181  landmarks.clear(); // Assert that the output CLandmarksMap is clear
1182 
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  {
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 
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 
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 -------------------------------------------------------------*/
1515  CFeatureList& leftList, CFeatureList& rightList,
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  {
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 
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 
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  ------------------------------------------------------- */
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 
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  ------------------------------------------------------- */
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 
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;
2139 
2140 } // end StereoObs2BRObs
2141 
2142 /* -------------------------------------------------------
2143  StereoObs2RBObs #3
2144  ------------------------------------------------------- */
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.
2153  for (itCloud = inObs.landmarks.landmarks.begin();
2154  itCloud != inObs.landmarks.landmarks.end(); ++itCloud)
2155  {
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  ------------------------------------------------------- */
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
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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:
2323  break;
2324  case 1:
2326  break;
2327  case 2:
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 -------------------------------------------------------------*/
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 -------------------------------------------------------------*/
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:
2459  break;
2460  case 1:
2462  break;
2463  case 2:
2465  break;
2466  case 3:
2468  break;
2469  case 4:
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
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
float fieldOfView_yaw
Information about the.
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
TMatchingOptions()
Intrinsic parameters of the stereo rig.
Scalar * iterator
Definition: eigen_plugins.h:26
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
uint32_t nrows
Definition: TCamera.h:41
float rCC_TH
Maximum Ratio Between the two highest CC values.
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::math::CMatrixDouble33 F
Stereo Fundamental matrix.
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
double maxORB_dist
Maximun distance between ORB descriptors.
Declares a matrix of booleans (non serializable).
std::vector< mrpt::vision::CFeature::Ptr > features
The set of features from which the landmark comes.
Definition: CLandmark.h:44
float range
The sensed landmark distance, in meters.
TInternalFeatList::iterator iterator
Definition: CFeature.h:365
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
bool estimateDepth
Whether or not estimate the 3D position of the real features for the matches (only with parallelOptic...
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
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:165
bool useXRestriction
Whether or not employ the x-coord restriction for finding correspondences (bumblebee camera...
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: img/CImage.h:891
void setRightMaxID(const TFeatureID &rightID)
Definition: CFeature.h:536
double SAD_RATIO
Boundary Ratio between the two highest SAD.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
size_t size() const
Definition: CFeature.h:386
void StereoObs2BRObs(const mrpt::obs::CObservationStereoImages &inObs, const std::vector< double > &sg, mrpt::obs::CObservationBearingRange &outObs)
Converts a stereo images observation into a bearing and range observation.
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
float min_disp
Disparity limits, see also &#39;useDisparityLimits&#39;.
double norm() const
Point norm.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
size_t getHeight() const override
Returns the height of the image in pixels.
Definition: CImage.cpp:892
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3551
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:20
STL namespace.
float epipolar_TH
Epipolar constraint (rows of pixels)
float EDSD_RATIO
Boundary Ratio between the two lowest SURF EDSD.
bool useEpipolarRestriction
Whether or not take into account the epipolar restriction for finding correspondences.
Matching by Hamming distance between ORB descriptors.
GLdouble s
Definition: glext.h:3676
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...
bool useDisparityLimits
Whether or not use limits (min,max) for the disparity, see also &#39;min_disp, max_disp&#39;.
float stdPixel
Standard deviation of the error in feature detection.
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
T square(const T x)
Inline function for the square of a number.
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:44
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Matching by Euclidean distance between SIFT descriptors.
bool parallelOpticalAxis
Whether or not the stereo rig has the optical axes parallel.
float minDCC_TH
Minimum Difference Between the Maximum Cross Correlation Values.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
size_t getWidth() const override
Returns the width of the image in pixels.
Definition: CImage.cpp:864
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:163
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:75
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1413
Parameters associated to a stereo system.
bool addMatches
Whether or not to add the matches found into the input matched list (if false the input list will be ...
GLint GLvoid * img
Definition: glext.h:3763
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Matching by Euclidean distance between SURF descriptors.
double x
Translation in x,y,z.
A list of TMatchingPair.
Definition: TMatchingPair.h:83
size_t rows() const
Number of rows in the matrix.
void rectangle(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1)
Draws a rectangle (an empty rectangle, without filling)
Definition: CCanvas.cpp:170
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
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.
double coefs[3]
Line coefficients, stored as an array: .
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
double x
X,Y,Z coordinates.
Uncertainty propagation through the Unscented Transformation.
double maxDepthThreshold
The maximum allowed depth for the matching.
double computeMsd(const mrpt::tfest::TMatchingPairList &list, const poses::CPose3D &Rt)
Computes the mean squared distance between a set of 3D correspondences ...
iterator erase(const iterator &it)
Definition: CFeature.h:379
void computeStereoRectificationMaps(const mrpt::img::TCamera &cam1, const mrpt::img::TCamera &cam2, const mrpt::poses::CPose3D &rightCameraPose, void *outMap1x, void *outMap1y, void *outMap2x, void *outMap2y)
Computes a pair of x-and-y maps for stereo rectification from a pair of cameras and the relative pose...
size_t cols() const
Number of columns in the matrix.
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:304
void rowChecking(CFeatureList &leftList, CFeatureList &rightList, float threshold=1.0)
Search for correspondences which are not in the same row and deletes them.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:33
CImage grayscale() const
Returns a grayscale version of the image, or itself if it is already a grayscale image.
Definition: CImage.cpp:994
TStereoSystemParams()
Initilization of default parameters.
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:366
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:47
Matching by sum of absolute differences of the image patches.
double maxSAD_TH
Minimum Euclidean Distance Between Sum of Absolute Differences.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:161
const int FEAT_FREE
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:906
const float R
float computeMainOrientation(const mrpt::img::CImage &image, unsigned int x, unsigned int y)
Computes the main orientation of a set of points with an image (for using in SIFT-based algorithms) ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::TPoint3D normal
The "normal" to the landmark, i.e.
Definition: CLandmark.h:50
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
GLuint id
Definition: glext.h:3909
size_t matchFeatures(const CFeatureList &list1, const CFeatureList &list2, CMatchedFeatureList &matches, const TMatchingOptions &options=TMatchingOptions(), const TStereoSystemParams &params=TStereoSystemParams())
Find the matches between two lists of features which must be of the same type.
#define MRPT_END
Definition: exceptions.h:266
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
A structure containing options for the matching.
Matching by cross correlation of the image patches.
void setLeftMaxID(const TFeatureID &leftID)
Explicitly set the max IDs values to certain values.
Definition: CFeature.h:535
void generateMask(const CMatchedFeatureList &mList, mrpt::math::CMatrixBool &mask1, mrpt::math::CMatrixBool &mask2, int wSize=10)
Calculates the Sum of Absolutes Differences (range [0,1]) between two patches.
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:30
bool hasFundamentalMatrix
Whether or not there is a fundamental matrix.
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
double computeSAD(const mrpt::img::CImage &patch1, const mrpt::img::CImage &patch2)
Calculates the Sum of Absolutes Differences (range [0,1]) between two patches.
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
Definition: CImage.cpp:1359
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:33
float maxEDSD_TH
Maximum Euclidean Distance Between SURF Descriptors.
GLenum GLint GLint y
Definition: glext.h:3538
float minCC_TH
Minimum Value of the Cross Correlation.
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
internal::TSequenceLandmarks::const_iterator const_iterator
void flip(mrpt::img::CImage &img)
Invert an image using OpenCV function.
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:46
void deleteRepeatedFeats(CFeatureList &list)
Explore the feature list and removes features which are in the same coordinates.
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
GLuint res
Definition: glext.h:7268
A RGB color - 8bit.
Definition: TColor.h:22
void getDispersion(const CFeatureList &list, mrpt::math::CVectorFloat &std, mrpt::math::CVectorFloat &mean)
Computes the dispersion of the features in the image.
GLenum GLint x
Definition: glext.h:3538
TFeatureType get_type() const
The type of the first feature in the list.
Definition: CFeature.h:314
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: CLandmark.h:47
Lightweight 3D point.
void cloudsToMatchedList(const mrpt::obs::CObservationVisualLandmarks &cloud1, const mrpt::obs::CObservationVisualLandmarks &cloud2, mrpt::tfest::TMatchingPairList &outList)
Transform two clouds of 3D points into a matched list of points ...
void openCV_cross_correlation(const mrpt::img::CImage &img, const mrpt::img::CImage &patch_img, size_t &x_max, size_t &y_max, double &max_val, int x_search_ini=-1, int y_search_ini=-1, int x_search_size=-1, int y_search_size=-1)
Computes the correlation between this image and another one, encapsulating the openCV function cvMatc...
double distance(const TPoint2D &point) const
Distance from a given point.
void projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::img::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:24
TMatchingMethod matching_method
Matching method.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
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
Lightweight 2D point.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
GLfloat param
Definition: glext.h:3831
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
void projectMatchedFeature(const CFeature::Ptr &leftFeat, const CFeature::Ptr &rightFeat, mrpt::math::TPoint3D &p3D, const TStereoSystemParams &params=TStereoSystemParams())
Computes the 3D position of a particular matched feature.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:41
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::math::CMatrixDouble33 defaultIntrinsicParamsMatrix(unsigned int camIndex=0, unsigned int resolutionX=320, unsigned int resolutionY=240)
Returns the stored, default intrinsic params matrix for a given camera:
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
mrpt::math::TPoint3D pixelTo3D(const mrpt::img::TPixelCoordf &xy, const mrpt::math::CMatrixDouble33 &A)
Extract a UNITARY 3D vector in the direction of a 3D point, given from its (x,y) pixels coordinates...
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:223
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
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
Uncertainty propagation through the Scaled Unscented Transformation.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:130
float stdDisp
Standard deviation of the error in disparity computation.
The central class from which images can be analyzed in search of different kinds of interest points a...
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CLandmark.h:75
TMeasurementList sensedData
The list of observed ranges:
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:5566
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
2D line without bounds, represented by its equation .
mrpt::math::CMatrixDouble33 buildIntrinsicParamsMatrix(const double focalLengthX, const double focalLengthY, const double centerX, const double centerY)
Builds the intrinsic parameters matrix A from parameters:
A list of features.
Definition: CFeature.h:503
void addFeaturesToImage(const mrpt::img::CImage &inImg, const CFeatureList &theList, mrpt::img::CImage &outImg)
Draw rectangles around each of the features on a copy of the input image.
mrpt::math::CMatrixDouble33 covariance
The covariance matrix of the landmark, with variable indices [0,1,2] being [range,yaw,pitch].
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019