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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019