Main MRPT website > C++ reference for MRPT 1.5.7
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>
15 #include <mrpt/vision/CFeatureExtraction.h>
16 #include <mrpt/vision/CFeature.h>
17 
18 #include <mrpt/poses/CPoint3D.h>
19 #include <mrpt/maps/CLandmarksMap.h>
20 #include <mrpt/obs/CObservationVisualLandmarks.h>
21 #include <mrpt/obs/CObservationStereoImages.h>
22 #include <mrpt/obs/CObservationBearingRange.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>
27 #include <mrpt/math/lightweight_geom_data.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 -------------------------------------------------------------*/
56 void vision::openCV_cross_correlation(
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 {
67  MRPT_START
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 -------------------------------------------------------------*/
157 void vision::flip(
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 -------------------------------------------------------------*/
175 TPoint3D vision::pixelTo3D(
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 -------------------------------------------------------------*/
197 CMatrixDouble33 vision::buildIntrinsicParamsMatrix(
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 -------------------------------------------------------------*/
218 CMatrixDouble33 vision::defaultIntrinsicParamsMatrix(
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 -------------------------------------------------------------*/
253 void vision::deleteRepeatedFeats(
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 -------------------------------------------------------------*/
287 void vision::rowChecking(
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 -------------------------------------------------------------*/
325 void vision::getDispersion(
326  const CFeatureList & list,
327  CVectorFloat & std,
328  CVectorFloat & mean )
329 {
330  std.assign(2,0);
331  mean.assign(2,0);
332 
333  CFeatureList::const_iterator it;
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 -------------------------------------------------------------*/
359 double vision::computeMsd(
360  const TMatchingPairList & feat_list,
361  const poses::CPose3D & Rt )
362 {
363  CMatrixDouble44 mat;
364  Rt.getHomogeneousMatrix(mat);
365  double acum = 0.0;
366 
367  TMatchingPairList::const_iterator it;
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 -------------------------------------------------------------*/
384 void vision::cloudsToMatchedList(
385  const CObservationVisualLandmarks & cloud1,
386  const CObservationVisualLandmarks & cloud2,
387  TMatchingPairList & outList)
388 {
389  CLandmarksMap::TCustomSequenceLandmarks::const_iterator itLand1, itLand2;
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 -------------------------------------------------------------*/
414 float vision::computeMainOrientation(
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 -------------------------------------------------------------*/
435 void vision::normalizeImage(
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 -------------------------------------------------------------*/
460 size_t vision::matchFeatures(
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 
533  CMatrixDouble31 l, p;
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 
556  case TMatchingOptions::mmDescriptorSIFT:
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 
578  case TMatchingOptions::mmCorrelation:
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 
602  case TMatchingOptions::mmDescriptorSURF:
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 
624  case TMatchingOptions::mmDescriptorORB:
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 
644  case TMatchingOptions::mmSAD:
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  {
708  case TMatchingOptions::mmDescriptorSIFT:
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;
713  case TMatchingOptions::mmCorrelation:
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;
718  case TMatchingOptions::mmDescriptorSURF:
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;
723  case TMatchingOptions::mmSAD:
724  cond1 = minSAD1 < options.maxSAD_TH;
725  cond2 = (minSAD1/minSAD2) < options.SAD_RATIO;
726  minVal = minSAD1;
727  break;
728  case TMatchingOptions::mmDescriptorORB:
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
847 void vision::generateMask(
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;
862  CMatchedFeatureList::const_iterator it;
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 -------------------------------------------------------------*/
888 double vision::computeSAD(
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 -------------------------------------------------------------*/
912 void vision::addFeaturesToImage(
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 -------------------------------------------------------------*/
925 void vision::projectMatchedFeatures(
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 -------------------------------------------------------------*/
949 void vision::projectMatchedFeatures(
950  const CFeatureList & leftList,
951  const CFeatureList & rightList,
952  vector<TPoint3D> & vP3D,
953  const TStereoSystemParams & params )
954 {
955  vP3D.reserve( leftList.size() );
956  CFeatureList::const_iterator it1, it2;
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 -------------------------------------------------------------*/
969 void vision::projectMatchedFeature(
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 -------------------------------------------------------------*/
1074 void vision::projectMatchedFeatures(
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 
1083  CMatchedFeatureList::iterator itList;
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  {
1126  case TStereoSystemParams::Prop_Linear:
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 
1144  case TStereoSystemParams::Prop_UT:
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 
1248  case TStereoSystemParams::Prop_SUT:
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 -------------------------------------------------------------*/
1372 void vision::projectMatchedFeatures(
1373  CFeatureList & leftList,
1374  CFeatureList & rightList,
1375  const vision::TStereoSystemParams & param,
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  {
1432  case TStereoSystemParams::Prop_Linear:
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 
1450  case TStereoSystemParams::Prop_UT:
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 
1554  case TStereoSystemParams::Prop_SUT:
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  ------------------------------------------------------- */
1679 void vision::StereoObs2BRObs(
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 
1720  CObservationBearingRange::TMeasurement m;
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  ------------------------------------------------------- */
1799 void vision::StereoObs2BRObs(
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 
1855  CObservationBearingRange::TMeasurement m;
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;
1931  outObs.setSensorPose( mrpt::poses::CPose3D(inObs.cameraPose) );
1932 
1933 } // end StereoObs2BRObs
1934 
1935 /* -------------------------------------------------------
1936  StereoObs2RBObs #3
1937  ------------------------------------------------------- */
1938 void vision::StereoObs2BRObs(
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.
1944  CLandmarksMap::TCustomSequenceLandmarks::const_iterator itCloud;
1945  for( itCloud = inObs.landmarks.landmarks.begin(); itCloud != inObs.landmarks.landmarks.end(); ++itCloud )
1946  {
1947  CObservationBearingRange::TMeasurement m;
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  ------------------------------------------------------- */
1963 void vision::computeStereoRectificationMaps(
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 -------------------------------------------------------------*/
2088 vision::TImageROI::TImageROI():
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 -------------------------------------------------------------*/
2101 TStereoSystemParams::TStereoSystemParams() :
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 -------------------------------------------------------------*/
2122 void TStereoSystemParams::loadFromConfigFile(
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:
2131  uncPropagation = Prop_Linear;
2132  break;
2133  case 1:
2134  uncPropagation = Prop_UT;
2135  break;
2136  case 2:
2137  uncPropagation = Prop_SUT;
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  ---------------------------------------------------------------*/
2167 void TStereoSystemParams::dumpToTextStream(
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 -------------------------------------------------------------*/
2209 TMatchingOptions::TMatchingOptions() :
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 -------------------------------------------------------------*/
2249 void TMatchingOptions::loadFromConfigFile(
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:
2257  matching_method = mmCorrelation;
2258  break;
2259  case 1:
2260  matching_method = mmDescriptorSIFT;
2261  break;
2262  case 2:
2263  matching_method = mmDescriptorSURF;
2264  break;
2265  case 3:
2266  matching_method = mmSAD;
2267  break;
2268  case 4:
2269  matching_method = mmDescriptorORB;
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  ---------------------------------------------------------------*/
2306 void TMatchingOptions::dumpToTextStream(
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
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
mrpt::vision::projectMatchedFeatures
void VISION_IMPEXP projectMatchedFeatures(const CMatchedFeatureList &matches, const mrpt::utils::TStereoCamera &stereo_camera, std::vector< mrpt::math::TPoint3D > &out_points)
Definition: vision_utils.cpp:925
mrpt::obs::CObservationBearingRange::fieldOfView_yaw
float fieldOfView_yaw
Information about the sensor: Ranges, in meters (0: there is no limits)
Definition: obs/CObservationBearingRange.h:40
mrpt::utils::TPixelCoordf
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
mrpt::vision::TMatchingOptions::EDD_RATIO
float EDD_RATIO
Boundary Ratio between the two lowest EDD.
Definition: vision/include/mrpt/vision/types.h:311
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
alpha
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
mrpt::vision::TMatchingOptions::rCC_TH
float rCC_TH
Maximum Ratio Between the two highest CC values.
Definition: vision/include/mrpt/vision/types.h:316
mrpt::vision::TStereoSystemParams::F
mrpt::math::CMatrixDouble33 F
Stereo Fundamental matrix.
Definition: vision/include/mrpt/vision/types.h:193
mrpt::utils::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:99
mrpt::vision::computeSAD
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.
Definition: vision_utils.cpp:888
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::vision::TMatchingOptions::maxORB_dist
double maxORB_dist
Maximun distance between ORB descriptors.
Definition: vision/include/mrpt/vision/types.h:327
mrpt::math::CMatrixBool
Declares a matrix of booleans (non serializable).
Definition: CMatrixTemplate.h:572
mrpt::obs::CObservationStereoImages::imageLeft
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
Definition: obs/CObservationStereoImages.h:69
mrpt::obs::CObservationBearingRange::TMeasurement::range
float range
The sensed landmark distance, in meters.
Definition: obs/CObservationBearingRange.h:52
mrpt::vision::TMatchingOptions::estimateDepth
bool estimateDepth
Whether or not estimate the 3D position of the real features for the matches (only with parallelOptic...
Definition: vision/include/mrpt/vision/types.h:330
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
mrpt::utils::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:73
mrpt::vision::TMatchingOptions::useXRestriction
bool useXRestriction
Whether or not employ the x-coord restriction for finding correspondences (bumblebee camera...
Definition: vision/include/mrpt/vision/types.h:296
mrpt::vision::cloudsToMatchedList
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 ...
Definition: vision_utils.cpp:384
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
mrpt::vision::CMatchedFeatureList::setRightMaxID
void setRightMaxID(const TFeatureID &rightID)
Definition: CFeature.h:385
mrpt::vision::TMatchingOptions::SAD_RATIO
double SAD_RATIO
Boundary Ratio between the two highest SAD.
Definition: vision/include/mrpt/vision/types.h:324
mrpt::vision::CFeatureList::size
size_t size() const
Definition: CFeature.h:280
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
mrpt::utils::CImage::resize
void resize(unsigned int width, unsigned int height, TImageChannels nChannels, bool originTopLeft)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
Definition: CImage.h:209
mrpt::vision::CFeatureExtraction::detectFeatures
void detectFeatures(const mrpt::utils::CImage &img, CFeatureList &feats, const unsigned int init_ID=0, const unsigned int nDesiredFeatures=0, const TImageROI &ROI=TImageROI()) const
Extract features from the image based on the method defined in TOptions.
Definition: CFeatureExtraction_common.cpp:46
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: maps/CColouredPointsMap.h:163
mrpt::vision::StereoObs2BRObs
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.
Definition: vision_utils.cpp:1799
mrpt::utils::TCamera::cy
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:156
ops_vectors.h
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::obs::CObservationVisualLandmarks
Declares a class derived from "CObservation" that stores a Landmarks Map as seen from a stereo camera...
Definition: obs/CObservationVisualLandmarks.h:29
mrpt::math::TPoint3D::norm
double norm() const
Point norm.
Definition: lightweight_geom_data.h:300
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
image
GLenum GLsizei GLenum GLenum const GLvoid * image
Definition: glext.h:3522
mrpt::vision::CFeatureList::const_iterator
TInternalFeatList::const_iterator const_iterator
Definition: CFeature.h:262
mrpt::utils::CCanvas::rectangle
void rectangle(int x0, int y0, int x1, int y1, const mrpt::utils::TColor color, unsigned int width=1)
Draws a rectangle (an empty rectangle, without filling)
Definition: CCanvas.cpp:176
std
STL namespace.
mrpt::vision::TMatchingOptions::epipolar_TH
float epipolar_TH
Epipolar constraint (rows of pixels)
Definition: vision/include/mrpt/vision/types.h:307
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::vision::TMatchingOptions::EDSD_RATIO
float EDSD_RATIO
Boundary Ratio between the two lowest SURF EDSD.
Definition: vision/include/mrpt/vision/types.h:320
CFeatureExtraction.h
mrpt::vision::addFeaturesToImage
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.
Definition: vision_utils.cpp:912
mrpt::math::TPoint3D::z
double z
X,Y,Z coordinates.
Definition: lightweight_geom_data.h:248
mrpt::vision::TMatchingOptions::useEpipolarRestriction
bool useEpipolarRestriction
Whether or not take into account the epipolar restriction for finding correspondences.
Definition: vision/include/mrpt/vision/types.h:293
mrpt::vision::projectMatchedFeature
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.
Definition: vision_utils.cpp:969
mrpt::vision::TMatchingOptions::mmDescriptorORB
Matching by Hamming distance between ORB descriptors.
Definition: vision/include/mrpt/vision/types.h:289
s
GLdouble s
Definition: glext.h:3602
mrpt::utils::CImage::getAs
const T * getAs() const
Returns a pointer to a const T* containing the image - the idea is to call like "img.getAs<IplImage>()" so we can avoid here including OpenCV&#39;s headers.
Definition: CImage.h:517
mrpt::vision::TMatchingOptions::loadFromConfigFile
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.
Definition: vision_utils.cpp:2249
CFeature.h
mrpt::utils::TStereoCamera
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:33
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::vision::TMatchingOptions::useDisparityLimits
bool useDisparityLimits
Whether or not use limits (min,max) for the disparity, see also &#39;min_disp, max_disp&#39;.
Definition: vision/include/mrpt/vision/types.h:298
mrpt::vision::TStereoSystemParams::stdPixel
float stdPixel
Standard deviation of the error in feature detection.
Definition: vision/include/mrpt/vision/types.h:203
mrpt::maps::CLandmark::features
std::vector< mrpt::vision::CFeaturePtr > features
The set of features from which the landmark comes.
Definition: maps/CLandmark.h:42
mrpt::obs::CObservationBearingRange::TMeasurement::yaw
float yaw
The sensed landmark direction, in radians, measured as the yaw (azimuth) and pitch (negative elevatio...
Definition: obs/CObservationBearingRange.h:58
mrpt::vision::pixelTo3D
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...
Definition: vision_utils.cpp:175
mrpt::utils::TCamera::fx
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:158
mrpt::utils::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:81
mrpt::obs::CObservationStereoImages::cameraPose
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
Definition: obs/CObservationStereoImages.h:87
mrpt::obs::CObservationBearingRange::TMeasurement::landmarkID
int32_t landmarkID
The ID of the sensed beacon, or INVALID_LANDMARK_ID (-1) if the sensor does not identify the landmark...
Definition: obs/CObservationBearingRange.h:61
mrpt::vision::TMatchingOptions::mmDescriptorSIFT
Matching by Euclidean distance between SIFT descriptors.
Definition: vision/include/mrpt/vision/types.h:280
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::vision::TMatchingOptions::parallelOpticalAxis
bool parallelOpticalAxis
Whether or not the stereo rig has the optical axes parallel.
Definition: vision/include/mrpt/vision/types.h:295
mrpt::vision::TMatchingOptions::minDCC_TH
float minDCC_TH
Minimum Difference Between the Maximum Cross Correlation Values.
Definition: vision/include/mrpt/vision/types.h:315
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: maps/CLandmarksMap.h:59
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::vision::CMatchedFeatureList::getMaxID
void getMaxID(const TListIdx &idx, TFeatureID &firstListID, TFeatureID &secondListID)
Returns the maximum ID of the features in the list.
Definition: CFeature.cpp:1165
mrpt::vision::TStereoSystemParams
Parameters associated to a stereo system.
Definition: vision/include/mrpt/vision/types.h:167
mrpt::vision::TMatchingOptions::addMatches
bool addMatches
Whether or not to add the matches found into the input matched list (if false the input list will be ...
Definition: vision/include/mrpt/vision/types.h:297
img
GLint GLvoid * img
Definition: glext.h:3645
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::push_back
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
Definition: CLandmarksMap.cpp:1723
mrpt::vision::TMatchingOptions::mmDescriptorSURF
Matching by Euclidean distance between SURF descriptors.
Definition: vision/include/mrpt/vision/types.h:283
mrpt::vision::openCV_cross_correlation
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...
Definition: vision_utils.cpp:56
mrpt::utils::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:78
mrpt::utils::CConfigFileBase::read_vector
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
Definition: CConfigFileBase.h:92
mrpt::vision::TMatchingOptions::max_disp
float max_disp
Disparity limits, see also &#39;useDisparityLimits&#39;.
Definition: vision/include/mrpt/vision/types.h:301
mrpt::utils::CImage::grayscale
CImage grayscale() const
Returns a grayscale version of the image, or itself if it is already a grayscale image.
Definition: CImage.cpp:992
mrpt::obs::CObservationStereoImages
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
Definition: obs/CObservationStereoImages.h:41
mrpt::obs::CObservationBearingRange::fieldOfView_pitch
float fieldOfView_pitch
Information about the sensor: The "field-of-view" of the sensor, in radians (for pitch )...
Definition: obs/CObservationBearingRange.h:41
mrpt::utils::TColor
A RGB color - 8bit.
Definition: TColor.h:26
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
mrpt::utils::TCamera::ncols
uint32_t ncols
Definition: TCamera.h:53
b
GLubyte GLubyte b
Definition: glext.h:5575
mrpt::math::TLine2D::coefs
double coefs[3]
Line coefficients, stored as an array: .
Definition: lightweight_geom_data.h:740
mrpt::vision
Classes for computer vision, detectors, features, etc.
Definition: bundle_adjustment.h:22
mrpt::vision::computeMsd
double VISION_IMPEXP computeMsd(const mrpt::utils::TMatchingPairList &list, const poses::CPose3D &Rt)
Computes the mean squared distance between a set of 3D correspondences ...
Definition: vision_utils.cpp:359
mrpt::vision::TStereoSystemParams::Prop_UT
Uncertainty propagation through the Unscented Transformation.
Definition: vision/include/mrpt/vision/types.h:184
mrpt::vision::TMatchingOptions::maxDepthThreshold
double maxDepthThreshold
The maximum allowed depth for the matching. If its computed depth is larger than this, the match won&#39;t be considered.
Definition: vision/include/mrpt/vision/types.h:331
mrpt::vision::CFeatureList::erase
iterator erase(const iterator &it)
Definition: CFeature.h:277
mrpt::vision::TFeatureID
uint64_t TFeatureID
Definition of a feature ID.
Definition: vision/include/mrpt/vision/types.h:27
mrpt::utils::CImage::setFromIplImageReadOnly
void setFromIplImageReadOnly(void *iplImage)
Reads the image from a OpenCV IplImage object (WITHOUT making a copy) and from now on the image canno...
Definition: CImage.cpp:343
mrpt::vision::flip
void VISION_IMPEXP flip(mrpt::utils::CImage &img)
Invert an image using OpenCV function.
Definition: vision_utils.cpp:157
mrpt::math::CMatrixTemplate::set_unsafe
void set_unsafe(size_t row, size_t col, const T &v)
Fast but unsafe method to write a value in the matrix.
Definition: CMatrixTemplate.h:347
mrpt::utils::TCamera::cx
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:154
mrpt::obs::CObservationStereoImages::leftCamera
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
Definition: obs/CObservationStereoImages.h:85
mrpt::vision::CFeatureList
A list of visual features, to be used as output by detectors, as input/output by trackers, etc.
Definition: CFeature.h:211
mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
mrpt::vision::rowChecking
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.
Definition: vision_utils.cpp:287
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::vision::computeMainOrientation
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) ...
Definition: vision_utils.cpp:414
mrpt::obs::CObservationBearingRange::setSensorPose
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
Definition: obs/CObservationBearingRange.h:87
mrpt::obs::CObservationStereoImages::imageRight
mrpt::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
Definition: obs/CObservationStereoImages.h:72
mrpt::utils::TCamera::intrinsicParams
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
mrpt::vision::TStereoSystemParams::TStereoSystemParams
TStereoSystemParams()
Initilization of default parameters.
Definition: vision_utils.cpp:2101
mrpt::vision::TMatchingOptions::mmSAD
Matching by sum of absolute differences of the image patches.
Definition: vision/include/mrpt/vision/types.h:286
CTicTac.h
mrpt::vision::TMatchingOptions::maxSAD_TH
double maxSAD_TH
Minimum Euclidean Distance Between Sum of Absolute Differences.
Definition: vision/include/mrpt/vision/types.h:323
mrpt::utils::CImage::setFromMatrix
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: CImage.h:746
FEAT_FREE
const int FEAT_FREE
Definition: vision_utils.cpp:48
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::maps::CLandmark
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: maps/CLandmark.h:34
mrpt::vision::TMatchingOptions::dumpToTextStream
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.
Definition: vision_utils.cpp:2306
lightweight_geom_data.h
mrpt::utils::CImage::setFromImageReadOnly
void setFromImageReadOnly(const CImage &other_img)
Sets the internal IplImage pointer to that of another given image, WITHOUT making a copy...
Definition: CImage.h:739
mrpt::utils::TCamera::dist
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:55
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::maps::CLandmark::normal
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.
Definition: maps/CLandmark.h:45
mrpt::obs::CObservationStereoImages::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
Definition: obs/CObservationStereoImages.h:94
mrpt::vision::computeStereoRectificationMaps
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...
Definition: vision_utils.cpp:1963
id
GLuint id
Definition: glext.h:3770
mrpt::vision::matchFeatures
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.
Definition: vision_utils.cpp:460
mrpt::obs::CObservationBearingRange
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
Definition: obs/CObservationBearingRange.h:29
mrpt::vision::TMatchingOptions
A structure containing options for the matching.
Definition: vision/include/mrpt/vision/types.h:268
mrpt::utils::CImage::extract_patch
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
Definition: CImage.cpp:1368
mrpt::utils::TCamera::nrows
uint32_t nrows
Camera resolution.
Definition: TCamera.h:53
mrpt::vision::TMatchingOptions::mmCorrelation
Matching by cross correlation of the image patches.
Definition: vision/include/mrpt/vision/types.h:277
mrpt::vision::CMatchedFeatureList::setLeftMaxID
void setLeftMaxID(const TFeatureID &leftID)
Explicitly set the max IDs values to certain values.
Definition: CFeature.h:384
mrpt::vision::generateMask
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.
Definition: vision_utils.cpp:847
mrpt::vision::TMatchingOptions::hasFundamentalMatrix
bool hasFundamentalMatrix
Whether or not there is a fundamental matrix.
Definition: vision/include/mrpt/vision/types.h:294
mrpt::obs::CObservationVisualLandmarks::landmarks
mrpt::maps::CLandmarksMap landmarks
The landmarks, with coordinates origin in the camera reference system.
Definition: obs/CObservationVisualLandmarks.h:38
mrpt::math::CMatrixTemplate::getColCount
size_t getColCount() const
Number of columns in the matrix.
Definition: CMatrixTemplate.h:244
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::math::CMatrixTemplate::getRowCount
size_t getRowCount() const
Number of rows in the matrix.
Definition: CMatrixTemplate.h:239
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::vision::CFeatureList::iterator
TInternalFeatList::iterator iterator
Definition: CFeature.h:261
mrpt::vision::TMatchingOptions::maxEDSD_TH
float maxEDSD_TH
Maximum Euclidean Distance Between SURF Descriptors.
Definition: vision/include/mrpt/vision/types.h:319
y
GLenum GLint GLint y
Definition: glext.h:3516
mrpt::vision::TMatchingOptions::minCC_TH
float minCC_TH
Minimum Value of the Cross Correlation.
Definition: vision/include/mrpt/vision/types.h:314
mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks::const_iterator
internal::TSequenceLandmarks::const_iterator const_iterator
Definition: maps/CLandmarksMap.h:122
mrpt::vision::TMatchingOptions::maxEDD_TH
float maxEDD_TH
Maximum Euclidean Distance Between SIFT Descriptors.
Definition: vision/include/mrpt/vision/types.h:310
mrpt::utils::CImage::isColor
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
Definition: CImage.cpp:898
mrpt::math::CVectorDouble
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:37
mrpt::vision::TStereoSystemParams::dumpToTextStream
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.
Definition: vision_utils.cpp:2167
mrpt::vision::deleteRepeatedFeats
void VISION_IMPEXP deleteRepeatedFeats(CFeatureList &list)
Explore the feature list and removes features which are in the same coordinates.
Definition: vision_utils.cpp:253
mrpt::obs::CObservationBearingRange::validCovariances
bool validCovariances
True: The individual 3x3 covariance matrices must be taken into account, false (default): All the mea...
Definition: obs/CObservationBearingRange.h:74
CPoint3D.h
res
GLuint res
Definition: glext.h:6298
mrpt::vision::getDispersion
void VISION_IMPEXP getDispersion(const CFeatureList &list, mrpt::math::CVectorFloat &std, mrpt::math::CVectorFloat &mean)
Computes the dispersion of the features in the image.
Definition: vision_utils.cpp:325
x
GLenum GLint x
Definition: glext.h:3516
mrpt::vision::CFeatureList::get_type
TFeatureType get_type() const
The type of the first feature in the list.
Definition: CFeature.h:220
mrpt::maps::CLandmark::pose_mean
mrpt::math::TPoint3D pose_mean
The mean of the landmark 3D position.
Definition: maps/CLandmark.h:44
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:246
mrpt::math::TLine2D::distance
double distance(const TPoint2D &point) const
Distance from a given point.
Definition: lightweight_geom_data.cpp:410
mrpt::math::CMatrix
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
mrpt::vision::TMatchingOptions::matching_method
TMatchingMethod matching_method
Matching method.
Definition: vision/include/mrpt/vision/types.h:306
mrpt::vision::TStereoSystemParams::loadFromConfigFile
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.
Definition: vision_utils.cpp:2122
mrpt::math::TPoint2D
Lightweight 2D point.
Definition: lightweight_geom_data.h:33
param
GLfloat param
Definition: glext.h:3705
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::vision::normalizeImage
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...
Definition: vision_utils.cpp:435
params
GLenum const GLfloat * params
Definition: glext.h:3514
mrpt::utils::CImage::getWidth
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
mrpt::vision::defaultIntrinsicParamsMatrix
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:
Definition: vision_utils.cpp:218
mrpt::utils::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
mrpt::poses::CPose3D::getHomogeneousMatrix
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.h:166
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:355
mrpt::utils::TStereoCamera::rightCameraPose
mrpt::poses::CPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:30
mrpt::utils::CStream::printf
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
mrpt::maps::CLandmarksMap::landmarks
struct VISION_IMPEXP mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
mrpt::utils::TCamera
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31
mrpt::vision::TStereoSystemParams::Prop_SUT
Uncertainty propagation through the Scaled Unscented Transformation.
Definition: vision/include/mrpt/vision/types.h:187
mrpt::utils::CImage::getHeight
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884
mrpt::vision::TStereoSystemParams::stdDisp
float stdDisp
Standard deviation of the error in disparity computation.
Definition: vision/include/mrpt/vision/types.h:206
mrpt::vision::CFeatureExtraction
The central class from which images can be analyzed in search of different kinds of interest points a...
Definition: CFeatureExtraction.h:59
mrpt::maps::CLandmark::ID
TLandmarkID ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: maps/CLandmark.h:57
mrpt::obs::CObservationBearingRange::sensedData
TMeasurementList sensedData
The list of observed ranges:
Definition: obs/CObservationBearingRange.h:70
mrpt::vision::TStereoSystemParams::K
mrpt::math::CMatrixDouble33 K
Intrinsic parameters.
Definition: vision/include/mrpt/vision/types.h:197
w1
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
Definition: glext.h:4993
mrpt::math::TLine2D
2D line without bounds, represented by its equation .
Definition: lightweight_geom_data.h:735
mrpt::vision::buildIntrinsicParamsMatrix
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:
Definition: vision_utils.cpp:197
mrpt::vision::CMatchedFeatureList
A list of features.
Definition: CFeature.h:361
mrpt::obs::CObservationBearingRange::TMeasurement::covariance
mrpt::math::CMatrixDouble33 covariance
The covariance matrix of the landmark, with variable indices [0,1,2] being [range,yaw,pitch].
Definition: obs/CObservationBearingRange.h:64


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019