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 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <mrpt/core/bits_mem.h> // vector_strong_clear
18 #include <mrpt/math/CMatrixF.h>
19 #include <mrpt/math/ops_containers.h> // norm(), etc.
22 #include <mrpt/poses/CPosePDF.h>
25 #include <mrpt/system/filesystem.h>
27 #include <cstring>
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(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg),
171 
172  sensorPose()
173 
174 {
175 }
176 
178 {
179 #ifdef COBS3DRANGE_USE_MEMPOOL
182 #endif
183 }
184 
185 uint8_t CObservation3DRangeScan::serializeGetVersion() const { return 9; }
188 {
189  // The data
190  out << maxRange << sensorPose;
191  out << hasPoints3D;
192  if (hasPoints3D)
193  {
194  ASSERT_(
195  points3D_x.size() == points3D_y.size() &&
196  points3D_x.size() == points3D_z.size() &&
197  points3D_idxs_x.size() == points3D_x.size() &&
198  points3D_idxs_y.size() == points3D_x.size());
199  uint32_t N = points3D_x.size();
200  out << N;
201  if (N)
202  {
203  out.WriteBufferFixEndianness(&points3D_x[0], N);
204  out.WriteBufferFixEndianness(&points3D_y[0], N);
205  out.WriteBufferFixEndianness(&points3D_z[0], N);
206  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
207  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
208  }
209  }
210 
211  out << hasRangeImage;
212  out << rangeUnits; // new in v9
213  if (hasRangeImage)
214  {
215  out.WriteAs<uint32_t>(rangeImage.rows());
216  out.WriteAs<uint32_t>(rangeImage.cols());
217  if (rangeImage.size() != 0)
218  out.WriteBufferFixEndianness<uint16_t>(
220  }
225 
226  out << cameraParams; // New in v2
227  out << cameraParamsIntensity; // New in v4
228  out << relativePoseIntensityWRTDepth; // New in v4
229 
230  out << stdError;
231  out << timestamp;
232  out << sensorLabel;
233 
234  // New in v3:
237 
238  // New in v5:
239  out << range_is_depth;
240 
241  // New in v6:
242  out << static_cast<int8_t>(intensityImageChannel);
243 
244  // New in v7:
245  out << hasPixelLabels();
246  if (hasPixelLabels())
247  {
248  pixelLabels->writeToStream(out);
249  }
250 }
251 
253  mrpt::serialization::CArchive& in, uint8_t version)
254 {
255  switch (version)
256  {
257  case 0:
258  case 1:
259  case 2:
260  case 3:
261  case 4:
262  case 5:
263  case 6:
264  case 7:
265  case 8:
266  case 9:
267  {
268  uint32_t N;
269 
270  in >> maxRange >> sensorPose;
271 
272  if (version > 0)
273  in >> hasPoints3D;
274  else
275  hasPoints3D = true;
276 
277  if (hasPoints3D)
278  {
279  in >> N;
281 
282  if (N)
283  {
287 
288  if (version == 0)
289  {
290  vector<char> validRange(N); // for v0.
291  in.ReadBuffer(
292  &validRange[0], sizeof(validRange[0]) * N);
293  }
294  if (version >= 8)
295  {
298  }
299  }
300  }
301  else
302  {
303  this->resizePoints3DVectors(0);
304  }
305 
306  if (version >= 1)
307  {
308  in >> hasRangeImage;
309  if (version >= 9)
310  in >> rangeUnits;
311  else
312  rangeUnits = 1e-3f; // default units
313 
314  if (hasRangeImage)
315  {
316  if (version < 9)
317  {
318  // Convert from old format:
320  in >> ri;
321  const uint32_t rows = ri.rows(), cols = ri.cols();
322  ASSERT_(rows > 0 && cols > 0);
323 
324  // Call "rangeImage_setSize()" to exploit the mempool:
325  rangeImage_setSize(rows, cols);
326 
327  for (uint32_t r = 0; r < rows; r++)
328  for (uint32_t c = 0; c < cols; c++)
329  rangeImage(r, c) = static_cast<uint16_t>(
330  mrpt::round(ri(r, c) / rangeUnits));
331  }
332  else
333  {
334  const uint32_t rows = in.ReadAs<uint32_t>();
335  const uint32_t cols = in.ReadAs<uint32_t>();
336 
337  // Call "rangeImage_setSize()" to exploit the mempool:
338  rangeImage_setSize(rows, cols);
339 
340  // new v9:
341  in.ReadBufferFixEndianness<uint16_t>(
343  }
344  }
345 
346  in >> hasIntensityImage;
348 
349  in >> hasConfidenceImage;
351 
352  if (version >= 2)
353  {
354  in >> cameraParams;
355 
356  if (version >= 4)
357  {
358  in >> cameraParamsIntensity >>
360  }
361  else
362  {
365  }
366  }
367  }
368 
369  in >> stdError;
370  in >> timestamp;
371  in >> sensorLabel;
372 
373  if (version >= 3)
374  {
375  // New in v3:
379  }
380  else
381  {
384  }
385 
386  if (version >= 5)
387  {
388  in >> range_is_depth;
389  }
390  else
391  {
392  range_is_depth = true;
393  }
394 
395  if (version >= 6)
396  {
397  int8_t i;
398  in >> i;
399  intensityImageChannel = static_cast<TIntensityChannelID>(i);
400  }
401  else
402  {
404  }
405 
406  pixelLabels.reset(); // Remove existing data first (_unique() is to
407  // leave alive any user copies of the shared
408  // pointer).
409  if (version >= 7)
410  {
411  bool do_have_labels;
412  in >> do_have_labels;
413 
414  if (do_have_labels)
415  pixelLabels.reset(
417  }
418  }
419  break;
420  default:
422  };
423 }
424 
426 {
428 
429  std::swap(hasPoints3D, o.hasPoints3D);
430  points3D_x.swap(o.points3D_x);
431  points3D_y.swap(o.points3D_y);
432  points3D_z.swap(o.points3D_z);
437 
438  std::swap(hasRangeImage, o.hasRangeImage);
442 
443  std::swap(hasIntensityImage, o.hasIntensityImage);
446 
449 
450  std::swap(pixelLabels, o.pixelLabels);
451 
453 
454  std::swap(cameraParams, o.cameraParams);
456 
457  std::swap(maxRange, o.maxRange);
458  std::swap(sensorPose, o.sensorPose);
459  std::swap(stdError, o.stdError);
460 }
461 
463 {
465  {
466  const string fil = points3D_getExternalStorageFileAbsolutePath();
468  "txt", mrpt::system::extractFileExtension(fil, true)))
469  {
470  CMatrixFloat M;
471  M.loadFromTextFile(fil);
472  ASSERT_EQUAL_(M.rows(), 3);
473  const auto N = M.cols();
474 
475  auto& xs = const_cast<std::vector<float>&>(points3D_x);
476  auto& ys = const_cast<std::vector<float>&>(points3D_y);
477  auto& zs = const_cast<std::vector<float>&>(points3D_z);
478  xs.resize(N);
479  ys.resize(N);
480  zs.resize(N);
481  std::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
482  std::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
483  std::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
484  }
485  else
486  {
489  f >> const_cast<std::vector<float>&>(points3D_x) >>
490  const_cast<std::vector<float>&>(points3D_y) >>
491  const_cast<std::vector<float>&>(points3D_z);
492  }
493  }
494 
496  {
499  "txt", mrpt::system::extractFileExtension(fil, true)))
500  {
501  const_cast<CMatrix_u16&>(rangeImage).loadFromTextFile(fil);
502  }
503  else
504  {
505  auto& me = const_cast<CObservation3DRangeScan&>(*this);
506 
509  const uint32_t rows = f.ReadAs<uint32_t>();
510  const uint32_t cols = f.ReadAs<uint32_t>();
511  me.rangeImage_setSize(rows, cols);
512  if (me.rangeImage.size() != 0)
513  f.ReadBufferFixEndianness<uint16_t>(
514  me.rangeImage.data(), me.rangeImage.size());
515  }
516  }
517 }
518 
520 {
522  {
526  }
527 
529 
532 }
533 
535  std::string& out_path) const
536 {
538  if (m_rangeImage_external_file[0] == '/' ||
539  (m_rangeImage_external_file[1] == ':' &&
540  m_rangeImage_external_file[2] == '\\'))
541  {
542  out_path = m_rangeImage_external_file;
543  }
544  else
545  {
546  out_path = CImage::getImagesPathBase();
547  size_t N = CImage::getImagesPathBase().size() - 1;
548  if (CImage::getImagesPathBase()[N] != '/' &&
549  CImage::getImagesPathBase()[N] != '\\')
550  out_path += "/";
551  out_path += m_rangeImage_external_file;
552  }
553 }
555  std::string& out_path) const
556 {
557  ASSERT_(m_points3D_external_file.size() > 2);
558  if (m_points3D_external_file[0] == '/' ||
559  (m_points3D_external_file[1] == ':' &&
560  m_points3D_external_file[2] == '\\'))
561  {
562  out_path = m_points3D_external_file;
563  }
564  else
565  {
566  out_path = CImage::getImagesPathBase();
567  size_t N = CImage::getImagesPathBase().size() - 1;
568  if (CImage::getImagesPathBase()[N] != '/' &&
569  CImage::getImagesPathBase()[N] != '\\')
570  out_path += "/";
571  out_path += m_points3D_external_file;
572  }
573 }
574 
576  const std::string& fileName_, const std::string& use_this_base_dir)
577 {
579  ASSERT_(
580  points3D_x.size() == points3D_y.size() &&
581  points3D_x.size() == points3D_z.size());
582 
585  mrpt::system::fileNameChangeExtension(fileName_, "txt");
586  else
588  mrpt::system::fileNameChangeExtension(fileName_, "bin");
589 
590  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
591  // instead of CImage::getImagesPathBase()
592  const string savedDir = CImage::getImagesPathBase();
593  CImage::setImagesPathBase(use_this_base_dir);
594  const string real_absolute_file_path =
596  CImage::setImagesPathBase(savedDir);
597 
599  {
600  const size_t nPts = points3D_x.size();
601 
602  CMatrixFloat M(3, nPts);
603  M.setRow(0, points3D_x);
604  M.setRow(1, points3D_y);
605  M.setRow(2, points3D_z);
606 
607  M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
608  }
609  else
610  {
611  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
613  f << points3D_x << points3D_y << points3D_z;
614  }
615 
617 
618  // Really dealloc memory, clear() is not enough:
624 }
626  const std::string& fileName_, const std::string& use_this_base_dir)
627 {
631  mrpt::system::fileNameChangeExtension(fileName_, "txt");
632  else
634  mrpt::system::fileNameChangeExtension(fileName_, "bin");
635 
636  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
637  // instead of CImage::getImagesPathBase()
638  const string savedDir = CImage::getImagesPathBase();
639  CImage::setImagesPathBase(use_this_base_dir);
640  const string real_absolute_file_path =
642  CImage::setImagesPathBase(savedDir);
643 
645  {
646  rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
647  }
648  else
649  {
650  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
652 
653  f.WriteAs<uint32_t>(rangeImage.rows());
654  f.WriteAs<uint32_t>(rangeImage.cols());
655  if (rangeImage.size() != 0)
656  f.WriteBufferFixEndianness<uint16_t>(
658  }
659 
661  rangeImage.setSize(0, 0);
662 }
663 
664 // ============== Auxiliary function for "recoverCameraCalibrationParameters"
665 // =========================
666 
667 #define CALIB_DECIMAT 15
668 
669 namespace mrpt::obs::detail
670 {
672 {
674  const double z_offset;
675  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
676  : obs(obs_), z_offset(z_offset_)
677  {
678  }
679 };
680 
681 void cam2vec(const TCamera& camPar, CVectorDouble& x)
682 {
683  if (x.size() < 4 + 4) x.resize(4 + 4);
684 
685  x[0] = camPar.fx();
686  x[1] = camPar.fy();
687  x[2] = camPar.cx();
688  x[3] = camPar.cy();
689 
690  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
691 }
692 void vec2cam(const CVectorDouble& x, TCamera& camPar)
693 {
694  camPar.intrinsicParams(0, 0) = x[0]; // fx
695  camPar.intrinsicParams(1, 1) = x[1]; // fy
696  camPar.intrinsicParams(0, 2) = x[2]; // cx
697  camPar.intrinsicParams(1, 2) = x[3]; // cy
698 
699  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
700 }
702  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
703 {
704  const CObservation3DRangeScan& obs = d.obs;
705 
706  TCamera params;
707  vec2cam(par, params);
708 
709  const size_t nC = obs.rangeImage.cols();
710  const size_t nR = obs.rangeImage.rows();
711 
712  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
713 
714  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
715  {
716  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
717  {
718  const size_t idx = nC * r + c;
719 
720  TPoint3D p(
721  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
722  obs.points3D_z[idx]);
723  TPoint3D P(-p.y, -p.z, p.x);
724  TPixelCoordf pixel;
725  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
726  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
727 
728  // Pinhole model:
729  const double x = P.x / P.z;
730  const double y = P.y / P.z;
731 
732  // Radial distortion:
733  const double r2 = square(x) + square(y);
734  const double r4 = square(r2);
735 
736  pixel.x =
737  params.cx() +
738  params.fx() *
739  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
740  2 * params.dist[2] * x * y +
741  params.dist[3] * (r2 + 2 * square(x))));
742  pixel.y =
743  params.cy() +
744  params.fy() *
745  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
746  2 * params.dist[3] * x * y +
747  params.dist[2] * (r2 + 2 * square(y))));
748  }
749 
750  // In theory, it should be (r,c):
751  err.push_back(c - pixel.x);
752  err.push_back(r - pixel.y);
753  }
754  }
755 } // end error_func
756 } // namespace mrpt::obs::detail
757 
758 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
759  * of a 3D camera given a range (depth) image and the corresponding 3D point
760  * cloud.
761  * \param camera_offset The offset (in meters) in the +X direction of the point
762  * cloud. It's 1cm for SwissRanger SR4000.
763  * \return The final average reprojection error per pixel (typ <0.05 px)
764  */
766  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
767  const double camera_offset)
768 {
769  MRPT_START
770 
771  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
772  ASSERT_(
773  obs.points3D_x.size() == obs.points3D_y.size() &&
774  obs.points3D_x.size() == obs.points3D_z.size());
775 
776  using TMyLevMar =
778  TMyLevMar::TResultInfo info;
779 
780  const size_t nR = obs.rangeImage.rows();
781  const size_t nC = obs.rangeImage.cols();
782 
783  TCamera camInit;
784  camInit.ncols = nC;
785  camInit.nrows = nR;
786  camInit.intrinsicParams(0, 0) = 250;
787  camInit.intrinsicParams(1, 1) = 250;
788  camInit.intrinsicParams(0, 2) = nC >> 1;
789  camInit.intrinsicParams(1, 2) = nR >> 1;
790 
791  CVectorDouble initial_x;
792  detail::cam2vec(camInit, initial_x);
793 
794  initial_x.resize(8);
795  CVectorDouble increments_x(initial_x.size());
796  increments_x.fill(1e-4);
797 
798  CVectorDouble optimal_x;
799 
800  TMyLevMar lm;
801  lm.execute(
802  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
803  detail::TLevMarData(obs, camera_offset), info,
804  mrpt::system::LVL_INFO, /* verbose */
805  1000, /* max iter */
806  1e-3, 1e-9, 1e-9, false);
807 
808  const double avr_px_err =
809  sqrt(info.final_sqr_err / double(nC * nR) / square(CALIB_DECIMAT));
810 
811  out_camParams.ncols = nC;
812  out_camParams.nrows = nR;
813  out_camParams.focalLengthMeters = camera_offset;
814  detail::vec2cam(optimal_x, out_camParams);
815 
816  return avr_px_err;
817 
818  MRPT_END
819 }
820 
822  CObservation3DRangeScan& obs, const unsigned int& r1,
823  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
824 {
825  unsigned int cols = cameraParams.ncols;
826  unsigned int rows = cameraParams.nrows;
827 
828  ASSERT_((r1 < r2) && (c1 < c2));
829  ASSERT_((r2 < rows) && (c2 < cols));
830  // Maybe we needed to copy more base obs atributes
831 
832  // Copy zone of range image
834  if (hasRangeImage)
835  obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);
836 
837  // Copy zone of intensity image
840  if (hasIntensityImage)
842  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
843 
844  // Copy zone of confidence image
846  if (hasConfidenceImage)
848  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
849 
850  // Zone labels: It's too complex, just document that pixel labels are NOT
851  // extracted.
852 
853  // Copy zone of scanned points
854  obs.hasPoints3D = hasPoints3D;
855  if (hasPoints3D)
856  {
857  // Erase a possible previous content
858  if (obs.points3D_x.size() > 0)
859  {
860  obs.points3D_x.clear();
861  obs.points3D_y.clear();
862  obs.points3D_z.clear();
863  }
864 
865  for (unsigned int i = r1; i < r2; i++)
866  for (unsigned int j = c1; j < c2; j++)
867  {
868  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
869  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
870  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
871  }
872  }
873 
874  obs.maxRange = maxRange;
875  obs.sensorPose = sensorPose;
876  obs.stdError = stdError;
877 
879 }
880 
881 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
882  * \a points3D_z to allow the usage of the internal memory pool. */
884 {
885 #ifdef COBS3DRANGE_USE_MEMPOOL
886  // If WH=0 this is a clear:
887  if (!WH)
888  {
894  return;
895  }
896 
897  if (WH <= points3D_x.size()) // reduce size, don't realloc
898  {
899  points3D_x.resize(WH);
900  points3D_y.resize(WH);
901  points3D_z.resize(WH);
902  points3D_idxs_x.resize(WH);
903  points3D_idxs_y.resize(WH);
904  return;
905  }
906 
907  // Request memory for the X,Y,Z buffers from the memory pool:
909  if (pool)
910  {
912  mem_params.WH = WH;
913 
915  pool->request_memory(mem_params);
916 
917  if (mem_block)
918  { // Take the memory via swaps:
919  points3D_x.swap(mem_block->pts_x);
920  points3D_y.swap(mem_block->pts_y);
921  points3D_z.swap(mem_block->pts_z);
922  points3D_idxs_x.swap(mem_block->idxs_x);
923  points3D_idxs_y.swap(mem_block->idxs_y);
924  delete mem_block;
925  }
926  }
927 #endif
928 
929  // Either if there was no pool memory or we got it, make sure the size of
930  // vectors is OK:
931  points3D_x.resize(WH);
932  points3D_y.resize(WH);
933  points3D_z.resize(WH);
934  points3D_idxs_x.resize(WH);
935  points3D_idxs_y.resize(WH);
936 }
937 
939 {
940  // x,y,z vectors have the same size.
941  return points3D_x.size();
942 }
943 
944 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
945 // pooling to speed-up the memory allocation.
946 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
947 {
948 #ifdef COBS3DRANGE_USE_MEMPOOL
949  // Request memory from the memory pool:
951  if (pool)
952  {
954  mem_params.H = H;
955  mem_params.W = W;
956 
958  pool->request_memory(mem_params);
959 
960  if (mem_block)
961  { // Take the memory via swaps:
962  rangeImage.swap(mem_block->rangeImage);
963  delete mem_block;
964  return;
965  }
966  }
967 // otherwise, continue with the normal method:
968 #endif
969  // Fall-back to normal method:
970  rangeImage.setSize(H, W);
971 }
972 
973 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
974 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
976 {
977  static const double EPSILON = 1e-7;
978  static mrpt::poses::CPose3D ref_pose(0, 0, 0, -90.0_deg, 0, -90.0_deg);
979 
980  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
981  .all() &&
982  ((ref_pose.getRotationMatrix() -
984  .array()
985  .abs() < EPSILON)
986  .all();
987 }
988 
992 {
993  out_scan2d.sensorLabel = sensorLabel;
994  out_scan2d.timestamp = this->timestamp;
995 
996  if (!this->hasRangeImage)
997  { // Nothing to do!
998  out_scan2d.resizeScan(0);
999  return;
1000  }
1001 
1002  const size_t nCols = this->rangeImage.cols();
1003  const size_t nRows = this->rangeImage.rows();
1004  if (fp.rangeMask_min)
1005  { // sanity check:
1008  }
1009  if (fp.rangeMask_max)
1010  { // sanity check:
1013  }
1014 
1015  // Compute the real horizontal FOV from the range camera intrinsic calib
1016  // data:
1017  // Note: this assumes the range image has been "undistorted", which is true
1018  // for data
1019  // from OpenNI, and will be in the future for libfreenect in MRPT,
1020  // but it's
1021  // not implemented yet (as of Mar 2012), so this is an approximation
1022  // in that case.
1023  const double cx = this->cameraParams.cx();
1024  const double cy = this->cameraParams.cy();
1025  const double fx = this->cameraParams.fx();
1026  const double fy = this->cameraParams.fy();
1027 
1028  // (Imagine the camera seen from above to understand this geometry)
1029  const double real_FOV_left = atan2(cx, fx);
1030  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
1031 
1032  // FOV of the equivalent "fake" "laser scanner":
1033  const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
1034 
1035  // Now, we should create more "fake laser" points than columns in the image,
1036  // since laser scans are assumed to sample space at evenly-spaced angles,
1037  // while in images it is like ~tan(angle).
1038  ASSERT_ABOVE_(
1039  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
1040  const auto nLaserRays = static_cast<size_t>(nCols * sp.oversampling_ratio);
1041 
1042  // Prepare 2D scan data fields:
1043  out_scan2d.aperture = FOV_equiv;
1044  out_scan2d.maxRange = this->maxRange;
1045  out_scan2d.resizeScan(nLaserRays);
1046 
1047  out_scan2d.resizeScanAndAssign(
1048  nLaserRays, 2.0 * this->maxRange,
1049  false); // default: all ranges=invalid
1050  if (sp.use_origin_sensor_pose)
1051  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1052  else
1053  out_scan2d.sensorPose = this->sensorPose;
1054 
1055  // The vertical FOVs given by the user can be translated into limits of the
1056  // tangents (tan>0 means above, i.e. z>0):
1057  const float tan_min = -tan(std::abs(sp.angle_inf));
1058  const float tan_max = tan(std::abs(sp.angle_sup));
1059 
1060  // Precompute the tangents of the vertical angles of each "ray"
1061  // for every row in the range image:
1062  std::vector<float> vert_ang_tan(nRows);
1063  for (size_t r = 0; r < nRows; r++)
1064  vert_ang_tan[r] = static_cast<float>((cy - r) / fy);
1065 
1066  if (!sp.use_origin_sensor_pose)
1067  {
1068  // Algorithm 1: each column in the range image corresponds to a known
1069  // orientation in the 2D scan:
1070  // -------------------
1071  out_scan2d.rightToLeft = false;
1072 
1073  // Angle "counter" for the fake laser scan direction, and the increment:
1074  double ang = -FOV_equiv * 0.5;
1075  const double A_ang = FOV_equiv / (nLaserRays - 1);
1076 
1077  TRangeImageFilter rif(fp);
1078 
1079  // Go thru columns, and keep the minimum distance (along the +X axis,
1080  // not 3D distance!)
1081  // for each direction (i.e. for each column) which also lies within the
1082  // vertical FOV passed
1083  // by the user.
1084  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1085  {
1086  // Equivalent column in the range image for the "i'th" ray:
1087  const double tan_ang = tan(ang);
1088  // make sure we don't go out of range (just in case):
1089  const size_t c = std::min(
1090  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1091  nCols - 1);
1092 
1093  bool any_valid = false;
1094  float closest_range = out_scan2d.maxRange;
1095 
1096  for (size_t r = 0; r < nRows; r++)
1097  {
1098  const float D = rangeImage.coeff(r, c) * rangeUnits;
1099  if (!rif.do_range_filter(r, c, D)) continue;
1100 
1101  // All filters passed:
1102  const float this_point_tan = vert_ang_tan[r] * D;
1103  if (this_point_tan > tan_min && this_point_tan < tan_max)
1104  {
1105  any_valid = true;
1106  mrpt::keep_min(closest_range, D);
1107  }
1108  }
1109 
1110  if (any_valid)
1111  {
1112  out_scan2d.setScanRangeValidity(i, true);
1113  // Compute the distance in 2D from the "depth" in closest_range:
1114  out_scan2d.setScanRange(
1115  i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1116  }
1117  } // end for columns
1118  }
1119  else
1120  {
1121  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1122  // at the origin)
1123  // ------------------------------------------------------------------------
1124  out_scan2d.rightToLeft = true;
1125 
1126  T3DPointsProjectionParams projParams;
1127  projParams.takeIntoAccountSensorPoseOnRobot = true;
1128 
1130  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1131 
1132  const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1133  &zs = pc->getArrayZ();
1134  const size_t N = xs.size();
1135 
1136  const double A_ang = FOV_equiv / (nLaserRays - 1);
1137  const double ang0 = -FOV_equiv * 0.5;
1138 
1139  for (size_t i = 0; i < N; i++)
1140  {
1141  if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;
1142 
1143  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1144 
1145  int i_range = (phi_wrt_origin - ang0) / A_ang;
1146  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1147 
1148  const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1149  if (out_scan2d.getScanRange(i_range) > r_wrt_origin)
1150  out_scan2d.setScanRange(i_range, r_wrt_origin);
1151  out_scan2d.setScanRangeValidity(i_range, true);
1152  }
1153  }
1154 }
1155 
1157 {
1159 
1160  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1161  // memory.
1162 
1163  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1164  "base:\n";
1166  << endl;
1167 
1168  o << "maxRange = " << maxRange << " m" << endl;
1169 
1170  o << "Has 3D point cloud? ";
1171  if (hasPoints3D)
1172  {
1173  o << "YES: " << points3D_x.size() << " points";
1175  o << ". External file: " << points3D_getExternalStorageFile()
1176  << endl;
1177  else
1178  o << " (embedded)." << endl;
1179  }
1180  else
1181  o << "NO" << endl;
1182 
1183  o << "Range is depth: " << (range_is_depth ? "YES" : "NO") << endl;
1184  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1185  if (hasRangeImage)
1186  {
1188  o << ". External file: " << rangeImage_getExternalStorageFile()
1189  << endl;
1190  else
1191  o << " (embedded)." << endl;
1192  }
1193 
1194  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1195  if (hasIntensityImage)
1196  {
1198  o << ". External file: " << intensityImage.getExternalStorageFile()
1199  << endl;
1200  else
1201  o << " (embedded).\n";
1202  // Channel?
1203  o << "Source channel: "
1206  value2name(intensityImageChannel)
1207  << endl;
1208  }
1209 
1210  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1211  if (hasConfidenceImage)
1212  {
1214  o << ". External file: " << confidenceImage.getExternalStorageFile()
1215  << endl;
1216  else
1217  o << " (embedded)." << endl;
1218  }
1219 
1220  o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1221  if (hasPixelLabels())
1222  {
1223  o << " Human readable labels:" << endl;
1224  for (auto it = pixelLabels->pixelLabelNames.begin();
1225  it != pixelLabels->pixelLabelNames.end(); ++it)
1226  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1227  }
1228 
1229  o << endl << endl;
1230  o << "Depth camera calibration parameters:" << endl;
1231  {
1232  CConfigFileMemory cfg;
1233  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1234  o << cfg.getContent() << endl;
1235  }
1236  o << endl << "Intensity camera calibration parameters:" << endl;
1237  {
1238  CConfigFileMemory cfg;
1239  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1240  o << cfg.getContent() << endl;
1241  }
1242  o << endl
1243  << endl
1244  << "Pose of the intensity cam. wrt the depth cam:\n"
1248  << endl;
1249 }
1250 
1252  : angle_sup(mrpt::DEG2RAD(5)),
1253  angle_inf(mrpt::DEG2RAD(5)),
1254  z_min(-std::numeric_limits<double>::max()),
1255  z_max(std::numeric_limits<double>::max())
1256 {
1257 }
1258 
1260 {
1261 #if MRPT_HAS_OPENCV
1262 
1263  // DEPTH image:
1264  {
1265  // OpenCV wrapper (copy-less) for rangeImage:
1266 
1267  cv::Mat rangeImg(
1268  rangeImage.rows(), rangeImage.cols(), CV_16UC1, &rangeImage(0, 0));
1269  const cv::Mat distortion(
1270  1, cameraParams.dist.size(), CV_64F, &cameraParams.dist[0]);
1271  const cv::Mat intrinsics(
1272  3, 3, CV_64F, &cameraParams.intrinsicParams(0, 0));
1273 
1274  const auto imgSize = cv::Size(rangeImage.rows(), rangeImage.cols());
1275 
1276  double alpha = 0; // all depth pixels are visible in the output
1277  const cv::Mat newIntrinsics = cv::getOptimalNewCameraMatrix(
1278  intrinsics, distortion, imgSize, alpha);
1279 
1280  cv::Mat outRangeImg(rangeImage.rows(), rangeImage.cols(), CV_16UC1);
1281 
1282  // Undistort:
1283  const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);
1284 
1285  cv::Mat m1, m2;
1286 
1287  cv::initUndistortRectifyMap(
1288  intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1,
1289  m2);
1290  cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
1291 
1292  // Overwrite:
1293  outRangeImg.copyTo(rangeImg);
1294  cameraParams.dist.fill(0);
1295  for (int r = 0; r < 3; r++)
1296  for (int c = 0; c < 3; c++)
1298  newIntrinsics.at<double>(r, c);
1299  }
1300 
1301  // RGB image:
1302  if (hasIntensityImage)
1303  {
1304  mrpt::img::CImage newIntImg;
1306 
1307  intensityImage = std::move(newIntImg);
1308  cameraParamsIntensity.dist.fill(0);
1309  }
1310 
1311 #else
1312  THROW_EXCEPTION("This method requires OpenCV");
1313 #endif
1314 }
1315 
1317  const std::optional<mrpt::img::TColormap> color,
1318  const std::optional<float> normMinRange,
1319  const std::optional<float> normMaxRange) const
1320 {
1321 #if MRPT_HAS_OPENCV
1322  const float val_min = normMinRange.value_or(.0f);
1323  const float val_max = normMaxRange.value_or(this->maxRange);
1324  ASSERT_ABOVE_(val_max, val_min);
1325 
1326  const float range_inv = rangeUnits / (val_max - val_min);
1327 
1328  ASSERT_(this->hasRangeImage);
1331 
1332  mrpt::img::CImage img;
1333  const int cols = rangeImage.cols(), rows = rangeImage.rows();
1334 
1335  const auto col = color.value_or(mrpt::img::TColormap::cmGRAYSCALE);
1336 
1337  const bool is_gray = (col == mrpt::img::TColormap::cmGRAYSCALE);
1338 
1339  img.resize(cols, rows, is_gray ? mrpt::img::CH_GRAY : mrpt::img::CH_RGB);
1340 
1341  for (int r = 0; r < rows; r++)
1342  {
1343  for (int c = 0; c < cols; c++)
1344  {
1345  // Normalized value in the range [0,1]:
1346  const float val_01 = (rangeImage.coeff(r, c) - val_min) * range_inv;
1347  if (is_gray)
1348  {
1349  img.setPixel(c, r, static_cast<uint8_t>(val_01 * 255));
1350  }
1351  else
1352  {
1353  float R, G, B;
1354  mrpt::img::colormap(col, val_01, R, G, B);
1355 
1356  img.setPixel(
1357  c, r,
1359  static_cast<uint8_t>(R * 255),
1360  static_cast<uint8_t>(G * 255),
1361  static_cast<uint8_t>(B * 255)));
1362  }
1363  }
1364  }
1365 
1366  return img;
1367 
1368 #else
1369  THROW_EXCEPTION("This method requires OpenCV");
1370 #endif
1371 }
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
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...
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
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
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.
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 ...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:174
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:97
const T * data() const
Return raw pointer to row-major data buffer.
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...
const double G
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...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt) const
Builds a visualization from the rangeImage.
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:176
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.
void undistort()
Removes the distortion in both, depth and intensity images.
mrpt::vision::TStereoCalibParams params
void setRow(const Index row, const VECTOR &v)
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
bool isExternallyStored() const noexcept
See setExternalStorage().
Definition: img/CImage.h:770
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:1493
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:171
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.
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:591
A generic system for versatile memory pooling.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:49
#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:55
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:172
CVectorDynamic< double > CVectorDouble
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
T square(const T x)
Inline function for the square of a number.
A helper class that can convert an enum value into its textual representation, and viceversa...
constexpr double DEG2RAD(const double x)
Degrees to radians.
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise...
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::serialization::CArchive &in)
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
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
std::string getExternalStorageFile() const noexcept
Only if isExternallyStored() returns true.
Definition: img/CImage.h:773
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.
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, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:52
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:170
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
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:54
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.
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
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:143
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
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".
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...
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
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:1162
void cam2vec(const TCamera &camPar, CVectorDouble &x)
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
void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
A RGB color - 8bit.
Definition: TColor.h:20
void resize(std::size_t N, bool zeroNewElements=false)
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:225
mrpt::config::CConfigFileMemory CConfigFileMemory
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image...
Definition: CImage.cpp:1600
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)
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
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.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
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...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
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.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020