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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019