MRPT  2.0.4
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-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/3rdparty/do_opencv_includes.h>
14 #include <mrpt/core/bits_mem.h> // vector_strong_clear
18 #include <mrpt/math/CMatrixF.h>
19 #include <mrpt/math/ops_containers.h> // norm(), etc.
22 #include <mrpt/poses/CPosePDF.h>
25 #include <mrpt/system/filesystem.h>
27 #include <cstring>
28 #include <limits>
29 #include <mutex>
30 #include <unordered_map>
31 
32 using namespace std;
33 using namespace mrpt::obs;
34 using namespace mrpt::poses;
35 using namespace mrpt::math;
36 using namespace mrpt::img;
38 
39 // This must be added to any CSerializable class implementation file.
41 
42 // Static LUT:
43 
44 // Index info: camera parameters + range_is_depth
45 struct LUT_info
46 {
50 };
51 
52 inline bool operator==(const LUT_info& a, const LUT_info& o)
53 {
54  return a.calib == o.calib && a.sensorPose == o.sensorPose &&
56 }
57 
58 namespace std
59 {
60 template <>
61 struct hash<LUT_info>
62 {
63  size_t operator()(const LUT_info& k) const
64  {
65  size_t res = 17;
66  res = res * 31 + hash<mrpt::img::TCamera>()(k.calib);
67  res = res * 31 + hash<bool>()(k.range_is_depth);
68  for (unsigned int i = 0; i < 6; i++)
69  res = res * 31 + hash<double>()(k.sensorPose[i]);
70  return res;
71  }
72 };
73 } // namespace std
74 
75 static std::unordered_map<LUT_info, CObservation3DRangeScan::unproject_LUT_t>
77 static std::mutex LUTs_mtx;
78 
80  CObservation3DRangeScan::get_unproj_lut() const
81 {
82 #if MRPT_HAS_OPENCV
83  // Access to, or create upon first usage:
84  LUT_info linfo;
85  linfo.calib = this->cameraParams;
86  linfo.sensorPose = this->sensorPose;
87  linfo.range_is_depth = this->range_is_depth;
88 
89  LUTs_mtx.lock();
90  // Protect against infinite memory growth: imagine sensorPose gets changed
91  // every time for a sweeping sensor, etc.
92  // Unlikely, but "just in case" (TM)
93  if (LUTs.size() > 100) LUTs.clear();
94 
95  const unproject_LUT_t& ret = LUTs[linfo];
96 
97  LUTs_mtx.unlock();
98 
99  ASSERT_EQUAL_(rangeImage.cols(), static_cast<int>(cameraParams.ncols));
100  ASSERT_EQUAL_(rangeImage.rows(), static_cast<int>(cameraParams.nrows));
101 
102  // already existed and was filled?
103  unsigned int H = cameraParams.nrows, W = cameraParams.ncols;
104  const size_t WH = W * H;
105  if (ret.Kxs.size() == WH) return ret;
106 
107  // fill LUT upon first use:
108  auto& lut = const_cast<unproject_LUT_t&>(ret);
109 
110  lut.Kxs.resize(WH);
111  lut.Kys.resize(WH);
112  lut.Kzs.resize(WH);
113  lut.Kxs_rot.resize(WH);
114  lut.Kys_rot.resize(WH);
115  lut.Kzs_rot.resize(WH);
116 
117  // Undistort all points:
118  cv::Mat pts(1, WH, CV_32FC2), undistort_pts(1, WH, CV_32FC2);
119 
120  const auto& intrMat = cameraParams.intrinsicParams;
121  const auto& dist = cameraParams.dist;
122  cv::Mat cv_distortion(
123  1, dist.size(), CV_64F, const_cast<double*>(&dist[0]));
124  cv::Mat cv_intrinsics(3, 3, CV_64F);
125  for (int i = 0; i < 3; i++)
126  for (int j = 0; j < 3; j++)
127  cv_intrinsics.at<double>(i, j) = intrMat(i, j);
128 
129  for (unsigned int r = 0; r < H; r++)
130  for (unsigned int c = 0; c < W; c++)
131  {
132  auto& p = pts.at<cv::Vec2f>(r * W + c);
133  p[0] = c;
134  p[1] = r;
135  }
136 
137  cv::undistortPoints(pts, undistort_pts, cv_intrinsics, cv_distortion);
138 
139  // Note: undistort_pts now holds point coordinates with (-1,-1)=top left,
140  // (1,1)=bottom-right
141 
142  ASSERT_EQUAL_(undistort_pts.size().area(), static_cast<int>(WH));
143  undistort_pts.reshape(WH);
144 
145  float* kxs = &lut.Kxs[0];
146  float* kys = &lut.Kys[0];
147  float* kzs = &lut.Kzs[0];
148  float* kxs_rot = &lut.Kxs_rot[0];
149  float* kys_rot = &lut.Kys_rot[0];
150  float* kzs_rot = &lut.Kzs_rot[0];
151 
152  for (size_t idx = 0; idx < WH; idx++)
153  {
154  const auto& p = undistort_pts.at<cv::Vec2f>(idx);
155  const float c = p[0], r = p[1];
156 
157  // XYZ -> (-Y,-Z, X)
158  auto v = mrpt::math::TPoint3Df(1.0f, -c, -r);
159 
160  // Range instead of depth? Use a unit vector:
161  if (!this->range_is_depth) v *= 1.0f / v.norm();
162 
163  // compute also the rotated version:
164  auto v_rot = sensorPose.rotateVector(v);
165 
166  *kxs++ = v.x;
167  *kys++ = v.y;
168  *kzs++ = v.z;
169 
170  *kxs_rot++ = v_rot.x;
171  *kys_rot++ = v_rot.y;
172  *kzs_rot++ = v_rot.z;
173  }
174 
175  return ret;
176 #else
177  THROW_EXCEPTION("This method requires MRPT built against OpenCV");
178 #endif
179 }
180 
181 static bool EXTERNALS_AS_TEXT_value = false;
182 void CObservation3DRangeScan::EXTERNALS_AS_TEXT(bool value)
183 {
184  EXTERNALS_AS_TEXT_value = value;
185 }
186 bool CObservation3DRangeScan::EXTERNALS_AS_TEXT()
187 {
189 }
190 
191 // Whether to use a memory pool for 3D points:
192 #define COBS3DRANGE_USE_MEMPOOL
193 
194 // Do performance time logging?
195 //#define PROJ3D_PERFLOG
196 
197 // Data types for memory pooling CObservation3DRangeScan:
198 #ifdef COBS3DRANGE_USE_MEMPOOL
199 
201 
202 // Memory pool for XYZ points ----------------
204 {
205  /** Width*Height, that is, the number of 3D points */
206  size_t WH{0};
207  inline bool isSuitable(
209  {
210  return WH >= req.WH;
211  }
212 };
214 {
215  std::vector<float> pts_x, pts_y, pts_z;
216  /** for each point, the corresponding (x,y) pixel coordinates */
217  std::vector<uint16_t> idxs_x, idxs_y;
218 };
222 
223 // Memory pool for the rangeImage matrix ----------------
225 {
226  /** Size of matrix */
227  int H{0}, W{0};
228  inline bool isSuitable(
230  {
231  return H == req.H && W == req.W;
232  }
233 };
235 {
237 };
241 
243 {
244  if (obs.points3D_x.empty()) return;
245  // Before dying, donate my memory to the pool for the joy of future
246  // class-brothers...
248  if (!pool) return;
249 
251  mem_params.WH = obs.points3D_x.capacity();
252  if (obs.points3D_y.capacity() != mem_params.WH)
253  obs.points3D_y.resize(mem_params.WH);
254  if (obs.points3D_z.capacity() != mem_params.WH)
255  obs.points3D_z.resize(mem_params.WH);
256  if (obs.points3D_idxs_x.capacity() != mem_params.WH)
257  obs.points3D_idxs_x.resize(mem_params.WH);
258  if (obs.points3D_idxs_y.capacity() != mem_params.WH)
259  obs.points3D_idxs_y.resize(mem_params.WH);
260 
261  auto* mem_block = new CObservation3DRangeScan_Points_MemPoolData();
262  obs.points3D_x.swap(mem_block->pts_x);
263  obs.points3D_y.swap(mem_block->pts_y);
264  obs.points3D_z.swap(mem_block->pts_z);
265  obs.points3D_idxs_x.swap(mem_block->idxs_x);
266  obs.points3D_idxs_y.swap(mem_block->idxs_y);
267 
268  pool->dump_to_pool(mem_params, mem_block);
269 }
271 {
272  if (obs.rangeImage.cols() == 0 || obs.rangeImage.rows() == 0) return;
273 
274  // Before dying, donate my memory to the pool for the joy of future
275  // class-brothers...
277  if (!pool) return;
278 
280  mem_params.H = obs.rangeImage.rows();
281  mem_params.W = obs.rangeImage.cols();
282 
283  auto* mem_block = new CObservation3DRangeScan_Ranges_MemPoolData();
284  obs.rangeImage.swap(mem_block->rangeImage);
285 
286  pool->dump_to_pool(mem_params, mem_block);
287 }
288 #endif
289 
290 CObservation3DRangeScan::~CObservation3DRangeScan()
291 {
292 #ifdef COBS3DRANGE_USE_MEMPOOL
295 #endif
296 }
297 
298 uint8_t CObservation3DRangeScan::serializeGetVersion() const { return 10; }
299 void CObservation3DRangeScan::serializeTo(
301 {
302  // The data
303  out << maxRange << sensorPose;
304  out << hasPoints3D;
305  if (hasPoints3D)
306  {
307  ASSERT_(
308  points3D_x.size() == points3D_y.size() &&
309  points3D_x.size() == points3D_z.size() &&
310  points3D_idxs_x.size() == points3D_x.size() &&
311  points3D_idxs_y.size() == points3D_x.size());
312  uint32_t N = points3D_x.size();
313  out << N;
314  if (N)
315  {
316  out.WriteBufferFixEndianness(&points3D_x[0], N);
317  out.WriteBufferFixEndianness(&points3D_y[0], N);
318  out.WriteBufferFixEndianness(&points3D_z[0], N);
319  out.WriteBufferFixEndianness(&points3D_idxs_x[0], N); // New in v8
320  out.WriteBufferFixEndianness(&points3D_idxs_y[0], N); // New in v8
321  }
322  }
323 
324  out << hasRangeImage;
325  out << rangeUnits; // new in v9
326  if (hasRangeImage)
327  {
328  out.WriteAs<uint32_t>(rangeImage.rows());
329  out.WriteAs<uint32_t>(rangeImage.cols());
330  if (rangeImage.size() != 0)
331  out.WriteBufferFixEndianness<uint16_t>(
332  rangeImage.data(), rangeImage.size());
333  }
334  out << hasIntensityImage;
335  if (hasIntensityImage) out << intensityImage;
336  out << hasConfidenceImage;
337  if (hasConfidenceImage) out << confidenceImage;
338  out << cameraParams; // New in v2
339  out << cameraParamsIntensity; // New in v4
340  out << relativePoseIntensityWRTDepth; // New in v4
341 
342  out << stdError;
343  out << timestamp;
344  out << sensorLabel;
345 
346  // New in v3:
347  out << m_points3D_external_stored << m_points3D_external_file;
348  out << m_rangeImage_external_stored << m_rangeImage_external_file;
349 
350  // New in v5:
351  out << range_is_depth;
352 
353  // New in v6:
354  out.WriteAs<int8_t>(intensityImageChannel);
355 
356  // New in v7:
357  out << hasPixelLabels();
358  if (hasPixelLabels())
359  {
360  pixelLabels->writeToStream(out);
361  }
362 
363  // v10:
364  out.WriteAs<uint8_t>(rangeImageOtherLayers.size());
365  for (const auto& kv : rangeImageOtherLayers)
366  {
367  out << kv.first;
368  const auto& ri = kv.second;
369  out.WriteAs<uint32_t>(ri.cols());
370  out.WriteAs<uint32_t>(ri.rows());
371  if (!ri.empty())
372  out.WriteBufferFixEndianness<uint16_t>(ri.data(), ri.size());
373  }
374 }
375 
376 void CObservation3DRangeScan::serializeFrom(
377  mrpt::serialization::CArchive& in, uint8_t version)
378 {
379  switch (version)
380  {
381  case 0:
382  case 1:
383  case 2:
384  case 3:
385  case 4:
386  case 5:
387  case 6:
388  case 7:
389  case 8:
390  case 9:
391  case 10:
392  {
393  uint32_t N;
394 
395  in >> maxRange >> sensorPose;
396 
397  if (version > 0)
398  in >> hasPoints3D;
399  else
400  hasPoints3D = true;
401 
402  if (hasPoints3D)
403  {
404  in >> N;
405  resizePoints3DVectors(N);
406 
407  if (N)
408  {
409  in.ReadBufferFixEndianness(&points3D_x[0], N);
410  in.ReadBufferFixEndianness(&points3D_y[0], N);
411  in.ReadBufferFixEndianness(&points3D_z[0], N);
412 
413  if (version == 0)
414  {
415  vector<char> validRange(N); // for v0.
416  in.ReadBuffer(
417  &validRange[0], sizeof(validRange[0]) * N);
418  }
419  if (version >= 8)
420  {
421  in.ReadBufferFixEndianness(&points3D_idxs_x[0], N);
422  in.ReadBufferFixEndianness(&points3D_idxs_y[0], N);
423  }
424  }
425  }
426  else
427  {
428  this->resizePoints3DVectors(0);
429  }
430 
431  if (version >= 1)
432  {
433  in >> hasRangeImage;
434  if (version >= 9)
435  in >> rangeUnits;
436  else
437  rangeUnits = 1e-3f; // default units
438 
439  if (hasRangeImage)
440  {
441  if (version < 9)
442  {
443  // Convert from old format:
445  in >> ri;
446  const uint32_t rows = ri.rows(), cols = ri.cols();
447  ASSERT_(rows > 0 && cols > 0);
448 
449  // Call "rangeImage_setSize()" to exploit the mempool:
450  rangeImage_setSize(rows, cols);
451 
452  for (uint32_t r = 0; r < rows; r++)
453  for (uint32_t c = 0; c < cols; c++)
454  rangeImage(r, c) = static_cast<uint16_t>(
455  mrpt::round(ri(r, c) / rangeUnits));
456  }
457  else
458  {
459  const uint32_t rows = in.ReadAs<uint32_t>();
460  const uint32_t cols = in.ReadAs<uint32_t>();
461 
462  // Call "rangeImage_setSize()" to exploit the mempool:
463  rangeImage_setSize(rows, cols);
464 
465  // new v9:
466  if (rangeImage.size() != 0)
467  in.ReadBufferFixEndianness<uint16_t>(
468  rangeImage.data(), rangeImage.size());
469  }
470  }
471 
472  in >> hasIntensityImage;
473  if (hasIntensityImage) in >> intensityImage;
474 
475  in >> hasConfidenceImage;
476  if (hasConfidenceImage) in >> confidenceImage;
477 
478  if (version >= 2)
479  {
480  in >> cameraParams;
481 
482  if (version >= 4)
483  {
484  in >> cameraParamsIntensity >>
485  relativePoseIntensityWRTDepth;
486  }
487  else
488  {
489  cameraParamsIntensity = cameraParams;
490  relativePoseIntensityWRTDepth = CPose3D();
491  }
492  }
493  }
494 
495  in >> stdError;
496  in >> timestamp;
497  in >> sensorLabel;
498 
499  if (version >= 3)
500  {
501  // New in v3:
502  in >> m_points3D_external_stored >> m_points3D_external_file;
503  in >> m_rangeImage_external_stored >>
504  m_rangeImage_external_file;
505  }
506  else
507  {
508  m_points3D_external_stored = false;
509  m_rangeImage_external_stored = false;
510  }
511 
512  if (version >= 5)
513  {
514  in >> range_is_depth;
515  }
516  else
517  {
518  range_is_depth = true;
519  }
520 
521  if (version >= 6)
522  {
523  int8_t i;
524  in >> i;
525  intensityImageChannel = static_cast<TIntensityChannelID>(i);
526  }
527  else
528  {
529  intensityImageChannel = CH_VISIBLE;
530  }
531 
532  pixelLabels.reset(); // Remove existing data first (_unique() is to
533  // leave alive any user copies of the shared
534  // pointer).
535  if (version >= 7)
536  {
537  bool do_have_labels;
538  in >> do_have_labels;
539 
540  if (do_have_labels)
541  pixelLabels.reset(
542  TPixelLabelInfoBase::readAndBuildFromStream(in));
543  }
544 
545  rangeImageOtherLayers.clear();
546  if (version >= 10)
547  {
548  const auto numLayers = in.ReadAs<uint8_t>();
549  for (size_t i = 0; i < numLayers; i++)
550  {
551  std::string name;
552  in >> name;
553  auto& ri = rangeImageOtherLayers[name];
554 
555  const uint32_t rows = in.ReadAs<uint32_t>();
556  const uint32_t cols = in.ReadAs<uint32_t>();
557  ri.resize(rows, cols);
558  if (ri.size() != 0)
559  in.ReadBufferFixEndianness<uint16_t>(
560  ri.data(), ri.size());
561  }
562  }
563 
564  // auto-fix wrong camera resolution in parameters:
565  if (hasRangeImage && !rangeImage_isExternallyStored() &&
566  (static_cast<int>(cameraParams.ncols) != rangeImage.cols() ||
567  static_cast<int>(cameraParams.nrows) != rangeImage.rows()))
568  {
569  std::cerr << "[CObservation3DRangeScan] Warning: autofixing "
570  "incorrect camera resolution in TCamera:"
571  << cameraParams.ncols << "x" << cameraParams.nrows
572  << " => " << rangeImage.cols() << "x"
573  << rangeImage.rows() << "\n";
574  cameraParams.ncols = rangeImage.cols();
575  cameraParams.nrows = rangeImage.rows();
576  }
577  }
578  break;
579  default:
581  };
582 }
583 
584 void CObservation3DRangeScan::swap(CObservation3DRangeScan& o)
585 {
586  CObservation::swap(o);
587 
588  std::swap(hasPoints3D, o.hasPoints3D);
589  points3D_x.swap(o.points3D_x);
590  points3D_y.swap(o.points3D_y);
591  points3D_z.swap(o.points3D_z);
592  points3D_idxs_x.swap(o.points3D_idxs_x);
593  points3D_idxs_y.swap(o.points3D_idxs_y);
594  std::swap(m_points3D_external_stored, o.m_points3D_external_stored);
595  std::swap(m_points3D_external_file, o.m_points3D_external_file);
596 
597  std::swap(hasRangeImage, o.hasRangeImage);
598  rangeImage.swap(o.rangeImage);
599  std::swap(m_rangeImage_external_stored, o.m_rangeImage_external_stored);
600  std::swap(m_rangeImage_external_file, o.m_rangeImage_external_file);
601 
602  std::swap(hasIntensityImage, o.hasIntensityImage);
603  std::swap(intensityImageChannel, o.intensityImageChannel);
604  intensityImage.swap(o.intensityImage);
605 
606  std::swap(hasConfidenceImage, o.hasConfidenceImage);
607  confidenceImage.swap(o.confidenceImage);
608 
609  std::swap(pixelLabels, o.pixelLabels);
610 
611  std::swap(relativePoseIntensityWRTDepth, o.relativePoseIntensityWRTDepth);
612 
613  std::swap(cameraParams, o.cameraParams);
614  std::swap(cameraParamsIntensity, o.cameraParamsIntensity);
615 
616  std::swap(maxRange, o.maxRange);
617  std::swap(sensorPose, o.sensorPose);
618  std::swap(stdError, o.stdError);
619 
620  rangeImageOtherLayers.swap(o.rangeImageOtherLayers);
621 }
622 
623 void CObservation3DRangeScan::load() const
624 {
625  if (hasPoints3D && m_points3D_external_stored)
626  {
627  const string fil = points3D_getExternalStorageFileAbsolutePath();
629  "txt", mrpt::system::extractFileExtension(fil, true)))
630  {
631  CMatrixFloat M;
632  M.loadFromTextFile(fil);
633  ASSERT_EQUAL_(M.rows(), 3);
634  const auto N = M.cols();
635 
636  auto& xs = const_cast<std::vector<float>&>(points3D_x);
637  auto& ys = const_cast<std::vector<float>&>(points3D_y);
638  auto& zs = const_cast<std::vector<float>&>(points3D_z);
639  xs.resize(N);
640  ys.resize(N);
641  zs.resize(N);
642  std::memcpy(&xs[0], &M(0, 0), sizeof(float) * N);
643  std::memcpy(&ys[0], &M(1, 0), sizeof(float) * N);
644  std::memcpy(&zs[0], &M(2, 0), sizeof(float) * N);
645  }
646  else
647  {
650  f >> const_cast<std::vector<float>&>(points3D_x) >>
651  const_cast<std::vector<float>&>(points3D_y) >>
652  const_cast<std::vector<float>&>(points3D_z);
653  }
654  }
655 
656  if (hasRangeImage && m_rangeImage_external_stored)
657  {
658  for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
659  {
660  std::string layerName;
661  mrpt::math::CMatrix_u16* ri = nullptr;
662  if (idx == 0)
663  ri = const_cast<mrpt::math::CMatrix_u16*>(&rangeImage);
664  else
665  {
666  auto it = rangeImageOtherLayers.begin();
667  std::advance(it, idx - 1);
668  layerName = it->first;
669  ri = const_cast<mrpt::math::CMatrix_u16*>(&it->second);
670  }
671  const string fil =
672  rangeImage_getExternalStorageFileAbsolutePath(layerName);
674  "txt", mrpt::system::extractFileExtension(fil, true)))
675  {
676  ri->loadFromTextFile(fil);
677  }
678  else
679  {
680  auto& me = const_cast<CObservation3DRangeScan&>(*this);
681 
684  const uint32_t rows = f.ReadAs<uint32_t>();
685  const uint32_t cols = f.ReadAs<uint32_t>();
686  me.rangeImage_setSize(rows, cols);
687  if (ri->size() != 0)
688  f.ReadBufferFixEndianness<uint16_t>(ri->data(), ri->size());
689  }
690 
691  } // end for each layer
692  }
693 }
694 
695 void CObservation3DRangeScan::unload()
696 {
697  if (hasPoints3D && m_points3D_external_stored)
698  {
699  mrpt::vector_strong_clear(points3D_x);
700  mrpt::vector_strong_clear(points3D_y);
701  mrpt::vector_strong_clear(points3D_z);
702  }
703 
704  if (hasRangeImage && m_rangeImage_external_stored) rangeImage.setSize(0, 0);
705 
706  intensityImage.unload();
707  confidenceImage.unload();
708 }
709 
710 std::string CObservation3DRangeScan::rangeImage_getExternalStorageFile(
711  const std::string& rangeImageLayer) const
712 {
713  std::string filName = m_rangeImage_external_file;
714  if (!rangeImageLayer.empty())
715  {
716  const auto curExt = mrpt::system::extractFileExtension(filName);
718  filName, std::string("layer_") + rangeImageLayer + curExt);
719  }
720  return filName;
721 }
722 
723 void CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath(
724  std::string& out_path, const std::string& rangeImageLayer) const
725 {
726  std::string filName = rangeImage_getExternalStorageFile(rangeImageLayer);
727 
728  ASSERT_(filName.size() > 2);
729  if (filName[0] == '/' || (filName[1] == ':' && filName[2] == '\\'))
730  {
731  out_path = filName;
732  }
733  else
734  {
735  out_path = CImage::getImagesPathBase();
736  size_t N = CImage::getImagesPathBase().size() - 1;
737  if (CImage::getImagesPathBase()[N] != '/' &&
738  CImage::getImagesPathBase()[N] != '\\')
739  out_path += "/";
740  out_path += filName;
741  }
742 }
743 void CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath(
744  std::string& out_path) const
745 {
746  ASSERT_(m_points3D_external_file.size() > 2);
747  if (m_points3D_external_file[0] == '/' ||
748  (m_points3D_external_file[1] == ':' &&
749  m_points3D_external_file[2] == '\\'))
750  {
751  out_path = m_points3D_external_file;
752  }
753  else
754  {
755  out_path = CImage::getImagesPathBase();
756  size_t N = CImage::getImagesPathBase().size() - 1;
757  if (CImage::getImagesPathBase()[N] != '/' &&
758  CImage::getImagesPathBase()[N] != '\\')
759  out_path += "/";
760  out_path += m_points3D_external_file;
761  }
762 }
763 
764 void CObservation3DRangeScan::points3D_convertToExternalStorage(
765  const std::string& fileName_, const std::string& use_this_base_dir)
766 {
767  ASSERT_(!points3D_isExternallyStored());
768  ASSERT_(
769  points3D_x.size() == points3D_y.size() &&
770  points3D_x.size() == points3D_z.size());
771 
773  m_points3D_external_file =
774  mrpt::system::fileNameChangeExtension(fileName_, "txt");
775  else
776  m_points3D_external_file =
777  mrpt::system::fileNameChangeExtension(fileName_, "bin");
778 
779  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
780  // instead of CImage::getImagesPathBase()
781  const string savedDir = CImage::getImagesPathBase();
782  CImage::setImagesPathBase(use_this_base_dir);
783  const string real_absolute_path =
784  points3D_getExternalStorageFileAbsolutePath();
785  CImage::setImagesPathBase(savedDir);
786 
788  {
789  const size_t nPts = points3D_x.size();
790 
791  CMatrixFloat M(nPts, 3);
792  M.setCol(0, points3D_x);
793  M.setCol(1, points3D_y);
794  M.setCol(2, points3D_z);
795 
796  M.saveToTextFile(real_absolute_path, MATRIX_FORMAT_FIXED);
797  }
798  else
799  {
800  mrpt::io::CFileGZOutputStream fo(real_absolute_path);
802  f << points3D_x << points3D_y << points3D_z;
803  }
804 
805  m_points3D_external_stored = true;
806 
807  // Really dealloc memory, clear() is not enough:
808  vector_strong_clear(points3D_x);
809  vector_strong_clear(points3D_y);
810  vector_strong_clear(points3D_z);
811  vector_strong_clear(points3D_idxs_x);
812  vector_strong_clear(points3D_idxs_y);
813 }
814 void CObservation3DRangeScan::rangeImage_convertToExternalStorage(
815  const std::string& fileName_, const std::string& use_this_base_dir)
816 {
817  ASSERT_(!rangeImage_isExternallyStored());
819  m_rangeImage_external_file =
820  mrpt::system::fileNameChangeExtension(fileName_, "txt");
821  else
822  m_rangeImage_external_file =
823  mrpt::system::fileNameChangeExtension(fileName_, "bin");
824 
825  // Use "use_this_base_dir" in "*_getExternalStorageFileAbsolutePath()"
826  // instead of CImage::getImagesPathBase()
827  const string savedDir = CImage::getImagesPathBase();
828  CImage::setImagesPathBase(use_this_base_dir);
829 
830  for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
831  {
832  std::string layerName;
833  mrpt::math::CMatrix_u16* ri = nullptr;
834  if (idx == 0)
835  ri = &rangeImage;
836  else
837  {
838  auto it = rangeImageOtherLayers.begin();
839  std::advance(it, idx - 1);
840  layerName = it->first;
841  ri = &it->second;
842  }
843  const string real_absolute_path =
844  rangeImage_getExternalStorageFileAbsolutePath(layerName);
845 
847  {
848  ri->saveToTextFile(real_absolute_path, MATRIX_FORMAT_FIXED);
849  }
850  else
851  {
852  mrpt::io::CFileGZOutputStream fo(real_absolute_path);
854 
855  f.WriteAs<uint32_t>(ri->rows());
856  f.WriteAs<uint32_t>(ri->cols());
857  if (ri->size() != 0)
858  f.WriteBufferFixEndianness<uint16_t>(ri->data(), ri->size());
859  }
860  }
861 
862  m_rangeImage_external_stored = true;
863  rangeImage.setSize(0, 0);
864 
865  CImage::setImagesPathBase(savedDir);
866 }
867 
868 // Auxiliary function for "recoverCameraCalibrationParameters"
869 #define CALIB_DECIMAT 15
870 
871 namespace mrpt::obs::detail
872 {
874 {
876  const double z_offset;
877  TLevMarData(const CObservation3DRangeScan& obs_, const double z_offset_)
878  : obs(obs_), z_offset(z_offset_)
879  {
880  }
881 };
882 
883 static void cam2vec(const TCamera& camPar, CVectorDouble& x)
884 {
885  if (x.size() < 4 + 4) x.resize(4 + 4);
886 
887  x[0] = camPar.fx();
888  x[1] = camPar.fy();
889  x[2] = camPar.cx();
890  x[3] = camPar.cy();
891 
892  for (size_t i = 0; i < 4; i++) x[4 + i] = camPar.dist[i];
893 }
894 static void vec2cam(const CVectorDouble& x, TCamera& camPar)
895 {
896  camPar.intrinsicParams(0, 0) = x[0]; // fx
897  camPar.intrinsicParams(1, 1) = x[1]; // fy
898  camPar.intrinsicParams(0, 2) = x[2]; // cx
899  camPar.intrinsicParams(1, 2) = x[3]; // cy
900 
901  for (size_t i = 0; i < 4; i++) camPar.dist[i] = x[4 + i];
902 }
903 static void cost_func(
904  const CVectorDouble& par, const TLevMarData& d, CVectorDouble& err)
905 {
906  const CObservation3DRangeScan& obs = d.obs;
907 
908  TCamera params;
909  vec2cam(par, params);
910 
911  const size_t nC = obs.rangeImage.cols();
912  const size_t nR = obs.rangeImage.rows();
913 
914  err = CVectorDouble(); // .resize( nC*nR/square(CALIB_DECIMAT) );
915 
916  for (size_t r = 0; r < nR; r += CALIB_DECIMAT)
917  {
918  for (size_t c = 0; c < nC; c += CALIB_DECIMAT)
919  {
920  const size_t idx = nC * r + c;
921 
922  TPoint3D p(
923  obs.points3D_x[idx] + d.z_offset, obs.points3D_y[idx],
924  obs.points3D_z[idx]);
925  TPoint3D P(-p.y, -p.z, p.x);
926  TPixelCoordf pixel;
927  { // mrpt-obs shouldn't depend on mrpt-vision just for this!
928  // pinhole::projectPoint_with_distortion(p_wrt_cam,cam,pixel);
929 
930  // Pinhole model:
931  const double x = P.x / P.z;
932  const double y = P.y / P.z;
933 
934  // Radial distortion:
935  const double r2 = square(x) + square(y);
936  const double r4 = square(r2);
937 
938  pixel.x = mrpt::d2f(
939  params.cx() +
940  params.fx() *
941  (x * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
942  2 * params.dist[2] * x * y +
943  params.dist[3] * (r2 + 2 * square(x)))));
944  pixel.y = mrpt::d2f(
945  params.cy() +
946  params.fy() *
947  (y * (1 + params.dist[0] * r2 + params.dist[1] * r4 +
948  2 * params.dist[3] * x * y +
949  params.dist[2] * (r2 + 2 * square(y)))));
950  }
951 
952  // In theory, it should be (r,c):
953  err.push_back(c - pixel.x);
954  err.push_back(r - pixel.y);
955  }
956  }
957 } // end error_func
958 } // namespace mrpt::obs::detail
959 
960 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters
961  * of a 3D camera given a range (depth) image and the corresponding 3D point
962  * cloud.
963  * \param camera_offset The offset (in meters) in the +X direction of the point
964  * cloud. It's 1cm for SwissRanger SR4000.
965  * \return The final average reprojection error per pixel (typ <0.05 px)
966  */
967 double CObservation3DRangeScan::recoverCameraCalibrationParameters(
968  const CObservation3DRangeScan& obs, mrpt::img::TCamera& out_camParams,
969  const double camera_offset)
970 {
971  MRPT_START
972 
973  ASSERT_(obs.hasRangeImage && obs.hasPoints3D);
974  ASSERT_(
975  obs.points3D_x.size() == obs.points3D_y.size() &&
976  obs.points3D_x.size() == obs.points3D_z.size());
977 
978  using TMyLevMar =
980  TMyLevMar::TResultInfo info;
981 
982  const size_t nR = obs.rangeImage.rows();
983  const size_t nC = obs.rangeImage.cols();
984 
985  TCamera camInit;
986  camInit.ncols = nC;
987  camInit.nrows = nR;
988  camInit.intrinsicParams(0, 0) = 250;
989  camInit.intrinsicParams(1, 1) = 250;
990  camInit.intrinsicParams(0, 2) = nC >> 1;
991  camInit.intrinsicParams(1, 2) = nR >> 1;
992 
993  CVectorDouble initial_x;
994  detail::cam2vec(camInit, initial_x);
995 
996  initial_x.resize(8);
997  CVectorDouble increments_x(initial_x.size());
998  increments_x.fill(1e-4);
999 
1000  CVectorDouble optimal_x;
1001 
1002  TMyLevMar lm;
1003  lm.execute(
1004  optimal_x, initial_x, &mrpt::obs::detail::cost_func, increments_x,
1005  detail::TLevMarData(obs, camera_offset), info,
1006  mrpt::system::LVL_INFO, /* verbose */
1007  1000, /* max iter */
1008  1e-3, 1e-9, 1e-9, false);
1009 
1010  const double avr_px_err =
1011  sqrt(info.final_sqr_err / double(nC * nR) / square(CALIB_DECIMAT));
1012 
1013  out_camParams.ncols = nC;
1014  out_camParams.nrows = nR;
1015  out_camParams.focalLengthMeters = camera_offset;
1016  detail::vec2cam(optimal_x, out_camParams);
1017 
1018  return avr_px_err;
1019 
1020  MRPT_END
1021 }
1022 
1023 void CObservation3DRangeScan::getZoneAsObs(
1024  CObservation3DRangeScan& obs, const unsigned int& r1,
1025  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2)
1026 {
1027  unsigned int cols = cameraParams.ncols;
1028  unsigned int rows = cameraParams.nrows;
1029 
1030  ASSERT_((r1 < r2) && (c1 < c2));
1031  ASSERT_((r2 < rows) && (c2 < cols));
1032  // Maybe we needed to copy more base obs atributes
1033 
1034  // Copy zone of range image
1035  obs.hasRangeImage = hasRangeImage;
1036  if (hasRangeImage)
1037  obs.rangeImage = rangeImage.asEigen().block(r2 - r1, c2 - c1, r1, c1);
1038 
1039  // Copy zone of intensity image
1040  obs.hasIntensityImage = hasIntensityImage;
1041  obs.intensityImageChannel = intensityImageChannel;
1042  if (hasIntensityImage)
1043  intensityImage.extract_patch(
1044  obs.intensityImage, c1, r1, c2 - c1, r2 - r1);
1045 
1046  // Copy zone of confidence image
1047  obs.hasConfidenceImage = hasConfidenceImage;
1048  if (hasConfidenceImage)
1049  confidenceImage.extract_patch(
1050  obs.confidenceImage, c1, r1, c2 - c1, r2 - r1);
1051 
1052  // Zone labels: It's too complex, just document that pixel labels are NOT
1053  // extracted.
1054 
1055  // Copy zone of scanned points
1056  obs.hasPoints3D = hasPoints3D;
1057  if (hasPoints3D)
1058  {
1059  // Erase a possible previous content
1060  if (obs.points3D_x.size() > 0)
1061  {
1062  obs.points3D_x.clear();
1063  obs.points3D_y.clear();
1064  obs.points3D_z.clear();
1065  }
1066 
1067  for (unsigned int i = r1; i < r2; i++)
1068  for (unsigned int j = c1; j < c2; j++)
1069  {
1070  obs.points3D_x.push_back(points3D_x.at(cols * i + j));
1071  obs.points3D_y.push_back(points3D_y.at(cols * i + j));
1072  obs.points3D_z.push_back(points3D_z.at(cols * i + j));
1073  }
1074  }
1075 
1076  obs.maxRange = maxRange;
1077  obs.sensorPose = sensorPose;
1078  obs.stdError = stdError;
1079 
1080  obs.cameraParams = cameraParams;
1081 }
1082 
1083 /** Use this method instead of resizing all three \a points3D_x, \a points3D_y &
1084  * \a points3D_z to allow the usage of the internal memory pool. */
1085 void CObservation3DRangeScan::resizePoints3DVectors(const size_t WH)
1086 {
1087 #ifdef COBS3DRANGE_USE_MEMPOOL
1088  // If WH=0 this is a clear:
1089  if (!WH)
1090  {
1091  vector_strong_clear(points3D_x);
1092  vector_strong_clear(points3D_y);
1093  vector_strong_clear(points3D_z);
1094  vector_strong_clear(points3D_idxs_x);
1095  vector_strong_clear(points3D_idxs_y);
1096  return;
1097  }
1098 
1099  if (WH <= points3D_x.size()) // reduce size, don't realloc
1100  {
1101  points3D_x.resize(WH);
1102  points3D_y.resize(WH);
1103  points3D_z.resize(WH);
1104  points3D_idxs_x.resize(WH);
1105  points3D_idxs_y.resize(WH);
1106  return;
1107  }
1108 
1109  // Request memory for the X,Y,Z buffers from the memory pool:
1111  if (pool)
1112  {
1114  mem_params.WH = WH;
1115 
1117  pool->request_memory(mem_params);
1118 
1119  if (mem_block)
1120  { // Take the memory via swaps:
1121  points3D_x.swap(mem_block->pts_x);
1122  points3D_y.swap(mem_block->pts_y);
1123  points3D_z.swap(mem_block->pts_z);
1124  points3D_idxs_x.swap(mem_block->idxs_x);
1125  points3D_idxs_y.swap(mem_block->idxs_y);
1126  delete mem_block;
1127  }
1128  }
1129 #endif
1130 
1131  // Either if there was no pool memory or we got it, make sure the size of
1132  // vectors is OK:
1133  points3D_x.resize(WH);
1134  points3D_y.resize(WH);
1135  points3D_z.resize(WH);
1136  points3D_idxs_x.resize(WH);
1137  points3D_idxs_y.resize(WH);
1138 }
1139 
1140 size_t CObservation3DRangeScan::getScanSize() const
1141 {
1142  // x,y,z vectors have the same size.
1143  return points3D_x.size();
1144 }
1145 
1146 // Similar to calling "rangeImage.setSize(H,W)" but this method provides memory
1147 // pooling to speed-up the memory allocation.
1148 void CObservation3DRangeScan::rangeImage_setSize(const int H, const int W)
1149 {
1150  bool ri_done = rangeImage.cols() == W && rangeImage.rows() == H;
1151 
1152 #ifdef COBS3DRANGE_USE_MEMPOOL
1153  // Request memory from the memory pool:
1155  if (pool && !ri_done)
1156  {
1158  mem_params.H = H;
1159  mem_params.W = W;
1160 
1162  pool->request_memory(mem_params);
1163 
1164  if (mem_block)
1165  { // Take the memory via swaps:
1166  rangeImage.swap(mem_block->rangeImage);
1167  delete mem_block;
1168  ri_done = true;
1169  }
1170  }
1171 // otherwise, continue with the normal method:
1172 #endif
1173  // Fall-back to normal method:
1174  if (!ri_done) rangeImage.setSize(H, W);
1175 
1176  // and in all cases, do the resize for the other layers:
1177  for (auto& layer : rangeImageOtherLayers) layer.second.setSize(H, W);
1178 }
1179 
1180 // Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
1181 // (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
1182 bool CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide() const
1183 {
1184  static const double EPSILON = 1e-7;
1185  static mrpt::poses::CPose3D ref_pose(0, 0, 0, -90.0_deg, 0, -90.0_deg);
1186 
1187  return (relativePoseIntensityWRTDepth.m_coords.array().abs() < EPSILON)
1188  .all() &&
1189  ((ref_pose.getRotationMatrix() -
1190  relativePoseIntensityWRTDepth.getRotationMatrix())
1191  .array()
1192  .abs() < EPSILON)
1193  .all();
1194 }
1195 
1196 void CObservation3DRangeScan::convertTo2DScan(
1198  const T3DPointsTo2DScanParams& sp, const TRangeImageFilterParams& fp)
1199 {
1200  out_scan2d.sensorLabel = sensorLabel;
1201  out_scan2d.timestamp = this->timestamp;
1202 
1203  if (!this->hasRangeImage)
1204  { // Nothing to do!
1205  out_scan2d.resizeScan(0);
1206  return;
1207  }
1208 
1209  const size_t nCols = this->rangeImage.cols();
1210  const size_t nRows = this->rangeImage.rows();
1211  if (fp.rangeMask_min)
1212  { // sanity check:
1213  ASSERT_EQUAL_(fp.rangeMask_min->cols(), rangeImage.cols());
1214  ASSERT_EQUAL_(fp.rangeMask_min->rows(), rangeImage.rows());
1215  }
1216  if (fp.rangeMask_max)
1217  { // sanity check:
1218  ASSERT_EQUAL_(fp.rangeMask_max->cols(), rangeImage.cols());
1219  ASSERT_EQUAL_(fp.rangeMask_max->rows(), rangeImage.rows());
1220  }
1221 
1222  // Compute the real horizontal FOV from the range camera intrinsic calib
1223  // data:
1224  // Note: this assumes the range image has been "undistorted", which is true
1225  // for data
1226  // from OpenNI, and will be in the future for libfreenect in MRPT,
1227  // but it's
1228  // not implemented yet (as of Mar 2012), so this is an approximation
1229  // in that case.
1230  const double cx = this->cameraParams.cx();
1231  const double cy = this->cameraParams.cy();
1232  const double fx = this->cameraParams.fx();
1233  const double fy = this->cameraParams.fy();
1234 
1235  // (Imagine the camera seen from above to understand this geometry)
1236  const double real_FOV_left = atan2(cx, fx);
1237  const double real_FOV_right = atan2(nCols - 1 - cx, fx);
1238 
1239  // FOV of the equivalent "fake" "laser scanner":
1240  const float FOV_equiv =
1241  mrpt::d2f(2 * std::max(real_FOV_left, real_FOV_right));
1242 
1243  // Now, we should create more "fake laser" points than columns in the image,
1244  // since laser scans are assumed to sample space at evenly-spaced angles,
1245  // while in images it is like ~tan(angle).
1246  ASSERT_ABOVE_(
1247  sp.oversampling_ratio, (sp.use_origin_sensor_pose ? 0.0 : 1.0));
1248  const auto nLaserRays = static_cast<size_t>(nCols * sp.oversampling_ratio);
1249 
1250  // Prepare 2D scan data fields:
1251  out_scan2d.aperture = FOV_equiv;
1252  out_scan2d.maxRange = this->maxRange;
1253  out_scan2d.resizeScan(nLaserRays);
1254 
1255  // default: all ranges=invalid
1256  out_scan2d.resizeScanAndAssign(nLaserRays, 2.0f * this->maxRange, false);
1257  if (sp.use_origin_sensor_pose)
1258  out_scan2d.sensorPose = mrpt::poses::CPose3D();
1259  else
1260  out_scan2d.sensorPose = this->sensorPose;
1261 
1262  // The vertical FOVs given by the user can be translated into limits of the
1263  // tangents (tan>0 means above, i.e. z>0):
1264  const float tan_min = mrpt::d2f(-tan(std::abs(sp.angle_inf)));
1265  const float tan_max = mrpt::d2f(tan(std::abs(sp.angle_sup)));
1266 
1267  // Precompute the tangents of the vertical angles of each "ray"
1268  // for every row in the range image:
1269  std::vector<float> vert_ang_tan(nRows);
1270  for (size_t r = 0; r < nRows; r++) vert_ang_tan[r] = d2f((cy - r) / fy);
1271 
1272  if (!sp.use_origin_sensor_pose)
1273  {
1274  // Algorithm 1: each column in the range image corresponds to a known
1275  // orientation in the 2D scan:
1276  // -------------------
1277  out_scan2d.rightToLeft = false;
1278 
1279  // Angle "counter" for the fake laser scan direction, and the increment:
1280  double ang = -FOV_equiv * 0.5;
1281  const double A_ang = FOV_equiv / (nLaserRays - 1);
1282 
1283  TRangeImageFilter rif(fp);
1284 
1285  // Go thru columns, and keep the minimum distance (along the +X axis,
1286  // not 3D distance!)
1287  // for each direction (i.e. for each column) which also lies within the
1288  // vertical FOV passed
1289  // by the user.
1290  for (size_t i = 0; i < nLaserRays; i++, ang += A_ang)
1291  {
1292  // Equivalent column in the range image for the "i'th" ray:
1293  const double tan_ang = tan(ang);
1294  // make sure we don't go out of range (just in case):
1295  const size_t c = std::min(
1296  static_cast<size_t>(std::max(0.0, cx + fx * tan_ang)),
1297  nCols - 1);
1298 
1299  bool any_valid = false;
1300  float closest_range = out_scan2d.maxRange;
1301 
1302  for (size_t r = 0; r < nRows; r++)
1303  {
1304  const float D = rangeImage.coeff(r, c) * rangeUnits;
1305  if (!rif.do_range_filter(r, c, D)) continue;
1306 
1307  // All filters passed:
1308  const float this_point_tan = vert_ang_tan[r] * D;
1309  if (this_point_tan > tan_min && this_point_tan < tan_max)
1310  {
1311  any_valid = true;
1312  mrpt::keep_min(closest_range, D);
1313  }
1314  }
1315 
1316  if (any_valid)
1317  {
1318  out_scan2d.setScanRangeValidity(i, true);
1319  // Compute the distance in 2D from the "depth" in closest_range:
1320  out_scan2d.setScanRange(
1321  i, mrpt::d2f(
1322  closest_range * std::sqrt(1.0 + tan_ang * tan_ang)));
1323  }
1324  } // end for columns
1325  }
1326  else
1327  {
1328  // Algorithm 2: project to 3D and reproject (for a different sensorPose
1329  // at the origin)
1330  // ------------------------------------------------------------------------
1331  out_scan2d.rightToLeft = true;
1332 
1333  T3DPointsProjectionParams projParams;
1334  projParams.takeIntoAccountSensorPoseOnRobot = true;
1335 
1337  this->unprojectInto(*pc, projParams, fp);
1338 
1339  const std::vector<mrpt::math::TPoint3Df>& pts = pc->getArrayPoints();
1340  const size_t N = pts.size();
1341 
1342  const double A_ang = FOV_equiv / (nLaserRays - 1);
1343  const double ang0 = -FOV_equiv * 0.5;
1344 
1345  for (size_t i = 0; i < N; i++)
1346  {
1347  if (pts[i].z < sp.z_min || pts[i].z > sp.z_max) continue;
1348 
1349  const double phi_wrt_origin = atan2(pts[i].y, pts[i].x);
1350 
1351  int i_range = mrpt::round((phi_wrt_origin - ang0) / A_ang);
1352  if (i_range < 0 || i_range >= int(nLaserRays)) continue;
1353 
1354  const float r_wrt_origin = ::hypotf(pts[i].x, pts[i].y);
1355  if (out_scan2d.getScanRange(i_range) > r_wrt_origin)
1356  out_scan2d.setScanRange(i_range, r_wrt_origin);
1357  out_scan2d.setScanRangeValidity(i_range, true);
1358  }
1359  }
1360 }
1361 
1362 void CObservation3DRangeScan::getDescriptionAsText(std::ostream& o) const
1363 {
1364  CObservation::getDescriptionAsText(o);
1365 
1366  this->load(); // Make sure the 3D point cloud, etc... are all loaded in
1367  // memory.
1368 
1369  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
1370  "base:\n";
1371  o << sensorPose.getHomogeneousMatrixVal<CMatrixDouble44>() << sensorPose
1372  << "\n";
1373 
1374  o << "maxRange = " << maxRange << " [meters]"
1375  << "\n";
1376  o << "rangeUnits = " << rangeUnits << " [meters]"
1377  << "\n";
1378 
1379  o << "Has 3D point cloud? ";
1380  if (hasPoints3D)
1381  {
1382  o << "YES: " << points3D_x.size() << " points";
1383  if (points3D_isExternallyStored())
1384  o << ". External file: " << points3D_getExternalStorageFile()
1385  << "\n";
1386  else
1387  o << " (embedded)."
1388  << "\n";
1389  }
1390  else
1391  o << "NO"
1392  << "\n";
1393 
1394  o << "Range is depth: " << (range_is_depth ? "YES" : "NO") << "\n";
1395  o << "Has raw range data? " << (hasRangeImage ? "YES" : "NO");
1396  if (hasRangeImage)
1397  {
1398  if (rangeImage_isExternallyStored())
1399  o << ". External file: " << rangeImage_getExternalStorageFile("");
1400  else
1401  o << " (embedded).";
1402  }
1403  o << "\n";
1404 
1405  for (auto& layer : rangeImageOtherLayers)
1406  {
1407  o << "Additional rangeImage layer: '" << layer.first << "'";
1408  if (rangeImage_isExternallyStored())
1409  o << ". External file: " << rangeImage_getExternalStorageFile("");
1410  else
1411  o << " (embedded).";
1412  o << "\n";
1413  }
1414 
1415  o << "\n"
1416  << "Has intensity data? " << (hasIntensityImage ? "YES" : "NO");
1417  if (hasIntensityImage)
1418  {
1419  if (intensityImage.isExternallyStored())
1420  o << ". External file: " << intensityImage.getExternalStorageFile()
1421  << "\n";
1422  else
1423  o << " (embedded).\n";
1424  // Channel?
1425  o << "Source channel: "
1428  value2name(intensityImageChannel)
1429  << "\n";
1430  }
1431 
1432  o << "\n"
1433  << "Has confidence data? " << (hasConfidenceImage ? "YES" : "NO");
1434  if (hasConfidenceImage)
1435  {
1436  if (confidenceImage.isExternallyStored())
1437  o << ". External file: " << confidenceImage.getExternalStorageFile()
1438  << "\n";
1439  else
1440  o << " (embedded)."
1441  << "\n";
1442  }
1443 
1444  o << "\n"
1445  << "Has pixel labels? " << (hasPixelLabels() ? "YES" : "NO");
1446  if (hasPixelLabels())
1447  {
1448  o << " Human readable labels:"
1449  << "\n";
1450  for (auto it = pixelLabels->pixelLabelNames.begin();
1451  it != pixelLabels->pixelLabelNames.end(); ++it)
1452  o << " label[" << it->first << "]: '" << it->second << "'"
1453  << "\n";
1454  }
1455 
1456  o << "\n"
1457  << "\n";
1458  o << "Depth camera calibration parameters:"
1459  << "\n";
1460  {
1461  CConfigFileMemory cfg;
1462  cameraParams.saveToConfigFile("DEPTH_CAM_PARAMS", cfg);
1463  o << cfg.getContent() << "\n";
1464  }
1465  o << "\n"
1466  << "Intensity camera calibration parameters:"
1467  << "\n";
1468  {
1469  CConfigFileMemory cfg;
1470  cameraParamsIntensity.saveToConfigFile("INTENSITY_CAM_PARAMS", cfg);
1471  o << cfg.getContent() << "\n";
1472  }
1473  o << "\n"
1474  << "\n"
1475  << "Pose of the intensity cam. wrt the depth cam:\n"
1476  << relativePoseIntensityWRTDepth << "\n"
1477  << relativePoseIntensityWRTDepth
1478  .getHomogeneousMatrixVal<CMatrixDouble44>()
1479  << "\n";
1480 }
1481 
1482 T3DPointsTo2DScanParams::T3DPointsTo2DScanParams()
1483  : angle_sup(mrpt::DEG2RAD(5)),
1484  angle_inf(mrpt::DEG2RAD(5)),
1485  z_min(-std::numeric_limits<double>::max()),
1486  z_max(std::numeric_limits<double>::max())
1487 {
1488 }
1489 
1491 {
1492 #if MRPT_HAS_OPENCV
1493 
1494  // DEPTH image:
1495  {
1496  // OpenCV wrapper (copy-less) for rangeImage:
1497 
1498  const cv::Mat distortion(
1499  1, cameraParams.dist.size(), CV_64F, &cameraParams.dist[0]);
1500  const cv::Mat intrinsics(
1501  3, 3, CV_64F, &cameraParams.intrinsicParams(0, 0));
1502 
1503  const auto imgSize = cv::Size(rangeImage.rows(), rangeImage.cols());
1504 
1505  double alpha = 0; // all depth pixels are visible in the output
1506  const cv::Mat newIntrinsics = cv::getOptimalNewCameraMatrix(
1507  intrinsics, distortion, imgSize, alpha);
1508 
1509  cv::Mat outRangeImg(rangeImage.rows(), rangeImage.cols(), CV_16UC1);
1510 
1511  // Undistort:
1512  const cv::Mat R_eye = cv::Mat::eye(3, 3, CV_32FC1);
1513 
1514  cv::Mat m1, m2;
1515 
1516  cv::initUndistortRectifyMap(
1517  intrinsics, distortion, R_eye, newIntrinsics, imgSize, CV_32FC1, m1,
1518  m2);
1519 
1520  for (size_t idx = 0; idx < 1 + rangeImageOtherLayers.size(); idx++)
1521  {
1522  mrpt::math::CMatrix_u16* ri = nullptr;
1523  if (idx == 0)
1524  ri = &rangeImage;
1525  else
1526  {
1527  auto it = rangeImageOtherLayers.begin();
1528  std::advance(it, idx - 1);
1529  ri = &it->second;
1530  }
1531  cv::Mat rangeImg(ri->rows(), ri->cols(), CV_16UC1, ri->data());
1532 
1533  // Remap:
1534  cv::remap(rangeImg, outRangeImg, m1, m2, cv::INTER_NEAREST);
1535  // Overwrite:
1536  outRangeImg.copyTo(rangeImg);
1537  }
1538 
1539  cameraParams.dist.fill(0);
1540  for (int r = 0; r < 3; r++)
1541  for (int c = 0; c < 3; c++)
1543  newIntrinsics.at<double>(r, c);
1544  }
1545 
1546  // RGB image:
1547  if (hasIntensityImage)
1548  {
1549  mrpt::img::CImage newIntImg;
1551 
1552  intensityImage = std::move(newIntImg);
1553  cameraParamsIntensity.dist.fill(0);
1554  }
1555 
1556 #else
1557  THROW_EXCEPTION("This method requires OpenCV");
1558 #endif
1559 }
1560 
1562  const mrpt::math::CMatrix_u16& ri, float val_min, float val_max,
1563  float rangeUnits, const std::optional<mrpt::img::TColormap> color)
1564 {
1565 #if MRPT_HAS_OPENCV
1566  if (val_max < 1e-4f) val_max = ri.maxCoeff() * rangeUnits;
1567 
1568  ASSERT_ABOVE_(val_max, val_min);
1569 
1570  const float range_inv = rangeUnits / (val_max - val_min);
1571 
1572  ASSERT_ABOVE_(ri.cols(), 0);
1573  ASSERT_ABOVE_(ri.rows(), 0);
1574 
1575  mrpt::img::CImage img;
1576  const int cols = ri.cols(), rows = ri.rows();
1577 
1578  const auto col = color.value_or(mrpt::img::TColormap::cmGRAYSCALE);
1579 
1580  const bool is_gray = (col == mrpt::img::TColormap::cmGRAYSCALE);
1581 
1582  img.resize(cols, rows, is_gray ? mrpt::img::CH_GRAY : mrpt::img::CH_RGB);
1583 
1584  for (int r = 0; r < rows; r++)
1585  {
1586  for (int c = 0; c < cols; c++)
1587  {
1588  // Normalized value in the range [0,1]:
1589  const float val_01 = (ri.coeff(r, c) - val_min) * range_inv;
1590  if (is_gray)
1591  {
1592  img.setPixel(c, r, static_cast<uint8_t>(val_01 * 255));
1593  }
1594  else
1595  {
1596  float R, G, B;
1597  mrpt::img::colormap(col, val_01, R, G, B);
1598 
1599  img.setPixel(
1600  c, r,
1602  static_cast<uint8_t>(R * 255),
1603  static_cast<uint8_t>(G * 255),
1604  static_cast<uint8_t>(B * 255)));
1605  }
1606  }
1607  }
1608 
1609  return img;
1610 #else
1611  THROW_EXCEPTION("This method requires OpenCV");
1612 #endif
1613 }
1614 
1616  const std::optional<mrpt::img::TColormap> color,
1617  const std::optional<float> normMinRange,
1618  const std::optional<float> normMaxRange,
1619  const std::optional<std::string> additionalLayerName) const
1620 {
1621  ASSERT_(this->hasRangeImage);
1622  const mrpt::math::CMatrix_u16* ri =
1623  (!additionalLayerName || additionalLayerName->empty())
1624  ? &rangeImage
1625  : &rangeImageOtherLayers.at(*additionalLayerName);
1626 
1627  const float val_min = normMinRange.value_or(.0f);
1628  const float val_max = normMaxRange.value_or(this->maxRange);
1629  ASSERT_ABOVE_(val_max, val_min);
1630 
1631  return rangeImageAsImage(*ri, val_min, val_max, rangeUnits, color);
1632 }
Scalar maxCoeff() const
Maximum value in the matrix/vector.
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:40
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
std::map< std::string, mrpt::math::CMatrix_u16 > rangeImageOtherLayers
Additional layer range/depth images.
mrpt::aligned_std_vector< float > Kxs
x,y,z: +X pointing forward (depth), +z up These doesn&#39;t account for the sensor pose.
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::unprojectInto()
#define MRPT_START
Definition: exceptions.h:241
bool isSuitable(const CObservation3DRangeScan_Ranges_MemPoolParams &req) const
mrpt::img::TCamera calib
static bool EXTERNALS_AS_TEXT_value
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
size_t WH
Width*Height, that is, the number of 3D points.
TIntensityChannelID
Enum type for intensityImageChannel.
void setCol(const Index col, const VECTOR &v)
#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:175
void mempool_donate_range_matrix(CObservation3DRangeScan &obs)
static void mempool_donate_xyz_buffers(CObservation3DRangeScan &obs)
const T * data() const
Return raw pointer to row-major data buffer.
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)
const double G
void fill(const Scalar &val)
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.
void swap(CMatrixDynamic< T > &o)
Swap with another matrix very efficiently (just swaps a pointer and two integer values).
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
void dump_to_pool(const DATA_PARAMS &params, POOLABLE_DATA *block)
Saves the passed data block (characterized by params) to the pool.
double angle_sup
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
This file implements several operations that operate element-wise on individual or pairs of container...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void undistort()
Removes the distortion in both, depth and intensity images.
mrpt::vision::TStereoCalibParams params
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:18
STL namespace.
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.
static std::unordered_map< LUT_info, CObservation3DRangeScan::unproject_LUT_t > LUTs
mrpt::poses::CPose3D sensorPose
static std::mutex LUTs_mtx
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::unprojectInto()
void push_back(const T &val)
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:49
#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:592
A generic system for versatile memory pooling.
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
double focalLengthMeters
The focal length of the camera, in meters (can be used among &#39;intrinsicParams&#39; to determine the pixel...
Definition: TCamera.h:56
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:173
CVectorDynamic< double > CVectorDouble
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
A helper class that can convert an enum value into its textual representation, and viceversa...
STORED_TYPE ReadAs()
De-serialize a variable and returns it by value.
Definition: CArchive.h:155
#define CALIB_DECIMAT
void unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
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>
bool operator==(const LUT_info &a, const LUT_info &o)
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:26
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
std::string 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...
bool hasPoints3D
true means the field points3D contains valid data.
size_type rows() const
Number of rows in the matrix.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
const Scalar & coeff(int r, int c) const
return_t square(const num_t x)
Inline function for the square of a number.
fixed floating point &#39;f&#39;
static void cost_func(const CVectorDouble &par, const TLevMarData &d, CVectorDouble &err)
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:171
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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.
static void cam2vec(const TCamera &camPar, CVectorDouble &x)
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
size_t operator()(const LUT_info &k) const
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void vector_strong_clear(VECTOR_T &v)
Like calling a std::vector<>&#39;s clear() method, but really forcing deallocating the memory...
Definition: bits_mem.h:18
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void resizeScan(const size_t len)
Resizes all data vectors to allocate a given number of scan rays.
TPixelLabelInfoBase::Ptr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Used in CObservation3DRangeScan::unprojectInto()
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
#define MRPT_END
Definition: exceptions.h:245
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
Builds a visualization from the rangeImage.
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
Transparently opens a compressed "gz" file and reads uncompressed data from it.
TPoint3D_< float > TPoint3Df
Definition: TPoint3D.h:269
A RGB color - 8bit.
Definition: TColor.h:25
void resize(std::size_t N, bool zeroNewElements=false)
Saves data to a file and transparently compress the data using the given compression level...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
mrpt::config::CConfigFileMemory CConfigFileMemory
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image...
Definition: CImage.cpp:1691
static void vec2cam(const CVectorDouble &x, TCamera &camPar)
bool strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
TLevMarData(const CObservation3DRangeScan &obs_, const double z_offset_)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
const CObservation3DRangeScan & obs
static mrpt::img::CImage rangeImageAsImage(const mrpt::math::CMatrix_u16 &ranges, float val_min, float val_max, float rangeUnits, const std::optional< mrpt::img::TColormap > color=std::nullopt)
Static method to convert a range matrix into an image.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
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".
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.
void setScanRangeValidity(const size_t i, const bool val)
bool hasConfidenceImage
true means the field confidenceImage contains valid data
bool do_range_filter(size_t r, size_t c, const float D) const
Returns true if the point (r,c) with depth D passes all filters.
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020