MRPT  1.9.9
CObservation3DRangeScan.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPosePDF.h>
16 
18 #include <mrpt/core/bits_mem.h> // vector_strong_clear
22 #include <mrpt/math/CMatrixF.h>
23 #include <mrpt/math/ops_containers.h> // norm(), etc.
26 #include <mrpt/system/filesystem.h>
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{0};
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{0}, W{0};
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 
129  auto* mem_block = new CObservation3DRangeScan_Points_MemPoolData();
130  obs.points3D_x.swap(mem_block->pts_x);
131  obs.points3D_y.swap(mem_block->pts_y);
132  obs.points3D_z.swap(mem_block->pts_z);
133  obs.points3D_idxs_x.swap(mem_block->idxs_x);
134  obs.points3D_idxs_y.swap(mem_block->idxs_y);
135 
136  pool->dump_to_pool(mem_params, mem_block);
137  }
138  }
139 }
141 {
142  if (obs.rangeImage.cols() > 1 && obs.rangeImage.rows() > 1)
143  {
144  // Before dying, donate my memory to the pool for the joy of future
145  // class-brothers...
147  if (pool)
148  {
150  mem_params.H = obs.rangeImage.rows();
151  mem_params.W = obs.rangeImage.cols();
152 
153  auto* mem_block = new CObservation3DRangeScan_Ranges_MemPoolData();
154  obs.rangeImage.swap(mem_block->rangeImage);
155 
156  pool->dump_to_pool(mem_params, mem_block);
157  }
158  }
159 }
160 
161 #endif
162 
163 /*---------------------------------------------------------------
164  Constructor
165  ---------------------------------------------------------------*/
166 CObservation3DRangeScan::CObservation3DRangeScan()
167  : pixelLabels(), // Start without label info
168  cameraParams(),
169  cameraParamsIntensity(),
170  relativePoseIntensityWRTDepth(
171  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
172 
173  sensorPose()
174 
175 {
176 }
177 
179 {
180 #ifdef COBS3DRANGE_USE_MEMPOOL
183 #endif
184 }
185 
189 {
190  // The data
191  out << maxRange << sensorPose;
192  out << hasPoints3D;
193  if (hasPoints3D)
194  {
195  ASSERT_(
196  points3D_x.size() == points3D_y.size() &&
197  points3D_x.size() == points3D_z.size() &&
198  points3D_idxs_x.size() == points3D_x.size() &&
199  points3D_idxs_y.size() == points3D_x.size());
200  uint32_t N = points3D_x.size();
201  out << N;
202  if (N)
203  {
207  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
208  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
209  }
210  }
211 
212  out << hasRangeImage;
213  if (hasRangeImage) out << rangeImage;
214  out << hasIntensityImage;
215  if (hasIntensityImage) out << intensityImage;
216  out << hasConfidenceImage;
218 
219  out << cameraParams; // New in v2
220  out << cameraParamsIntensity; // New in v4
221  out << relativePoseIntensityWRTDepth; // New in v4
222 
223  out << stdError;
224  out << timestamp;
225  out << sensorLabel;
226 
227  // New in v3:
230 
231  // New in v5:
232  out << range_is_depth;
233 
234  // New in v6:
235  out << static_cast<int8_t>(intensityImageChannel);
236 
237  // New in v7:
238  out << hasPixelLabels();
239  if (hasPixelLabels())
240  {
241  pixelLabels->writeToStream(out);
242  }
243 }
244 
247 {
248  switch (version)
249  {
250  case 0:
251  case 1:
252  case 2:
253  case 3:
254  case 4:
255  case 5:
256  case 6:
257  case 7:
258  case 8:
259  {
260  uint32_t N;
261 
262  in >> maxRange >> sensorPose;
263 
264  if (version > 0)
265  in >> hasPoints3D;
266  else
267  hasPoints3D = true;
268 
269  if (hasPoints3D)
270  {
271  in >> N;
273 
274  if (N)
275  {
276  in.ReadBufferFixEndianness(&points3D_x[0], N);
277  in.ReadBufferFixEndianness(&points3D_y[0], N);
278  in.ReadBufferFixEndianness(&points3D_z[0], N);
279 
280  if (version == 0)
281  {
282  vector<char> validRange(N); // for v0.
283  in.ReadBuffer(
284  &validRange[0], sizeof(validRange[0]) * N);
285  }
286  if (version >= 8)
287  {
288  in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
289  in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
290  }
291  }
292  }
293  else
294  {
295  this->resizePoints3DVectors(0);
296  }
297 
298  if (version >= 1)
299  {
300  in >> hasRangeImage;
301  if (hasRangeImage)
302  {
303 #ifdef COBS3DRANGE_USE_MEMPOOL
304  // We should call "rangeImage_setSize()" to exploit the
305  // mempool:
306  this->rangeImage_setSize(480, 640);
307 #endif
308  in >> rangeImage;
309  }
310 
313 
316 
317  if (version >= 2)
318  {
319  in >> cameraParams;
320 
321  if (version >= 4)
322  {
325  }
326  else
327  {
330  }
331  }
332  }
333 
334  in >> stdError;
335  in >> timestamp;
336  in >> sensorLabel;
337 
338  if (version >= 3)
339  {
340  // New in v3:
344  }
345  else
346  {
349  }
350 
351  if (version >= 5)
352  {
353  in >> range_is_depth;
354  }
355  else
356  {
357  range_is_depth = true;
358  }
359 
360  if (version >= 6)
361  {
362  int8_t i;
363  in >> i;
364  intensityImageChannel = static_cast<TIntensityChannelID>(i);
365  }
366  else
367  {
369  }
370 
371  pixelLabels.reset(); // Remove existing data first (_unique() is to
372  // leave alive any user copies of the shared
373  // pointer).
374  if (version >= 7)
375  {
376  bool do_have_labels;
377  in >> do_have_labels;
378 
379  if (do_have_labels)
380  pixelLabels.reset(
382  }
383  }
384  break;
385  default:
387  };
388 }
389 
391 {
393 
394  std::swap(hasPoints3D, o.hasPoints3D);
395  points3D_x.swap(o.points3D_x);
396  points3D_y.swap(o.points3D_y);
397  points3D_z.swap(o.points3D_z);
402 
403  std::swap(hasRangeImage, o.hasRangeImage);
407 
408  std::swap(hasIntensityImage, o.hasIntensityImage);
411 
414 
415  std::swap(pixelLabels, o.pixelLabels);
416 
418 
419  std::swap(cameraParams, o.cameraParams);
421 
422  std::swap(maxRange, o.maxRange);
423  std::swap(sensorPose, o.sensorPose);
424  std::swap(stdError, o.stdError);
425 }
426 
428 {
430  {
431  const string fil = points3D_getExternalStorageFileAbsolutePath();
433  "txt", mrpt::system::extractFileExtension(fil, true)))
434  {
435  CMatrixFloat M;
436  M.loadFromTextFile(fil);
437  ASSERT_EQUAL_(M.rows(), 3);
438  const auto N = M.cols();
439 
440  auto& xs = const_cast<std::vector<float>&>(points3D_x);
441  auto& ys = const_cast<std::vector<float>&>(points3D_y);
442  auto& zs = const_cast<std::vector<float>&>(points3D_z);
443  xs.resize(N);
444  ys.resize(N);
445  zs.resize(N);
446  ::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
447  ::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
448  ::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
449  }
450  else
451  {
454  f >> const_cast<std::vector<float>&>(points3D_x) >>
455  const_cast<std::vector<float>&>(points3D_y) >>
456  const_cast<std::vector<float>&>(points3D_z);
457  }
458  }
459 
461  {
464  "txt", mrpt::system::extractFileExtension(fil, true)))
465  {
466  const_cast<CMatrixF&>(rangeImage).loadFromTextFile(fil);
467  }
468  else
469  {
472  f >> const_cast<CMatrixF&>(rangeImage);
473  }
474  }
475 }
476 
478 {
480  {
484  }
485 
487 
490 }
491 
493  std::string& out_path) const
494 {
496  if (m_rangeImage_external_file[0] == '/' ||
497  (m_rangeImage_external_file[1] == ':' &&
498  m_rangeImage_external_file[2] == '\\'))
499  {
500  out_path = m_rangeImage_external_file;
501  }
502  else
503  {
504  out_path = CImage::getImagesPathBase();
505  size_t N = CImage::getImagesPathBase().size() - 1;
506  if (CImage::getImagesPathBase()[N] != '/' &&
507  CImage::getImagesPathBase()[N] != '\\')
508  out_path += "/";
509  out_path += m_rangeImage_external_file;
510  }
511 }
513  std::string& out_path) const
514 {
515  ASSERT_(m_points3D_external_file.size() > 2);
516  if (m_points3D_external_file[0] == '/' ||
517  (m_points3D_external_file[1] == ':' &&
518  m_points3D_external_file[2] == '\\'))
519  {
520  out_path = m_points3D_external_file;
521  }
522  else
523  {
524  out_path = CImage::getImagesPathBase();
525  size_t N = CImage::getImagesPathBase().size() - 1;
526  if (CImage::getImagesPathBase()[N] != '/' &&
527  CImage::getImagesPathBase()[N] != '\\')
528  out_path += "/";
529  out_path += m_points3D_external_file;
530  }
531 }
532 
534  const std::string& fileName_, const std::string& use_this_base_dir)
535 {
537  ASSERT_(
538  points3D_x.size() == points3D_y.size() &&
539  points3D_x.size() == points3D_z.size());
540 
543  mrpt::system::fileNameChangeExtension(fileName_, "txt");
544  else
546  mrpt::system::fileNameChangeExtension(fileName_, "bin");
547 
548  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
549  // instead of CImage::getImagesPathBase()
550  const string savedDir = CImage::getImagesPathBase();
551  CImage::setImagesPathBase(use_this_base_dir);
552  const string real_absolute_file_path =
554  CImage::setImagesPathBase(savedDir);
555 
557  {
558  const size_t nPts = points3D_x.size();
559 
560  CMatrixFloat M(3, nPts);
561  M.setRow(0, points3D_x);
562  M.setRow(1, points3D_y);
563  M.setRow(2, points3D_z);
564 
565  M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
566  }
567  else
568  {
569  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
571  f << points3D_x << points3D_y << points3D_z;
572  }
573 
575 
576  // Really dealloc memory, clear() is not enough:
582 }
584  const std::string& fileName_, const std::string& use_this_base_dir)
585 {
589  mrpt::system::fileNameChangeExtension(fileName_, "txt");
590  else
592  mrpt::system::fileNameChangeExtension(fileName_, "bin");
593 
594  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
595  // instead of CImage::getImagesPathBase()
596  const string savedDir = CImage::getImagesPathBase();
597  CImage::setImagesPathBase(use_this_base_dir);
598  const string real_absolute_file_path =
600  CImage::setImagesPathBase(savedDir);
601 
603  {
604  rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
605  }
606  else
607  {
608  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
610  f << rangeImage;
611  }
612 
614  rangeImage.setSize(0, 0);
615 }
616 
617 // ============== Auxiliary function for "recoverCameraCalibrationParameters"
618 // =========================
619 
620 #define CALIB_DECIMAT 15
621 
622 namespace mrpt::obs::detail
623 {
625 {
627  const double z_offset;
628  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
629  : obs(obs_), z_offset(z_offset_)
630  {
631  }
632 };
633 
634 void cam2vec(const TCamera& camPar, CVectorDouble& x)
635 {
636  if (x.size() < 4 + 4) x.resize(4 + 4);
637 
638  x[0] = camPar.fx();
639  x[1] = camPar.fy();
640  x[2] = camPar.cx();
641  x[3] = camPar.cy();
642 
643  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
644 }
645 void vec2cam(const CVectorDouble& x, TCamera& camPar)
646 {
647  camPar.intrinsicParams(0, 0) = x[0]; // fx
648  camPar.intrinsicParams(1, 1) = x[1]; // fy
649  camPar.intrinsicParams(0, 2) = x[2]; // cx
650  camPar.intrinsicParams(1, 2) = x[3]; // cy
651 
652  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
653 }
655  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
656 {
657  const CObservation3DRangeScan& obs = d.obs;
658 
659  TCamera params;
660  vec2cam(par, params);
661 
662  const size_t nC = obs.rangeImage.cols();
663  const size_t nR = obs.rangeImage.rows();
664 
665  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
666 
667  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
668  {
669  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
670  {
671  const size_t idx = nC * r + c;
672 
673  TPoint3D p(
674  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
675  obs.points3D_z[idx]);
676  TPoint3D P(-p.y, -p.z, p.x);
677  TPixelCoordf pixel;
678  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
679  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
680 
681  // Pinhole model:
682  const double x = P.x / P.z;
683  const double y = P.y / P.z;
684 
685  // Radial distortion:
686  const double r2 = square(x) + square(y);
687  const double r4 = square(r2);
688 
689  pixel.x =
690  params.cx() +
691  params.fx() *
692  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
693  2 * params.dist[2] * x * y +
694  params.dist[3] * (r2 + 2 * square(x))));
695  pixel.y =
696  params.cy() +
697  params.fy() *
698  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
699  2 * params.dist[3] * x * y +
700  params.dist[2] * (r2 + 2 * square(y))));
701  }
702 
703  // In theory, it should be (r,c):
704  err.push_back(c - pixel.x);
705  err.push_back(r - pixel.y);
706  }
707  }
708 } // end error_func
709 } // namespace mrpt::obs::detail
710 
711 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
712  * of a 3D camera given a range (depth) image and the corresponding 3D point
713  * cloud.
714  * \param camera_offset The offset (in meters) in the +X direction of the point
715  * cloud. It's 1cm for SwissRanger SR4000.
716  * \return The final average reprojection error per pixel (typ <0.05 px)
717  */
719  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
720  const double camera_offset)
721 {
722  MRPT_START
723 
724  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
725  ASSERT_(
726  obs.points3D_x.size() == obs.points3D_y.size() &&
727  obs.points3D_x.size() == obs.points3D_z.size());
728 
729  using TMyLevMar =
731  TMyLevMar::TResultInfo info;
732 
733  const size_t nR = obs.rangeImage.rows();
734  const size_t nC = obs.rangeImage.cols();
735 
736  TCamera camInit;
737  camInit.ncols = nC;
738  camInit.nrows = nR;
739  camInit.intrinsicParams(0, 0) = 250;
740  camInit.intrinsicParams(1, 1) = 250;
741  camInit.intrinsicParams(0, 2) = nC >> 1;
742  camInit.intrinsicParams(1, 2) = nR >> 1;
743 
744  CVectorDouble initial_x;
745  detail::cam2vec(camInit, initial_x);
746 
747  initial_x.resize(8);
748  CVectorDouble increments_x(initial_x.size());
749  increments_x.fill(1e-4);
750 
751  CVectorDouble optimal_x;
752 
753  TMyLevMar lm;
754  lm.execute(
755  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
756  detail::TLevMarData(obs, camera_offset), info,
757  mrpt::system::LVL_INFO, /* verbose */
758  1000, /* max iter */
759  1e-3, 1e-9, 1e-9, false);
760 
761  const double avr_px_err =
762  sqrt(info.final_sqr_err / double(nC * nR / square(CALIB_DECIMAT)));
763 
764  out_camParams.ncols = nC;
765  out_camParams.nrows = nR;
766  out_camParams.focalLengthMeters = camera_offset;
767  detail::vec2cam(optimal_x, out_camParams);
768 
769  return avr_px_err;
770 
771  MRPT_END
772 }
773 
775  CObservation3DRangeScan& obs, const unsigned int& r1,
776  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
777 {
778  unsigned int cols = cameraParams.ncols;
779  unsigned int rows = cameraParams.nrows;
780 
781  ASSERT_((r1 < r2) && (c1 < c2));
782  ASSERT_((r2 < rows) && (c2 < cols));
783  // Maybe we needed to copy more base obs atributes
784 
785  // Copy zone of range image
787  if (hasRangeImage)
788  obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);
789 
790  // Copy zone of intensity image
793  if (hasIntensityImage)
795  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
796 
797  // Copy zone of confidence image
799  if (hasConfidenceImage)
801  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
802 
803  // Zone labels: It's too complex, just document that pixel labels are NOT
804  // extracted.
805 
806  // Copy zone of scanned points
807  obs.hasPoints3D = hasPoints3D;
808  if (hasPoints3D)
809  {
810  // Erase a possible previous content
811  if (obs.points3D_x.size() > 0)
812  {
813  obs.points3D_x.clear();
814  obs.points3D_y.clear();
815  obs.points3D_z.clear();
816  }
817 
818  for (unsigned int i = r1; i < r2; i++)
819  for (unsigned int j = c1; j < c2; j++)
820  {
821  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
822  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
823  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
824  }
825  }
826 
827  obs.maxRange = maxRange;
828  obs.sensorPose = sensorPose;
829  obs.stdError = stdError;
830 
832 }
833 
834 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
835  * \a points3D_z to allow the usage of the internal memory pool. */
837 {
838 #ifdef COBS3DRANGE_USE_MEMPOOL
839  // If WH=0 this is a clear:
840  if (!WH)
841  {
847  return;
848  }
849 
850  if (WH <= points3D_x.size()) // reduce size, don't realloc
851  {
852  points3D_x.resize(WH);
853  points3D_y.resize(WH);
854  points3D_z.resize(WH);
855  points3D_idxs_x.resize(WH);
856  points3D_idxs_y.resize(WH);
857  return;
858  }
859 
860  // Request memory for the X,Y,Z buffers from the memory pool:
862  if (pool)
863  {
865  mem_params.WH = WH;
866 
868  pool->request_memory(mem_params);
869 
870  if (mem_block)
871  { // Take the memory via swaps:
872  points3D_x.swap(mem_block->pts_x);
873  points3D_y.swap(mem_block->pts_y);
874  points3D_z.swap(mem_block->pts_z);
875  points3D_idxs_x.swap(mem_block->idxs_x);
876  points3D_idxs_y.swap(mem_block->idxs_y);
877  delete mem_block;
878  }
879  }
880 #endif
881 
882  // Either if there was no pool memory or we got it, make sure the size of
883  // vectors is OK:
884  points3D_x.resize(WH);
885  points3D_y.resize(WH);
886  points3D_z.resize(WH);
887  points3D_idxs_x.resize(WH);
888  points3D_idxs_y.resize(WH);
889 }
890 
892 {
893  // x,y,z vectors have the same size.
894  return points3D_x.size();
895 }
896 
897 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
898 // pooling to speed-up the memory allocation.
899 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
900 {
901 #ifdef COBS3DRANGE_USE_MEMPOOL
902  // Request memory from the memory pool:
904  if (pool)
905  {
907  mem_params.H = H;
908  mem_params.W = W;
909 
911  pool->request_memory(mem_params);
912 
913  if (mem_block)
914  { // Take the memory via swaps:
915  rangeImage.swap(mem_block->rangeImage);
916  delete mem_block;
917  return;
918  }
919  }
920 // otherwise, continue with the normal method:
921 #endif
922  // Fall-back to normal method:
923  rangeImage.setSize(H, W);
924 }
925 
926 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
927 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
929 {
930  static const double EPSILON = 1e-7;
931  static mrpt::poses::CPose3D ref_pose(
932  0, 0, 0, DEG2RAD(-90), 0, DEG2RAD(-90));
933 
934  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
935  .all() &&
936  ((ref_pose.getRotationMatrix() -
938  .array()
939  .abs() < EPSILON)
940  .all();
941 }
942 
946 {
947  out_scan2d.sensorLabel = sensorLabel;
948  out_scan2d.timestamp = this->timestamp;
949 
950  if (!this->hasRangeImage)
951  { // Nothing to do!
952  out_scan2d.resizeScan(0);
953  return;
954  }
955 
956  const size_t nCols = this->rangeImage.cols();
957  const size_t nRows = this->rangeImage.rows();
958  if (fp.rangeMask_min)
959  { // sanity check:
962  }
963  if (fp.rangeMask_max)
964  { // sanity check:
967  }
968 
969  // Compute the real horizontal FOV from the range camera intrinsic calib
970  // data:
971  // Note: this assumes the range image has been "undistorted", which is true
972  // for data
973  // from OpenNI, and will be in the future for libfreenect in MRPT,
974  // but it's
975  // not implemented yet (as of Mar 2012), so this is an approximation
976  // in that case.
977  const double cx = this->cameraParams.cx();
978  const double cy = this->cameraParams.cy();
979  const double fx = this->cameraParams.fx();
980  const double fy = this->cameraParams.fy();
981 
982  // (Imagine the camera seen from above to understand this geometry)
983  const double real_FOV_left = atan2(cx, fx);
984  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
985 
986  // FOV of the equivalent "fake" "laser scanner":
987  const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
988 
989  // Now, we should create more "fake laser" points than columns in the image,
990  // since laser scans are assumed to sample space at evenly-spaced angles,
991  // while in images it is like ~tan(angle).
993  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
994  const auto nLaserRays = static_cast<size_t>(nCols * sp.oversampling_ratio);
995 
996  // Prepare 2D scan data fields:
997  out_scan2d.aperture = FOV_equiv;
998  out_scan2d.maxRange = this->maxRange;
999  out_scan2d.resizeScan(nLaserRays);
1000 
1001  out_scan2d.resizeScanAndAssign(
1002  nLaserRays, 2.0 * this->maxRange,
1003  false); // default: all ranges=invalid
1004  if (sp.use_origin_sensor_pose)
1005  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1006  else
1007  out_scan2d.sensorPose = this->sensorPose;
1008 
1009  // The vertical FOVs given by the user can be translated into limits of the
1010  // tangents (tan>0 means above, i.e. z>0):
1011  const float tan_min = -tan(std::abs(sp.angle_inf));
1012  const float tan_max = tan(std::abs(sp.angle_sup));
1013 
1014  // Precompute the tangents of the vertical angles of each "ray"
1015  // for every row in the range image:
1016  std::vector<float> vert_ang_tan(nRows);
1017  for (size_t r = 0; r < nRows; r++)
1018  vert_ang_tan[r] = static_cast<float>((cy - r) / fy);
1019 
1020  if (!sp.use_origin_sensor_pose)
1021  {
1022  // Algorithm 1: each column in the range image corresponds to a known
1023  // orientation in the 2D scan:
1024  // -------------------
1025  out_scan2d.rightToLeft = false;
1026 
1027  // Angle "counter" for the fake laser scan direction, and the increment:
1028  double ang = -FOV_equiv * 0.5;
1029  const double A_ang = FOV_equiv / (nLaserRays - 1);
1030 
1031  TRangeImageFilter rif(fp);
1032 
1033  // Go thru columns, and keep the minimum distance (along the +X axis,
1034  // not 3D distance!)
1035  // for each direction (i.e. for each column) which also lies within the
1036  // vertical FOV passed
1037  // by the user.
1038  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1039  {
1040  // Equivalent column in the range image for the "i'th" ray:
1041  const double tan_ang = tan(ang);
1042  // make sure we don't go out of range (just in case):
1043  const size_t c = std::min(
1044  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1045  nCols - 1);
1046 
1047  bool any_valid = false;
1048  float closest_range = out_scan2d.maxRange;
1049 
1050  for (size_t r = 0; r < nRows; r++)
1051  {
1052  const float D = this->rangeImage.coeff(r, c);
1053  if (!rif.do_range_filter(r, c, D)) continue;
1054 
1055  // All filters passed:
1056  const float this_point_tan = vert_ang_tan[r] * D;
1057  if (this_point_tan > tan_min && this_point_tan < tan_max)
1058  {
1059  any_valid = true;
1060  mrpt::keep_min(closest_range, D);
1061  }
1062  }
1063 
1064  if (any_valid)
1065  {
1066  out_scan2d.setScanRangeValidity(i, true);
1067  // Compute the distance in 2D from the "depth" in closest_range:
1068  out_scan2d.setScanRange(
1069  i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1070  }
1071  } // end for columns
1072  }
1073  else
1074  {
1075  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1076  // at the origin)
1077  // ------------------------------------------------------------------------
1078  out_scan2d.rightToLeft = true;
1079 
1080  T3DPointsProjectionParams projParams;
1081  projParams.takeIntoAccountSensorPoseOnRobot = true;
1082 
1084  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1085 
1086  const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1087  &zs = pc->getArrayZ();
1088  const size_t N = xs.size();
1089 
1090  const double A_ang = FOV_equiv / (nLaserRays - 1);
1091  const double ang0 = -FOV_equiv * 0.5;
1092 
1093  for (size_t i = 0; i < N; i++)
1094  {
1095  if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;
1096 
1097  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1098 
1099  int i_range = (phi_wrt_origin - ang0) / A_ang;
1100  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1101 
1102  const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1103  if (out_scan2d.scan[i_range] > r_wrt_origin)
1104  out_scan2d.setScanRange(i_range, r_wrt_origin);
1105  out_scan2d.setScanRangeValidity(i_range, true);
1106  }
1107  }
1108 }
1109 
1111 {
1113 
1114  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1115  // memory.
1116 
1117  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1118  "base:\n";
1120  << endl;
1121 
1122  o << "maxRange = " << maxRange << " m" << endl;
1123 
1124  o << "Has 3D point cloud? ";
1125  if (hasPoints3D)
1126  {
1127  o << "YES: " << points3D_x.size() << " points";
1129  o << ". External file: " << points3D_getExternalStorageFile()
1130  << endl;
1131  else
1132  o << " (embedded)." << endl;
1133  }
1134  else
1135  o << "NO" << endl;
1136 
1137  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1138  if (hasRangeImage)
1139  {
1141  o << ". External file: " << rangeImage_getExternalStorageFile()
1142  << endl;
1143  else
1144  o << " (embedded)." << endl;
1145  }
1146 
1147  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1148  if (hasIntensityImage)
1149  {
1151  o << ". External file: " << intensityImage.getExternalStorageFile()
1152  << endl;
1153  else
1154  o << " (embedded).\n";
1155  // Channel?
1156  o << "Source channel: "
1159  value2name(intensityImageChannel)
1160  << endl;
1161  }
1162 
1163  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1164  if (hasConfidenceImage)
1165  {
1167  o << ". External file: " << confidenceImage.getExternalStorageFile()
1168  << endl;
1169  else
1170  o << " (embedded)." << endl;
1171  }
1172 
1173  o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1174  if (hasPixelLabels())
1175  {
1176  o << " Human readable labels:" << endl;
1177  for (auto it = pixelLabels->pixelLabelNames.begin();
1178  it != pixelLabels->pixelLabelNames.end(); ++it)
1179  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1180  }
1181 
1182  o << endl << endl;
1183  o << "Depth camera calibration parameters:" << endl;
1184  {
1185  CConfigFileMemory cfg;
1186  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1187  o << cfg.getContent() << endl;
1188  }
1189  o << endl << "Intensity camera calibration parameters:" << endl;
1190  {
1191  CConfigFileMemory cfg;
1192  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1193  o << cfg.getContent() << endl;
1194  }
1195  o << endl
1196  << endl
1197  << "Pose of the intensity cam. wrt the depth cam:\n"
1201  << endl;
1202 }
1203 
1206 {
1207  const uint8_t version = 1; // for possible future changes.
1208  out << version;
1209  // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the
1210  // object in the class factory later on.
1211  out << BITFIELD_BYTES;
1212  // 2nd: data-specific serialization:
1213  this->internal_writeToStream(out);
1214 }
1215 
1216 template <unsigned int BYTES_REQUIRED_>
1218  BYTES_REQUIRED_>::internal_readFromStream(mrpt::serialization::CArchive& in)
1219 {
1220  {
1221  uint32_t nR, nC;
1222  in >> nR >> nC;
1223  pixelLabels.resize(nR, nC);
1224  for (uint32_t c = 0; c < nC; c++)
1225  for (uint32_t r = 0; r < nR; r++) in >> pixelLabels.coeffRef(r, c);
1226  }
1227  in >> pixelLabelNames;
1228 }
1229 template <unsigned int BYTES_REQUIRED_>
1232 {
1233  {
1234  const auto nR = static_cast<uint32_t>(pixelLabels.rows());
1235  const auto nC = static_cast<uint32_t>(pixelLabels.cols());
1236  out << nR << nC;
1237  for (uint32_t c = 0; c < nC; c++)
1238  for (uint32_t r = 0; r < nR; r++) out << pixelLabels.coeff(r, c);
1239  }
1240  out << pixelLabelNames;
1241 }
1242 
1243 // Deserialization and class factory. All in one, ladies and gentlemen
1246 {
1247  uint8_t version;
1248  in >> version;
1249 
1250  switch (version)
1251  {
1252  case 1:
1253  {
1254  // 1st: Read NUM BYTES
1255  uint8_t bitfield_bytes;
1256  in >> bitfield_bytes;
1257 
1258  // Hand-made class factory. May be a good solution if there will be
1259  // not too many different classes:
1261  switch (bitfield_bytes)
1262  {
1263  case 1:
1265  break;
1266  case 2:
1268  break;
1269  case 3:
1270  case 4:
1272  break;
1273  case 5:
1274  case 6:
1275  case 7:
1276  case 8:
1278  break;
1279  default:
1280  throw std::runtime_error(
1281  "Unknown type of pixelLabel inner class while "
1282  "deserializing!");
1283  };
1284  // 2nd: data-specific serialization:
1285  new_obj->internal_readFromStream(in);
1286 
1287  return new_obj;
1288  }
1289  break;
1290 
1291  default:
1293  break;
1294  };
1295 }
1296 
1298  : angle_sup(mrpt::DEG2RAD(5)),
1299  angle_inf(mrpt::DEG2RAD(5)),
1300  z_min(-std::numeric_limits<double>::max()),
1301  z_max(std::numeric_limits<double>::max())
1302 
1303 {
1304 }
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:37
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Mainly for internal use within CObservation3DRangeScan::project3DPointsFromDepthImageInto() ...
#define MRPT_START
Definition: exceptions.h:241
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)
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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:161
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
Definition: CPose3D.h:96
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)
To be added to 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.
void fill(const Scalar &val)
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.
void swap(CMatrixDynamic< T > &o)
Swap with another matrix very efficiently (just swaps a pointer and two integer values).
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...
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
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:128
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:163
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
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.
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::serialization::CArchive &in)
void setRow(const Index row, const VECTOR &v)
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
signed char int8_t
Definition: rptypes.h:43
STL namespace.
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:768
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:1527
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
void push_back(const T &val)
void swap(CImage &o)
Efficiently swap of two images.
Definition: CImage.cpp:177
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.
unsigned char uint8_t
Definition: rptypes.h:44
const mrpt::math::CMatrixF * rangeMask_max
float maxRange
The maximum range allowed by the device, in meters (e.g.
static Ptr Create(Args &&... args)
Definition: CPointCloud.h:48
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:586
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:40
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double focalLengthMeters
The focal length of the camera, in meters (can be used among &#39;intrinsicParams&#39; to determine the pixel...
Definition: TCamera.h:46
std::string rangeImage_getExternalStorageFileAbsolutePath() const
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:159
CVectorDynamic< double > CVectorDouble
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:137
A helper class that can convert an enum value into its textual representation, and viceversa...
const GLubyte * c
Definition: glext.h:6406
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:373
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:25
std::string getExternalStorageFile() const noexcept
Only if isExternallyStored() returns true.
Definition: img/CImage.h:771
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:98
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...
const mrpt::math::CMatrixF * rangeMask_min
(Default: nullptr) If provided, each data range will be tested to be greater-than (rangeMask_min) or ...
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:4116
size_type rows() const
Number of rows in the matrix.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
size_type cols() const
Number of columns in the matrix.
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:43
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
const Scalar & coeff(int r, int c) const
fixed floating point &#39;f&#39;
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:157
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
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:53
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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:18
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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:115
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
GLuint in
Definition: glext.h:7391
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:1193
void cam2vec(const TCamera &camPar, CVectorDouble &x)
GLenum GLint GLint y
Definition: glext.h:3542
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)
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
GLsizei const GLfloat * value
Definition: glext.h:4134
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
void resize(std::size_t N, bool zeroNewElements=false)
GLenum GLint x
Definition: glext.h:3542
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:224
Lightweight 3D point.
Definition: TPoint3D.h:90
mrpt::config::CConfigFileMemory CConfigFileMemory
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
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:50
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_)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
uint32_t ncols
Camera resolution.
Definition: TCamera.h:37
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
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
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 memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
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.
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019