MRPT  1.9.9
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-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPosePDF.h>
16 
17 #include <mrpt/math/CMatrix.h>
19 #include <mrpt/math/ops_containers.h> // norm(), etc.
24 #include <mrpt/system/filesystem.h>
27 #include <mrpt/core/bits_mem.h> // vector_strong_clear
28 #include <limits>
29 
30 using namespace std;
31 using namespace mrpt::obs;
32 using namespace mrpt::poses;
33 using namespace mrpt::math;
34 using namespace mrpt::img;
36 
37 // This must be added to any CSerializable class implementation file.
39 
40 // Static LUT:
41 static CObservation3DRangeScan::TCached3DProjTables lut_3dproj;
42 CObservation3DRangeScan::TCached3DProjTables&
43  CObservation3DRangeScan::get_3dproj_lut()
44 {
45  return lut_3dproj;
46 }
47 
48 static bool EXTERNALS_AS_TEXT_value = false;
49 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(bool value)
50 {
52 }
53 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
54 {
56 }
57 
58 // Whether to use a memory pool for 3D points:
59 #define COBS3DRANGE_USE_MEMPOOL
60 
61 // Do performance time logging?
62 //#define PROJ3D_PERFLOG
63 
64 // Data types for memory pooling CObservation3DRangeScan:
65 #ifdef COBS3DRANGE_USE_MEMPOOL
66 
68 
69 // Memory pool for XYZ points ----------------
71 {
72  /** Width*Height, that is, the number of 3D points */
73  size_t WH;
74  inline bool isSuitable(
76  {
77  return WH >= req.WH;
78  }
79 };
81 {
82  std::vector<float> pts_x, pts_y, pts_z;
83  /** for each point, the corresponding (x,y) pixel coordinates */
84  std::vector<uint16_t> idxs_x, idxs_y;
85 };
89 
90 // Memory pool for the rangeImage matrix ----------------
92 {
93  /** Size of matrix */
94  int H, W;
95  inline bool isSuitable(
97  {
98  return H == req.H && W == req.W;
99  }
100 };
102 {
104 };
108 
110 {
111  if (!obs.points3D_x.empty())
112  {
113  // Before dying, donate my memory to the pool for the joy of future
114  // class-brothers...
116  if (pool)
117  {
119  mem_params.WH = obs.points3D_x.capacity();
120  if (obs.points3D_y.capacity() != mem_params.WH)
121  obs.points3D_y.resize(mem_params.WH);
122  if (obs.points3D_z.capacity() != mem_params.WH)
123  obs.points3D_z.resize(mem_params.WH);
124  if (obs.points3D_idxs_x.capacity() != mem_params.WH)
125  obs.points3D_idxs_x.resize(mem_params.WH);
126  if (obs.points3D_idxs_y.capacity() != mem_params.WH)
127  obs.points3D_idxs_y.resize(mem_params.WH);
128 
131  obs.points3D_x.swap(mem_block->pts_x);
132  obs.points3D_y.swap(mem_block->pts_y);
133  obs.points3D_z.swap(mem_block->pts_z);
134  obs.points3D_idxs_x.swap(mem_block->idxs_x);
135  obs.points3D_idxs_y.swap(mem_block->idxs_y);
136 
137  pool->dump_to_pool(mem_params, mem_block);
138  }
139  }
140 }
142 {
143  if (obs.rangeImage.cols() > 1 && obs.rangeImage.rows() > 1)
144  {
145  // Before dying, donate my memory to the pool for the joy of future
146  // class-brothers...
148  if (pool)
149  {
151  mem_params.H = obs.rangeImage.rows();
152  mem_params.W = obs.rangeImage.cols();
153 
156  obs.rangeImage.swap(mem_block->rangeImage);
157 
158  pool->dump_to_pool(mem_params, mem_block);
159  }
160  }
161 }
162 
163 #endif
164 
165 /*---------------------------------------------------------------
166  Constructor
167  ---------------------------------------------------------------*/
168 CObservation3DRangeScan::CObservation3DRangeScan()
169  : m_points3D_external_stored(false),
170  m_rangeImage_external_stored(false),
171  hasPoints3D(false),
172  hasRangeImage(false),
173  range_is_depth(true),
174  hasIntensityImage(false),
175  intensityImageChannel(CH_VISIBLE),
176  hasConfidenceImage(false),
177  pixelLabels(), // Start without label info
178  cameraParams(),
179  cameraParamsIntensity(),
180  relativePoseIntensityWRTDepth(
181  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
182  maxRange(5.0f),
183  sensorPose(),
184  stdError(0.01f)
185 {
186 }
187 
189 {
190 #ifdef COBS3DRANGE_USE_MEMPOOL
193 #endif
194 }
195 
199 {
200  // The data
201  out << maxRange << sensorPose;
202  out << hasPoints3D;
203  if (hasPoints3D)
204  {
205  ASSERT_(
206  points3D_x.size() == points3D_y.size() &&
207  points3D_x.size() == points3D_z.size() &&
208  points3D_idxs_x.size() == points3D_x.size() &&
209  points3D_idxs_y.size() == points3D_x.size());
210  uint32_t N = points3D_x.size();
211  out << N;
212  if (N)
213  {
217  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
218  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
219  }
220  }
221 
222  out << hasRangeImage;
223  if (hasRangeImage) out << rangeImage;
224  out << hasIntensityImage;
225  if (hasIntensityImage) out << intensityImage;
226  out << hasConfidenceImage;
228 
229  out << cameraParams; // New in v2
230  out << cameraParamsIntensity; // New in v4
231  out << relativePoseIntensityWRTDepth; // New in v4
232 
233  out << stdError;
234  out << timestamp;
235  out << sensorLabel;
236 
237  // New in v3:
240 
241  // New in v5:
242  out << range_is_depth;
243 
244  // New in v6:
245  out << static_cast<int8_t>(intensityImageChannel);
246 
247  // New in v7:
248  out << hasPixelLabels();
249  if (hasPixelLabels())
250  {
251  pixelLabels->writeToStream(out);
252  }
253 }
254 
257 {
258  switch (version)
259  {
260  case 0:
261  case 1:
262  case 2:
263  case 3:
264  case 4:
265  case 5:
266  case 6:
267  case 7:
268  case 8:
269  {
270  uint32_t N;
271 
272  in >> maxRange >> sensorPose;
273 
274  if (version > 0)
275  in >> hasPoints3D;
276  else
277  hasPoints3D = true;
278 
279  if (hasPoints3D)
280  {
281  in >> N;
283 
284  if (N)
285  {
286  in.ReadBufferFixEndianness(&points3D_x[0], N);
287  in.ReadBufferFixEndianness(&points3D_y[0], N);
288  in.ReadBufferFixEndianness(&points3D_z[0], N);
289 
290  if (version == 0)
291  {
292  vector<char> validRange(N); // for v0.
293  in.ReadBuffer(
294  &validRange[0], sizeof(validRange[0]) * N);
295  }
296  if (version >= 8)
297  {
298  in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
299  in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
300  }
301  }
302  }
303  else
304  {
305  this->resizePoints3DVectors(0);
306  }
307 
308  if (version >= 1)
309  {
310  in >> hasRangeImage;
311  if (hasRangeImage)
312  {
313 #ifdef COBS3DRANGE_USE_MEMPOOL
314  // We should call "rangeImage_setSize()" to exploit the
315  // mempool:
316  this->rangeImage_setSize(480, 640);
317 #endif
318  in >> rangeImage;
319  }
320 
323 
326 
327  if (version >= 2)
328  {
329  in >> cameraParams;
330 
331  if (version >= 4)
332  {
335  }
336  else
337  {
340  }
341  }
342  }
343 
344  in >> stdError;
345  in >> timestamp;
346  in >> sensorLabel;
347 
348  if (version >= 3)
349  {
350  // New in v3:
354  }
355  else
356  {
359  }
360 
361  if (version >= 5)
362  {
363  in >> range_is_depth;
364  }
365  else
366  {
367  range_is_depth = true;
368  }
369 
370  if (version >= 6)
371  {
372  int8_t i;
373  in >> i;
374  intensityImageChannel = static_cast<TIntensityChannelID>(i);
375  }
376  else
377  {
379  }
380 
381  pixelLabels.reset(); // Remove existing data first (_unique() is to
382  // leave alive any user copies of the shared
383  // pointer).
384  if (version >= 7)
385  {
386  bool do_have_labels;
387  in >> do_have_labels;
388 
389  if (do_have_labels)
390  pixelLabels.reset(
392  }
393  }
394  break;
395  default:
397  };
398 }
399 
401 {
403 
404  std::swap(hasPoints3D, o.hasPoints3D);
405  points3D_x.swap(o.points3D_x);
406  points3D_y.swap(o.points3D_y);
407  points3D_z.swap(o.points3D_z);
412 
413  std::swap(hasRangeImage, o.hasRangeImage);
414  rangeImage.swap(o.rangeImage);
417 
418  std::swap(hasIntensityImage, o.hasIntensityImage);
421 
424 
425  std::swap(pixelLabels, o.pixelLabels);
426 
428 
429  std::swap(cameraParams, o.cameraParams);
431 
432  std::swap(maxRange, o.maxRange);
433  std::swap(sensorPose, o.sensorPose);
434  std::swap(stdError, o.stdError);
435 }
436 
438 {
440  {
441  const string fil = points3D_getExternalStorageFileAbsolutePath();
443  "txt", mrpt::system::extractFileExtension(fil, true)))
444  {
445  CMatrixFloat M;
446  M.loadFromTextFile(fil);
447  ASSERT_EQUAL_(M.rows(), 3);
448 
449  M.extractRow(0, const_cast<std::vector<float>&>(points3D_x));
450  M.extractRow(1, const_cast<std::vector<float>&>(points3D_y));
451  M.extractRow(2, const_cast<std::vector<float>&>(points3D_z));
452  }
453  else
454  {
457  f >> const_cast<std::vector<float>&>(points3D_x) >>
458  const_cast<std::vector<float>&>(points3D_y) >>
459  const_cast<std::vector<float>&>(points3D_z);
460  }
461  }
462 
464  {
467  "txt", mrpt::system::extractFileExtension(fil, true)))
468  {
469  const_cast<CMatrix&>(rangeImage).loadFromTextFile(fil);
470  }
471  else
472  {
475  f >> const_cast<CMatrix&>(rangeImage);
476  }
477  }
478 }
479 
481 {
483  {
487  }
488 
490 
493 }
494 
496  std::string& out_path) const
497 {
499  if (m_rangeImage_external_file[0] == '/' ||
500  (m_rangeImage_external_file[1] == ':' &&
501  m_rangeImage_external_file[2] == '\\'))
502  {
503  out_path = m_rangeImage_external_file;
504  }
505  else
506  {
507  out_path = CImage::getImagesPathBase();
508  size_t N = CImage::getImagesPathBase().size() - 1;
509  if (CImage::getImagesPathBase()[N] != '/' &&
510  CImage::getImagesPathBase()[N] != '\\')
511  out_path += "/";
512  out_path += m_rangeImage_external_file;
513  }
514 }
516  std::string& out_path) const
517 {
518  ASSERT_(m_points3D_external_file.size() > 2);
519  if (m_points3D_external_file[0] == '/' ||
520  (m_points3D_external_file[1] == ':' &&
521  m_points3D_external_file[2] == '\\'))
522  {
523  out_path = m_points3D_external_file;
524  }
525  else
526  {
527  out_path = CImage::getImagesPathBase();
528  size_t N = CImage::getImagesPathBase().size() - 1;
529  if (CImage::getImagesPathBase()[N] != '/' &&
530  CImage::getImagesPathBase()[N] != '\\')
531  out_path += "/";
532  out_path += m_points3D_external_file;
533  }
534 }
535 
537  const std::string& fileName_, const std::string& use_this_base_dir)
538 {
540  ASSERT_(
541  points3D_x.size() == points3D_y.size() &&
542  points3D_x.size() == points3D_z.size());
543 
546  mrpt::system::fileNameChangeExtension(fileName_, "txt");
547  else
549  mrpt::system::fileNameChangeExtension(fileName_, "bin");
550 
551  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
552  // instead of CImage::getImagesPathBase()
553  const string savedDir = CImage::getImagesPathBase();
554  CImage::setImagesPathBase(use_this_base_dir);
555  const string real_absolute_file_path =
557  CImage::setImagesPathBase(savedDir);
558 
560  {
561  const size_t nPts = points3D_x.size();
562 
563  CMatrixFloat M(3, nPts);
564  M.insertRow(0, points3D_x);
565  M.insertRow(1, points3D_y);
566  M.insertRow(2, points3D_z);
567 
568  M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
569  }
570  else
571  {
572  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
574  f << points3D_x << points3D_y << points3D_z;
575  }
576 
578 
579  // Really dealloc memory, clear() is not enough:
585 }
587  const std::string& fileName_, const std::string& use_this_base_dir)
588 {
592  mrpt::system::fileNameChangeExtension(fileName_, "txt");
593  else
595  mrpt::system::fileNameChangeExtension(fileName_, "bin");
596 
597  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
598  // instead of CImage::getImagesPathBase()
599  const string savedDir = CImage::getImagesPathBase();
600  CImage::setImagesPathBase(use_this_base_dir);
601  const string real_absolute_file_path =
603  CImage::setImagesPathBase(savedDir);
604 
606  {
607  rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
608  }
609  else
610  {
611  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
613  f << rangeImage;
614  }
615 
617  rangeImage.setSize(0, 0);
618 }
619 
620 // ============== Auxiliary function for "recoverCameraCalibrationParameters"
621 // =========================
622 
623 #define CALIB_DECIMAT 15
624 
625 namespace mrpt::obs::detail
626 {
628 {
630  const double z_offset;
631  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
632  : obs(obs_), z_offset(z_offset_)
633  {
634  }
635 };
636 
637 void cam2vec(const TCamera& camPar, CVectorDouble& x)
638 {
639  if (x.size() < 4 + 4) x.resize(4 + 4);
640 
641  x[0] = camPar.fx();
642  x[1] = camPar.fy();
643  x[2] = camPar.cx();
644  x[3] = camPar.cy();
645 
646  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
647 }
648 void vec2cam(const CVectorDouble& x, TCamera& camPar)
649 {
650  camPar.intrinsicParams(0, 0) = x[0]; // fx
651  camPar.intrinsicParams(1, 1) = x[1]; // fy
652  camPar.intrinsicParams(0, 2) = x[2]; // cx
653  camPar.intrinsicParams(1, 2) = x[3]; // cy
654 
655  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
656 }
658  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
659 {
660  const CObservation3DRangeScan& obs = d.obs;
661 
662  TCamera params;
663  vec2cam(par, params);
664 
665  const size_t nC = obs.rangeImage.cols();
666  const size_t nR = obs.rangeImage.rows();
667 
668  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
669 
670  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
671  {
672  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
673  {
674  const size_t idx = nC * r + c;
675 
676  TPoint3D p(
677  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
678  obs.points3D_z[idx]);
679  TPoint3D P(-p.y, -p.z, p.x);
680  TPixelCoordf pixel;
681  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
682  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
683 
684  // Pinhole model:
685  const double x = P.x / P.z;
686  const double y = P.y / P.z;
687 
688  // Radial distortion:
689  const double r2 = square(x) + square(y);
690  const double r4 = square(r2);
691 
692  pixel.x =
693  params.cx() +
694  params.fx() *
695  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
696  2 * params.dist[2] * x * y +
697  params.dist[3] * (r2 + 2 * square(x))));
698  pixel.y =
699  params.cy() +
700  params.fy() *
701  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
702  2 * params.dist[3] * x * y +
703  params.dist[2] * (r2 + 2 * square(y))));
704  }
705 
706  // In theory, it should be (r,c):
707  err.push_back(c - pixel.x);
708  err.push_back(r - pixel.y);
709  }
710  }
711 } // end error_func
712 }
713 
714 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
715  * of a 3D camera given a range (depth) image and the corresponding 3D point
716  * cloud.
717  * \param camera_offset The offset (in meters) in the +X direction of the point
718  * cloud. It's 1cm for SwissRanger SR4000.
719  * \return The final average reprojection error per pixel (typ <0.05 px)
720  */
722  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
723  const double camera_offset)
724 {
725  MRPT_START
726 
727  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
728  ASSERT_(
729  obs.points3D_x.size() == obs.points3D_y.size() &&
730  obs.points3D_x.size() == obs.points3D_z.size());
731 
732  using TMyLevMar =
734  TMyLevMar::TResultInfo info;
735 
736  const size_t nR = obs.rangeImage.rows();
737  const size_t nC = obs.rangeImage.cols();
738 
739  TCamera camInit;
740  camInit.ncols = nC;
741  camInit.nrows = nR;
742  camInit.intrinsicParams(0, 0) = 250;
743  camInit.intrinsicParams(1, 1) = 250;
744  camInit.intrinsicParams(0, 2) = nC >> 1;
745  camInit.intrinsicParams(1, 2) = nR >> 1;
746 
747  CVectorDouble initial_x;
748  detail::cam2vec(camInit, initial_x);
749 
750  initial_x.resize(8);
751  CVectorDouble increments_x(initial_x.size());
752  increments_x.assign(1e-4);
753 
754  CVectorDouble optimal_x;
755 
756  TMyLevMar lm;
757  lm.execute(
758  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
759  detail::TLevMarData(obs, camera_offset), info,
760  mrpt::system::LVL_INFO, /* verbose */
761  1000, /* max iter */
762  1e-3, 1e-9, 1e-9, false);
763 
764  const double avr_px_err =
765  sqrt(info.final_sqr_err / (nC * nR / square(CALIB_DECIMAT)));
766 
767  out_camParams.ncols = nC;
768  out_camParams.nrows = nR;
769  out_camParams.focalLengthMeters = camera_offset;
770  detail::vec2cam(optimal_x, out_camParams);
771 
772  return avr_px_err;
773 
774  MRPT_END
775 }
776 
778  CObservation3DRangeScan& obs, const unsigned int& r1,
779  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
780 {
781  unsigned int cols = cameraParams.ncols;
782  unsigned int rows = cameraParams.nrows;
783 
784  ASSERT_((r1 < r2) && (c1 < c2));
785  ASSERT_((r2 < rows) && (c2 < cols));
786  // Maybe we needed to copy more base obs atributes
787 
788  // Copy zone of range image
790  if (hasRangeImage)
791  rangeImage.extractSubmatrix(r1, r2, c1, c2, obs.rangeImage);
792 
793  // Copy zone of intensity image
796  if (hasIntensityImage)
798  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
799 
800  // Copy zone of confidence image
802  if (hasConfidenceImage)
804  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
805 
806  // Zone labels: It's too complex, just document that pixel labels are NOT
807  // extracted.
808 
809  // Copy zone of scanned points
810  obs.hasPoints3D = hasPoints3D;
811  if (hasPoints3D)
812  {
813  // Erase a possible previous content
814  if (obs.points3D_x.size() > 0)
815  {
816  obs.points3D_x.clear();
817  obs.points3D_y.clear();
818  obs.points3D_z.clear();
819  }
820 
821  for (unsigned int i = r1; i < r2; i++)
822  for (unsigned int j = c1; j < c2; j++)
823  {
824  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
825  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
826  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
827  }
828  }
829 
830  obs.maxRange = maxRange;
831  obs.sensorPose = sensorPose;
832  obs.stdError = stdError;
833 
835 }
836 
837 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
838  * \a points3D_z to allow the usage of the internal memory pool. */
840 {
841 #ifdef COBS3DRANGE_USE_MEMPOOL
842  // If WH=0 this is a clear:
843  if (!WH)
844  {
850  return;
851  }
852 
853  if (WH <= points3D_x.size()) // reduce size, don't realloc
854  {
855  points3D_x.resize(WH);
856  points3D_y.resize(WH);
857  points3D_z.resize(WH);
858  points3D_idxs_x.resize(WH);
859  points3D_idxs_y.resize(WH);
860  return;
861  }
862 
863  // Request memory for the X,Y,Z buffers from the memory pool:
865  if (pool)
866  {
868  mem_params.WH = WH;
869 
871  pool->request_memory(mem_params);
872 
873  if (mem_block)
874  { // Take the memory via swaps:
875  points3D_x.swap(mem_block->pts_x);
876  points3D_y.swap(mem_block->pts_y);
877  points3D_z.swap(mem_block->pts_z);
878  points3D_idxs_x.swap(mem_block->idxs_x);
879  points3D_idxs_y.swap(mem_block->idxs_y);
880  delete mem_block;
881  }
882  }
883 #endif
884 
885  // Either if there was no pool memory or we got it, make sure the size of
886  // vectors is OK:
887  points3D_x.resize(WH);
888  points3D_y.resize(WH);
889  points3D_z.resize(WH);
890  points3D_idxs_x.resize(WH);
891  points3D_idxs_y.resize(WH);
892 }
893 
895 {
896  // x,y,z vectors have the same size.
897  return points3D_x.size();
898 }
899 
900 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
901 // pooling to speed-up the memory allocation.
902 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
903 {
904 #ifdef COBS3DRANGE_USE_MEMPOOL
905  // Request memory from the memory pool:
907  if (pool)
908  {
910  mem_params.H = H;
911  mem_params.W = W;
912 
914  pool->request_memory(mem_params);
915 
916  if (mem_block)
917  { // Take the memory via swaps:
918  rangeImage.swap(mem_block->rangeImage);
919  delete mem_block;
920  return;
921  }
922  }
923 // otherwise, continue with the normal method:
924 #endif
925  // Fall-back to normal method:
926  rangeImage.setSize(H, W);
927 }
928 
929 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
930 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
932 {
933  static const double EPSILON = 1e-7;
934  static mrpt::poses::CPose3D ref_pose(
935  0, 0, 0, DEG2RAD(-90), 0, DEG2RAD(-90));
936 
937  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
938  .all() &&
939  ((ref_pose.getRotationMatrix() -
941  .array()
942  .abs() < EPSILON)
943  .all();
944 }
945 
949 {
950  out_scan2d.sensorLabel = sensorLabel;
951  out_scan2d.timestamp = this->timestamp;
952 
953  if (!this->hasRangeImage)
954  { // Nothing to do!
955  out_scan2d.resizeScan(0);
956  return;
957  }
958 
959  const size_t nCols = this->rangeImage.cols();
960  const size_t nRows = this->rangeImage.rows();
961  if (fp.rangeMask_min)
962  { // sanity check:
963  ASSERT_EQUAL_(fp.rangeMask_min->cols(), rangeImage.cols());
964  ASSERT_EQUAL_(fp.rangeMask_min->rows(), rangeImage.rows());
965  }
966  if (fp.rangeMask_max)
967  { // sanity check:
968  ASSERT_EQUAL_(fp.rangeMask_max->cols(), rangeImage.cols());
969  ASSERT_EQUAL_(fp.rangeMask_max->rows(), rangeImage.rows());
970  }
971 
972  // Compute the real horizontal FOV from the range camera intrinsic calib
973  // data:
974  // Note: this assumes the range image has been "undistorted", which is true
975  // for data
976  // from OpenNI, and will be in the future for libfreenect in MRPT,
977  // but it's
978  // not implemented yet (as of Mar 2012), so this is an approximation
979  // in that case.
980  const double cx = this->cameraParams.cx();
981  const double cy = this->cameraParams.cy();
982  const double fx = this->cameraParams.fx();
983  const double fy = this->cameraParams.fy();
984 
985  // (Imagine the camera seen from above to understand this geometry)
986  const double real_FOV_left = atan2(cx, fx);
987  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
988 
989  // FOV of the equivalent "fake" "laser scanner":
990  const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
991 
992  // Now, we should create more "fake laser" points than columns in the image,
993  // since laser scans are assumed to sample space at evenly-spaced angles,
994  // while in images it is like ~tan(angle).
996  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
997  const size_t nLaserRays =
998  static_cast<size_t>(nCols * sp.oversampling_ratio);
999 
1000  // Prepare 2D scan data fields:
1001  out_scan2d.aperture = FOV_equiv;
1002  out_scan2d.maxRange = this->maxRange;
1003  out_scan2d.resizeScan(nLaserRays);
1004 
1005  out_scan2d.resizeScanAndAssign(
1006  nLaserRays, 2.0 * this->maxRange,
1007  false); // default: all ranges=invalid
1008  if (sp.use_origin_sensor_pose)
1009  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1010  else
1011  out_scan2d.sensorPose = this->sensorPose;
1012 
1013  // The vertical FOVs given by the user can be translated into limits of the
1014  // tangents (tan>0 means above, i.e. z>0):
1015  const float tan_min = -tan(std::abs(sp.angle_inf));
1016  const float tan_max = tan(std::abs(sp.angle_sup));
1017 
1018  // Precompute the tangents of the vertical angles of each "ray"
1019  // for every row in the range image:
1020  std::vector<float> vert_ang_tan(nRows);
1021  for (size_t r = 0; r < nRows; r++)
1022  vert_ang_tan[r] = static_cast<float>((cy - r) / fy);
1023 
1024  if (!sp.use_origin_sensor_pose)
1025  {
1026  // Algorithm 1: each column in the range image corresponds to a known
1027  // orientation in the 2D scan:
1028  // -------------------
1029  out_scan2d.rightToLeft = false;
1030 
1031  // Angle "counter" for the fake laser scan direction, and the increment:
1032  double ang = -FOV_equiv * 0.5;
1033  const double A_ang = FOV_equiv / (nLaserRays - 1);
1034 
1035  TRangeImageFilter rif(fp);
1036 
1037  // Go thru columns, and keep the minimum distance (along the +X axis,
1038  // not 3D distance!)
1039  // for each direction (i.e. for each column) which also lies within the
1040  // vertical FOV passed
1041  // by the user.
1042  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1043  {
1044  // Equivalent column in the range image for the "i'th" ray:
1045  const double tan_ang = tan(ang);
1046  // make sure we don't go out of range (just in case):
1047  const size_t c = std::min(
1048  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1049  nCols - 1);
1050 
1051  bool any_valid = false;
1052  float closest_range = out_scan2d.maxRange;
1053 
1054  for (size_t r = 0; r < nRows; r++)
1055  {
1056  const float D = this->rangeImage.coeff(r, c);
1057  if (!rif.do_range_filter(r, c, D)) continue;
1058 
1059  // All filters passed:
1060  const float this_point_tan = vert_ang_tan[r] * D;
1061  if (this_point_tan > tan_min && this_point_tan < tan_max)
1062  {
1063  any_valid = true;
1064  mrpt::keep_min(closest_range, D);
1065  }
1066  }
1067 
1068  if (any_valid)
1069  {
1070  out_scan2d.setScanRangeValidity(i, true);
1071  // Compute the distance in 2D from the "depth" in closest_range:
1072  out_scan2d.setScanRange(
1073  i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1074  }
1075  } // end for columns
1076  }
1077  else
1078  {
1079  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1080  // at the origin)
1081  // ------------------------------------------------------------------------
1082  out_scan2d.rightToLeft = true;
1083 
1084  T3DPointsProjectionParams projParams;
1085  projParams.takeIntoAccountSensorPoseOnRobot = true;
1086 
1088  mrpt::make_aligned_shared<mrpt::opengl::CPointCloud>();
1089  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1090 
1091  const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1092  &zs = pc->getArrayZ();
1093  const size_t N = xs.size();
1094 
1095  const double A_ang = FOV_equiv / (nLaserRays - 1);
1096  const double ang0 = -FOV_equiv * 0.5;
1097 
1098  for (size_t i = 0; i < N; i++)
1099  {
1100  if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;
1101 
1102  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1103 
1104  int i_range = (phi_wrt_origin - ang0) / A_ang;
1105  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1106 
1107  const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1108  if (out_scan2d.scan[i_range] > r_wrt_origin)
1109  out_scan2d.setScanRange(i_range, r_wrt_origin);
1110  out_scan2d.setScanRangeValidity(i_range, true);
1111  }
1112  }
1113 }
1114 
1116 {
1118 
1119  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1120  // memory.
1121 
1122  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1123  "base:\n";
1125  << endl;
1126 
1127  o << "maxRange = " << maxRange << " m" << endl;
1128 
1129  o << "Has 3D point cloud? ";
1130  if (hasPoints3D)
1131  {
1132  o << "YES: " << points3D_x.size() << " points";
1134  o << ". External file: " << points3D_getExternalStorageFile()
1135  << endl;
1136  else
1137  o << " (embedded)." << endl;
1138  }
1139  else
1140  o << "NO" << endl;
1141 
1142  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1143  if (hasRangeImage)
1144  {
1146  o << ". External file: " << rangeImage_getExternalStorageFile()
1147  << endl;
1148  else
1149  o << " (embedded)." << endl;
1150  }
1151 
1152  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1153  if (hasIntensityImage)
1154  {
1156  o << ". External file: " << intensityImage.getExternalStorageFile()
1157  << endl;
1158  else
1159  o << " (embedded).\n";
1160  // Channel?
1161  o << "Source channel: "
1164  value2name(intensityImageChannel)
1165  << endl;
1166  }
1167 
1168  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1169  if (hasConfidenceImage)
1170  {
1172  o << ". External file: " << confidenceImage.getExternalStorageFile()
1173  << endl;
1174  else
1175  o << " (embedded)." << endl;
1176  }
1177 
1178  o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1179  if (hasPixelLabels())
1180  {
1181  o << " Human readable labels:" << endl;
1183  pixelLabels->pixelLabelNames.begin();
1184  it != pixelLabels->pixelLabelNames.end(); ++it)
1185  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1186  }
1187 
1188  o << endl << endl;
1189  o << "Depth camera calibration parameters:" << endl;
1190  {
1191  CConfigFileMemory cfg;
1192  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1193  o << cfg.getContent() << endl;
1194  }
1195  o << endl << "Intensity camera calibration parameters:" << endl;
1196  {
1197  CConfigFileMemory cfg;
1198  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1199  o << cfg.getContent() << endl;
1200  }
1201  o << endl
1202  << endl
1203  << "Pose of the intensity cam. wrt the depth cam:\n"
1207  << endl;
1208 }
1209 
1212 {
1213  const uint8_t version = 1; // for possible future changes.
1214  out << version;
1215  // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the
1216  // object in the class factory later on.
1217  out << BITFIELD_BYTES;
1218  // 2nd: data-specific serialization:
1219  this->internal_writeToStream(out);
1220 }
1221 
1222 template <unsigned int BYTES_REQUIRED_>
1224  BYTES_REQUIRED_>::internal_readFromStream(mrpt::serialization::CArchive& in)
1225 {
1226  {
1227  uint32_t nR, nC;
1228  in >> nR >> nC;
1229  pixelLabels.resize(nR, nC);
1230  for (uint32_t c = 0; c < nC; c++)
1231  for (uint32_t r = 0; r < nR; r++) in >> pixelLabels.coeffRef(r, c);
1232  }
1233  in >> pixelLabelNames;
1234 }
1235 template <unsigned int BYTES_REQUIRED_>
1238 {
1239  {
1240  const uint32_t nR = static_cast<uint32_t>(pixelLabels.rows());
1241  const uint32_t nC = static_cast<uint32_t>(pixelLabels.cols());
1242  out << nR << nC;
1243  for (uint32_t c = 0; c < nC; c++)
1244  for (uint32_t r = 0; r < nR; r++) out << pixelLabels.coeff(r, c);
1245  }
1246  out << pixelLabelNames;
1247 }
1248 
1249 // Deserialization and class factory. All in one, ladies and gentlemen
1252 {
1253  uint8_t version;
1254  in >> version;
1255 
1256  switch (version)
1257  {
1258  case 1:
1259  {
1260  // 1st: Read NUM BYTES
1261  uint8_t bitfield_bytes;
1262  in >> bitfield_bytes;
1263 
1264  // Hand-made class factory. May be a good solution if there will be
1265  // not too many different classes:
1267  switch (bitfield_bytes)
1268  {
1269  case 1:
1271  break;
1272  case 2:
1274  break;
1275  case 3:
1276  case 4:
1278  break;
1279  case 5:
1280  case 6:
1281  case 7:
1282  case 8:
1284  break;
1285  default:
1286  throw std::runtime_error(
1287  "Unknown type of pixelLabel inner class while "
1288  "deserializing!");
1289  };
1290  // 2nd: data-specific serialization:
1291  new_obj->internal_readFromStream(in);
1292 
1293  return new_obj;
1294  }
1295  break;
1296 
1297  default:
1299  break;
1300  };
1301 }
1302 
1304  : angle_sup(mrpt::DEG2RAD(5)),
1305  angle_inf(mrpt::DEG2RAD(5)),
1306  z_min(-std::numeric_limits<double>::max()),
1307  z_max(std::numeric_limits<double>::max()),
1308  oversampling_ratio(1.2),
1309  use_origin_sensor_pose(false)
1310 {
1311 }
bool isSuitable(const CObservation3DRangeScan_Points_MemPoolParams &req) const
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.
This class implements a config file-like interface over a memory-stored string list.
uint32_t nrows
Definition: TCamera.h:39
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
#define MRPT_START
Definition: exceptions.h:262
void writeToStream(mrpt::serialization::CArchive &out) const
bool isSuitable(const CObservation3DRangeScan_Ranges_MemPoolParams &req) const
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)
static bool EXTERNALS_AS_TEXT_value
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 ...
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:163
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
std::vector< uint16_t > idxs_x
for each point, the corresponding (x,y) pixel coordinates
#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)
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::img::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...
double DEG2RAD(const double x)
Degrees to radians.
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.
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.
double angle_sup
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
This file implements several operations that operate element-wise on individual or pairs of container...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)=0
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: CArchive.h:127
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:165
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: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::serialization::CArchive &in)
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
signed char int8_t
Definition: rptypes.h:40
STL namespace.
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:793
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...
void swap(CObservation &o)
Swap with another observation, ONLY the data defined here in the base class CObservation.
double z_min
(Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates wi...
POOLABLE_DATA * request_memory(const DATA_PARAMS &params)
Request a block of data which fulfils the size requirements stated in params.
void unload() const noexcept
For external storage image objects only, this method unloads the image from memory (or does nothing i...
Definition: CImage.cpp:1897
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
void swap(CImage &o)
Very efficient swap of two images (just swap the internal pointers)
Definition: CImage.cpp:133
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won&#39;t normally want to call this, it&#39;s only used from internal MRPT programs.
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:41
float maxRange
The maximum range allowed by the device, in meters (e.g.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream
Definition: CArchive.h:555
T square(const T x)
Inline function for the square of a number.
A generic system for versatile memory pooling.
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:42
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double focalLengthMeters
The focal length of the camera, in meters (can be used among &#39;intrinsicParams&#39; to determine the pixel...
Definition: TCamera.h:48
std::string rangeImage_getExternalStorageFileAbsolutePath() const
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:161
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:96
mrpt::containers::ContainerReadOnlyProxyAccessor< mrpt::aligned_std_vector< float > > scan
The range values of the scan, in meters.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:153
A helper class that can convert an enum value into its textual representation, and viceversa...
const GLubyte * c
Definition: glext.h:6313
virtual void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise...
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
#define CALIB_DECIMAT
void vec2cam(const CVectorDouble &x, TCamera &camPar)
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
std::string fileNameChangeExtension(const std::string &filename, const std::string &newExtension)
Replace the filename extension by another one.
Definition: filesystem.cpp:370
Used in CObservation3DRangeScan::convertTo2DScan()
std::string m_points3D_external_file
3D points are in CImage::getImagesPathBase()+<this_file_name>
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:27
std::string getExternalStorageFile() const noexcept
Only if isExternallyStored() returns true.
Definition: img/CImage.h:796
double x
X,Y,Z coordinates.
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 extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:97
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
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...
std::string points3D_getExternalStorageFileAbsolutePath() const
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
Definition: glext.h:4101
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::array< double, 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:45
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:159
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.
Definition: CObservation.h:62
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
const mrpt::math::CMatrix * rangeMask_max
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
TPixelLabelInfoBase::Ptr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
void saveToConfigFile(const std::string &section, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:114
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:275
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:171
#define MRPT_END
Definition: exceptions.h:266
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
GLuint in
Definition: glext.h:7274
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
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 ...
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
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...
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
Definition: CImage.cpp:1359
void cam2vec(const TCamera &camPar, CVectorDouble &x)
GLenum GLint GLint y
Definition: glext.h:3538
static CObservation3DRangeScan::TCached3DProjTables lut_3dproj
Transparently opens a compressed "gz" file and reads uncompressed data from it.
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won&#39;t normally want to call this, it&#39;s only used from internal MRPT programs.
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:46
GLsizei const GLfloat * value
Definition: glext.h:4117
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
GLenum GLint x
Definition: glext.h:3538
Saves data to a file and transparently compress the data using the given compression level...
void mempool_donate_xyz_buffers(CObservation3DRangeScan &obs)
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:230
Lightweight 3D point.
mrpt::config::CConfigFileMemory CConfigFileMemory
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:22
size_t getScanSize() const
Get the size of the scan pointcloud.
bool 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:47
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_)
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits_mem.h:16
GLfloat GLfloat p
Definition: glext.h:6305
GLenum const GLfloat * params
Definition: glext.h:3534
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
Virtual interface to all pixel-label information structs.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
const CObservation3DRangeScan & obs
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
fixed floating point &#39;f&#39;
Definition: math_frwds.h:65
virtual void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
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...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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...
float maxRange
The maximum range allowed by the device, in meters (e.g.
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
void setScanRangeValidity(const size_t i, const bool val)
bool hasConfidenceImage
true means the field confidenceImage contains valid data
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020