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 
13 #include <mrpt/core/bits_mem.h> // vector_strong_clear
17 #include <mrpt/math/CMatrixF.h>
18 #include <mrpt/math/ops_containers.h> // norm(), etc.
21 #include <mrpt/otherlibs/do_opencv_includes.h>
22 #include <mrpt/poses/CPosePDF.h>
26 #include <mrpt/system/filesystem.h>
28 #include <cstring>
29 #include <limits>
30 
31 using namespace std;
32 using namespace mrpt::obs;
33 using namespace mrpt::poses;
34 using namespace mrpt::math;
35 using namespace mrpt::img;
37 
38 // This must be added to any CSerializable class implementation file.
40 
41 // Static LUT:
42 static CObservation3DRangeScan::TCached3DProjTables lut_3dproj;
43 CObservation3DRangeScan::TCached3DProjTables&
44  CObservation3DRangeScan::get_3dproj_lut()
45 {
46  return lut_3dproj;
47 }
48 
49 static bool EXTERNALS_AS_TEXT_value = false;
50 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(bool value)
51 {
53 }
54 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
55 {
57 }
58 
59 // Whether to use a memory pool for 3D points:
60 #define COBS3DRANGE_USE_MEMPOOL
61 
62 // Do performance time logging?
63 //#define PROJ3D_PERFLOG
64 
65 // Data types for memory pooling CObservation3DRangeScan:
66 #ifdef COBS3DRANGE_USE_MEMPOOL
67 
69 
70 // Memory pool for XYZ points ----------------
72 {
73  /** Width*Height, that is, the number of 3D points */
74  size_t WH{0};
75  inline bool isSuitable(
77  {
78  return WH >= req.WH;
79  }
80 };
82 {
83  std::vector<float> pts_x, pts_y, pts_z;
84  /** for each point, the corresponding (x,y) pixel coordinates */
85  std::vector<uint16_t> idxs_x, idxs_y;
86 };
90 
91 // Memory pool for the rangeImage matrix ----------------
93 {
94  /** Size of matrix */
95  int H{0}, W{0};
96  inline bool isSuitable(
98  {
99  return H == req.H && W == req.W;
100  }
101 };
103 {
105 };
109 
111 {
112  if (!obs.points3D_x.empty())
113  {
114  // Before dying, donate my memory to the pool for the joy of future
115  // class-brothers...
117  if (pool)
118  {
120  mem_params.WH = obs.points3D_x.capacity();
121  if (obs.points3D_y.capacity() != mem_params.WH)
122  obs.points3D_y.resize(mem_params.WH);
123  if (obs.points3D_z.capacity() != mem_params.WH)
124  obs.points3D_z.resize(mem_params.WH);
125  if (obs.points3D_idxs_x.capacity() != mem_params.WH)
126  obs.points3D_idxs_x.resize(mem_params.WH);
127  if (obs.points3D_idxs_y.capacity() != mem_params.WH)
128  obs.points3D_idxs_y.resize(mem_params.WH);
129 
130  auto* mem_block = new CObservation3DRangeScan_Points_MemPoolData();
131  obs.points3D_x.swap(mem_block->pts_x);
132  obs.points3D_y.swap(mem_block->pts_y);
133  obs.points3D_z.swap(mem_block->pts_z);
134  obs.points3D_idxs_x.swap(mem_block->idxs_x);
135  obs.points3D_idxs_y.swap(mem_block->idxs_y);
136 
137  pool->dump_to_pool(mem_params, mem_block);
138  }
139  }
140 }
142 {
143  if (obs.rangeImage.cols() > 1 && obs.rangeImage.rows() > 1)
144  {
145  // Before dying, donate my memory to the pool for the joy of future
146  // class-brothers...
148  if (pool)
149  {
151  mem_params.H = obs.rangeImage.rows();
152  mem_params.W = obs.rangeImage.cols();
153 
154  auto* mem_block = new CObservation3DRangeScan_Ranges_MemPoolData();
155  obs.rangeImage.swap(mem_block->rangeImage);
156 
157  pool->dump_to_pool(mem_params, mem_block);
158  }
159  }
160 }
161 
162 #endif
163 
164 /*---------------------------------------------------------------
165  Constructor
166  ---------------------------------------------------------------*/
167 CObservation3DRangeScan::CObservation3DRangeScan()
168  : pixelLabels(), // Start without label info
169  cameraParams(),
170  cameraParamsIntensity(),
171  relativePoseIntensityWRTDepth(
172  0, 0, 0, DEG2RAD(-90), DEG2RAD(0), DEG2RAD(-90)),
173 
174  sensorPose()
175 
176 {
177 }
178 
180 {
181 #ifdef COBS3DRANGE_USE_MEMPOOL
184 #endif
185 }
186 
190 {
191  // The data
192  out << maxRange << sensorPose;
193  out << hasPoints3D;
194  if (hasPoints3D)
195  {
196  ASSERT_(
197  points3D_x.size() == points3D_y.size() &&
198  points3D_x.size() == points3D_z.size() &&
199  points3D_idxs_x.size() == points3D_x.size() &&
200  points3D_idxs_y.size() == points3D_x.size());
201  uint32_t N = points3D_x.size();
202  out << N;
203  if (N)
204  {
208  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
209  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
210  }
211  }
212 
213  out << hasRangeImage;
214  if (hasRangeImage) out << rangeImage;
215  out << hasIntensityImage;
216  if (hasIntensityImage) out << intensityImage;
217  out << hasConfidenceImage;
219 
220  out << cameraParams; // New in v2
221  out << cameraParamsIntensity; // New in v4
222  out << relativePoseIntensityWRTDepth; // New in v4
223 
224  out << stdError;
225  out << timestamp;
226  out << sensorLabel;
227 
228  // New in v3:
231 
232  // New in v5:
233  out << range_is_depth;
234 
235  // New in v6:
236  out << static_cast<int8_t>(intensityImageChannel);
237 
238  // New in v7:
239  out << hasPixelLabels();
240  if (hasPixelLabels())
241  {
242  pixelLabels->writeToStream(out);
243  }
244 }
245 
248 {
249  switch (version)
250  {
251  case 0:
252  case 1:
253  case 2:
254  case 3:
255  case 4:
256  case 5:
257  case 6:
258  case 7:
259  case 8:
260  {
261  uint32_t N;
262 
263  in >> maxRange >> sensorPose;
264 
265  if (version > 0)
266  in >> hasPoints3D;
267  else
268  hasPoints3D = true;
269 
270  if (hasPoints3D)
271  {
272  in >> N;
274 
275  if (N)
276  {
277  in.ReadBufferFixEndianness(&points3D_x[0], N);
278  in.ReadBufferFixEndianness(&points3D_y[0], N);
279  in.ReadBufferFixEndianness(&points3D_z[0], N);
280 
281  if (version == 0)
282  {
283  vector<char> validRange(N); // for v0.
284  in.ReadBuffer(
285  &validRange[0], sizeof(validRange[0]) * N);
286  }
287  if (version >= 8)
288  {
289  in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
290  in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
291  }
292  }
293  }
294  else
295  {
296  this->resizePoints3DVectors(0);
297  }
298 
299  if (version >= 1)
300  {
301  in >> hasRangeImage;
302  if (hasRangeImage)
303  {
304 #ifdef COBS3DRANGE_USE_MEMPOOL
305  // We should call "rangeImage_setSize()" to exploit the
306  // mempool:
307  this->rangeImage_setSize(480, 640);
308 #endif
309  in >> rangeImage;
310  }
311 
314 
317 
318  if (version >= 2)
319  {
320  in >> cameraParams;
321 
322  if (version >= 4)
323  {
326  }
327  else
328  {
331  }
332  }
333  }
334 
335  in >> stdError;
336  in >> timestamp;
337  in >> sensorLabel;
338 
339  if (version >= 3)
340  {
341  // New in v3:
345  }
346  else
347  {
350  }
351 
352  if (version >= 5)
353  {
354  in >> range_is_depth;
355  }
356  else
357  {
358  range_is_depth = true;
359  }
360 
361  if (version >= 6)
362  {
363  int8_t i;
364  in >> i;
365  intensityImageChannel = static_cast<TIntensityChannelID>(i);
366  }
367  else
368  {
370  }
371 
372  pixelLabels.reset(); // Remove existing data first (_unique() is to
373  // leave alive any user copies of the shared
374  // pointer).
375  if (version >= 7)
376  {
377  bool do_have_labels;
378  in >> do_have_labels;
379 
380  if (do_have_labels)
381  pixelLabels.reset(
383  }
384  }
385  break;
386  default:
388  };
389 }
390 
392 {
394 
395  std::swap(hasPoints3D, o.hasPoints3D);
396  points3D_x.swap(o.points3D_x);
397  points3D_y.swap(o.points3D_y);
398  points3D_z.swap(o.points3D_z);
403 
404  std::swap(hasRangeImage, o.hasRangeImage);
408 
409  std::swap(hasIntensityImage, o.hasIntensityImage);
412 
415 
416  std::swap(pixelLabels, o.pixelLabels);
417 
419 
420  std::swap(cameraParams, o.cameraParams);
422 
423  std::swap(maxRange, o.maxRange);
424  std::swap(sensorPose, o.sensorPose);
425  std::swap(stdError, o.stdError);
426 }
427 
429 {
431  {
432  const string fil = points3D_getExternalStorageFileAbsolutePath();
434  "txt", mrpt::system::extractFileExtension(fil, true)))
435  {
436  CMatrixFloat M;
437  M.loadFromTextFile(fil);
438  ASSERT_EQUAL_(M.rows(), 3);
439  const auto N = M.cols();
440 
441  auto& xs = const_cast<std::vector<float>&>(points3D_x);
442  auto& ys = const_cast<std::vector<float>&>(points3D_y);
443  auto& zs = const_cast<std::vector<float>&>(points3D_z);
444  xs.resize(N);
445  ys.resize(N);
446  zs.resize(N);
447  std::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
448  std::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
449  std::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
450  }
451  else
452  {
455  f >> const_cast<std::vector<float>&>(points3D_x) >>
456  const_cast<std::vector<float>&>(points3D_y) >>
457  const_cast<std::vector<float>&>(points3D_z);
458  }
459  }
460 
462  {
465  "txt", mrpt::system::extractFileExtension(fil, true)))
466  {
467  const_cast<CMatrixF&>(rangeImage).loadFromTextFile(fil);
468  }
469  else
470  {
473  f >> const_cast<CMatrixF&>(rangeImage);
474  }
475  }
476 }
477 
479 {
481  {
485  }
486 
488 
491 }
492 
494  std::string& out_path) const
495 {
497  if (m_rangeImage_external_file[0] == '/' ||
498  (m_rangeImage_external_file[1] == ':' &&
499  m_rangeImage_external_file[2] == '\\'))
500  {
501  out_path = m_rangeImage_external_file;
502  }
503  else
504  {
505  out_path = CImage::getImagesPathBase();
506  size_t N = CImage::getImagesPathBase().size() - 1;
507  if (CImage::getImagesPathBase()[N] != '/' &&
508  CImage::getImagesPathBase()[N] != '\\')
509  out_path += "/";
510  out_path += m_rangeImage_external_file;
511  }
512 }
514  std::string& out_path) const
515 {
516  ASSERT_(m_points3D_external_file.size() > 2);
517  if (m_points3D_external_file[0] == '/' ||
518  (m_points3D_external_file[1] == ':' &&
519  m_points3D_external_file[2] == '\\'))
520  {
521  out_path = m_points3D_external_file;
522  }
523  else
524  {
525  out_path = CImage::getImagesPathBase();
526  size_t N = CImage::getImagesPathBase().size() - 1;
527  if (CImage::getImagesPathBase()[N] != '/' &&
528  CImage::getImagesPathBase()[N] != '\\')
529  out_path += "/";
530  out_path += m_points3D_external_file;
531  }
532 }
533 
535  const std::string& fileName_, const std::string& use_this_base_dir)
536 {
538  ASSERT_(
539  points3D_x.size() == points3D_y.size() &&
540  points3D_x.size() == points3D_z.size());
541 
544  mrpt::system::fileNameChangeExtension(fileName_, "txt");
545  else
547  mrpt::system::fileNameChangeExtension(fileName_, "bin");
548 
549  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
550  // instead of CImage::getImagesPathBase()
551  const string savedDir = CImage::getImagesPathBase();
552  CImage::setImagesPathBase(use_this_base_dir);
553  const string real_absolute_file_path =
555  CImage::setImagesPathBase(savedDir);
556 
558  {
559  const size_t nPts = points3D_x.size();
560 
561  CMatrixFloat M(3, nPts);
562  M.setRow(0, points3D_x);
563  M.setRow(1, points3D_y);
564  M.setRow(2, points3D_z);
565 
566  M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
567  }
568  else
569  {
570  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
572  f << points3D_x << points3D_y << points3D_z;
573  }
574 
576 
577  // Really dealloc memory, clear() is not enough:
583 }
585  const std::string& fileName_, const std::string& use_this_base_dir)
586 {
590  mrpt::system::fileNameChangeExtension(fileName_, "txt");
591  else
593  mrpt::system::fileNameChangeExtension(fileName_, "bin");
594 
595  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
596  // instead of CImage::getImagesPathBase()
597  const string savedDir = CImage::getImagesPathBase();
598  CImage::setImagesPathBase(use_this_base_dir);
599  const string real_absolute_file_path =
601  CImage::setImagesPathBase(savedDir);
602 
604  {
605  rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
606  }
607  else
608  {
609  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
611  f << rangeImage;
612  }
613 
615  rangeImage.setSize(0, 0);
616 }
617 
618 // ============== Auxiliary function for "recoverCameraCalibrationParameters"
619 // =========================
620 
621 #define CALIB_DECIMAT 15
622 
623 namespace mrpt::obs::detail
624 {
626 {
628  const double z_offset;
629  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
630  : obs(obs_), z_offset(z_offset_)
631  {
632  }
633 };
634 
635 void cam2vec(const TCamera& camPar, CVectorDouble& x)
636 {
637  if (x.size() < 4 + 4) x.resize(4 + 4);
638 
639  x[0] = camPar.fx();
640  x[1] = camPar.fy();
641  x[2] = camPar.cx();
642  x[3] = camPar.cy();
643 
644  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
645 }
646 void vec2cam(const CVectorDouble& x, TCamera& camPar)
647 {
648  camPar.intrinsicParams(0, 0) = x[0]; // fx
649  camPar.intrinsicParams(1, 1) = x[1]; // fy
650  camPar.intrinsicParams(0, 2) = x[2]; // cx
651  camPar.intrinsicParams(1, 2) = x[3]; // cy
652 
653  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
654 }
656  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
657 {
658  const CObservation3DRangeScan& obs = d.obs;
659 
660  TCamera params;
661  vec2cam(par, params);
662 
663  const size_t nC = obs.rangeImage.cols();
664  const size_t nR = obs.rangeImage.rows();
665 
666  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
667 
668  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
669  {
670  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
671  {
672  const size_t idx = nC * r + c;
673 
674  TPoint3D p(
675  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
676  obs.points3D_z[idx]);
677  TPoint3D P(-p.y, -p.z, p.x);
678  TPixelCoordf pixel;
679  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
680  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
681 
682  // Pinhole model:
683  const double x = P.x / P.z;
684  const double y = P.y / P.z;
685 
686  // Radial distortion:
687  const double r2 = square(x) + square(y);
688  const double r4 = square(r2);
689 
690  pixel.x =
691  params.cx() +
692  params.fx() *
693  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
694  2 * params.dist[2] * x * y +
695  params.dist[3] * (r2 + 2 * square(x))));
696  pixel.y =
697  params.cy() +
698  params.fy() *
699  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
700  2 * params.dist[3] * x * y +
701  params.dist[2] * (r2 + 2 * square(y))));
702  }
703 
704  // In theory, it should be (r,c):
705  err.push_back(c - pixel.x);
706  err.push_back(r - pixel.y);
707  }
708  }
709 } // end error_func
710 } // namespace mrpt::obs::detail
711 
712 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
713  * of a 3D camera given a range (depth) image and the corresponding 3D point
714  * cloud.
715  * \param camera_offset The offset (in meters) in the +X direction of the point
716  * cloud. It's 1cm for SwissRanger SR4000.
717  * \return The final average reprojection error per pixel (typ <0.05 px)
718  */
720  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
721  const double camera_offset)
722 {
723  MRPT_START
724 
725  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
726  ASSERT_(
727  obs.points3D_x.size() == obs.points3D_y.size() &&
728  obs.points3D_x.size() == obs.points3D_z.size());
729 
730  using TMyLevMar =
732  TMyLevMar::TResultInfo info;
733 
734  const size_t nR = obs.rangeImage.rows();
735  const size_t nC = obs.rangeImage.cols();
736 
737  TCamera camInit;
738  camInit.ncols = nC;
739  camInit.nrows = nR;
740  camInit.intrinsicParams(0, 0) = 250;
741  camInit.intrinsicParams(1, 1) = 250;
742  camInit.intrinsicParams(0, 2) = nC >> 1;
743  camInit.intrinsicParams(1, 2) = nR >> 1;
744 
745  CVectorDouble initial_x;
746  detail::cam2vec(camInit, initial_x);
747 
748  initial_x.resize(8);
749  CVectorDouble increments_x(initial_x.size());
750  increments_x.fill(1e-4);
751 
752  CVectorDouble optimal_x;
753 
754  TMyLevMar lm;
755  lm.execute(
756  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
757  detail::TLevMarData(obs, camera_offset), info,
758  mrpt::system::LVL_INFO, /* verbose */
759  1000, /* max iter */
760  1e-3, 1e-9, 1e-9, false);
761 
762  const double avr_px_err =
763  sqrt(info.final_sqr_err / double(nC * nR) / square(CALIB_DECIMAT));
764 
765  out_camParams.ncols = nC;
766  out_camParams.nrows = nR;
767  out_camParams.focalLengthMeters = camera_offset;
768  detail::vec2cam(optimal_x, out_camParams);
769 
770  return avr_px_err;
771 
772  MRPT_END
773 }
774 
776  CObservation3DRangeScan& obs, const unsigned int& r1,
777  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
778 {
779  unsigned int cols = cameraParams.ncols;
780  unsigned int rows = cameraParams.nrows;
781 
782  ASSERT_((r1 < r2) && (c1 < c2));
783  ASSERT_((r2 < rows) && (c2 < cols));
784  // Maybe we needed to copy more base obs atributes
785 
786  // Copy zone of range image
788  if (hasRangeImage)
789  obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);
790 
791  // Copy zone of intensity image
794  if (hasIntensityImage)
796  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
797 
798  // Copy zone of confidence image
800  if (hasConfidenceImage)
802  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
803 
804  // Zone labels: It's too complex, just document that pixel labels are NOT
805  // extracted.
806 
807  // Copy zone of scanned points
808  obs.hasPoints3D = hasPoints3D;
809  if (hasPoints3D)
810  {
811  // Erase a possible previous content
812  if (obs.points3D_x.size() > 0)
813  {
814  obs.points3D_x.clear();
815  obs.points3D_y.clear();
816  obs.points3D_z.clear();
817  }
818 
819  for (unsigned int i = r1; i < r2; i++)
820  for (unsigned int j = c1; j < c2; j++)
821  {
822  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
823  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
824  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
825  }
826  }
827 
828  obs.maxRange = maxRange;
829  obs.sensorPose = sensorPose;
830  obs.stdError = stdError;
831 
833 }
834 
835 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
836  * \a points3D_z to allow the usage of the internal memory pool. */
838 {
839 #ifdef COBS3DRANGE_USE_MEMPOOL
840  // If WH=0 this is a clear:
841  if (!WH)
842  {
848  return;
849  }
850 
851  if (WH <= points3D_x.size()) // reduce size, don't realloc
852  {
853  points3D_x.resize(WH);
854  points3D_y.resize(WH);
855  points3D_z.resize(WH);
856  points3D_idxs_x.resize(WH);
857  points3D_idxs_y.resize(WH);
858  return;
859  }
860 
861  // Request memory for the X,Y,Z buffers from the memory pool:
863  if (pool)
864  {
866  mem_params.WH = WH;
867 
869  pool->request_memory(mem_params);
870 
871  if (mem_block)
872  { // Take the memory via swaps:
873  points3D_x.swap(mem_block->pts_x);
874  points3D_y.swap(mem_block->pts_y);
875  points3D_z.swap(mem_block->pts_z);
876  points3D_idxs_x.swap(mem_block->idxs_x);
877  points3D_idxs_y.swap(mem_block->idxs_y);
878  delete mem_block;
879  }
880  }
881 #endif
882 
883  // Either if there was no pool memory or we got it, make sure the size of
884  // vectors is OK:
885  points3D_x.resize(WH);
886  points3D_y.resize(WH);
887  points3D_z.resize(WH);
888  points3D_idxs_x.resize(WH);
889  points3D_idxs_y.resize(WH);
890 }
891 
893 {
894  // x,y,z vectors have the same size.
895  return points3D_x.size();
896 }
897 
898 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
899 // pooling to speed-up the memory allocation.
900 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
901 {
902 #ifdef COBS3DRANGE_USE_MEMPOOL
903  // Request memory from the memory pool:
905  if (pool)
906  {
908  mem_params.H = H;
909  mem_params.W = W;
910 
912  pool->request_memory(mem_params);
913 
914  if (mem_block)
915  { // Take the memory via swaps:
916  rangeImage.swap(mem_block->rangeImage);
917  delete mem_block;
918  return;
919  }
920  }
921 // otherwise, continue with the normal method:
922 #endif
923  // Fall-back to normal method:
924  rangeImage.setSize(H, W);
925 }
926 
927 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
928 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
930 {
931  static const double EPSILON = 1e-7;
932  static mrpt::poses::CPose3D ref_pose(
933  0, 0, 0, DEG2RAD(-90), 0, DEG2RAD(-90));
934 
935  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
936  .all() &&
937  ((ref_pose.getRotationMatrix() -
939  .array()
940  .abs() < EPSILON)
941  .all();
942 }
943 
947 {
948  out_scan2d.sensorLabel = sensorLabel;
949  out_scan2d.timestamp = this->timestamp;
950 
951  if (!this->hasRangeImage)
952  { // Nothing to do!
953  out_scan2d.resizeScan(0);
954  return;
955  }
956 
957  const size_t nCols = this->rangeImage.cols();
958  const size_t nRows = this->rangeImage.rows();
959  if (fp.rangeMask_min)
960  { // sanity check:
963  }
964  if (fp.rangeMask_max)
965  { // sanity check:
968  }
969 
970  // Compute the real horizontal FOV from the range camera intrinsic calib
971  // data:
972  // Note: this assumes the range image has been "undistorted", which is true
973  // for data
974  // from OpenNI, and will be in the future for libfreenect in MRPT,
975  // but it's
976  // not implemented yet (as of Mar 2012), so this is an approximation
977  // in that case.
978  const double cx = this->cameraParams.cx();
979  const double cy = this->cameraParams.cy();
980  const double fx = this->cameraParams.fx();
981  const double fy = this->cameraParams.fy();
982 
983  // (Imagine the camera seen from above to understand this geometry)
984  const double real_FOV_left = atan2(cx, fx);
985  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
986 
987  // FOV of the equivalent "fake" "laser scanner":
988  const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
989 
990  // Now, we should create more "fake laser" points than columns in the image,
991  // since laser scans are assumed to sample space at evenly-spaced angles,
992  // while in images it is like ~tan(angle).
994  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
995  const auto nLaserRays = static_cast<size_t>(nCols * sp.oversampling_ratio);
996 
997  // Prepare 2D scan data fields:
998  out_scan2d.aperture = FOV_equiv;
999  out_scan2d.maxRange = this->maxRange;
1000  out_scan2d.resizeScan(nLaserRays);
1001 
1002  out_scan2d.resizeScanAndAssign(
1003  nLaserRays, 2.0 * this->maxRange,
1004  false); // default: all ranges=invalid
1005  if (sp.use_origin_sensor_pose)
1006  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1007  else
1008  out_scan2d.sensorPose = this->sensorPose;
1009 
1010  // The vertical FOVs given by the user can be translated into limits of the
1011  // tangents (tan>0 means above, i.e. z>0):
1012  const float tan_min = -tan(std::abs(sp.angle_inf));
1013  const float tan_max = tan(std::abs(sp.angle_sup));
1014 
1015  // Precompute the tangents of the vertical angles of each "ray"
1016  // for every row in the range image:
1017  std::vector<float> vert_ang_tan(nRows);
1018  for (size_t r = 0; r < nRows; r++)
1019  vert_ang_tan[r] = static_cast<float>((cy - r) / fy);
1020 
1021  if (!sp.use_origin_sensor_pose)
1022  {
1023  // Algorithm 1: each column in the range image corresponds to a known
1024  // orientation in the 2D scan:
1025  // -------------------
1026  out_scan2d.rightToLeft = false;
1027 
1028  // Angle "counter" for the fake laser scan direction, and the increment:
1029  double ang = -FOV_equiv * 0.5;
1030  const double A_ang = FOV_equiv / (nLaserRays - 1);
1031 
1032  TRangeImageFilter rif(fp);
1033 
1034  // Go thru columns, and keep the minimum distance (along the +X axis,
1035  // not 3D distance!)
1036  // for each direction (i.e. for each column) which also lies within the
1037  // vertical FOV passed
1038  // by the user.
1039  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1040  {
1041  // Equivalent column in the range image for the "i'th" ray:
1042  const double tan_ang = tan(ang);
1043  // make sure we don't go out of range (just in case):
1044  const size_t c = std::min(
1045  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1046  nCols - 1);
1047 
1048  bool any_valid = false;
1049  float closest_range = out_scan2d.maxRange;
1050 
1051  for (size_t r = 0; r < nRows; r++)
1052  {
1053  const float D = this->rangeImage.coeff(r, c);
1054  if (!rif.do_range_filter(r, c, D)) continue;
1055 
1056  // All filters passed:
1057  const float this_point_tan = vert_ang_tan[r] * D;
1058  if (this_point_tan > tan_min && this_point_tan < tan_max)
1059  {
1060  any_valid = true;
1061  mrpt::keep_min(closest_range, D);
1062  }
1063  }
1064 
1065  if (any_valid)
1066  {
1067  out_scan2d.setScanRangeValidity(i, true);
1068  // Compute the distance in 2D from the "depth" in closest_range:
1069  out_scan2d.setScanRange(
1070  i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1071  }
1072  } // end for columns
1073  }
1074  else
1075  {
1076  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1077  // at the origin)
1078  // ------------------------------------------------------------------------
1079  out_scan2d.rightToLeft = true;
1080 
1081  T3DPointsProjectionParams projParams;
1082  projParams.takeIntoAccountSensorPoseOnRobot = true;
1083 
1085  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1086 
1087  const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1088  &zs = pc->getArrayZ();
1089  const size_t N = xs.size();
1090 
1091  const double A_ang = FOV_equiv / (nLaserRays - 1);
1092  const double ang0 = -FOV_equiv * 0.5;
1093 
1094  for (size_t i = 0; i < N; i++)
1095  {
1096  if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;
1097 
1098  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1099 
1100  int i_range = (phi_wrt_origin - ang0) / A_ang;
1101  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1102 
1103  const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1104  if (out_scan2d.scan[i_range] > r_wrt_origin)
1105  out_scan2d.setScanRange(i_range, r_wrt_origin);
1106  out_scan2d.setScanRangeValidity(i_range, true);
1107  }
1108  }
1109 }
1110 
1112 {
1114 
1115  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1116  // memory.
1117 
1118  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1119  "base:\n";
1121  << endl;
1122 
1123  o << "maxRange = " << maxRange << " m" << endl;
1124 
1125  o << "Has 3D point cloud? ";
1126  if (hasPoints3D)
1127  {
1128  o << "YES: " << points3D_x.size() << " points";
1130  o << ". External file: " << points3D_getExternalStorageFile()
1131  << endl;
1132  else
1133  o << " (embedded)." << endl;
1134  }
1135  else
1136  o << "NO" << endl;
1137 
1138  o << "Range is depth: " << (range_is_depth ? "YES" : "NO") << endl;
1139  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1140  if (hasRangeImage)
1141  {
1143  o << ". External file: " << rangeImage_getExternalStorageFile()
1144  << endl;
1145  else
1146  o << " (embedded)." << endl;
1147  }
1148 
1149  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1150  if (hasIntensityImage)
1151  {
1153  o << ". External file: " << intensityImage.getExternalStorageFile()
1154  << endl;
1155  else
1156  o << " (embedded).\n";
1157  // Channel?
1158  o << "Source channel: "
1161  value2name(intensityImageChannel)
1162  << endl;
1163  }
1164 
1165  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1166  if (hasConfidenceImage)
1167  {
1169  o << ". External file: " << confidenceImage.getExternalStorageFile()
1170  << endl;
1171  else
1172  o << " (embedded)." << endl;
1173  }
1174 
1175  o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1176  if (hasPixelLabels())
1177  {
1178  o << " Human readable labels:" << endl;
1179  for (auto it = pixelLabels->pixelLabelNames.begin();
1180  it != pixelLabels->pixelLabelNames.end(); ++it)
1181  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1182  }
1183 
1184  o << endl << endl;
1185  o << "Depth camera calibration parameters:" << endl;
1186  {
1187  CConfigFileMemory cfg;
1188  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1189  o << cfg.getContent() << endl;
1190  }
1191  o << endl << "Intensity camera calibration parameters:" << endl;
1192  {
1193  CConfigFileMemory cfg;
1194  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1195  o << cfg.getContent() << endl;
1196  }
1197  o << endl
1198  << endl
1199  << "Pose of the intensity cam. wrt the depth cam:\n"
1203  << endl;
1204 }
1205 
1208 {
1209  const uint8_t version = 1; // for possible future changes.
1210  out << version;
1211  // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the
1212  // object in the class factory later on.
1213  out << BITFIELD_BYTES;
1214  // 2nd: data-specific serialization:
1215  this->internal_writeToStream(out);
1216 }
1217 
1218 template <unsigned int BYTES_REQUIRED_>
1220  BYTES_REQUIRED_>::internal_readFromStream(mrpt::serialization::CArchive& in)
1221 {
1222  {
1223  uint32_t nR, nC;
1224  in >> nR >> nC;
1225  pixelLabels.resize(nR, nC);
1226  for (uint32_t c = 0; c < nC; c++)
1227  for (uint32_t r = 0; r < nR; r++) in >> pixelLabels.coeffRef(r, c);
1228  }
1229  in >> pixelLabelNames;
1230 }
1231 template <unsigned int BYTES_REQUIRED_>
1234 {
1235  {
1236  const auto nR = static_cast<uint32_t>(pixelLabels.rows());
1237  const auto nC = static_cast<uint32_t>(pixelLabels.cols());
1238  out << nR << nC;
1239  for (uint32_t c = 0; c < nC; c++)
1240  for (uint32_t r = 0; r < nR; r++) out << pixelLabels.coeff(r, c);
1241  }
1242  out << pixelLabelNames;
1243 }
1244 
1245 // Deserialization and class factory. All in one, ladies and gentlemen
1248 {
1249  uint8_t version;
1250  in >> version;
1251 
1252  switch (version)
1253  {
1254  case 1:
1255  {
1256  // 1st: Read NUM BYTES
1257  uint8_t bitfield_bytes;
1258  in >> bitfield_bytes;
1259 
1260  // Hand-made class factory. May be a good solution if there will be
1261  // not too many different classes:
1263  switch (bitfield_bytes)
1264  {
1265  case 1:
1267  break;
1268  case 2:
1270  break;
1271  case 3:
1272  case 4:
1274  break;
1275  case 5:
1276  case 6:
1277  case 7:
1278  case 8:
1280  break;
1281  default:
1282  throw std::runtime_error(
1283  "Unknown type of pixelLabel inner class while "
1284  "deserializing!");
1285  };
1286  // 2nd: data-specific serialization:
1287  new_obj->internal_readFromStream(in);
1288 
1289  return new_obj;
1290  }
1291  break;
1292 
1293  default:
1295  break;
1296  };
1297 }
1298 
1300  : angle_sup(mrpt::DEG2RAD(5)),
1301  angle_inf(mrpt::DEG2RAD(5)),
1302  z_min(-std::numeric_limits<double>::max()),
1303  z_max(std::numeric_limits<double>::max())
1304 {
1305 }
1306 
1308 {
1309 #if MRPT_HAS_OPENCV
1310 
1311  // DEPTH image:
1312  {
1313  // OpenCV wrapper (copy-less) for rangeImage:
1314 
1315  cv::Mat rangeImg(
1316  rangeImage.rows(), rangeImage.cols(), CV_32FC1, &rangeImage(0, 0));
1317  const cv::Mat distortion(
1318  1, cameraParams.dist.size(), CV_64F, &cameraParams.dist[0]);
1319  const cv::Mat intrinsics(
1320  3, 3, CV_64F, &cameraParams.intrinsicParams(0, 0));
1321 
1322  const auto imgSize = cv::Size(rangeImage.rows(), rangeImage.cols());
1323 
1324  double alpha = 0; // all depth pixels are visible in the output
1325  const cv::Mat newIntrinsics = cv::getOptimalNewCameraMatrix(
1326  intrinsics, distortion, imgSize, alpha);
1327 
1328  cv::Mat outRangeImg(rangeImage.rows(), rangeImage.cols(), CV_32FC1);
1329 
1330  // Undistort:
1331  const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);
1332 
1333  cv::Mat m1, m2;
1334 
1335  cv::initUndistortRectifyMap(
1336  intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1,
1337  m2);
1338  cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
1339 
1340  // Overwrite:
1341  outRangeImg.copyTo(rangeImg);
1342  cameraParams.dist.fill(0);
1343  for (int r = 0; r < 3; r++)
1344  for (int c = 0; c < 3; c++)
1346  newIntrinsics.at<double>(r, c);
1347  }
1348 
1349  // RGB image:
1350  if (hasIntensityImage)
1351  {
1352  mrpt::img::CImage newIntImg;
1354 
1355  intensityImage = std::move(newIntImg);
1356  cameraParamsIntensity.dist.fill(0);
1357  }
1358 
1359 #else
1360  THROW_EXCEPTION("This method requires OpenCV");
1361 #endif
1362 }
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.
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3529
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 ...
#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:172
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:129
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:174
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.
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:770
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:1525
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:587
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:47
#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:53
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:170
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
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
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.
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, 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:50
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:168
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
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:137
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:1192
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.
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:1632
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...
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019