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(0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg),
172 
173  sensorPose()
174 
175 {
176 }
177 
179 {
180 #ifdef COBS3DRANGE_USE_MEMPOOL
183 #endif
184 }
185 
186 uint8_t CObservation3DRangeScan::serializeGetVersion() const { return 8; }
189 {
190  // The data
191  out << maxRange << sensorPose;
192  out << hasPoints3D;
193  if (hasPoints3D)
194  {
195  ASSERT_(
196  points3D_x.size() == points3D_y.size() &&
197  points3D_x.size() == points3D_z.size() &&
198  points3D_idxs_x.size() == points3D_x.size() &&
199  points3D_idxs_y.size() == points3D_x.size());
200  uint32_t N = points3D_x.size();
201  out << N;
202  if (N)
203  {
204  out.WriteBufferFixEndianness(&points3D_x[0], N);
205  out.WriteBufferFixEndianness(&points3D_y[0], N);
206  out.WriteBufferFixEndianness(&points3D_z[0], N);
207  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
208  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
209  }
210  }
211 
212  out << hasRangeImage;
213  if (hasRangeImage) out << rangeImage;
218 
219  out << cameraParams; // New in v2
220  out << cameraParamsIntensity; // New in v4
221  out << relativePoseIntensityWRTDepth; // New in v4
222 
223  out << stdError;
224  out << timestamp;
225  out << sensorLabel;
226 
227  // New in v3:
230 
231  // New in v5:
232  out << range_is_depth;
233 
234  // New in v6:
235  out << static_cast<int8_t>(intensityImageChannel);
236 
237  // New in v7:
238  out << hasPixelLabels();
239  if (hasPixelLabels())
240  {
241  pixelLabels->writeToStream(out);
242  }
243 }
244 
246  mrpt::serialization::CArchive& in, uint8_t version)
247 {
248  switch (version)
249  {
250  case 0:
251  case 1:
252  case 2:
253  case 3:
254  case 4:
255  case 5:
256  case 6:
257  case 7:
258  case 8:
259  {
260  uint32_t N;
261 
262  in >> maxRange >> sensorPose;
263 
264  if (version > 0)
265  in >> hasPoints3D;
266  else
267  hasPoints3D = true;
268 
269  if (hasPoints3D)
270  {
271  in >> N;
273 
274  if (N)
275  {
276  in.ReadBufferFixEndianness(&points3D_x[0], N);
277  in.ReadBufferFixEndianness(&points3D_y[0], N);
278  in.ReadBufferFixEndianness(&points3D_z[0], N);
279 
280  if (version == 0)
281  {
282  vector<char> validRange(N); // for v0.
283  in.ReadBuffer(
284  &validRange[0], sizeof(validRange[0]) * N);
285  }
286  if (version >= 8)
287  {
288  in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
289  in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
290  }
291  }
292  }
293  else
294  {
295  this->resizePoints3DVectors(0);
296  }
297 
298  if (version >= 1)
299  {
300  in >> hasRangeImage;
301  if (hasRangeImage)
302  {
303 #ifdef COBS3DRANGE_USE_MEMPOOL
304  // We should call "rangeImage_setSize()" to exploit the
305  // mempool:
306  this->rangeImage_setSize(480, 640);
307 #endif
308  in >> rangeImage;
309  }
310 
313 
316 
317  if (version >= 2)
318  {
319  in >> cameraParams;
320 
321  if (version >= 4)
322  {
325  }
326  else
327  {
330  }
331  }
332  }
333 
334  in >> stdError;
335  in >> timestamp;
336  in >> sensorLabel;
337 
338  if (version >= 3)
339  {
340  // New in v3:
344  }
345  else
346  {
349  }
350 
351  if (version >= 5)
352  {
353  in >> range_is_depth;
354  }
355  else
356  {
357  range_is_depth = true;
358  }
359 
360  if (version >= 6)
361  {
362  int8_t i;
363  in >> i;
364  intensityImageChannel = static_cast<TIntensityChannelID>(i);
365  }
366  else
367  {
369  }
370 
371  pixelLabels.reset(); // Remove existing data first (_unique() is to
372  // leave alive any user copies of the shared
373  // pointer).
374  if (version >= 7)
375  {
376  bool do_have_labels;
377  in >> do_have_labels;
378 
379  if (do_have_labels)
380  pixelLabels.reset(
382  }
383  }
384  break;
385  default:
387  };
388 }
389 
391 {
393 
394  std::swap(hasPoints3D, o.hasPoints3D);
395  points3D_x.swap(o.points3D_x);
396  points3D_y.swap(o.points3D_y);
397  points3D_z.swap(o.points3D_z);
402 
403  std::swap(hasRangeImage, o.hasRangeImage);
407 
408  std::swap(hasIntensityImage, o.hasIntensityImage);
411 
414 
415  std::swap(pixelLabels, o.pixelLabels);
416 
418 
419  std::swap(cameraParams, o.cameraParams);
421 
422  std::swap(maxRange, o.maxRange);
423  std::swap(sensorPose, o.sensorPose);
424  std::swap(stdError, o.stdError);
425 }
426 
428 {
430  {
431  const string fil = points3D_getExternalStorageFileAbsolutePath();
433  "txt", mrpt::system::extractFileExtension(fil, true)))
434  {
435  CMatrixFloat M;
436  M.loadFromTextFile(fil);
437  ASSERT_EQUAL_(M.rows(), 3);
438  const auto N = M.cols();
439 
440  auto& xs = const_cast<std::vector<float>&>(points3D_x);
441  auto& ys = const_cast<std::vector<float>&>(points3D_y);
442  auto& zs = const_cast<std::vector<float>&>(points3D_z);
443  xs.resize(N);
444  ys.resize(N);
445  zs.resize(N);
446  std::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
447  std::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
448  std::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
449  }
450  else
451  {
454  f >> const_cast<std::vector<float>&>(points3D_x) >>
455  const_cast<std::vector<float>&>(points3D_y) >>
456  const_cast<std::vector<float>&>(points3D_z);
457  }
458  }
459 
461  {
464  "txt", mrpt::system::extractFileExtension(fil, true)))
465  {
466  const_cast<CMatrixF&>(rangeImage).loadFromTextFile(fil);
467  }
468  else
469  {
472  f >> const_cast<CMatrixF&>(rangeImage);
473  }
474  }
475 }
476 
478 {
480  {
484  }
485 
487 
490 }
491 
493  std::string& out_path) const
494 {
496  if (m_rangeImage_external_file[0] == '/' ||
497  (m_rangeImage_external_file[1] == ':' &&
498  m_rangeImage_external_file[2] == '\\'))
499  {
500  out_path = m_rangeImage_external_file;
501  }
502  else
503  {
504  out_path = CImage::getImagesPathBase();
505  size_t N = CImage::getImagesPathBase().size() - 1;
506  if (CImage::getImagesPathBase()[N] != '/' &&
507  CImage::getImagesPathBase()[N] != '\\')
508  out_path += "/";
509  out_path += m_rangeImage_external_file;
510  }
511 }
513  std::string& out_path) const
514 {
515  ASSERT_(m_points3D_external_file.size() > 2);
516  if (m_points3D_external_file[0] == '/' ||
517  (m_points3D_external_file[1] == ':' &&
518  m_points3D_external_file[2] == '\\'))
519  {
520  out_path = m_points3D_external_file;
521  }
522  else
523  {
524  out_path = CImage::getImagesPathBase();
525  size_t N = CImage::getImagesPathBase().size() - 1;
526  if (CImage::getImagesPathBase()[N] != '/' &&
527  CImage::getImagesPathBase()[N] != '\\')
528  out_path += "/";
529  out_path += m_points3D_external_file;
530  }
531 }
532 
534  const std::string& fileName_, const std::string& use_this_base_dir)
535 {
537  ASSERT_(
538  points3D_x.size() == points3D_y.size() &&
539  points3D_x.size() == points3D_z.size());
540 
543  mrpt::system::fileNameChangeExtension(fileName_, "txt");
544  else
546  mrpt::system::fileNameChangeExtension(fileName_, "bin");
547 
548  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
549  // instead of CImage::getImagesPathBase()
550  const string savedDir = CImage::getImagesPathBase();
551  CImage::setImagesPathBase(use_this_base_dir);
552  const string real_absolute_file_path =
554  CImage::setImagesPathBase(savedDir);
555 
557  {
558  const size_t nPts = points3D_x.size();
559 
560  CMatrixFloat M(3, nPts);
561  M.setRow(0, points3D_x);
562  M.setRow(1, points3D_y);
563  M.setRow(2, points3D_z);
564 
565  M.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
566  }
567  else
568  {
569  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
571  f << points3D_x << points3D_y << points3D_z;
572  }
573 
575 
576  // Really dealloc memory, clear() is not enough:
582 }
584  const std::string& fileName_, const std::string& use_this_base_dir)
585 {
589  mrpt::system::fileNameChangeExtension(fileName_, "txt");
590  else
592  mrpt::system::fileNameChangeExtension(fileName_, "bin");
593 
594  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
595  // instead of CImage::getImagesPathBase()
596  const string savedDir = CImage::getImagesPathBase();
597  CImage::setImagesPathBase(use_this_base_dir);
598  const string real_absolute_file_path =
600  CImage::setImagesPathBase(savedDir);
601 
603  {
604  rangeImage.saveToTextFile(real_absolute_file_path, MATRIX_FORMAT_FIXED);
605  }
606  else
607  {
608  mrpt::io::CFileGZOutputStream fo(real_absolute_file_path);
610  f << rangeImage;
611  }
612 
614  rangeImage.setSize(0, 0);
615 }
616 
617 // ============== Auxiliary function for "recoverCameraCalibrationParameters"
618 // =========================
619 
620 #define CALIB_DECIMAT 15
621 
622 namespace mrpt::obs::detail
623 {
625 {
627  const double z_offset;
628  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
629  : obs(obs_), z_offset(z_offset_)
630  {
631  }
632 };
633 
634 void cam2vec(const TCamera& camPar, CVectorDouble& x)
635 {
636  if (x.size() < 4 + 4) x.resize(4 + 4);
637 
638  x[0] = camPar.fx();
639  x[1] = camPar.fy();
640  x[2] = camPar.cx();
641  x[3] = camPar.cy();
642 
643  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
644 }
645 void vec2cam(const CVectorDouble& x, TCamera& camPar)
646 {
647  camPar.intrinsicParams(0, 0) = x[0]; // fx
648  camPar.intrinsicParams(1, 1) = x[1]; // fy
649  camPar.intrinsicParams(0, 2) = x[2]; // cx
650  camPar.intrinsicParams(1, 2) = x[3]; // cy
651 
652  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
653 }
655  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
656 {
657  const CObservation3DRangeScan& obs = d.obs;
658 
659  TCamera params;
660  vec2cam(par, params);
661 
662  const size_t nC = obs.rangeImage.cols();
663  const size_t nR = obs.rangeImage.rows();
664 
665  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
666 
667  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
668  {
669  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
670  {
671  const size_t idx = nC * r + c;
672 
673  TPoint3D p(
674  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
675  obs.points3D_z[idx]);
676  TPoint3D P(-p.y, -p.z, p.x);
677  TPixelCoordf pixel;
678  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
679  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
680 
681  // Pinhole model:
682  const double x = P.x / P.z;
683  const double y = P.y / P.z;
684 
685  // Radial distortion:
686  const double r2 = square(x) + square(y);
687  const double r4 = square(r2);
688 
689  pixel.x =
690  params.cx() +
691  params.fx() *
692  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
693  2 * params.dist[2] * x * y +
694  params.dist[3] * (r2 + 2 * square(x))));
695  pixel.y =
696  params.cy() +
697  params.fy() *
698  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
699  2 * params.dist[3] * x * y +
700  params.dist[2] * (r2 + 2 * square(y))));
701  }
702 
703  // In theory, it should be (r,c):
704  err.push_back(c - pixel.x);
705  err.push_back(r - pixel.y);
706  }
707  }
708 } // end error_func
709 } // namespace mrpt::obs::detail
710 
711 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
712  * of a 3D camera given a range (depth) image and the corresponding 3D point
713  * cloud.
714  * \param camera_offset The offset (in meters) in the +X direction of the point
715  * cloud. It's 1cm for SwissRanger SR4000.
716  * \return The final average reprojection error per pixel (typ <0.05 px)
717  */
719  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
720  const double camera_offset)
721 {
722  MRPT_START
723 
724  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
725  ASSERT_(
726  obs.points3D_x.size() == obs.points3D_y.size() &&
727  obs.points3D_x.size() == obs.points3D_z.size());
728 
729  using TMyLevMar =
731  TMyLevMar::TResultInfo info;
732 
733  const size_t nR = obs.rangeImage.rows();
734  const size_t nC = obs.rangeImage.cols();
735 
736  TCamera camInit;
737  camInit.ncols = nC;
738  camInit.nrows = nR;
739  camInit.intrinsicParams(0, 0) = 250;
740  camInit.intrinsicParams(1, 1) = 250;
741  camInit.intrinsicParams(0, 2) = nC >> 1;
742  camInit.intrinsicParams(1, 2) = nR >> 1;
743 
744  CVectorDouble initial_x;
745  detail::cam2vec(camInit, initial_x);
746 
747  initial_x.resize(8);
748  CVectorDouble increments_x(initial_x.size());
749  increments_x.fill(1e-4);
750 
751  CVectorDouble optimal_x;
752 
753  TMyLevMar lm;
754  lm.execute(
755  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
756  detail::TLevMarData(obs, camera_offset), info,
757  mrpt::system::LVL_INFO, /* verbose */
758  1000, /* max iter */
759  1e-3, 1e-9, 1e-9, false);
760 
761  const double avr_px_err =
762  sqrt(info.final_sqr_err / double(nC * nR) / square(CALIB_DECIMAT));
763 
764  out_camParams.ncols = nC;
765  out_camParams.nrows = nR;
766  out_camParams.focalLengthMeters = camera_offset;
767  detail::vec2cam(optimal_x, out_camParams);
768 
769  return avr_px_err;
770 
771  MRPT_END
772 }
773 
775  CObservation3DRangeScan& obs, const unsigned int& r1,
776  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
777 {
778  unsigned int cols = cameraParams.ncols;
779  unsigned int rows = cameraParams.nrows;
780 
781  ASSERT_((r1 < r2) && (c1 < c2));
782  ASSERT_((r2 < rows) && (c2 < cols));
783  // Maybe we needed to copy more base obs atributes
784 
785  // Copy zone of range image
787  if (hasRangeImage)
788  obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);
789 
790  // Copy zone of intensity image
793  if (hasIntensityImage)
795  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
796 
797  // Copy zone of confidence image
799  if (hasConfidenceImage)
801  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
802 
803  // Zone labels: It's too complex, just document that pixel labels are NOT
804  // extracted.
805 
806  // Copy zone of scanned points
807  obs.hasPoints3D = hasPoints3D;
808  if (hasPoints3D)
809  {
810  // Erase a possible previous content
811  if (obs.points3D_x.size() > 0)
812  {
813  obs.points3D_x.clear();
814  obs.points3D_y.clear();
815  obs.points3D_z.clear();
816  }
817 
818  for (unsigned int i = r1; i < r2; i++)
819  for (unsigned int j = c1; j < c2; j++)
820  {
821  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
822  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
823  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
824  }
825  }
826 
827  obs.maxRange = maxRange;
828  obs.sensorPose = sensorPose;
829  obs.stdError = stdError;
830 
832 }
833 
834 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
835  * \a points3D_z to allow the usage of the internal memory pool. */
837 {
838 #ifdef COBS3DRANGE_USE_MEMPOOL
839  // If WH=0 this is a clear:
840  if (!WH)
841  {
847  return;
848  }
849 
850  if (WH <= points3D_x.size()) // reduce size, don't realloc
851  {
852  points3D_x.resize(WH);
853  points3D_y.resize(WH);
854  points3D_z.resize(WH);
855  points3D_idxs_x.resize(WH);
856  points3D_idxs_y.resize(WH);
857  return;
858  }
859 
860  // Request memory for the X,Y,Z buffers from the memory pool:
862  if (pool)
863  {
865  mem_params.WH = WH;
866 
868  pool->request_memory(mem_params);
869 
870  if (mem_block)
871  { // Take the memory via swaps:
872  points3D_x.swap(mem_block->pts_x);
873  points3D_y.swap(mem_block->pts_y);
874  points3D_z.swap(mem_block->pts_z);
875  points3D_idxs_x.swap(mem_block->idxs_x);
876  points3D_idxs_y.swap(mem_block->idxs_y);
877  delete mem_block;
878  }
879  }
880 #endif
881 
882  // Either if there was no pool memory or we got it, make sure the size of
883  // vectors is OK:
884  points3D_x.resize(WH);
885  points3D_y.resize(WH);
886  points3D_z.resize(WH);
887  points3D_idxs_x.resize(WH);
888  points3D_idxs_y.resize(WH);
889 }
890 
892 {
893  // x,y,z vectors have the same size.
894  return points3D_x.size();
895 }
896 
897 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
898 // pooling to speed-up the memory allocation.
899 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
900 {
901 #ifdef COBS3DRANGE_USE_MEMPOOL
902  // Request memory from the memory pool:
904  if (pool)
905  {
907  mem_params.H = H;
908  mem_params.W = W;
909 
911  pool->request_memory(mem_params);
912 
913  if (mem_block)
914  { // Take the memory via swaps:
915  rangeImage.swap(mem_block->rangeImage);
916  delete mem_block;
917  return;
918  }
919  }
920 // otherwise, continue with the normal method:
921 #endif
922  // Fall-back to normal method:
923  rangeImage.setSize(H, W);
924 }
925 
926 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
927 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
929 {
930  static const double EPSILON = 1e-7;
931  static mrpt::poses::CPose3D ref_pose(0, 0, 0, -90.0_deg, 0, -90.0_deg);
932 
933  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
934  .all() &&
935  ((ref_pose.getRotationMatrix() -
937  .array()
938  .abs() < EPSILON)
939  .all();
940 }
941 
945 {
946  out_scan2d.sensorLabel = sensorLabel;
947  out_scan2d.timestamp = this->timestamp;
948 
949  if (!this->hasRangeImage)
950  { // Nothing to do!
951  out_scan2d.resizeScan(0);
952  return;
953  }
954 
955  const size_t nCols = this->rangeImage.cols();
956  const size_t nRows = this->rangeImage.rows();
957  if (fp.rangeMask_min)
958  { // sanity check:
961  }
962  if (fp.rangeMask_max)
963  { // sanity check:
966  }
967 
968  // Compute the real horizontal FOV from the range camera intrinsic calib
969  // data:
970  // Note: this assumes the range image has been "undistorted", which is true
971  // for data
972  // from OpenNI, and will be in the future for libfreenect in MRPT,
973  // but it's
974  // not implemented yet (as of Mar 2012), so this is an approximation
975  // in that case.
976  const double cx = this->cameraParams.cx();
977  const double cy = this->cameraParams.cy();
978  const double fx = this->cameraParams.fx();
979  const double fy = this->cameraParams.fy();
980 
981  // (Imagine the camera seen from above to understand this geometry)
982  const double real_FOV_left = atan2(cx, fx);
983  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
984 
985  // FOV of the equivalent "fake" "laser scanner":
986  const float FOV_equiv = 2. * std::max(real_FOV_left, real_FOV_right);
987 
988  // Now, we should create more "fake laser" points than columns in the image,
989  // since laser scans are assumed to sample space at evenly-spaced angles,
990  // while in images it is like ~tan(angle).
992  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
993  const auto nLaserRays = static_cast<size_t>(nCols * sp.oversampling_ratio);
994 
995  // Prepare 2D scan data fields:
996  out_scan2d.aperture = FOV_equiv;
997  out_scan2d.maxRange = this->maxRange;
998  out_scan2d.resizeScan(nLaserRays);
999 
1000  out_scan2d.resizeScanAndAssign(
1001  nLaserRays, 2.0 * this->maxRange,
1002  false); // default: all ranges=invalid
1003  if (sp.use_origin_sensor_pose)
1004  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1005  else
1006  out_scan2d.sensorPose = this->sensorPose;
1007 
1008  // The vertical FOVs given by the user can be translated into limits of the
1009  // tangents (tan>0 means above, i.e. z>0):
1010  const float tan_min = -tan(std::abs(sp.angle_inf));
1011  const float tan_max = tan(std::abs(sp.angle_sup));
1012 
1013  // Precompute the tangents of the vertical angles of each "ray"
1014  // for every row in the range image:
1015  std::vector<float> vert_ang_tan(nRows);
1016  for (size_t r = 0; r < nRows; r++)
1017  vert_ang_tan[r] = static_cast<float>((cy - r) / fy);
1018 
1019  if (!sp.use_origin_sensor_pose)
1020  {
1021  // Algorithm 1: each column in the range image corresponds to a known
1022  // orientation in the 2D scan:
1023  // -------------------
1024  out_scan2d.rightToLeft = false;
1025 
1026  // Angle "counter" for the fake laser scan direction, and the increment:
1027  double ang = -FOV_equiv * 0.5;
1028  const double A_ang = FOV_equiv / (nLaserRays - 1);
1029 
1030  TRangeImageFilter rif(fp);
1031 
1032  // Go thru columns, and keep the minimum distance (along the +X axis,
1033  // not 3D distance!)
1034  // for each direction (i.e. for each column) which also lies within the
1035  // vertical FOV passed
1036  // by the user.
1037  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1038  {
1039  // Equivalent column in the range image for the "i'th" ray:
1040  const double tan_ang = tan(ang);
1041  // make sure we don't go out of range (just in case):
1042  const size_t c = std::min(
1043  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1044  nCols - 1);
1045 
1046  bool any_valid = false;
1047  float closest_range = out_scan2d.maxRange;
1048 
1049  for (size_t r = 0; r < nRows; r++)
1050  {
1051  const float D = this->rangeImage.coeff(r, c);
1052  if (!rif.do_range_filter(r, c, D)) continue;
1053 
1054  // All filters passed:
1055  const float this_point_tan = vert_ang_tan[r] * D;
1056  if (this_point_tan > tan_min && this_point_tan < tan_max)
1057  {
1058  any_valid = true;
1059  mrpt::keep_min(closest_range, D);
1060  }
1061  }
1062 
1063  if (any_valid)
1064  {
1065  out_scan2d.setScanRangeValidity(i, true);
1066  // Compute the distance in 2D from the "depth" in closest_range:
1067  out_scan2d.setScanRange(
1068  i, closest_range * std::sqrt(1.0 + tan_ang * tan_ang));
1069  }
1070  } // end for columns
1071  }
1072  else
1073  {
1074  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1075  // at the origin)
1076  // ------------------------------------------------------------------------
1077  out_scan2d.rightToLeft = true;
1078 
1079  T3DPointsProjectionParams projParams;
1080  projParams.takeIntoAccountSensorPoseOnRobot = true;
1081 
1083  this->project3DPointsFromDepthImageInto(*pc, projParams, fp);
1084 
1085  const std::vector<float>&xs = pc->getArrayX(), &ys = pc->getArrayY(),
1086  &zs = pc->getArrayZ();
1087  const size_t N = xs.size();
1088 
1089  const double A_ang = FOV_equiv / (nLaserRays - 1);
1090  const double ang0 = -FOV_equiv * 0.5;
1091 
1092  for (size_t i = 0; i < N; i++)
1093  {
1094  if (zs[i] < sp.z_min || zs[i] > sp.z_max) continue;
1095 
1096  const double phi_wrt_origin = atan2(ys[i], xs[i]);
1097 
1098  int i_range = (phi_wrt_origin - ang0) / A_ang;
1099  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1100 
1101  const float r_wrt_origin = ::hypotf(xs[i], ys[i]);
1102  if (out_scan2d.getScanRange(i_range) > r_wrt_origin)
1103  out_scan2d.setScanRange(i_range, r_wrt_origin);
1104  out_scan2d.setScanRangeValidity(i_range, true);
1105  }
1106  }
1107 }
1108 
1110 {
1112 
1113  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1114  // memory.
1115 
1116  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1117  "base:\n";
1119  << endl;
1120 
1121  o << "maxRange = " << maxRange << " m" << endl;
1122 
1123  o << "Has 3D point cloud? ";
1124  if (hasPoints3D)
1125  {
1126  o << "YES: " << points3D_x.size() << " points";
1128  o << ". External file: " << points3D_getExternalStorageFile()
1129  << endl;
1130  else
1131  o << " (embedded)." << endl;
1132  }
1133  else
1134  o << "NO" << endl;
1135 
1136  o << "Range is depth: " << (range_is_depth ? "YES" : "NO") << endl;
1137  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1138  if (hasRangeImage)
1139  {
1141  o << ". External file: " << rangeImage_getExternalStorageFile()
1142  << endl;
1143  else
1144  o << " (embedded)." << endl;
1145  }
1146 
1147  o << endl << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1148  if (hasIntensityImage)
1149  {
1151  o << ". External file: " << intensityImage.getExternalStorageFile()
1152  << endl;
1153  else
1154  o << " (embedded).\n";
1155  // Channel?
1156  o << "Source channel: "
1159  value2name(intensityImageChannel)
1160  << endl;
1161  }
1162 
1163  o << endl << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1164  if (hasConfidenceImage)
1165  {
1167  o << ". External file: " << confidenceImage.getExternalStorageFile()
1168  << endl;
1169  else
1170  o << " (embedded)." << endl;
1171  }
1172 
1173  o << endl << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1174  if (hasPixelLabels())
1175  {
1176  o << " Human readable labels:" << endl;
1177  for (auto it = pixelLabels->pixelLabelNames.begin();
1178  it != pixelLabels->pixelLabelNames.end(); ++it)
1179  o << " label[" << it->first << "]: '" << it->second << "'" << endl;
1180  }
1181 
1182  o << endl << endl;
1183  o << "Depth camera calibration parameters:" << endl;
1184  {
1185  CConfigFileMemory cfg;
1186  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1187  o << cfg.getContent() << endl;
1188  }
1189  o << endl << "Intensity camera calibration parameters:" << endl;
1190  {
1191  CConfigFileMemory cfg;
1192  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1193  o << cfg.getContent() << endl;
1194  }
1195  o << endl
1196  << endl
1197  << "Pose of the intensity cam. wrt the depth cam:\n"
1201  << endl;
1202 }
1203 
1206 {
1207  const uint8_t version = 1; // for possible future changes.
1208  out << version;
1209  // 1st: Save number MAX_NUM_DIFFERENT_LABELS so we can reconstruct the
1210  // object in the class factory later on.
1211  out << BITFIELD_BYTES;
1212  // 2nd: data-specific serialization:
1213  this->internal_writeToStream(out);
1214 }
1215 
1216 template <unsigned int BYTES_REQUIRED_>
1218  BYTES_REQUIRED_>::internal_readFromStream(mrpt::serialization::CArchive& in)
1219 {
1220  {
1221  uint32_t nR, nC;
1222  in >> nR >> nC;
1223  pixelLabels.resize(nR, nC);
1224  for (uint32_t c = 0; c < nC; c++)
1225  for (uint32_t r = 0; r < nR; r++) in >> pixelLabels.coeffRef(r, c);
1226  }
1227  in >> pixelLabelNames;
1228 }
1229 template <unsigned int BYTES_REQUIRED_>
1232 {
1233  {
1234  const auto nR = static_cast<uint32_t>(pixelLabels.rows());
1235  const auto nC = static_cast<uint32_t>(pixelLabels.cols());
1236  out << nR << nC;
1237  for (uint32_t c = 0; c < nC; c++)
1238  for (uint32_t r = 0; r < nR; r++) out << pixelLabels.coeff(r, c);
1239  }
1240  out << pixelLabelNames;
1241 }
1242 
1243 // Deserialization and class factory. All in one, ladies and gentlemen
1246 {
1247  uint8_t version;
1248  in >> version;
1249 
1250  switch (version)
1251  {
1252  case 1:
1253  {
1254  // 1st: Read NUM BYTES
1255  uint8_t bitfield_bytes;
1256  in >> bitfield_bytes;
1257 
1258  // Hand-made class factory. May be a good solution if there will be
1259  // not too many different classes:
1261  switch (bitfield_bytes)
1262  {
1263  case 1:
1265  break;
1266  case 2:
1268  break;
1269  case 3:
1270  case 4:
1272  break;
1273  case 5:
1274  case 6:
1275  case 7:
1276  case 8:
1278  break;
1279  default:
1280  throw std::runtime_error(
1281  "Unknown type of pixelLabel inner class while "
1282  "deserializing!");
1283  };
1284  // 2nd: data-specific serialization:
1285  new_obj->internal_readFromStream(in);
1286 
1287  return new_obj;
1288  }
1289  break;
1290 
1291  default:
1293  break;
1294  };
1295 }
1296 
1298  : angle_sup(mrpt::DEG2RAD(5)),
1299  angle_inf(mrpt::DEG2RAD(5)),
1300  z_min(-std::numeric_limits<double>::max()),
1301  z_max(std::numeric_limits<double>::max())
1302 {
1303 }
1304 
1306 {
1307 #if MRPT_HAS_OPENCV
1308 
1309  // DEPTH image:
1310  {
1311  // OpenCV wrapper (copy-less) for rangeImage:
1312 
1313  cv::Mat rangeImg(
1314  rangeImage.rows(), rangeImage.cols(), CV_32FC1, &rangeImage(0, 0));
1315  const cv::Mat distortion(
1316  1, cameraParams.dist.size(), CV_64F, &cameraParams.dist[0]);
1317  const cv::Mat intrinsics(
1318  3, 3, CV_64F, &cameraParams.intrinsicParams(0, 0));
1319 
1320  const auto imgSize = cv::Size(rangeImage.rows(), rangeImage.cols());
1321 
1322  double alpha = 0; // all depth pixels are visible in the output
1323  const cv::Mat newIntrinsics = cv::getOptimalNewCameraMatrix(
1324  intrinsics, distortion, imgSize, alpha);
1325 
1326  cv::Mat outRangeImg(rangeImage.rows(), rangeImage.cols(), CV_32FC1);
1327 
1328  // Undistort:
1329  const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);
1330 
1331  cv::Mat m1, m2;
1332 
1333  cv::initUndistortRectifyMap(
1334  intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1,
1335  m2);
1336  cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
1337 
1338  // Overwrite:
1339  outRangeImg.copyTo(rangeImg);
1340  cameraParams.dist.fill(0);
1341  for (int r = 0; r < 3; r++)
1342  for (int c = 0; c < 3; c++)
1344  newIntrinsics.at<double>(r, c);
1345  }
1346 
1347  // RGB image:
1348  if (hasIntensityImage)
1349  {
1350  mrpt::img::CImage newIntImg;
1352 
1353  intensityImage = std::move(newIntImg);
1354  cameraParamsIntensity.dist.fill(0);
1355  }
1356 
1357 #else
1358  THROW_EXCEPTION("This method requires OpenCV");
1359 #endif
1360 }
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: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>
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.
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: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
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...
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 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.
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
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:1528
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:178
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.
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: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.
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: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
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: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
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".
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:1195
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:225
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:1635
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)
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:39
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: 70be1f8ba Thu Nov 14 20:53:42 2019 +0100 at jue nov 14 21:00:10 CET 2019