Main MRPT website > C++ reference for MRPT 1.5.6
CObservation3DRangeScan.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 "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPosePDF.h>
14 #include <mrpt/utils/CStream.h>
16 
17 #include <mrpt/math/CMatrix.h>
19 #include <mrpt/math/ops_containers.h> // norm(), etc.
22 #include <mrpt/utils/CTimeLogger.h>
24 #include <mrpt/system/filesystem.h>
26 
27 #include <limits>
28 
29 using namespace std;
30 using namespace mrpt::obs;
31 using namespace mrpt::utils;
32 using namespace mrpt::poses;
33 using namespace mrpt::math;
34 
35 // This must be added to any CSerializable class implementation file.
37 
38 // Static LUT:
39 CObservation3DRangeScan::TCached3DProjTables CObservation3DRangeScan::m_3dproj_lut;
40 
41 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT = false;
42 
43 
44 // Whether to use a memory pool for 3D points:
45 #define COBS3DRANGE_USE_MEMPOOL
46 
47 // Do performance time logging?
48 //#define PROJ3D_PERFLOG
49 
50 
51 // Data types for memory pooling CObservation3DRangeScan:
52 #ifdef COBS3DRANGE_USE_MEMPOOL
53 
55 
56  // Memory pool for XYZ points ----------------
58  {
59  size_t WH; //!< Width*Height, that is, the number of 3D points
61  return WH>=req.WH;
62  }
63  };
65  {
66  std::vector<float> pts_x,pts_y,pts_z;
67  std::vector<uint16_t> idxs_x, idxs_y; //!< for each point, the corresponding (x,y) pixel coordinates
68  };
70 
71  // Memory pool for the rangeImage matrix ----------------
73  {
74  int H,W; //!< Size of matrix
76  return H==req.H && W==req.W;
77  }
78  };
80  {
82  };
84 
86  {
87  if (!obs.points3D_x.empty())
88  {
89  // Before dying, donate my memory to the pool for the joy of future class-brothers...
91  if (pool)
92  {
94  mem_params.WH = obs.points3D_x.capacity();
95  if (obs.points3D_y.capacity()!=mem_params.WH) obs.points3D_y.resize(mem_params.WH);
96  if (obs.points3D_z.capacity()!=mem_params.WH) obs.points3D_z.resize(mem_params.WH);
97  if (obs.points3D_idxs_x.capacity()!=mem_params.WH) obs.points3D_idxs_x.resize(mem_params.WH);
98  if (obs.points3D_idxs_y.capacity()!=mem_params.WH) obs.points3D_idxs_y.resize(mem_params.WH);
99 
101  obs.points3D_x.swap( mem_block->pts_x );
102  obs.points3D_y.swap( mem_block->pts_y );
103  obs.points3D_z.swap( mem_block->pts_z );
104  obs.points3D_idxs_x.swap( mem_block->idxs_x );
105  obs.points3D_idxs_y.swap( mem_block->idxs_y );
106 
107  pool->dump_to_pool(mem_params, mem_block);
108  }
109  }
110  }
112  {
113  if (obs.rangeImage.cols()>1 && obs.rangeImage.rows()>1)
114  {
115  // Before dying, donate my memory to the pool for the joy of future class-brothers...
117  if (pool)
118  {
120  mem_params.H = obs.rangeImage.rows();
121  mem_params.W = obs.rangeImage.cols();
122 
124  obs.rangeImage.swap( mem_block->rangeImage );
125 
126  pool->dump_to_pool(mem_params, mem_block);
127  }
128  }
129  }
130 
131 #endif
132 
133 /*---------------------------------------------------------------
134  Constructor
135  ---------------------------------------------------------------*/
136 CObservation3DRangeScan::CObservation3DRangeScan( ) :
137  m_points3D_external_stored(false),
138  m_rangeImage_external_stored(false),
139  hasPoints3D(false),
140  hasRangeImage(false),
141  range_is_depth(true),
142  hasIntensityImage(false),
143  intensityImageChannel(CH_VISIBLE),
144  hasConfidenceImage(false),
145  pixelLabels(), // Start without label info
146  cameraParams(),
147  cameraParamsIntensity(),
148  relativePoseIntensityWRTDepth(0,0,0,DEG2RAD(-90),DEG2RAD(0),DEG2RAD(-90)),
149  maxRange( 5.0f ),
150  sensorPose(),
151  stdError( 0.01f )
152 {
153 }
154 
155 /*---------------------------------------------------------------
156  Destructor
157  ---------------------------------------------------------------*/
158 
160 {
161 #ifdef COBS3DRANGE_USE_MEMPOOL
164 #endif
165 }
166 
167 /*---------------------------------------------------------------
168  Implements the writing to a CStream capability of CSerializable objects
169  ---------------------------------------------------------------*/
171 {
172  if (version)
173  *version = 8;
174  else
175  {
176  // The data
177  out << maxRange << sensorPose;
178 
179  out << hasPoints3D;
180  if (hasPoints3D)
181  {
182  ASSERT_(points3D_x.size()==points3D_y.size() && points3D_x.size()==points3D_z.size() && points3D_idxs_x.size()==points3D_x.size() && points3D_idxs_y.size()==points3D_x.size())
183  uint32_t N = points3D_x.size();
184  out << N;
185  if (N)
186  {
187  out.WriteBufferFixEndianness( &points3D_x[0], N );
188  out.WriteBufferFixEndianness( &points3D_y[0], N );
189  out.WriteBufferFixEndianness( &points3D_z[0], N );
190  out.WriteBufferFixEndianness( &points3D_idxs_x[0], N ); // New in v8
191  out.WriteBufferFixEndianness( &points3D_idxs_y[0], N ); // New in v8
192  }
193  }
194 
195  out << hasRangeImage; if (hasRangeImage) out << rangeImage;
196  out << hasIntensityImage; if (hasIntensityImage) out << intensityImage;
197  out << hasConfidenceImage; if (hasConfidenceImage) out << confidenceImage;
198 
199  out << cameraParams; // New in v2
200  out << cameraParamsIntensity; // New in v4
201  out << relativePoseIntensityWRTDepth; // New in v4
202 
203  out << stdError;
204  out << timestamp;
205  out << sensorLabel;
206 
207  // New in v3:
210 
211  // New in v5:
212  out << range_is_depth;
213 
214  // New in v6:
215  out << static_cast<int8_t>(intensityImageChannel);
216 
217  // New in v7:
218  out << hasPixelLabels();
219  if (hasPixelLabels())
220  {
221  pixelLabels->writeToStream(out);
222  }
223  }
224 }
225 
226 /*---------------------------------------------------------------
227  Implements the reading from a CStream capability of CSerializable objects
228  ---------------------------------------------------------------*/
230 {
231  switch(version)
232  {
233  case 0:
234  case 1:
235  case 2:
236  case 3:
237  case 4:
238  case 5:
239  case 6:
240  case 7:
241  case 8:
242  {
243  uint32_t N;
244 
245  in >> maxRange >> sensorPose;
246 
247  if (version>0)
248  in >> hasPoints3D;
249  else hasPoints3D = true;
250 
251  if (hasPoints3D)
252  {
253  in >> N;
255 
256  if (N)
257  {
261 
262  if (version==0)
263  {
264  vector<char> validRange(N); // for v0.
265  in.ReadBuffer( &validRange[0], sizeof(validRange[0])*N );
266  }
267  if (version>=8) {
270  }
271  }
272  }
273  else
274  {
275  this->resizePoints3DVectors(0);
276  }
277 
278  if (version>=1)
279  {
280  in >> hasRangeImage;
281  if (hasRangeImage)
282  {
283 #ifdef COBS3DRANGE_USE_MEMPOOL
284  // We should call "rangeImage_setSize()" to exploit the mempool:
285  this->rangeImage_setSize(480,640);
286 #endif
287  in >> rangeImage;
288  }
289 
290  in >> hasIntensityImage;
291  if (hasIntensityImage)
292  in >>intensityImage;
293 
294  in >> hasConfidenceImage;
295  if (hasConfidenceImage)
296  in >> confidenceImage;
297 
298  if (version>=2)
299  {
300  in >> cameraParams;
301 
302  if (version>=4)
303  {
306  }
307  else
308  {
311  }
312  }
313  }
314 
315  in >> stdError;
316  in >> timestamp;
317  in >> sensorLabel;
318 
319  if (version>=3)
320  {
321  // New in v3:
324  }
325  else
326  {
329  }
330 
331  if (version>=5)
332  {
333  in >> range_is_depth;
334  }
335  else
336  {
337  range_is_depth = true;
338  }
339 
340  if (version>=6)
341  {
342  int8_t i;
343  in >> i;
344  intensityImageChannel = static_cast<TIntensityChannelID>(i);
345  }
346  else
347  {
349  }
350 
351  pixelLabels.clear_unique(); // Remove existing data first (_unique() is to leave alive any user copies of the shared pointer).
352  if (version>=7)
353  {
354 
355  bool do_have_labels;
356  in >> do_have_labels;
357 
358  if (do_have_labels)
360  }
361 
362  } break;
363  default:
365 
366  };
367 
368 }
369 
371 {
373 
374  std::swap(hasPoints3D,o.hasPoints3D);
375  points3D_x.swap(o.points3D_x);
376  points3D_y.swap(o.points3D_y);
377  points3D_z.swap(o.points3D_z);
382 
383  std::swap(hasRangeImage,o.hasRangeImage);
384  rangeImage.swap(o.rangeImage);
387 
391 
394 
395  std::swap(pixelLabels,o.pixelLabels);
396 
398 
399  std::swap(cameraParams,o.cameraParams);
401 
402  std::swap(maxRange, o.maxRange);
403  std::swap(sensorPose, o.sensorPose);
404  std::swap(stdError, o.stdError);
405 
406 }
407 
409 {
411  {
412  const string fil = points3D_getExternalStorageFileAbsolutePath();
414  {
415  CMatrixFloat M;
416  M.loadFromTextFile(fil);
417  ASSERT_EQUAL_(M.rows(),3);
418 
419  M.extractRow(0,const_cast<std::vector<float>&>(points3D_x));
420  M.extractRow(1,const_cast<std::vector<float>&>(points3D_y));
421  M.extractRow(2,const_cast<std::vector<float>&>(points3D_z));
422  }
423  else
424  {
426  f >> const_cast<std::vector<float>&>(points3D_x) >> const_cast<std::vector<float>&>(points3D_y) >> const_cast<std::vector<float>&>(points3D_z);
427  }
428  }
429 
431  {
434  {
435  const_cast<CMatrix&>(rangeImage).loadFromTextFile(fil);
436  }
437  else
438  {
440  f >> const_cast<CMatrix&>(rangeImage);
441  }
442  }
443 }
444 
446 {
448  {
452  }
453 
455  rangeImage.setSize(0,0);
456 
459 }
460 
462 {
465  {
466  out_path= m_rangeImage_external_file;
467  }
468  else
469  {
470  out_path = CImage::IMAGES_PATH_BASE;
471  size_t N=CImage::IMAGES_PATH_BASE.size()-1;
472  if (CImage::IMAGES_PATH_BASE[N]!='/' && CImage::IMAGES_PATH_BASE[N]!='\\' )
473  out_path+= "/";
474  out_path+= m_rangeImage_external_file;
475  }
476 }
478 {
480  if (m_points3D_external_file[0]=='/' || ( m_points3D_external_file[1]==':' && m_points3D_external_file[2]=='\\' ) )
481  {
482  out_path= m_points3D_external_file;
483  }
484  else
485  {
486  out_path = CImage::IMAGES_PATH_BASE;
487  size_t N=CImage::IMAGES_PATH_BASE.size()-1;
488  if (CImage::IMAGES_PATH_BASE[N]!='/' && CImage::IMAGES_PATH_BASE[N]!='\\' )
489  out_path+= "/";
490  out_path+= m_points3D_external_file;
491  }
492 }
493 
495 {
497  ASSERT_(points3D_x.size()==points3D_y.size() && points3D_x.size()==points3D_z.size())
498 
499  if (EXTERNALS_AS_TEXT)
502 
503  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()" instead of CImage::IMAGES_PATH_BASE
504  const string savedDir = CImage::IMAGES_PATH_BASE;
505  CImage::IMAGES_PATH_BASE = use_this_base_dir;
506  const string real_absolute_file_path = points3D_getExternalStorageFileAbsolutePath();
507  CImage::IMAGES_PATH_BASE = savedDir;
508 
509  if (EXTERNALS_AS_TEXT)
510  {
511  const size_t nPts = points3D_x.size();
512 
513  CMatrixFloat M(3,nPts);
514  M.insertRow(0,points3D_x);
515  M.insertRow(1,points3D_y);
516  M.insertRow(2,points3D_z);
517 
518  M.saveToTextFile(
519  real_absolute_file_path,
521  }
522  else
523  {
524  mrpt::utils::CFileGZOutputStream f(real_absolute_file_path);
525  f << points3D_x << points3D_y << points3D_z;
526  }
527 
529 
530  // Really dealloc memory, clear() is not enough:
536 }
538 {
540  if (EXTERNALS_AS_TEXT)
543 
544  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()" instead of CImage::IMAGES_PATH_BASE
545  const string savedDir = CImage::IMAGES_PATH_BASE;
546  CImage::IMAGES_PATH_BASE = use_this_base_dir;
547  const string real_absolute_file_path = rangeImage_getExternalStorageFileAbsolutePath();
548  CImage::IMAGES_PATH_BASE = savedDir;
549 
550  if (EXTERNALS_AS_TEXT)
551  {
552  rangeImage.saveToTextFile(
553  real_absolute_file_path,
555  }
556  else
557  {
558  mrpt::utils::CFileGZOutputStream f(real_absolute_file_path);
559  f << rangeImage;
560  }
561 
563  rangeImage.setSize(0,0);
564 }
565 
566 // ============== Auxiliary function for "recoverCameraCalibrationParameters" =========================
567 
568 #define CALIB_DECIMAT 15
569 
570 namespace mrpt
571 {
572  namespace obs
573  {
574  namespace detail
575  {
576  struct TLevMarData
577  {
579  const double z_offset;
580  TLevMarData(const CObservation3DRangeScan &obs_, const double z_offset_ ) :
581  obs(obs_),z_offset(z_offset_) {}
582  };
583 
584  void cam2vec(const TCamera &camPar,CVectorDouble &x)
585  {
586  if (x.size()<4+4) x.resize(4+4);
587 
588  x[0] = camPar.fx();
589  x[1] = camPar.fy();
590  x[2] = camPar.cx();
591  x[3] = camPar.cy();
592 
593  for (size_t i=0;i<4;i++)
594  x[4+i] = camPar.dist[i];
595  }
596  void vec2cam(const CVectorDouble &x, TCamera &camPar)
597  {
598  camPar.intrinsicParams(0,0) = x[0]; // fx
599  camPar.intrinsicParams(1,1) = x[1]; // fy
600  camPar.intrinsicParams(0,2) = x[2]; // cx
601  camPar.intrinsicParams(1,2) = x[3]; // cy
602 
603  for (size_t i=0;i<4;i++)
604  camPar.dist[i] = x[4+i];
605  }
606  void cost_func(
607  const CVectorDouble &par,
608  const TLevMarData &d,
609  CVectorDouble &err)
610  {
611  const CObservation3DRangeScan &obs = d.obs;
612 
613  TCamera params;
614  vec2cam(par,params);
615 
616  const size_t nC = obs.rangeImage.getColCount();
617  const size_t nR = obs.rangeImage.getRowCount();
618 
619 
620  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
621 
622  for (size_t r=0;r<nR;r+=CALIB_DECIMAT)
623  {
624  for (size_t c=0;c<nC;c+=CALIB_DECIMAT)
625  {
626  const size_t idx = nC*r + c;
627 
628  TPoint3D p( obs.points3D_x[idx]+d.z_offset, obs.points3D_y[idx], obs.points3D_z[idx] );
629  TPoint3D P(-p.y, -p.z , p.x );
630  TPixelCoordf pixel;
631  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
632  //pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
633 
634  // Pinhole model:
635  const double x = P.x/P.z;
636  const double y = P.y/P.z;
637 
638  // Radial distortion:
639  const double r2 = square(x)+square(y);
640  const double r4 = square(r2);
641 
642  pixel.x = params.cx() + params.fx() *( x*(1+params.dist[0]*r2+params.dist[1]*r4+ 2*params.dist[2]*x*y+params.dist[3]*(r2+2*square(x)) ) );
643  pixel.y = params.cy() + params.fy() *( y*(1+params.dist[0]*r2+params.dist[1]*r4+ 2*params.dist[3]*x*y+params.dist[2]*(r2+2*square(y))) );
644  }
645 
646 
647  // In theory, it should be (r,c):
648  err.push_back( c-pixel.x );
649  err.push_back( r-pixel.y );
650  }
651  }
652  } // end error_func
653  }
654  }
655 }
656 
657 
658 
659 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud.
660  * \param camera_offset The offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000.
661  * \return The final average reprojection error per pixel (typ <0.05 px)
662  */
664  const CObservation3DRangeScan &obs,
665  mrpt::utils::TCamera &out_camParams,
666  const double camera_offset)
667 {
668  MRPT_START
669 
670  ASSERT_(obs.hasRangeImage && obs.hasPoints3D)
671  ASSERT_(obs.points3D_x.size() == obs.points3D_y.size() && obs.points3D_x.size() == obs.points3D_z.size())
672 
674  TMyLevMar::TResultInfo info;
675 
676  const size_t nR = obs.rangeImage.getRowCount();
677  const size_t nC = obs.rangeImage.getColCount();
678 
679  TCamera camInit;
680  camInit.ncols = nC;
681  camInit.nrows = nR;
682  camInit.intrinsicParams(0,0) = 250;
683  camInit.intrinsicParams(1,1) = 250;
684  camInit.intrinsicParams(0,2) = nC >> 1;
685  camInit.intrinsicParams(1,2) = nR >> 1;
686 
687  CVectorDouble initial_x;
688  detail::cam2vec(camInit,initial_x);
689 
690  initial_x.resize(8);
691  CVectorDouble increments_x(initial_x.size());
692  increments_x.assign(1e-4);
693 
694  CVectorDouble optimal_x;
695 
696  TMyLevMar lm;
697  lm.execute(
698  optimal_x,
699  initial_x,
701  increments_x,
702  detail::TLevMarData(obs,camera_offset),
703  info,
704  mrpt::utils::LVL_INFO, /* verbose */
705  1000 , /* max iter */
706  1e-3,
707  1e-9,
708  1e-9,
709  false
710  );
711 
712  const double avr_px_err = sqrt(info.final_sqr_err/(nC*nR/square(CALIB_DECIMAT)));
713 
714  out_camParams.ncols = nC;
715  out_camParams.nrows = nR;
716  out_camParams.focalLengthMeters = camera_offset;
717  detail::vec2cam(optimal_x,out_camParams);
718 
719  return avr_px_err;
720 
721  MRPT_END
722 }
723 
726  const unsigned int &r1, const unsigned int &r2,
727  const unsigned int &c1, const unsigned int &c2 )
728 {
729  unsigned int cols = cameraParams.ncols;
730  unsigned int rows = cameraParams.nrows;
731 
732  ASSERT_( (r1<r2) && (c1<c2) )
733  ASSERT_( (r2<rows) && (c2<cols) )
734 
735  // Maybe we needed to copy more base obs atributes
736 
737  // Copy zone of range image
739  if ( hasRangeImage )
740  rangeImage.extractSubmatrix( r1, r2, c1, c2, obs.rangeImage );
741 
742  // Copy zone of intensity image
745  if ( hasIntensityImage )
746  intensityImage.extract_patch( obs.intensityImage, c1, r1, c2-c1, r2-r1 );
747 
748  // Copy zone of confidence image
750  if ( hasConfidenceImage )
751  confidenceImage.extract_patch( obs.confidenceImage, c1, r1, c2-c1, r2-r1 );
752 
753  // Zone labels: It's too complex, just document that pixel labels are NOT extracted.
754 
755  // Copy zone of scanned points
756  obs.hasPoints3D = hasPoints3D;
757  if ( hasPoints3D )
758  {
759  // Erase a possible previous content
760  if ( obs.points3D_x.size() > 0 )
761  {
762  obs.points3D_x.clear();
763  obs.points3D_y.clear();
764  obs.points3D_z.clear();
765  }
766 
767  for ( unsigned int i = r1; i < r2; i++ )
768  for ( unsigned int j = c1; j < c2; j++ )
769  {
770  obs.points3D_x.push_back( points3D_x.at( cols*i + j ) );
771  obs.points3D_y.push_back( points3D_y.at( cols*i + j ) );
772  obs.points3D_z.push_back( points3D_z.at( cols*i + j ) );
773  }
774  }
775 
776  obs.maxRange = maxRange;
777  obs.sensorPose = sensorPose;
778  obs.stdError = stdError;
779 
781 }
782 
783 
784 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y & \a points3D_z to allow the usage of the internal memory pool. */
786 {
787 #ifdef COBS3DRANGE_USE_MEMPOOL
788  // If WH=0 this is a clear:
789  if (!WH)
790  {
796  return;
797  }
798 
799  if (WH<=points3D_x.size()) // reduce size, don't realloc
800  {
801  points3D_x.resize( WH );
802  points3D_y.resize( WH );
803  points3D_z.resize( WH );
804  points3D_idxs_x.resize( WH );
805  points3D_idxs_y.resize( WH );
806  return;
807  }
808 
809  // Request memory for the X,Y,Z buffers from the memory pool:
811  if (pool)
812  {
814  mem_params.WH = WH;
815 
816  CObservation3DRangeScan_Points_MemPoolData *mem_block = pool->request_memory(mem_params);
817 
818  if (mem_block)
819  { // Take the memory via swaps:
820  points3D_x.swap( mem_block->pts_x );
821  points3D_y.swap( mem_block->pts_y );
822  points3D_z.swap( mem_block->pts_z );
823  points3D_idxs_x.swap( mem_block->idxs_x );
824  points3D_idxs_y.swap( mem_block->idxs_y );
825  delete mem_block;
826  }
827  }
828 #endif
829 
830  // Either if there was no pool memory or we got it, make sure the size of vectors is OK:
831  points3D_x.resize( WH );
832  points3D_y.resize( WH );
833  points3D_z.resize( WH );
834  points3D_idxs_x.resize( WH );
835  points3D_idxs_y.resize( WH );
836 }
837 
838 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the memory allocation.
839 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
840 {
841 #ifdef COBS3DRANGE_USE_MEMPOOL
842  // Request memory from the memory pool:
844  if (pool)
845  {
847  mem_params.H = H;
848  mem_params.W = W;
849 
850  CObservation3DRangeScan_Ranges_MemPoolData *mem_block = pool->request_memory(mem_params);
851 
852  if (mem_block)
853  { // Take the memory via swaps:
854  rangeImage.swap(mem_block->rangeImage);
855  delete mem_block;
856  return;
857  }
858  }
859  // otherwise, continue with the normal method:
860 #endif
861  // Fall-back to normal method:
862  rangeImage.setSize(H,W);
863 }
864 
865 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
867 {
868  static const double EPSILON=1e-7;
869  static mrpt::poses::CPose3D ref_pose(0,0,0,DEG2RAD(-90),0,DEG2RAD(-90));
870 
871  return
872  (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON ).all() &&
873  ((ref_pose.getRotationMatrix() - relativePoseIntensityWRTDepth.getRotationMatrix()).array().abs() < EPSILON).all();
874 }
875 
876 
877 // Convert into equivalent 2D "fake" laser scan. See .h for doc
879  CObservation2DRangeScan &out_scan2d,
880  const std::string &sensorLabel,
881  const double angle_sup,
882  const double angle_inf,
883  const double oversampling_ratio,
884  const mrpt::math::CMatrix * rangeMask_min
885  )
886 {
889  sp.angle_sup = angle_sup;
890  sp.angle_inf = angle_inf;
891  sp.oversampling_ratio = oversampling_ratio;
893  fp.rangeMask_min = rangeMask_min;
894  convertTo2DScan(out_scan2d, sp,fp);
895 }
896 
898 {
899  out_scan2d.sensorLabel = sensorLabel;
900  out_scan2d.timestamp = this->timestamp;
901 
902  if (!this->hasRangeImage)
903  { // Nothing to do!
904  out_scan2d.resizeScan(0);
905  return;
906  }
907 
908  const size_t nCols = this->rangeImage.cols();
909  const size_t nRows = this->rangeImage.rows();
910  if (fp.rangeMask_min) { // sanity check:
911  ASSERT_EQUAL_(fp.rangeMask_min->cols(), rangeImage.cols());
912  ASSERT_EQUAL_(fp.rangeMask_min->rows(), rangeImage.rows());
913  }
914  if (fp.rangeMask_max) { // sanity check:
915  ASSERT_EQUAL_(fp.rangeMask_max->cols(), rangeImage.cols());
916  ASSERT_EQUAL_(fp.rangeMask_max->rows(), rangeImage.rows());
917  }
918 
919  // Compute the real horizontal FOV from the range camera intrinsic calib data:
920  // Note: this assumes the range image has been "undistorted", which is true for data
921  // from OpenNI, and will be in the future for libfreenect in MRPT, but it's
922  // not implemented yet (as of Mar 2012), so this is an approximation in that case.
923  const double cx = this->cameraParams.cx();
924  const double cy = this->cameraParams.cy();
925  const double fx = this->cameraParams.fx();
926  const double fy = this->cameraParams.fy();
927 
928  // (Imagine the camera seen from above to understand this geometry)
929  const double real_FOV_left = atan2(cx, fx);
930  const double real_FOV_right = atan2(nCols-1-cx, fx);
931 
932  // FOV of the equivalent "fake" "laser scanner":
933  const float FOV_equiv = 2. * std::max(real_FOV_left,real_FOV_right);
934 
935  // Now, we should create more "fake laser" points than columns in the image,
936  // since laser scans are assumed to sample space at evenly-spaced angles,
937  // while in images it is like ~tan(angle).
939  const size_t nLaserRays = static_cast<size_t>( nCols * sp.oversampling_ratio );
940 
941  // Prepare 2D scan data fields:
942  out_scan2d.aperture = FOV_equiv;
943  out_scan2d.maxRange = this->maxRange;
944  out_scan2d.resizeScan(nLaserRays);
945 
946  out_scan2d.resizeScanAndAssign(nLaserRays, 2.0 * this->maxRange, false ); // default: all ranges=invalid
947  if (sp.use_origin_sensor_pose)
948  out_scan2d.sensorPose = mrpt::poses::CPose3D();
949  else out_scan2d.sensorPose = this->sensorPose;
950 
951  // The vertical FOVs given by the user can be translated into limits of the tangents (tan>0 means above, i.e. z>0):
952  const float tan_min = -tan( std::abs(sp.angle_inf) );
953  const float tan_max = tan( std::abs(sp.angle_sup) );
954 
955  // Precompute the tangents of the vertical angles of each "ray"
956  // for every row in the range image:
957  std::vector<float> vert_ang_tan(nRows);
958  for (size_t r=0;r<nRows;r++)
959  vert_ang_tan[r] = static_cast<float>( (cy-r)/fy );
960 
961  if (!sp.use_origin_sensor_pose)
962  {
963  // Algorithm 1: each column in the range image corresponds to a known orientation in the 2D scan:
964  // -------------------
965  out_scan2d.rightToLeft = false;
966 
967  // Angle "counter" for the fake laser scan direction, and the increment:
968  double ang = -FOV_equiv*0.5;
969  const double A_ang = FOV_equiv/(nLaserRays-1);
970 
971  TRangeImageFilter rif(fp);
972 
973  // Go thru columns, and keep the minimum distance (along the +X axis, not 3D distance!)
974  // for each direction (i.e. for each column) which also lies within the vertical FOV passed
975  // by the user.
976  for (size_t i=0;i<nLaserRays;i++, ang+=A_ang )
977  {
978  // Equivalent column in the range image for the "i'th" ray:
979  const double tan_ang = tan(ang);
980  // make sure we don't go out of range (just in case):
981  const size_t c = std::min(static_cast<size_t>(std::max(0.0,cx + fx*tan_ang)),nCols-1);
982 
983  bool any_valid = false;
984  float closest_range = out_scan2d.maxRange;
985 
986  for (size_t r=0;r<nRows;r++)
987  {
988  const float D = this->rangeImage.coeff(r,c);
989  if (!rif.do_range_filter(r,c,D))
990  continue;
991 
992  // All filters passed:
993  const float this_point_tan = vert_ang_tan[r] * D;
994  if (this_point_tan>tan_min && this_point_tan<tan_max)
995  {
996  any_valid = true;
997  mrpt::utils::keep_min(closest_range, D);
998  }
999  }
1000 
1001  if (any_valid)
1002  {
1003  out_scan2d.setScanRangeValidity(i, true);
1004  // Compute the distance in 2D from the "depth" in closest_range:
1005  out_scan2d.setScanRange(i, closest_range*std::sqrt(1.0+tan_ang*tan_ang) );
1006  }
1007  } // end for columns
1008  }
1009  else
1010  {
1011  // Algorithm 2: project to 3D and reproject (for a different sensorPose at the origin)
1012  // ------------------------------------------------------------------------
1013  out_scan2d.rightToLeft = true;
1014 
1015  T3DPointsProjectionParams projParams;
1016  projParams.takeIntoAccountSensorPoseOnRobot = true;
1017 
1018  mrpt::opengl::CPointCloudPtr pc = mrpt::opengl::CPointCloud::Create();
1019  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1020 
1021  const std::vector<float> & xs = pc->getArrayX(), &ys = pc->getArrayY(), &zs = pc->getArrayZ();
1022  const size_t N = xs.size();
1023 
1024  const double A_ang = FOV_equiv/(nLaserRays-1);
1025  const double ang0 = -FOV_equiv*0.5;
1026 
1027  for (size_t i=0;i<N;i++)
1028  {
1029  if (zs[i]<sp.z_min || zs[i]>sp.z_max)
1030  continue;
1031 
1032  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1033 
1034  int i_range = (phi_wrt_origin-ang0)/A_ang;
1035  if (i_range<0 || i_range>=int(nLaserRays))
1036  continue;
1037 
1038  const float r_wrt_origin = ::hypotf(xs[i],ys[i]);
1039  if (out_scan2d.scan[i_range]> r_wrt_origin) out_scan2d.setScanRange(i_range, r_wrt_origin);
1040  out_scan2d.setScanRangeValidity(i_range, true);
1041  }
1042 
1043  }
1044 }
1045 
1047 {
1049 
1050  this->load(); // Make sure the 3D point cloud, etc... are all loaded in memory.
1051 
1052  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n";
1053  o << sensorPose.getHomogeneousMatrixVal() << sensorPose << endl;
1054 
1055  o << "maxRange = " << maxRange << " m" << endl;
1056 
1057  o << "Has 3D point cloud? ";
1058  if (hasPoints3D)
1059  {
1060  o << "YES: " << points3D_x.size() << " points";
1062  o << ". External file: " << points3D_getExternalStorageFile() << endl;
1063  else o << " (embedded)." << endl;
1064  }
1065  else o << "NO" << endl;
1066 
1067  o << "Has raw range data? " << (hasRangeImage ? "YES": "NO");
1068  if (hasRangeImage)
1069  {
1071  o << ". External file: " << rangeImage_getExternalStorageFile() << endl;
1072  else o << " (embedded)." << endl;
1073  }
1074 
1075  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES": "NO");
1076  if (hasIntensityImage)
1077  {
1079  o << ". External file: " << intensityImage.getExternalStorageFile() << endl;
1080  else o << " (embedded).\n";
1081  // Channel?
1083  }
1084 
1085  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES": "NO");
1086  if (hasConfidenceImage)
1087  {
1089  o << ". External file: " << confidenceImage.getExternalStorageFile() << endl;
1090  else o << " (embedded)." << endl;
1091  }
1092 
1093  o << endl << "Has pixel labels? " << (hasPixelLabels()? "YES": "NO");
1094  if (hasPixelLabels())
1095  {
1096  o << " Human readable labels:" << endl;
1097  for (TPixelLabelInfoBase::TMapLabelID2Name::const_iterator it=pixelLabels->pixelLabelNames.begin();it!=pixelLabels->pixelLabelNames.end();++it)
1098  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1099  }
1100 
1101  o << endl << endl;
1102  o << "Depth camera calibration parameters:" << endl;
1103  {
1104  CConfigFileMemory cfg;
1105  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS",cfg);
1106  o << cfg.getContent() << endl;
1107  }
1108  o << endl << "Intensity camera calibration parameters:" << endl;
1109  {
1110  CConfigFileMemory cfg;
1111  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS",cfg);
1112  o << cfg.getContent() << endl;
1113  }
1114  o << endl << endl << "Pose of the intensity cam. wrt the depth cam:\n"
1117 
1118 }
1119 
1121 {
1122  const uint8_t version = 1; // for possible future changes.
1123  out << version;
1124 
1125  // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the object in the class factory later on.
1126  out << BITFIELD_BYTES;
1127 
1128  // 2nd: data-specific serialization:
1129  this->internal_writeToStream(out);
1130 }
1131 
1132 
1133 // Deserialization and class factory. All in one, ladies and gentlemen
1135 {
1136  uint8_t version;
1137  in >> version;
1138 
1139  switch (version)
1140  {
1141  case 1:
1142  {
1143  // 1st: Read NUM BYTES
1144  uint8_t bitfield_bytes;
1145  in >> bitfield_bytes;
1146 
1147  // Hand-made class factory. May be a good solution if there will be not too many different classes:
1149  switch (bitfield_bytes)
1150  {
1151  case 1: new_obj = new CObservation3DRangeScan::TPixelLabelInfo<1>(); break;
1152  case 2: new_obj = new CObservation3DRangeScan::TPixelLabelInfo<2>(); break;
1153  case 3:
1154  case 4: new_obj = new CObservation3DRangeScan::TPixelLabelInfo<4>(); break;
1155  case 5:
1156  case 6:
1157  case 7:
1158  case 8: new_obj = new CObservation3DRangeScan::TPixelLabelInfo<8>(); break;
1159  default:
1160  throw std::runtime_error("Unknown type of pixelLabel inner class while deserializing!");
1161  };
1162  // 2nd: data-specific serialization:
1163  new_obj->internal_readFromStream(in);
1164 
1165  return new_obj;
1166  }
1167  break;
1168 
1169  default:
1171  break;
1172  };
1173 
1174 }
1175 
1177  angle_sup(mrpt::utils::DEG2RAD(5)), angle_inf(mrpt::utils::DEG2RAD(5)),
1178  z_min(-std::numeric_limits<double>::max() ),z_max(std::numeric_limits<double>::max()),
1179  oversampling_ratio(1.2),use_origin_sensor_pose(false)
1180 {
1181 }
#define ASSERT_EQUAL_(__A, __B)
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
virtual void internal_writeToStream(mrpt::utils::CStream &out) const =0
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
std::string m_rangeImage_external_file
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
mrpt::system::CGenericMemoryPool< CObservation3DRangeScan_Ranges_MemPoolParams, CObservation3DRangeScan_Ranges_MemPoolData > TMyRangesMemPool
void getZoneAsObs(CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
Extract a ROI of the 3D observation as a new one.
#define min(a, b)
size_t WH
Width*Height, that is, the number of 3D points.
TIntensityChannelID
Enum type for intensityImageChannel.
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
#define ASSERT_ABOVE_(__A, __B)
double focalLengthMeters
The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel...
Definition: TCamera.h:56
bool isSuitable(const CObservation3DRangeScan_Ranges_MemPoolParams &req) const
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
stlplus::smart_ptr< TPixelLabelInfoBase > TPixelLabelInfoPtr
Used in CObservation3DRangeScan::pixelLabels.
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
TPixelLabelInfoPtr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void setScanRange(const size_t i, const float val)
const GLfloat * c
Definition: glew.h:10088
std::string sensorLabel
The sensor label that will have the newly created observation.
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
void dump_to_pool(const DATA_PARAMS &params, POOLABLE_DATA *block)
Saves the passed data block (characterized by params) to the pool.
std::string getExternalStorageFile() const MRPT_NO_THROWS
< Only if isExternallyStored() returns true.
Definition: CImage.h:673
This file implements several operations that operate element-wise on individual or pairs of container...
void unload() const MRPT_NO_THROWS
For external storage image objects only, this method unloads the image from memory (or does nothing i...
Definition: CImage.cpp:1905
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
bool isSuitable(const CObservation3DRangeScan_Points_MemPoolParams &req) const
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
const mrpt::math::CMatrix * rangeMask_min
(Default: NULL) If provided, each data range will be tested to be greater-than (rangeMask_min) or les...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
signed char int8_t
Definition: rptypes.h:42
const Scalar * const_iterator
Definition: eigen_plugins.h:24
virtual void unload() MRPT_OVERRIDE
Unload all images, for the case they being delayed-load images stored in external files (othewise...
double z
X,Y,Z coordinates.
void swap(CObservation &o)
Swap with another observation, ONLY the data defined here in the base class CObservation. It's protected since it'll be only called from child classes that should know what else to swap appart from these common data.
POOLABLE_DATA * request_memory(const DATA_PARAMS &params)
Request a block of data which fulfils the size requirements stated in params.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
GLuint in
Definition: glew.h:7146
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
std::string rangeImage_getExternalStorageFileAbsolutePath() const
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
unsigned char uint8_t
Definition: rptypes.h:43
float maxRange
The maximum range allowed by the device, in meters (e.g. 80m, 50m,...)
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
A generic system for versatile memory pooling.
This class implements a config file-like interface over a memory-stored string list.
void WriteBufferFixEndianness(const T *ptr, size_t ElementCount)
Writes a sequence of elemental datatypes, taking care of reordering their bytes from the running arch...
Definition: CStream.h:139
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:160
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
std::vector< uint16_t > points3D_idxs_y
//!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z
#define MRPT_END
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:81
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
#define CALIB_DECIMAT
Transparently opens a compressed "gz" file and reads uncompressed data from it.
void vec2cam(const CVectorDouble &x, TCamera &camPar)
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
std::string BASE_IMPEXP fileNameChangeExtension(const std::string &filename, const std::string &newExtension)
Replace the filename extension by another one.
Definition: filesystem.cpp:356
Used in CObservation3DRangeScan::convertTo2DScan()
std::string m_points3D_external_file
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
uint32_t ncols
Definition: TCamera.h:53
bool hasRangeImage
true means the field rangeImage contains valid data
double angle_inf
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
bool isExternallyStored() const MRPT_NO_THROWS
See setExternalStorage().
Definition: CImage.h:671
void convertTo2DScan(mrpt::obs::CObservation2DRangeScan &out_scan2d, const T3DPointsTo2DScanParams &scanParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV...
std::string BASE_IMPEXP extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:96
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
int version
Definition: mrpt_jpeglib.h:898
GLfloat GLfloat p
Definition: glew.h:10113
#define DEG2RAD
void resizeScanAndAssign(const size_t len, const float rangeVal, const bool rangeValidity, const int32_t rangeIntensity=0)
Resizes all data vectors to allocate a given number of scan rays and assign default values...
bool hasPoints3D
true means the field points3D contains valid data.
void swap(CImage &o)
Very efficient swap of two images (just swap the internal pointers)
Definition: CImage.cpp:135
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
virtual void internal_readFromStream(mrpt::utils::CStream &in)=0
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
std::vector< uint16_t > idxs_y
for each point, the corresponding (x,y) pixel coordinates
#define MRPT_START
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>'s clear() method, but really forcing deallocating the memory...
Definition: bits.h:205
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::utils::CStream &in)
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CStream.h:95
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:55
const mrpt::math::CMatrix * rangeMask_max
static CGenericMemoryPool< DATA_PARAMS, POOLABLE_DATA > * getInstance(const size_t max_pool_entries=5)
Construct-on-first-use (~singleton) pattern: Return the unique instance of this class for a given tem...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot's observation.
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
static std::string value2name(const ENUMTYPE val)
Gives the textual name for a given enum value on unknown enum value name.
Definition: TEnumType.h:41
double z_max
(Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates wi...
backing_store_ptr info
Definition: jmemsys.h:170
mrpt::system::CGenericMemoryPool< CObservation3DRangeScan_Points_MemPoolParams, CObservation3DRangeScan_Points_MemPoolData > TMyPointsMemPool
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
uint32_t nrows
Camera resolution.
Definition: TCamera.h:53
void rangeImage_setSize(const int HEIGHT, const int WIDTH)
Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the ...
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
#define ASSERT_(f)
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::utils::TCamera &out_camParams, const double camera_offset=0.01)
A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a ran...
GLfloat * params
Definition: glew.h:1436
void project3DPointsFromDepthImageInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optiona...
bool do_range_filter(size_t r, size_t c, const float D) const
Returns true if the point (r,c) with depth D passes all filters.
void cam2vec(const TCamera &camPar, CVectorDouble &x)
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:158
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:37
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Definition: CPose3D.h:173
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
static CPointCloudPtr Create()
void mempool_donate_xyz_buffers(CObservation3DRangeScan &obs)
mrpt::utils::ContainerReadOnlyProxyAccessor< std::vector< float > > scan
The range values of the scan, in meters. Must have same length than validRange.
Lightweight 3D point.
virtual void load() const MRPT_OVERRIDE
Makes sure all images and other fields which may be externally stored are loaded in memory...
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
void saveToConfigFile(const std::string &section, mrpt::utils::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:117
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
unsigned __int32 uint32_t
Definition: rptypes.h:49
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
TLevMarData(const CObservation3DRangeScan &obs_, const double z_offset_)
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:154
Virtual interface to all pixel-label information structs.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
const CObservation3DRangeScan & obs
static bool EXTERNALS_AS_TEXT
Whether external files (3D points, range and confidence) are to be saved as .txt text files (MATLAB c...
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated. Otherwise, points are transformed with sensorPose. Furthermore, if provided, those coordinates are transformed with robotPoseInTheWorld
fixed floating point 'f'
Definition: math_frwds.h:66
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
bool use_origin_sensor_pose
(Default:false) If false, the conversion will be such that the 2D observation pose on the robot coinc...
float maxRange
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
void setScanRangeValidity(const size_t i, const bool val)
bool hasConfidenceImage
true means the field confidenceImage contains valid data
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:156
std::string points3D_getExternalStorageFileAbsolutePath() const



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