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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018