MRPT  2.0.2
CPointsMap.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 "maps-precomp.h" // Precomp header
11 
13 #include <mrpt/maps/CPointsMap.h>
15 #include <mrpt/math/TPose2D.h>
16 #include <mrpt/math/geometry.h>
25 #include <mrpt/system/CTicTac.h>
27 #include <mrpt/system/os.h>
28 
29 #include <mrpt/core/SSE_macros.h>
30 #include <mrpt/core/SSE_types.h>
31 
32 #if MRPT_HAS_MATLAB
33 #include <mexplus.h>
34 #endif
35 
36 using namespace mrpt::poses;
37 using namespace mrpt::maps;
38 using namespace mrpt::math;
39 using namespace mrpt::tfest;
40 using namespace mrpt::obs;
41 using namespace mrpt::img;
42 using namespace mrpt::system;
43 using namespace std;
44 
46 
47 /*---------------------------------------------------------------
48  Constructor
49  ---------------------------------------------------------------*/
51  : insertionOptions(), likelihoodOptions(), m_x(), m_y(), m_z()
52 
53 {
54  mark_as_modified();
55 }
56 
57 /*---------------------------------------------------------------
58  Destructor
59  ---------------------------------------------------------------*/
60 CPointsMap::~CPointsMap() = default;
61 /*---------------------------------------------------------------
62  save2D_to_text_file
63  Save to a text file. In each line there are a point coordinates.
64  Returns false if any error occured, true elsewere.
65  ---------------------------------------------------------------*/
66 bool CPointsMap::save2D_to_text_file(const string& file) const
67 {
68  FILE* f = os::fopen(file.c_str(), "wt");
69  if (!f) return false;
70 
71  for (unsigned int i = 0; i < m_x.size(); i++)
72  os::fprintf(f, "%f %f\n", m_x[i], m_y[i]);
73 
74  os::fclose(f);
75  return true;
76 }
77 
78 /*---------------------------------------------------------------
79  save3D_to_text_file
80  Save to a text file. In each line there are a point coordinates.
81  Returns false if any error occured, true elsewere.
82  ---------------------------------------------------------------*/
83 bool CPointsMap::save3D_to_text_file(const string& file) const
84 {
85  FILE* f = os::fopen(file.c_str(), "wt");
86  if (!f) return false;
87 
88  for (unsigned int i = 0; i < m_x.size(); i++)
89  os::fprintf(f, "%f %f %f\n", m_x[i], m_y[i], m_z[i]);
90 
91  os::fclose(f);
92  return true;
93 }
94 
95 /*---------------------------------------------------------------
96  load2Dor3D_from_text_file
97  Load from a text file. In each line there are a point coordinates.
98  Returns false if any error occured, true elsewere.
99  ---------------------------------------------------------------*/
100 bool CPointsMap::load2Dor3D_from_text_file(
101  const std::string& file, const bool is_3D)
102 {
103  MRPT_START
104 
105  mark_as_modified();
106 
107  FILE* f = os::fopen(file.c_str(), "rt");
108  if (!f) return false;
109 
110  char str[1024];
111  char *ptr, *ptr1, *ptr2, *ptr3;
112 
113  // Clear current map:
114  this->clear();
115 
116  while (!feof(f))
117  {
118  // Read one string line:
119  str[0] = 0;
120  if (!fgets(str, sizeof(str), f)) break;
121 
122  // Find the first digit:
123  ptr = str;
124  while (ptr[0] && (ptr[0] == ' ' || ptr[0] == '\t' || ptr[0] == '\r' ||
125  ptr[0] == '\n'))
126  ptr++;
127 
128  // And try to parse it:
129  float xx = strtod(ptr, &ptr1);
130  if (ptr1 != str)
131  {
132  float yy = strtod(ptr1, &ptr2);
133  if (ptr2 != ptr1)
134  {
135  if (!is_3D)
136  {
137  this->insertPoint(xx, yy, 0);
138  }
139  else
140  {
141  float zz = strtod(ptr2, &ptr3);
142  if (ptr3 != ptr2)
143  {
144  this->insertPoint(xx, yy, zz);
145  }
146  }
147  }
148  }
149  }
150 
151  os::fclose(f);
152  return true;
153 
154  MRPT_END
155 }
156 
157 /*---------------------------------------------------------------
158  Implements the writing to a mxArray for Matlab
159  ---------------------------------------------------------------*/
160 #if MRPT_HAS_MATLAB
161 // Add to implement mexplus::from template specialization
163 #endif
164 
165 mxArray* CPointsMap::writeToMatlab() const
166 {
167 #if MRPT_HAS_MATLAB
168  MRPT_TODO("Create 3xN array xyz of points coordinates")
169  const char* fields[] = {"x", "y", "z"};
170  mexplus::MxArray map_struct(
171  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
172 
173  map_struct.set("x", m_x);
174  map_struct.set("y", m_y);
175  map_struct.set("z", m_z);
176  return map_struct.release();
177 #else
178  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
179 #endif
180 }
181 
182 /*---------------------------------------------------------------
183  getPoint
184  Access to stored points coordinates:
185  ---------------------------------------------------------------*/
186 void CPointsMap::getPoint(size_t index, float& x, float& y) const
187 {
188  ASSERT_BELOW_(index, m_x.size());
189  x = m_x[index];
190  y = m_y[index];
191 }
192 void CPointsMap::getPoint(size_t index, float& x, float& y, float& z) const
193 {
194  ASSERT_BELOW_(index, m_x.size());
195  x = m_x[index];
196  y = m_y[index];
197  z = m_z[index];
198 }
199 void CPointsMap::getPoint(size_t index, double& x, double& y) const
200 {
201  ASSERT_BELOW_(index, m_x.size());
202  x = m_x[index];
203  y = m_y[index];
204 }
205 void CPointsMap::getPoint(size_t index, double& x, double& y, double& z) const
206 {
207  ASSERT_BELOW_(index, m_x.size());
208  x = m_x[index];
209  y = m_y[index];
210  z = m_z[index];
211 }
212 
213 /*---------------------------------------------------------------
214  getPointsBuffer
215  ---------------------------------------------------------------*/
216 void CPointsMap::getPointsBuffer(
217  size_t& outPointsCount, const float*& xs, const float*& ys,
218  const float*& zs) const
219 {
220  outPointsCount = size();
221 
222  if (outPointsCount > 0)
223  {
224  xs = &m_x[0];
225  ys = &m_y[0];
226  zs = &m_z[0];
227  }
228  else
229  {
230  xs = ys = zs = nullptr;
231  }
232 }
233 
234 /*---------------------------------------------------------------
235  clipOutOfRangeInZ
236  ---------------------------------------------------------------*/
237 void CPointsMap::clipOutOfRangeInZ(float zMin, float zMax)
238 {
239  const size_t n = size();
240  vector<bool> deletionMask(n);
241 
242  // Compute it:
243  for (size_t i = 0; i < n; i++)
244  deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);
245 
246  // Perform deletion:
247  applyDeletionMask(deletionMask);
248 
249  mark_as_modified();
250 }
251 
252 /*---------------------------------------------------------------
253  clipOutOfRange
254  ---------------------------------------------------------------*/
255 void CPointsMap::clipOutOfRange(const TPoint2D& p, float maxRange)
256 {
257  size_t i, n = size();
258  vector<bool> deletionMask;
259 
260  // The deletion mask:
261  deletionMask.resize(n);
262 
263  const auto max_sq = maxRange * maxRange;
264 
265  // Compute it:
266  for (i = 0; i < n; i++)
267  deletionMask[i] = square(p.x - m_x[i]) + square(p.y - m_y[i]) > max_sq;
268 
269  // Perform deletion:
270  applyDeletionMask(deletionMask);
271 
272  mark_as_modified();
273 }
274 
275 void CPointsMap::determineMatching2D(
276  const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
277  TMatchingPairList& correspondences, const TMatchingParams& params,
278  TMatchingExtraResults& extraResults) const
279 {
280  MRPT_START
281 
282  extraResults = TMatchingExtraResults(); // Clear output
283 
284  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
286  params.offset_other_map_points, params.decimation_other_map_points);
287  ASSERT_(IS_DERIVED(*otherMap2, CPointsMap));
288  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
289 
290  const TPose2D otherMapPose(
291  otherMapPose_.x(), otherMapPose_.y(), otherMapPose_.phi());
292 
293  const size_t nLocalPoints = otherMap->size();
294  const size_t nGlobalPoints = this->size();
295  float _sumSqrDist = 0;
296  size_t _sumSqrCount = 0;
297  size_t nOtherMapPointsWithCorrespondence =
298  0; // Number of points with one corrs. at least
299 
300  float local_x_min = std::numeric_limits<float>::max(),
301  local_x_max = -std::numeric_limits<float>::max();
302  float global_x_min = std::numeric_limits<float>::max(),
303  global_x_max = -std::numeric_limits<float>::max();
304  float local_y_min = std::numeric_limits<float>::max(),
305  local_y_max = -std::numeric_limits<float>::max();
306  float global_y_min = std::numeric_limits<float>::max(),
307  global_y_max = -std::numeric_limits<float>::max();
308 
309  double maxDistForCorrespondenceSquared;
310  float x_local, y_local;
311  unsigned int localIdx;
312 
313  const float *x_other_it, *y_other_it, *z_other_it;
314 
315  // Prepare output: no correspondences initially:
316  correspondences.clear();
317  correspondences.reserve(nLocalPoints);
318  extraResults.correspondencesRatio = 0;
319 
320  TMatchingPairList _correspondences;
321  _correspondences.reserve(nLocalPoints);
322 
323  // Nothing to do if we have an empty map!
324  if (!nGlobalPoints || !nLocalPoints) return;
325 
326  const double sin_phi = sin(otherMapPose.phi);
327  const double cos_phi = cos(otherMapPose.phi);
328 
329 // Do matching only there is any chance of the two maps to overlap:
330 // -----------------------------------------------------------
331 // Translate and rotate all local points, while simultaneously
332 // estimating the bounding box:
333 #if MRPT_HAS_SSE2
334  // Number of 4-floats:
335  size_t nPackets = nLocalPoints / 4;
336 
337  Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);
338 
339  // load 4 copies of the same value
340  const __m128 cos_4val = _mm_set1_ps(cos_phi);
341  const __m128 sin_4val = _mm_set1_ps(sin_phi);
342  const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
343  const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);
344 
345  // For the bounding box:
346  __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
347  __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
348  __m128 y_mins = x_mins;
349  __m128 y_maxs = x_maxs;
350 
351  const float* ptr_in_x = &otherMap->m_x[0];
352  const float* ptr_in_y = &otherMap->m_y[0];
353  float* ptr_out_x = &x_locals[0];
354  float* ptr_out_y = &y_locals[0];
355 
356  for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
357  ptr_out_y += 4)
358  {
359  const __m128 xs = _mm_loadu_ps(ptr_in_x); // *Unaligned* load
360  const __m128 ys = _mm_loadu_ps(ptr_in_y);
361 
362  const __m128 lxs = _mm_add_ps(
363  x0_4val,
364  _mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
365  const __m128 lys = _mm_add_ps(
366  y0_4val,
367  _mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
368  _mm_store_ps(ptr_out_x, lxs);
369  _mm_store_ps(ptr_out_y, lys);
370 
371  x_mins = _mm_min_ps(x_mins, lxs);
372  x_maxs = _mm_max_ps(x_maxs, lxs);
373  y_mins = _mm_min_ps(y_mins, lys);
374  y_maxs = _mm_max_ps(y_maxs, lys);
375  }
376 
377  // Recover the min/max:
378  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float temp_nums[4];
379 
380  _mm_store_ps(temp_nums, x_mins);
381  local_x_min =
382  min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
383  _mm_store_ps(temp_nums, y_mins);
384  local_y_min =
385  min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
386  _mm_store_ps(temp_nums, x_maxs);
387  local_x_max =
388  max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
389  _mm_store_ps(temp_nums, y_maxs);
390  local_y_max =
391  max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
392 
393  // transform extra pts & update BBox
394  for (size_t k = 0; k < nLocalPoints % 4; k++)
395  {
396  float x = ptr_in_x[k];
397  float y = ptr_in_y[k];
398  float out_x = otherMapPose.x + cos_phi * x - sin_phi * y;
399  float out_y = otherMapPose.y + sin_phi * x + cos_phi * y;
400 
401  local_x_min = std::min(local_x_min, out_x);
402  local_x_max = std::max(local_x_max, out_x);
403 
404  local_y_min = std::min(local_y_min, out_y);
405  local_y_max = std::max(local_y_max, out_y);
406 
407  ptr_out_x[k] = out_x;
408  ptr_out_y[k] = out_y;
409  }
410 
411 #else
412  // Non SSE2 version:
414  const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
416  const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);
417 
418  Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
419  otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
420  Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
421  otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();
422 
423  local_x_min = x_locals.minCoeff();
424  local_y_min = y_locals.minCoeff();
425  local_x_max = x_locals.maxCoeff();
426  local_y_max = y_locals.maxCoeff();
427 #endif
428 
429  // Find the bounding box:
430  float global_z_min, global_z_max;
431  this->boundingBox(
432  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
433  global_z_max);
434 
435  // Only try doing a matching if there exist any chance of both maps
436  // touching/overlaping:
437  if (local_x_min > global_x_max || local_x_max < global_x_min ||
438  local_y_min > global_y_max || local_y_max < global_y_min)
439  return; // We know for sure there is no matching at all
440 
441  // Loop for each point in local map:
442  // --------------------------------------------------
443  for (localIdx = params.offset_other_map_points,
444  x_other_it = &otherMap->m_x[params.offset_other_map_points],
445  y_other_it = &otherMap->m_y[params.offset_other_map_points],
446  z_other_it = &otherMap->m_z[params.offset_other_map_points];
447  localIdx < nLocalPoints;
448  x_other_it += params.decimation_other_map_points,
449  y_other_it += params.decimation_other_map_points,
450  z_other_it += params.decimation_other_map_points,
451  localIdx += params.decimation_other_map_points)
452  {
453  // For speed-up:
454  x_local = x_locals[localIdx]; // *x_locals_it;
455  y_local = y_locals[localIdx]; // *y_locals_it;
456 
457  // Find all the matchings in the requested distance:
458 
459  // KD-TREE implementation =================================
460  // Use a KD-tree to look for the nearnest neighbor of:
461  // (x_local, y_local, z_local)
462  // In "this" (global/reference) points map.
463 
464  float tentativ_err_sq;
465  unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
466  x_local, y_local, // Look closest to this guy
467  tentativ_err_sq // save here the min. distance squared
468  );
469 
470  // Compute max. allowed distance:
471  maxDistForCorrespondenceSquared = square(
472  params.maxAngularDistForCorrespondence *
473  std::sqrt(
474  square(params.angularDistPivotPoint.x - x_local) +
475  square(params.angularDistPivotPoint.y - y_local)) +
476  params.maxDistForCorrespondence);
477 
478  // Distance below the threshold??
479  if (tentativ_err_sq < maxDistForCorrespondenceSquared)
480  {
481  // Save all the correspondences:
482  _correspondences.resize(_correspondences.size() + 1);
483 
484  TMatchingPair& p = _correspondences.back();
485 
486  p.this_idx = tentativ_this_idx;
487  p.this_x = m_x[tentativ_this_idx];
488  p.this_y = m_y[tentativ_this_idx];
489  p.this_z = m_z[tentativ_this_idx];
490 
491  p.other_idx = localIdx;
492  p.other_x = *x_other_it;
493  p.other_y = *y_other_it;
494  p.other_z = *z_other_it;
495 
496  p.errorSquareAfterTransformation = tentativ_err_sq;
497 
498  // At least one:
499  nOtherMapPointsWithCorrespondence++;
500 
501  // Accumulate the MSE:
502  _sumSqrDist += p.errorSquareAfterTransformation;
503  _sumSqrCount++;
504  }
505 
506  } // For each local point
507 
508  // Additional consistency filter: "onlyKeepTheClosest" up to now
509  // led to just one correspondence for each "local map" point, but
510  // many of them may have as corresponding pair the same "global point"!!
511  // -------------------------------------------------------------------------
512  if (params.onlyUniqueRobust)
513  {
514  ASSERTMSG_(
515  params.onlyKeepTheClosest,
516  "ERROR: onlyKeepTheClosest must be also set to true when "
517  "onlyUniqueRobust=true.");
518  _correspondences.filterUniqueRobustPairs(
519  nGlobalPoints, correspondences);
520  }
521  else
522  {
523  correspondences.swap(_correspondences);
524  }
525 
526  // If requested, copy sum of squared distances to output pointer:
527  // -------------------------------------------------------------------
528  if (_sumSqrCount)
529  extraResults.sumSqrDist =
530  _sumSqrDist / static_cast<double>(_sumSqrCount);
531  else
532  extraResults.sumSqrDist = 0;
533 
534  // The ratio of points in the other map with corrs:
535  extraResults.correspondencesRatio = params.decimation_other_map_points *
536  nOtherMapPointsWithCorrespondence /
537  d2f(nLocalPoints);
538 
539  MRPT_END
540 }
541 
542 /*---------------------------------------------------------------
543  changeCoordinatesReference
544  ---------------------------------------------------------------*/
545 void CPointsMap::changeCoordinatesReference(const CPose2D& newBase)
546 {
547  const size_t N = m_x.size();
548 
549  const CPose3D newBase3D(newBase);
550 
551  for (size_t i = 0; i < N; i++)
552  newBase3D.composePoint(
553  m_x[i], m_y[i], m_z[i], // In
554  m_x[i], m_y[i], m_z[i] // Out
555  );
556 
557  mark_as_modified();
558 }
559 
560 /*---------------------------------------------------------------
561  changeCoordinatesReference
562  ---------------------------------------------------------------*/
563 void CPointsMap::changeCoordinatesReference(const CPose3D& newBase)
564 {
565  const size_t N = m_x.size();
566 
567  for (size_t i = 0; i < N; i++)
568  newBase.composePoint(
569  m_x[i], m_y[i], m_z[i], // In
570  m_x[i], m_y[i], m_z[i] // Out
571  );
572 
573  mark_as_modified();
574 }
575 
576 /*---------------------------------------------------------------
577  changeCoordinatesReference
578  ---------------------------------------------------------------*/
579 void CPointsMap::changeCoordinatesReference(
580  const CPointsMap& other, const CPose3D& newBase)
581 {
582  *this = other;
583  changeCoordinatesReference(newBase);
584 }
585 
586 /*---------------------------------------------------------------
587  isEmpty
588  ---------------------------------------------------------------*/
589 bool CPointsMap::isEmpty() const { return m_x.empty(); }
590 /*---------------------------------------------------------------
591  TInsertionOptions
592  ---------------------------------------------------------------*/
593 CPointsMap::TInsertionOptions::TInsertionOptions()
594  : horizontalTolerance(0.05_deg)
595 
596 {
597 }
598 
599 // Binary dump to/read from stream - for usage in derived classes' serialization
602 {
603  const int8_t version = 0;
604  out << version;
605 
606  out << minDistBetweenLaserPoints << addToExistingPointsMap
607  << also_interpolate << disableDeletion << fuseWithExisting
608  << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
609  << insertInvalidPoints; // v0
610 }
611 
614 {
615  int8_t version;
616  in >> version;
617  switch (version)
618  {
619  case 0:
620  {
621  in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
622  also_interpolate >> disableDeletion >> fuseWithExisting >>
623  isPlanarMap >> horizontalTolerance >>
624  maxDistForInterpolatePoints >> insertInvalidPoints; // v0
625  }
626  break;
627  default:
629  }
630 }
631 
633 
634  = default;
635 
638 {
639  const int8_t version = 0;
640  out << version;
641  out << sigma_dist << max_corr_distance << decimation;
642 }
643 
646 {
647  int8_t version;
648  in >> version;
649  switch (version)
650  {
651  case 0:
652  {
653  in >> sigma_dist >> max_corr_distance >> decimation;
654  }
655  break;
656  default:
658  }
659 }
660 
663 {
664  const int8_t version = 0;
665  out << version;
666  out << point_size << color << int8_t(colormap);
667 }
668 
671 {
672  int8_t version;
673  in >> version;
674  switch (version)
675  {
676  case 0:
677  {
678  in >> point_size >> this->color;
679  in.ReadAsAndCastTo<int8_t>(this->colormap);
680  }
681  break;
682  default:
684  }
685 }
686 
688 {
689  out << "\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
690 
691  LOADABLEOPTS_DUMP_VAR(minDistBetweenLaserPoints, double);
692  LOADABLEOPTS_DUMP_VAR(maxDistForInterpolatePoints, double);
693  LOADABLEOPTS_DUMP_VAR_DEG(horizontalTolerance);
694 
695  LOADABLEOPTS_DUMP_VAR(addToExistingPointsMap, bool);
696  LOADABLEOPTS_DUMP_VAR(also_interpolate, bool);
697  LOADABLEOPTS_DUMP_VAR(disableDeletion, bool);
698  LOADABLEOPTS_DUMP_VAR(fuseWithExisting, bool);
699  LOADABLEOPTS_DUMP_VAR(isPlanarMap, bool);
700 
701  LOADABLEOPTS_DUMP_VAR(insertInvalidPoints, bool);
702 
703  out << endl;
704 }
705 
707 {
708  out << "\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
709 
710  LOADABLEOPTS_DUMP_VAR(sigma_dist, double);
711  LOADABLEOPTS_DUMP_VAR(max_corr_distance, double);
712  LOADABLEOPTS_DUMP_VAR(decimation, int);
713 }
714 
716 {
717  out << "\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
718 
719  LOADABLEOPTS_DUMP_VAR(point_size, float);
720  LOADABLEOPTS_DUMP_VAR(color.R, float);
721  LOADABLEOPTS_DUMP_VAR(color.G, float);
722  LOADABLEOPTS_DUMP_VAR(color.B, float);
723  // LOADABLEOPTS_DUMP_VAR(colormap, int);
724 }
725 /*---------------------------------------------------------------
726  loadFromConfigFile
727  ---------------------------------------------------------------*/
729  const mrpt::config::CConfigFileBase& iniFile, const string& section)
730 {
731  MRPT_LOAD_CONFIG_VAR(minDistBetweenLaserPoints, float, iniFile, section);
732  MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);
733 
734  MRPT_LOAD_CONFIG_VAR(addToExistingPointsMap, bool, iniFile, section);
735  MRPT_LOAD_CONFIG_VAR(also_interpolate, bool, iniFile, section);
736  MRPT_LOAD_CONFIG_VAR(disableDeletion, bool, iniFile, section);
737  MRPT_LOAD_CONFIG_VAR(fuseWithExisting, bool, iniFile, section);
738  MRPT_LOAD_CONFIG_VAR(isPlanarMap, bool, iniFile, section);
739 
740  MRPT_LOAD_CONFIG_VAR(maxDistForInterpolatePoints, float, iniFile, section);
741 
742  MRPT_LOAD_CONFIG_VAR(insertInvalidPoints, bool, iniFile, section);
743 }
744 
746  const mrpt::config::CConfigFileBase& iniFile, const string& section)
747 {
748  MRPT_LOAD_CONFIG_VAR(sigma_dist, double, iniFile, section);
749  MRPT_LOAD_CONFIG_VAR(max_corr_distance, double, iniFile, section);
750  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
751 }
752 
754  const mrpt::config::CConfigFileBase& iniFile, const string& section)
755 {
756  MRPT_LOAD_CONFIG_VAR(point_size, float, iniFile, section);
757  MRPT_LOAD_CONFIG_VAR(color.R, float, iniFile, section);
758  MRPT_LOAD_CONFIG_VAR(color.G, float, iniFile, section);
759  MRPT_LOAD_CONFIG_VAR(color.B, float, iniFile, section);
760  colormap = iniFile.read_enum(section, "colormap", this->colormap);
761 }
762 
763 /*---------------------------------------------------------------
764  getAs3DObject
765 ---------------------------------------------------------------*/
767 {
768  MRPT_START
770 
772  {
773  // Single color:
774  auto obj = opengl::CPointCloud::Create();
775  obj->loadFromPointsMap(this);
776  obj->setColor(renderOptions.color);
777  obj->setPointSize(renderOptions.point_size);
778  obj->enableColorFromZ(false);
779  outObj->insert(obj);
780  }
781  else
782  {
784  obj->loadFromPointsMap(this);
785  obj->setPointSize(renderOptions.point_size);
786  mrpt::math::TPoint3D pMin, pMax;
787  this->boundingBox(pMin, pMax);
788  obj->recolorizeByCoordinate(
789  pMin.z, pMax.z, 2 /*z*/, renderOptions.colormap);
790  outObj->insert(obj);
791  }
792  MRPT_END
793 }
794 
796  const mrpt::maps::CMetricMap* otherMap2,
797  const mrpt::poses::CPose3D& otherMapPose,
798  const TMatchingRatioParams& mrp) const
799 {
800  TMatchingPairList correspondences;
802  TMatchingExtraResults extraResults;
803 
804  params.maxDistForCorrespondence = mrp.maxDistForCorr;
805 
806  this->determineMatching3D(
807  otherMap2->getAsSimplePointsMap(), otherMapPose, correspondences,
808  params, extraResults);
809 
810  return extraResults.correspondencesRatio;
811 }
812 
813 /*---------------------------------------------------------------
814  getLargestDistanceFromOrigin
815 ---------------------------------------------------------------*/
817 {
818  // Updated?
820  {
821  // NO: Update it:
822  float maxDistSq = 0, d;
823  for (auto X = m_x.begin(), Y = m_y.begin(), Z = m_z.begin();
824  X != m_x.end(); ++X, ++Y, ++Z)
825  {
826  d = square(*X) + square(*Y) + square(*Z);
827  maxDistSq = max(d, maxDistSq);
828  }
829 
830  m_largestDistanceFromOrigin = sqrt(maxDistSq);
832  }
834 }
835 
836 /*---------------------------------------------------------------
837  getAllPoints
838 ---------------------------------------------------------------*/
840  vector<float>& xs, vector<float>& ys, size_t decimation) const
841 {
842  MRPT_START
843  ASSERT_(decimation > 0);
844  if (decimation == 1)
845  {
846  xs = vector<float>(m_x.begin(), m_x.end());
847  ys = vector<float>(m_y.begin(), m_y.end());
848  }
849  else
850  {
851  size_t N = m_x.size() / decimation;
852 
853  xs.resize(N);
854  ys.resize(N);
855 
856  auto X = m_x.begin();
857  auto Y = m_y.begin();
858  for (auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
859  X += decimation, Y += decimation, ++oX, ++oY)
860  {
861  *oX = *X;
862  *oY = *Y;
863  }
864  }
865  MRPT_END
866 }
867 
868 /*---------------------------------------------------------------
869  squareDistanceToClosestCorrespondence
870 ---------------------------------------------------------------*/
872  float x0, float y0) const
873 {
874  // Just the closest point:
875 
876 #if 1
877  return kdTreeClosestPoint2DsqrError(x0, y0);
878 #else
879  // The distance to the line that interpolates the TWO closest points:
880  float x1, y1, x2, y2, d1, d2;
882  x0, y0, // The query
883  x1, y1, // Closest point #1
884  x2, y2, // Closest point #2
885  d1, d2);
886 
887  ASSERT_(d2 >= d1);
888 
889  // If the two points are too far, do not interpolate:
890  float d12 = square(x1 - x2) + square(y1 - y2);
891  if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
892  {
893  return square(x1 - x0) + square(y1 - y0);
894  }
895  else
896  { // Interpolate
897  double interp_x, interp_y;
898 
899  // math::closestFromPointToSegment(
901  x0, y0, // the point
902  x1, y1, x2, y2, // The segment
903  interp_x, interp_y // out
904  );
905 
906  return square(interp_x - x0) + square(interp_y - y0);
907  }
908 #endif
909 }
910 
911 /*---------------------------------------------------------------
912  boundingBox
913 ---------------------------------------------------------------*/
915  float& min_x, float& max_x, float& min_y, float& max_y, float& min_z,
916  float& max_z) const
917 {
918  MRPT_START
919 
920  const size_t nPoints = m_x.size();
921 
923  {
924  if (!nPoints)
925  {
927  m_bb_max_z = 0;
928  }
929  else
930  {
931 #if MRPT_HAS_SSE2
932  // Vectorized version: ~ 9x times faster
933 
934  // Number of 4-floats:
935  size_t nPackets = nPoints / 4;
936 
937  // For the bounding box:
938  __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
939  __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
940  __m128 y_mins = x_mins, y_maxs = x_maxs;
941  __m128 z_mins = x_mins, z_maxs = x_maxs;
942 
943  const float* ptr_in_x = &m_x[0];
944  const float* ptr_in_y = &m_y[0];
945  const float* ptr_in_z = &m_z[0];
946 
947  for (; nPackets;
948  nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
949  {
950  const __m128 xs = _mm_loadu_ps(ptr_in_x); // *Unaligned* load
951  x_mins = _mm_min_ps(x_mins, xs);
952  x_maxs = _mm_max_ps(x_maxs, xs);
953 
954  const __m128 ys = _mm_loadu_ps(ptr_in_y);
955  y_mins = _mm_min_ps(y_mins, ys);
956  y_maxs = _mm_max_ps(y_maxs, ys);
957 
958  const __m128 zs = _mm_loadu_ps(ptr_in_z);
959  z_mins = _mm_min_ps(z_mins, zs);
960  z_maxs = _mm_max_ps(z_maxs, zs);
961  }
962 
963  // Recover the min/max:
964  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float temp_nums[4];
965 
966  _mm_store_ps(temp_nums, x_mins);
967  m_bb_min_x =
968  min(min(temp_nums[0], temp_nums[1]),
969  min(temp_nums[2], temp_nums[3]));
970  _mm_store_ps(temp_nums, y_mins);
971  m_bb_min_y =
972  min(min(temp_nums[0], temp_nums[1]),
973  min(temp_nums[2], temp_nums[3]));
974  _mm_store_ps(temp_nums, z_mins);
975  m_bb_min_z =
976  min(min(temp_nums[0], temp_nums[1]),
977  min(temp_nums[2], temp_nums[3]));
978  _mm_store_ps(temp_nums, x_maxs);
979  m_bb_max_x =
980  max(max(temp_nums[0], temp_nums[1]),
981  max(temp_nums[2], temp_nums[3]));
982  _mm_store_ps(temp_nums, y_maxs);
983  m_bb_max_y =
984  max(max(temp_nums[0], temp_nums[1]),
985  max(temp_nums[2], temp_nums[3]));
986  _mm_store_ps(temp_nums, z_maxs);
987  m_bb_max_z =
988  max(max(temp_nums[0], temp_nums[1]),
989  max(temp_nums[2], temp_nums[3]));
990 
991  // extra
992  for (size_t k = 0; k < nPoints % 4; k++)
993  {
994  m_bb_min_x = std::min(m_bb_min_x, ptr_in_x[k]);
995  m_bb_max_x = std::max(m_bb_max_x, ptr_in_x[k]);
996 
997  m_bb_min_y = std::min(m_bb_min_y, ptr_in_y[k]);
998  m_bb_max_y = std::max(m_bb_max_y, ptr_in_y[k]);
999 
1000  m_bb_min_z = std::min(m_bb_min_z, ptr_in_z[k]);
1001  m_bb_max_z = std::max(m_bb_max_z, ptr_in_z[k]);
1002  }
1003 #else
1004  // Non vectorized version:
1006  (std::numeric_limits<float>::max)();
1007 
1009  -(std::numeric_limits<float>::max)();
1010 
1011  for (auto xi = m_x.begin(), yi = m_y.begin(), zi = m_z.begin();
1012  xi != m_x.end(); xi++, yi++, zi++)
1013  {
1014  m_bb_min_x = min(m_bb_min_x, *xi);
1015  m_bb_max_x = max(m_bb_max_x, *xi);
1016  m_bb_min_y = min(m_bb_min_y, *yi);
1017  m_bb_max_y = max(m_bb_max_y, *yi);
1018  m_bb_min_z = min(m_bb_min_z, *zi);
1019  m_bb_max_z = max(m_bb_max_z, *zi);
1020  }
1021 #endif
1022  }
1023  m_boundingBoxIsUpdated = true;
1024  }
1025 
1026  min_x = m_bb_min_x;
1027  max_x = m_bb_max_x;
1028  min_y = m_bb_min_y;
1029  max_y = m_bb_max_y;
1030  min_z = m_bb_min_z;
1031  max_z = m_bb_max_z;
1032  MRPT_END
1033 }
1034 
1035 /*---------------------------------------------------------------
1036  computeMatchingWith3D
1037 ---------------------------------------------------------------*/
1039  const mrpt::maps::CMetricMap* otherMap2, const CPose3D& otherMapPose,
1040  TMatchingPairList& correspondences, const TMatchingParams& params,
1041  TMatchingExtraResults& extraResults) const
1042 {
1043  MRPT_START
1044 
1045  extraResults = TMatchingExtraResults();
1046 
1047  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
1048  ASSERT_BELOW_(
1049  params.offset_other_map_points, params.decimation_other_map_points);
1050 
1052  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
1053 
1054  const size_t nLocalPoints = otherMap->size();
1055  const size_t nGlobalPoints = this->size();
1056  float _sumSqrDist = 0;
1057  size_t _sumSqrCount = 0;
1058  size_t nOtherMapPointsWithCorrespondence =
1059  0; // Number of points with one corrs. at least
1060 
1061  float local_x_min = std::numeric_limits<float>::max(),
1062  local_x_max = -std::numeric_limits<float>::max();
1063  float local_y_min = std::numeric_limits<float>::max(),
1064  local_y_max = -std::numeric_limits<float>::max();
1065  float local_z_min = std::numeric_limits<float>::max(),
1066  local_z_max = -std::numeric_limits<float>::max();
1067 
1068  double maxDistForCorrespondenceSquared;
1069 
1070  // Prepare output: no correspondences initially:
1071  correspondences.clear();
1072  correspondences.reserve(nLocalPoints);
1073 
1074  TMatchingPairList _correspondences;
1075  _correspondences.reserve(nLocalPoints);
1076 
1077  // Empty maps? Nothing to do
1078  if (!nGlobalPoints || !nLocalPoints) return;
1079 
1080  // Try to do matching only if the bounding boxes have some overlap:
1081  // Transform all local points:
1082  vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1083  z_locals(nLocalPoints);
1084 
1085  for (unsigned int localIdx = params.offset_other_map_points;
1086  localIdx < nLocalPoints;
1087  localIdx += params.decimation_other_map_points)
1088  {
1089  float x_local, y_local, z_local;
1090  otherMapPose.composePoint(
1091  otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1092  otherMap->m_z[localIdx], x_local, y_local, z_local);
1093 
1094  x_locals[localIdx] = x_local;
1095  y_locals[localIdx] = y_local;
1096  z_locals[localIdx] = z_local;
1097 
1098  // Find the bounding box:
1099  local_x_min = min(local_x_min, x_local);
1100  local_x_max = max(local_x_max, x_local);
1101  local_y_min = min(local_y_min, y_local);
1102  local_y_max = max(local_y_max, y_local);
1103  local_z_min = min(local_z_min, z_local);
1104  local_z_max = max(local_z_max, z_local);
1105  }
1106 
1107  // Find the bounding box:
1108  float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1109  global_z_max;
1110  this->boundingBox(
1111  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1112  global_z_max);
1113 
1114  // Solo hacer matching si existe alguna posibilidad de que
1115  // los dos mapas se toquen:
1116  if (local_x_min > global_x_max || local_x_max < global_x_min ||
1117  local_y_min > global_y_max || local_y_max < global_y_min)
1118  return; // No need to compute: matching is ZERO.
1119 
1120  // Loop for each point in local map:
1121  // --------------------------------------------------
1122  for (unsigned int localIdx = params.offset_other_map_points;
1123  localIdx < nLocalPoints;
1124  localIdx += params.decimation_other_map_points)
1125  {
1126  // For speed-up:
1127  const float x_local = x_locals[localIdx];
1128  const float y_local = y_locals[localIdx];
1129  const float z_local = z_locals[localIdx];
1130 
1131  {
1132  // KD-TREE implementation
1133  // Use a KD-tree to look for the nearnest neighbor of:
1134  // (x_local, y_local, z_local)
1135  // In "this" (global/reference) points map.
1136 
1137  float tentativ_err_sq;
1138  const unsigned int tentativ_this_idx = kdTreeClosestPoint3D(
1139  x_local, y_local, z_local, // Look closest to this guy
1140  tentativ_err_sq // save here the min. distance squared
1141  );
1142 
1143  // Compute max. allowed distance:
1144  maxDistForCorrespondenceSquared = square(
1145  params.maxAngularDistForCorrespondence *
1146  params.angularDistPivotPoint.distanceTo(
1147  TPoint3D(x_local, y_local, z_local)) +
1148  params.maxDistForCorrespondence);
1149 
1150  // Distance below the threshold??
1151  if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1152  {
1153  // Save all the correspondences:
1154  _correspondences.resize(_correspondences.size() + 1);
1155 
1156  TMatchingPair& p = _correspondences.back();
1157 
1158  p.this_idx = tentativ_this_idx;
1159  p.this_x = m_x[tentativ_this_idx];
1160  p.this_y = m_y[tentativ_this_idx];
1161  p.this_z = m_z[tentativ_this_idx];
1162 
1163  p.other_idx = localIdx;
1164  p.other_x = otherMap->m_x[localIdx];
1165  p.other_y = otherMap->m_y[localIdx];
1166  p.other_z = otherMap->m_z[localIdx];
1167 
1168  p.errorSquareAfterTransformation = tentativ_err_sq;
1169 
1170  // At least one:
1171  nOtherMapPointsWithCorrespondence++;
1172 
1173  // Accumulate the MSE:
1174  _sumSqrDist += p.errorSquareAfterTransformation;
1175  _sumSqrCount++;
1176  }
1177 
1178  } // End of test_match
1179  } // For each local point
1180 
1181  // Additional consistency filter: "onlyKeepTheClosest" up to now
1182  // led to just one correspondence for each "local map" point, but
1183  // many of them may have as corresponding pair the same "global point"!!
1184  // -------------------------------------------------------------------------
1185  if (params.onlyUniqueRobust)
1186  {
1187  ASSERTMSG_(
1188  params.onlyKeepTheClosest,
1189  "ERROR: onlyKeepTheClosest must be also set to true when "
1190  "onlyUniqueRobust=true.");
1191  _correspondences.filterUniqueRobustPairs(
1192  nGlobalPoints, correspondences);
1193  }
1194  else
1195  {
1196  correspondences.swap(_correspondences);
1197  }
1198 
1199  // If requested, copy sum of squared distances to output pointer:
1200  // -------------------------------------------------------------------
1201  extraResults.sumSqrDist =
1202  (_sumSqrCount) ? _sumSqrDist / static_cast<double>(_sumSqrCount) : 0;
1203  extraResults.correspondencesRatio = params.decimation_other_map_points *
1204  nOtherMapPointsWithCorrespondence /
1205  d2f(nLocalPoints);
1206 
1207  MRPT_END
1208 }
1209 
1210 /*---------------------------------------------------------------
1211  extractCylinder
1212 ---------------------------------------------------------------*/
1214  const TPoint2D& center, const double radius, const double zmin,
1215  const double zmax, CPointsMap* outMap)
1216 {
1217  outMap->clear();
1218  for (size_t k = 0; k < m_x.size(); k++)
1219  {
1220  if ((m_z[k] <= zmax && m_z[k] >= zmin) &&
1221  (sqrt(square(center.x - m_x[k]) + square(center.y - m_y[k])) <
1222  radius))
1223  outMap->insertPoint(m_x[k], m_y[k], m_z[k]);
1224  }
1225 }
1226 
1227 /*---------------------------------------------------------------
1228  extractPoints
1229 ---------------------------------------------------------------*/
1231  const TPoint3D& corner1, const TPoint3D& corner2, CPointsMap* outMap,
1232  double R, double G, double B)
1233 {
1234  outMap->clear();
1235  double minX, maxX, minY, maxY, minZ, maxZ;
1236  minX = min(corner1.x, corner2.x);
1237  maxX = max(corner1.x, corner2.x);
1238  minY = min(corner1.y, corner2.y);
1239  maxY = max(corner1.y, corner2.y);
1240  minZ = min(corner1.z, corner2.z);
1241  maxZ = max(corner1.z, corner2.z);
1242  for (size_t k = 0; k < m_x.size(); k++)
1243  {
1244  if ((m_x[k] >= minX && m_x[k] <= maxX) &&
1245  (m_y[k] >= minY && m_y[k] <= maxY) &&
1246  (m_z[k] >= minZ && m_z[k] <= maxZ))
1247  outMap->insertPointRGB(m_x[k], m_y[k], m_z[k], R, G, B);
1248  }
1249 }
1250 
1251 /*---------------------------------------------------------------
1252  compute3DDistanceToMesh
1253 ---------------------------------------------------------------*/
1255  const mrpt::maps::CMetricMap* otherMap2,
1256  [[maybe_unused]] const CPose3D& otherMapPose,
1257  float maxDistForCorrespondence, TMatchingPairList& correspondences,
1258  float& correspondencesRatio)
1259 {
1260  MRPT_START
1261 
1262  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
1263 
1264  const size_t nLocalPoints = otherMap->size();
1265  const size_t nGlobalPoints = this->size();
1266  size_t nOtherMapPointsWithCorrespondence =
1267  0; // Number of points with one corrs. at least
1268 
1269  // Prepare output: no correspondences initially:
1270  correspondences.clear();
1271  correspondences.reserve(nLocalPoints);
1272  correspondencesRatio = 0;
1273 
1274  // aux correspondence vector
1275  TMatchingPairList _correspondences;
1276  _correspondences.reserve(nLocalPoints);
1277 
1278  // Hay mapa global?
1279  if (!nGlobalPoints) return; // No
1280 
1281  // Hay mapa local?
1282  if (!nLocalPoints) return; // No
1283 
1284  // we'll assume by now both reference systems are the same
1285  float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1286  local_z_max;
1287  otherMap->boundingBox(
1288  local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1289  local_z_max);
1290 
1291  // Find the bounding box:
1292  float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1293  global_z_max;
1294  this->boundingBox(
1295  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1296  global_z_max);
1297 
1298  // Solo hacer matching si existe alguna posibilidad de que
1299  // los dos mapas se toquen:
1300  if (local_x_min > global_x_max || local_x_max < global_x_min ||
1301  local_y_min > global_y_max || local_y_max < global_y_min)
1302  return; // No hace falta hacer matching,
1303  // porque es de CERO.
1304 
1305  std::vector<std::vector<size_t>> vIdx;
1306 
1307  // Loop for each point in local map:
1308  // --------------------------------------------------
1309  std::vector<float> outX, outY, outZ, tentativeErrSq;
1310  std::vector<size_t> outIdx;
1311  for (unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1312  {
1313  // For speed-up:
1314  const float x_local = otherMap->m_x[localIdx];
1315  const float y_local = otherMap->m_y[localIdx];
1316  const float z_local = otherMap->m_z[localIdx];
1317 
1318  {
1319  // KD-TREE implementation
1320  // Use a KD-tree to look for the nearnest neighbor of:
1321  // (x_local, y_local, z_local)
1322  // In "this" (global/reference) points map.
1324  x_local, y_local, z_local, // Look closest to this guy
1325  3, // get the three closest points
1326  outX, outY, outZ, // output vectors
1327  outIdx, // output indexes
1328  tentativeErrSq // save here the min. distance squared
1329  );
1330 
1331  // get the centroid
1332  const float mX = (outX[0] + outX[1] + outX[2]) / 3.0;
1333  const float mY = (outY[0] + outY[1] + outY[2]) / 3.0;
1334  const float mZ = (outZ[0] + outZ[1] + outZ[2]) / 3.0;
1335 
1336  const float distanceForThisPoint = fabs(mrpt::math::distance(
1337  TPoint3D(x_local, y_local, z_local), TPoint3D(mX, mY, mZ)));
1338 
1339  // Distance below the threshold??
1340  if (distanceForThisPoint < maxDistForCorrespondence)
1341  {
1342  // Save all the correspondences:
1343  _correspondences.resize(_correspondences.size() + 1);
1344 
1345  TMatchingPair& p = _correspondences.back();
1346 
1347  p.this_idx = nOtherMapPointsWithCorrespondence++; // insert a
1348  // consecutive index
1349  // here
1350  p.this_x = mX;
1351  p.this_y = mY;
1352  p.this_z = mZ;
1353 
1354  p.other_idx = localIdx;
1355  p.other_x = otherMap->m_x[localIdx];
1356  p.other_y = otherMap->m_y[localIdx];
1357  p.other_z = otherMap->m_z[localIdx];
1358 
1359  p.errorSquareAfterTransformation = distanceForThisPoint;
1360 
1361  // save the indexes
1362  std::sort(outIdx.begin(), outIdx.end());
1363  vIdx.push_back(outIdx);
1364  }
1365  } // End of test_match
1366  } // For each local point
1367 
1368  // Additional consistency filter: "onlyKeepTheClosest" up to now
1369  // led to just one correspondence for each "local map" point, but
1370  // many of them may have as corresponding pair the same "global point"!!
1371  // -------------------------------------------------------------------------
1372  std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1373  best; // 3D associative map
1374  TMatchingPairList::iterator it;
1375  for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1376  {
1377  const size_t i0 = vIdx[it->this_idx][0];
1378  const size_t i1 = vIdx[it->this_idx][1];
1379  const size_t i2 = vIdx[it->this_idx][2];
1380 
1381  if (best.find(i0) != best.end() &&
1382  best[i0].find(i1) != best[i0].end() &&
1383  best[i0][i1].find(i2) !=
1384  best[i0][i1]
1385  .end()) // if there is a match, check if it is better
1386  {
1387  if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1388  {
1389  best[i0][i1][i2].first = it->this_idx;
1390  best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1391  }
1392  }
1393  else // if there is no match
1394  {
1395  best[i0][i1][i2].first = it->this_idx;
1396  best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1397  }
1398  } // end it correspondences
1399 
1400  for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1401  {
1402  const size_t i0 = vIdx[it->this_idx][0];
1403  const size_t i1 = vIdx[it->this_idx][1];
1404  const size_t i2 = vIdx[it->this_idx][2];
1405 
1406  if (best[i0][i1][i2].first == it->this_idx)
1407  correspondences.push_back(*it);
1408  }
1409 
1410  // The ratio of points in the other map with corrs:
1411  correspondencesRatio =
1412  nOtherMapPointsWithCorrespondence / d2f(nLocalPoints);
1413 
1414  MRPT_END
1415 }
1416 
1418  const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys,
1419  const float* zs, const std::size_t num_pts)
1420 {
1422 
1423  float closest_x, closest_y, closest_z;
1424  float closest_err;
1425  const float max_sqr_err = square(likelihoodOptions.max_corr_distance);
1426  double sumSqrDist = 0;
1427 
1428  std::size_t nPtsForAverage = 0;
1429  for (std::size_t i = 0; i < num_pts;
1430  i += likelihoodOptions.decimation, nPtsForAverage++)
1431  {
1432  // Transform the point from the scan reference to its global 3D
1433  // position:
1434  float xg, yg, zg;
1435  pc_in_map.composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1436 
1438  xg, yg, zg, // Look for the closest to this guy
1439  closest_x, closest_y,
1440  closest_z, // save here the closest match
1441  closest_err // save here the min. distance squared
1442  );
1443 
1444  // Put a limit:
1445  mrpt::keep_min(closest_err, max_sqr_err);
1446 
1447  sumSqrDist += static_cast<double>(closest_err);
1448  }
1449  if (nPtsForAverage) sumSqrDist /= nPtsForAverage;
1450 
1451  // Log-likelihood:
1452  return -sumSqrDist / likelihoodOptions.sigma_dist;
1453 
1454  MRPT_TRY_END
1455 }
1456 
1458  const CObservation& obs, const CPose3D& takenFrom)
1459 {
1460  // This function depends on the observation type:
1461  // -----------------------------------------------------
1463  {
1464  // Observation is a laser range scan:
1465  // -------------------------------------------
1466  const auto& o = static_cast<const CObservation2DRangeScan&>(obs);
1467 
1468  // Build (if not done before) the points map representation of this
1469  // observation:
1470  const auto* scanPoints = o.buildAuxPointsMap<CPointsMap>();
1471 
1472  const size_t N = scanPoints->m_x.size();
1473  if (!N || !this->size()) return -100;
1474 
1475  const float* xs = &scanPoints->m_x[0];
1476  const float* ys = &scanPoints->m_y[0];
1477  const float* zs = &scanPoints->m_z[0];
1478 
1479  if (takenFrom.isHorizontal())
1480  {
1481  double sumSqrDist = 0;
1482  float closest_x, closest_y;
1483  float closest_err;
1484  const float max_sqr_err =
1486 
1487  // optimized 2D version ---------------------------
1488  TPose2D takenFrom2D = CPose2D(takenFrom).asTPose();
1489 
1490  const double ccos = cos(takenFrom2D.phi);
1491  const double csin = sin(takenFrom2D.phi);
1492  int nPtsForAverage = 0;
1493 
1494  for (size_t i = 0; i < N;
1495  i += likelihoodOptions.decimation, nPtsForAverage++)
1496  {
1497  // Transform the point from the scan reference to its global
1498  // 3D position:
1499  const float xg = takenFrom2D.x + ccos * xs[i] - csin * ys[i];
1500  const float yg = takenFrom2D.y + csin * xs[i] + ccos * ys[i];
1501 
1503  xg, yg, // Look for the closest to this guy
1504  closest_x, closest_y, // save here the closest match
1505  closest_err // save here the min. distance squared
1506  );
1507 
1508  // Put a limit:
1509  mrpt::keep_min(closest_err, max_sqr_err);
1510 
1511  sumSqrDist += static_cast<double>(closest_err);
1512  }
1513  sumSqrDist /= nPtsForAverage;
1514  // Log-likelihood:
1515  return -sumSqrDist / likelihoodOptions.sigma_dist;
1516  }
1517  else
1518  {
1519  // Generic 3D version ---------------------------
1521  takenFrom, xs, ys, zs, N);
1522  }
1523  }
1524  else if (IS_CLASS(obs, CObservationVelodyneScan))
1525  {
1526  const auto& o = dynamic_cast<const CObservationVelodyneScan&>(obs);
1527 
1528  // Automatically generate pointcloud if needed:
1529  if (!o.point_cloud.size())
1530  const_cast<CObservationVelodyneScan&>(o).generatePointCloud();
1531 
1532  const size_t N = o.point_cloud.size();
1533  if (!N || !this->size()) return -100;
1534 
1535  const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1536 
1537  const float* xs = &o.point_cloud.x[0];
1538  const float* ys = &o.point_cloud.y[0];
1539  const float* zs = &o.point_cloud.z[0];
1540 
1542  sensorAbsPose, xs, ys, zs, N);
1543  }
1544  else if (IS_CLASS(obs, CObservationPointCloud))
1545  {
1546  const auto& o = dynamic_cast<const CObservationPointCloud&>(obs);
1547 
1548  const size_t N = o.pointcloud->size();
1549  if (!N || !this->size()) return -100;
1550 
1551  const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1552 
1553  auto xs = o.pointcloud->getPointsBufferRef_x();
1554  auto ys = o.pointcloud->getPointsBufferRef_y();
1555  auto zs = o.pointcloud->getPointsBufferRef_z();
1556 
1558  sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1559  }
1560 
1561  return .0;
1562 }
1563 
1564 namespace mrpt::obs
1565 {
1566 // Tricky way to call to a library that depends on us, a sort of "run-time"
1567 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
1568 // "mrpt-obs", set by "mrpt-maps" at its startup.
1569 using scan2pts_functor = void (*)(
1571  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
1572 
1574 } // namespace mrpt::obs
1575 
1578  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps)
1579 {
1580  // Create on first call:
1581  if (out_map) return; // Already done!
1582 
1583  out_map = std::make_shared<CSimplePointsMap>();
1584 
1585  if (insertOps)
1586  static_cast<CSimplePointsMap*>(out_map.get())->insertionOptions =
1587  *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1588 
1589  out_map->insertObservation(obs, nullptr);
1590 }
1591 
1593 {
1595  {
1598  }
1599 };
1600 
1601 // used just to set "ptr_internal_build_points_map_from_scan2D"
1603 
1604 // ================================ PLY files import & export virtual
1605 // methods
1606 // ================================
1607 
1608 /** In a base class, will be called after PLY_import_set_vertex_count() once
1609  * for each loaded point. \param pt_color Will be nullptr if the loaded file
1610  * does not provide color info.
1611  */
1613  const size_t idx, const mrpt::math::TPoint3Df& pt,
1614  [[maybe_unused]] const mrpt::img::TColorf* pt_color)
1615 {
1616  this->setPoint(idx, pt.x, pt.y, pt.z);
1617 }
1618 
1619 /** In a base class, return the number of vertices */
1620 size_t CPointsMap::PLY_export_get_vertex_count() const { return this->size(); }
1621 /** In a base class, will be called after PLY_export_get_vertex_count() once
1622  * for each exported point. \param pt_color Will be nullptr if the loaded
1623  * file does not provide color info.
1624  */
1626  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
1627  [[maybe_unused]] mrpt::img::TColorf& pt_color) const
1628 {
1629  pt_has_color = false;
1630 
1631  pt.x = m_x[idx];
1632  pt.y = m_y[idx];
1633  pt.z = m_z[idx];
1634 }
1635 
1636 // Generic implementation (a more optimized one should exist in derived
1637 // classes):
1638 void CPointsMap::addFrom(const CPointsMap& anotherMap)
1639 {
1640  const size_t nThis = this->size();
1641  const size_t nOther = anotherMap.size();
1642 
1643  const size_t nTot = nThis + nOther;
1644 
1645  this->resize(nTot);
1646 
1647  for (size_t i = 0, j = nThis; i < nOther; i++, j++)
1648  {
1649  m_x[j] = anotherMap.m_x[i];
1650  m_y[j] = anotherMap.m_y[i];
1651  m_z[j] = anotherMap.m_z[i];
1652  }
1653 
1654  // Also copy other data fields (color, ...)
1655  addFrom_classSpecific(anotherMap, nThis);
1656 
1657  mark_as_modified();
1658 }
1659 
1660 /*---------------------------------------------------------------
1661  applyDeletionMask
1662  ---------------------------------------------------------------*/
1663 void CPointsMap::applyDeletionMask(const std::vector<bool>& mask)
1664 {
1665  ASSERT_EQUAL_(size(), mask.size());
1666  // Remove marked points:
1667  const size_t n = mask.size();
1668  vector<float> Pt;
1669  size_t i, j;
1670  for (i = 0, j = 0; i < n; i++)
1671  {
1672  if (!mask[i])
1673  {
1674  // Pt[j] <---- Pt[i]
1675  this->getPointAllFieldsFast(i, Pt);
1676  this->setPointAllFieldsFast(j++, Pt);
1677  }
1678  }
1679 
1680  // Set new correct size:
1681  this->resize(j);
1682 
1683  mark_as_modified();
1684 }
1685 
1686 /*---------------------------------------------------------------
1687  insertAnotherMap
1688  ---------------------------------------------------------------*/
1690  const CPointsMap* otherMap, const CPose3D& otherPose)
1691 {
1692  const size_t N_this = size();
1693  const size_t N_other = otherMap->size();
1694 
1695  // Set the new size:
1696  this->resize(N_this + N_other);
1697 
1699  size_t src, dst;
1700  for (src = 0, dst = N_this; src < N_other; src++, dst++)
1701  {
1702  // Load the next point:
1703  otherMap->getPointFast(src, pt.x, pt.y, pt.z);
1704 
1705  // Translation:
1706  double gx, gy, gz;
1707  otherPose.composePoint(pt.x, pt.y, pt.z, gx, gy, gz);
1708 
1709  // Add to this map:
1710  this->setPointFast(dst, gx, gy, gz);
1711  }
1712 
1713  // Also copy other data fields (color, ...)
1714  addFrom_classSpecific(*otherMap, N_this);
1715 
1716  mark_as_modified();
1717 }
1718 
1719 /** Helper method for ::copyFrom() */
1721 {
1722  MRPT_START
1723 
1724  if (this == &obj) return;
1725 
1726  m_x = obj.m_x;
1727  m_y = obj.m_y;
1728  m_z = obj.m_z;
1729 
1733 
1734  // Fill missing fields (R,G,B,min_dist) with default values.
1735  this->resize(m_x.size());
1736 
1738 
1739  MRPT_END
1740 }
1741 
1742 /*---------------------------------------------------------------
1743  internal_insertObservation
1744 
1745  Insert the observation information into this map.
1746  ---------------------------------------------------------------*/
1748  const CObservation& obs, const CPose3D* robotPose)
1749 {
1750  MRPT_START
1751 
1752  CPose2D robotPose2D;
1753  CPose3D robotPose3D;
1754 
1755  if (robotPose)
1756  {
1757  robotPose2D = CPose2D(*robotPose);
1758  robotPose3D = (*robotPose);
1759  }
1760  else
1761  {
1762  // Default values are (0,0,0)
1763  }
1764 
1766  {
1767  /********************************************************************
1768  OBSERVATION TYPE: CObservation2DRangeScan
1769  ********************************************************************/
1770  mark_as_modified();
1771 
1772  const auto& o = static_cast<const CObservation2DRangeScan&>(obs);
1773  // Insert only HORIZONTAL scans??
1774  bool reallyInsertIt;
1775 
1777  reallyInsertIt =
1779  else
1780  reallyInsertIt = true;
1781 
1782  if (reallyInsertIt)
1783  {
1784  std::vector<bool> checkForDeletion;
1785 
1786  // 1) Fuse into the points map or add directly?
1787  // ----------------------------------------------
1789  {
1790  CSimplePointsMap auxMap;
1791  // Fuse:
1794 
1795  auxMap.loadFromRangeScan(
1796  o, // The laser range scan observation
1797  &robotPose3D // The robot pose
1798  );
1799 
1800  fuseWith(
1801  &auxMap, // Fuse with this map
1803  &checkForDeletion // Set to "false" if a point in "map"
1804  // has
1805  // been fused.
1806  );
1807 
1809  {
1810  // 2) Delete points in newly added free
1811  // region, thus dynamic areas:
1812  // --------------------------------------
1813  // Load scan as a polygon:
1814  CPolygon pol;
1815  const float *xs, *ys, *zs;
1816  size_t n;
1817  auxMap.getPointsBuffer(n, xs, ys, zs);
1818  pol.setAllVertices(n, xs, ys);
1819 
1820  // Check for deletion of points in "map"
1821  n = size();
1822  for (size_t i = 0; i < n; i++)
1823  {
1824  if (checkForDeletion[i]) // Default to true, unless
1825  // a
1826  // fused point, which must be
1827  // kept.
1828  {
1829  float x, y;
1830  getPoint(i, x, y);
1831  if (!pol.PointIntoPolygon(x, y))
1832  checkForDeletion[i] =
1833  false; // Out of polygon, don't delete
1834  }
1835  }
1836 
1837  // Create a new points list just with non-deleted
1838  // points.
1839  // ----------------------------------------------------------
1840  applyDeletionMask(checkForDeletion);
1841  }
1842  }
1843  else
1844  {
1845  // Don't fuse: Simply add
1848  o, // The laser range scan observation
1849  &robotPose3D // The robot pose
1850  );
1851  }
1852 
1853  return true;
1854  }
1855  // A planar map and a non-horizontal scan.
1856  else
1857  return false;
1858  }
1859  else if (IS_CLASS(obs, CObservation3DRangeScan))
1860  {
1861  /********************************************************************
1862  OBSERVATION TYPE: CObservation3DRangeScan
1863  ********************************************************************/
1864  mark_as_modified();
1865 
1866  const auto& o = static_cast<const CObservation3DRangeScan&>(obs);
1867  // Insert only HORIZONTAL scans??
1868  bool reallyInsertIt;
1869 
1871  reallyInsertIt =
1872  false; // Don't insert 3D range observation into planar map
1873  else
1874  reallyInsertIt = true;
1875 
1876  if (reallyInsertIt)
1877  {
1878  // 1) Fuse into the points map or add directly?
1879  // ----------------------------------------------
1881  {
1882  // Fuse:
1883  CSimplePointsMap auxMap;
1886 
1887  auxMap.loadFromRangeScan(
1888  o, // The laser range scan observation
1889  &robotPose3D // The robot pose
1890  );
1891 
1892  fuseWith(
1893  &auxMap, // Fuse with this map
1895  nullptr // rather than &checkForDeletion which we don't
1896  // need
1897  // for 3D observations
1898  );
1899  }
1900  else
1901  {
1902  // Don't fuse: Simply add
1905  o, // The laser range scan observation
1906  &robotPose3D // The robot pose
1907  );
1908  }
1909 
1910  return true;
1911  }
1912  // A planar map and a non-horizontal scan.
1913  else
1914  return false;
1915  }
1916  else if (IS_CLASS(obs, CObservationRange))
1917  {
1918  /********************************************************************
1919  OBSERVATION TYPE: CObservationRange (IRs, Sonars, etc.)
1920  ********************************************************************/
1921  mark_as_modified();
1922 
1923  const auto& o = static_cast<const CObservationRange&>(obs);
1924 
1925  const double aper_2 = 0.5 * o.sensorConeApperture;
1926 
1927  this->reserve(
1928  this->size() + o.sensedData.size() * 30); // faster push_back's.
1929 
1930  for (auto it = o.begin(); it != o.end(); ++it)
1931  {
1932  const CPose3D sensorPose = robotPose3D + CPose3D(it->sensorPose);
1933  const double rang = it->sensedDistance;
1934 
1935  if (rang <= 0 || rang < o.minSensorDistance ||
1936  rang > o.maxSensorDistance)
1937  continue;
1938 
1939  // Insert a few points with a given maximum separation between
1940  // them:
1941  const double arc_len = o.sensorConeApperture * rang;
1942  const unsigned int nSteps = round(1 + arc_len / 0.05);
1943  const double Aa = o.sensorConeApperture / double(nSteps);
1944  TPoint3D loc, glob;
1945 
1946  for (double a1 = -aper_2; a1 < aper_2; a1 += Aa)
1947  {
1948  for (double a2 = -aper_2; a2 < aper_2; a2 += Aa)
1949  {
1950  loc.x = cos(a1) * cos(a2) * rang;
1951  loc.y = cos(a1) * sin(a2) * rang;
1952  loc.z = sin(a1) * rang;
1953  sensorPose.composePoint(loc, glob);
1954 
1955  this->insertPointFast(glob.x, glob.y, glob.z);
1956  }
1957  }
1958  }
1959  return true;
1960  }
1961  else if (IS_CLASS(obs, CObservationVelodyneScan))
1962  {
1963  /********************************************************************
1964  OBSERVATION TYPE: CObservationVelodyneScan
1965  ********************************************************************/
1966  mark_as_modified();
1967 
1968  const auto& o = static_cast<const CObservationVelodyneScan&>(obs);
1969 
1970  // Automatically generate pointcloud if needed:
1971  if (!o.point_cloud.size())
1972  const_cast<CObservationVelodyneScan&>(o).generatePointCloud();
1973 
1975  {
1976  // Fuse:
1977  CSimplePointsMap auxMap;
1980  auxMap.loadFromVelodyneScan(o, &robotPose3D);
1981  fuseWith(
1982  &auxMap, insertionOptions.minDistBetweenLaserPoints, nullptr /* rather than &checkForDeletion which we don't need for 3D observations */);
1983  }
1984  else
1985  {
1986  // Don't fuse: Simply add
1988  loadFromVelodyneScan(o, &robotPose3D);
1989  }
1990  return true;
1991  }
1992  else if (IS_CLASS(obs, CObservationPointCloud))
1993  {
1994  mark_as_modified();
1995 
1996  const auto& o = static_cast<const CObservationPointCloud&>(obs);
1997  ASSERT_(o.pointcloud);
1998 
2000  {
2001  fuseWith(
2002  o.pointcloud.get(), insertionOptions.minDistBetweenLaserPoints, nullptr /* rather than &checkForDeletion which we don't need for 3D observations */);
2003  }
2004  else
2005  {
2006  // Don't fuse: Simply add
2008  this->insertAnotherMap(o.pointcloud.get(), o.sensorPose);
2009  }
2010  return true;
2011  }
2012  else
2013  {
2014  /********************************************************************
2015  OBSERVATION TYPE: Unknown
2016  ********************************************************************/
2017  return false;
2018  }
2019 
2020  MRPT_END
2021 }
2022 
2023 /*---------------------------------------------------------------
2024 Insert the contents of another map into this one, fusing the previous
2025 content with the new one. This means that points very close to existing ones
2026 will be "fused", rather than "added". This prevents the unbounded increase
2027 in size of these class of maps.
2028  ---------------------------------------------------------------*/
2030  CPointsMap* otherMap, float minDistForFuse,
2031  std::vector<bool>* notFusedPoints)
2032 {
2033  TMatchingPairList correspondences;
2034  TPoint3D a, b;
2035  const CPose2D nullPose(0, 0, 0);
2036 
2037  mark_as_modified();
2038 
2039  // const size_t nThis = this->size();
2040  const size_t nOther = otherMap->size();
2041 
2042  // Find correspondences between this map and the other one:
2043  // ------------------------------------------------------------
2045  TMatchingExtraResults extraResults;
2046 
2047  params.maxAngularDistForCorrespondence = 0;
2048  params.maxDistForCorrespondence = minDistForFuse;
2049 
2051  otherMap, // The other map
2052  nullPose, // The other map's pose
2053  correspondences, params, extraResults);
2054 
2055  // Initially, all set to "true" -> "not fused".
2056  if (notFusedPoints)
2057  {
2058  notFusedPoints->clear();
2059  notFusedPoints->reserve(m_x.size() + nOther);
2060  notFusedPoints->resize(m_x.size(), true);
2061  }
2062 
2063  // Speeds-up possible memory reallocations:
2064  reserve(m_x.size() + nOther);
2065 
2066  // Merge matched points from both maps:
2067  // AND add new points which have been not matched:
2068  // -------------------------------------------------
2069  for (size_t i = 0; i < nOther; i++)
2070  {
2071  otherMap->getPoint(i, a); // Get "local" point into "a"
2072  const unsigned long w_a = otherMap->getPointWeight(i);
2073 
2074  // Find closest correspondence of "a":
2075  int closestCorr = -1;
2076  float minDist = std::numeric_limits<float>::max();
2077  for (auto corrsIt = correspondences.begin();
2078  corrsIt != correspondences.end(); ++corrsIt)
2079  {
2080  if (corrsIt->other_idx == i)
2081  {
2082  float dist = square(corrsIt->other_x - corrsIt->this_x) +
2083  square(corrsIt->other_y - corrsIt->this_y) +
2084  square(corrsIt->other_z - corrsIt->this_z);
2085  if (dist < minDist)
2086  {
2087  minDist = dist;
2088  closestCorr = corrsIt->this_idx;
2089  }
2090  }
2091  } // End of for each correspondence...
2092 
2093  if (closestCorr != -1)
2094  { // Merge: FUSION
2095  getPoint(closestCorr, b);
2096  unsigned long w_b = getPointWeight(closestCorr);
2097 
2098  ASSERT_((w_a + w_b) > 0);
2099 
2100  const float F = 1.0f / (w_a + w_b);
2101 
2102  m_x[closestCorr] = F * (w_a * a.x + w_b * b.x);
2103  m_y[closestCorr] = F * (w_a * a.y + w_b * b.y);
2104  m_z[closestCorr] = F * (w_a * a.z + w_b * b.z);
2105 
2106  this->setPointWeight(closestCorr, w_a + w_b);
2107 
2108  // Append to fused points list
2109  if (notFusedPoints) (*notFusedPoints)[closestCorr] = false;
2110  }
2111  else
2112  { // New point: ADDITION
2113  this->insertPointFast(a.x, a.y, a.z);
2114  if (notFusedPoints) (*notFusedPoints).push_back(false);
2115  }
2116  }
2117 }
2118 
2121  const mrpt::poses::CPose3D* robotPose)
2122 {
2123  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.y.size());
2124  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.z.size());
2125  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.intensity.size());
2126 
2127  if (scan.point_cloud.x.empty()) return;
2128 
2129  this->mark_as_modified();
2130 
2131  // Insert vs. load and replace:
2133  resize(0); // Resize to 0 instead of clear() so the std::vector<>
2134  // memory is not actually deallocated and can be reused.
2135 
2136  // Alloc space:
2137  const size_t nOldPtsCount = this->size();
2138  const size_t nScanPts = scan.point_cloud.size();
2139  const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2140  this->resize(nNewPtsCount);
2141 
2142  const float K = 1.0f / 255; // Intensity scale.
2143 
2144  // global 3D pose:
2145  CPose3D sensorGlobalPose;
2146  if (robotPose)
2147  sensorGlobalPose = *robotPose + scan.sensorPose;
2148  else
2149  sensorGlobalPose = scan.sensorPose;
2150 
2152  sensorGlobalPose.getHomogeneousMatrix(HM);
2153 
2154  const double m00 = HM(0, 0), m01 = HM(0, 1), m02 = HM(0, 2), m03 = HM(0, 3);
2155  const double m10 = HM(1, 0), m11 = HM(1, 1), m12 = HM(1, 2), m13 = HM(1, 3);
2156  const double m20 = HM(2, 0), m21 = HM(2, 1), m22 = HM(2, 2), m23 = HM(2, 3);
2157 
2158  // Copy points:
2159  for (size_t i = 0; i < nScanPts; i++)
2160  {
2161  const float inten = scan.point_cloud.intensity[i] * K;
2162  const double lx = scan.point_cloud.x[i];
2163  const double ly = scan.point_cloud.y[i];
2164  const double lz = scan.point_cloud.z[i];
2165 
2166  const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2167  const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2168  const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2169 
2170  this->setPointRGB(
2171  nOldPtsCount + i, gx, gy, gz, // XYZ
2172  inten, inten, inten // RGB
2173  );
2174  }
2175 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:715
void closestFromPointToLine(double Px, double Py, double x1, double y1, double x2, double y2, double &out_x, double &out_y)
Computes the closest point from a given point to a (infinite) line.
Definition: geometry.cpp:77
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - used in derived classes&#39; serialization.
Definition: CPointsMap.cpp:661
TLikelihoodOptions()
Initilization of default parameters.
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:636
void kdTreeNClosestPoint3DWithIdx(float x0, float y0, float z0, size_t knn, std::vector< float > &out_x, std::vector< float > &out_y, std::vector< float > &out_z, std::vector< size_t > &out_idx, std::vector< float > &out_dist_sqr) const
KD Tree-based search for the N closest points to some given 3D coordinates.
void insertAnotherMap(const CPointsMap *otherMap, const mrpt::poses::CPose3D &otherPose)
Insert the contents of another map into this one with some geometric transformation, without fusing close points.
Parameters for CMetricMap::compute3DMatchingRatio()
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 getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:192
void colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:114
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:252
mrpt::img::TColormap colormap
Colormap for points (index is "z" coordinates)
Definition: CPointsMap.h:329
virtual void addFrom(const CPointsMap &anotherMap)
Adds all the points from anotherMap to this map, without fusing.
void getPointFast(size_t index, float &x, float &y, float &z) const
Just like getPoint() but without checking out-of-bound index and without returning the point weight...
Definition: CPointsMap.h:480
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:241
A CObservation-derived class for RAW DATA (and optionally, point cloud) of scans from 3D Velodyne LID...
virtual void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr)=0
Transform the range scan into a set of cartessian coordinated points.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:753
static Ptr Create(Args &&... args)
#define MRPT_TRY_END
The end of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ex...
Definition: exceptions.h:213
void internal_build_points_map_from_scan2D(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps)
float sensorConeApperture
Cone aperture of each ultrasonic beam, in radians.
double x
X,Y coordinates.
Definition: TPose2D.h:30
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
T x
X,Y coordinates.
Definition: TPoint2D.h:25
An observation from any sensor that can be summarized as a pointcloud.
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
size_t size(const MATRIXLIKE &m, const int dim)
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards)...
Definition: CPose3D.cpp:599
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:327
virtual void setPointAllFieldsFast(const size_t index, const std::vector< float > &point_data)=0
Set all the data fields for one point as a vector: depending on the implementation class this can be ...
const double G
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:766
void base_copyFrom(const CPointsMap &obj)
Helper method for ::copyFrom()
void setAllVertices(const std::vector< double > &x, const std::vector< double > &y)
Set all vertices at once.
Definition: CPolygon.cpp:121
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
mrpt::vision::TStereoCalibParams params
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void internal_set_build_points_map_from_scan2D(scan2pts_functor fn)
void insertPoint(float x, float y, float z=0)
Provides a way to insert (append) individual points into the map: the missing fields of child classes...
Definition: CPointsMap.h:639
STL namespace.
virtual void getPointAllFieldsFast(const size_t index, std::vector< float > &point_data) const =0
Get all the data fields for one point as a vector: depending on the implementation class this can be ...
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
virtual void reserve(size_t newLength)=0
Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:728
virtual void insertPointRGB(float x, float y, float z, [[maybe_unused]] float R, [[maybe_unused]] float G, [[maybe_unused]] float B)
overload (RGB data is ignored in classes without color information)
Definition: CPointsMap.h:650
void generatePointCloud(const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
void PLY_import_set_vertex(const size_t idx, const mrpt::math::TPoint3Df &pt, const mrpt::img::TColorf *pt_color=nullptr) override
In a base class, will be called after PLY_import_set_vertex_count() once for each loaded point...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot/vehicle frame of reference.
size_t PLY_export_get_vertex_count() const override
In a base class, return the number of vertices.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:706
virtual unsigned int getPointWeight([[maybe_unused]] size_t index) const
Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually st...
Definition: CPointsMap.h:531
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
float maxDistForCorr
(Default: 0.10f) The minimum distance between 2 non-probabilistic map elements for counting them as a...
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
#define MRPT_TRY_START
The start of a standard MRPT "try...catch()" block that allows tracing throw the call stack after an ...
Definition: exceptions.h:206
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
TPointCloud point_cloud
Optionally, raw data can be converted into a 3D point cloud (local coordinates wrt the sensor...
float d2f(const double d)
shortcut for static_cast<float>(double)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
double internal_computeObservationLikelihoodPointCloud3D(const mrpt::poses::CPose3D &pc_in_map, const float *xs, const float *ys, const float *zs, const std::size_t num_pts)
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
Definition: CArchive.h:147
bool isPlanarScan(const double tolerance=0) const
Return true if the laser scanner is "horizontal", so it has an absolute value of "pitch" and "roll" l...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
double sigma_dist
Sigma squared (variance, in meters) of the exponential used to model the likelihood (default= 0...
Definition: CPointsMap.h:299
void getAllPoints(VECTOR &xs, VECTOR &ys, VECTOR &zs, size_t decimation=1) const
Returns a copy of the 2D/3D points as a std::vector of float coordinates.
Definition: CPointsMap.h:571
void loadFromRangeScan(const mrpt::obs::CObservation2DRangeScan &rangeScan, const mrpt::poses::CPose3D *robotPose=nullptr) override
See CPointsMap::loadFromRangeScan()
bool PointIntoPolygon(double x, double y) const
Check if a point is inside the polygon.
Definition: CPolygon.h:63
A list of TMatchingPair.
Definition: TMatchingPair.h:70
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
Definition: CObject.h:151
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:1129
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - used in derived classes&#39; serialization.
Definition: CPointsMap.cpp:669
void extractCylinder(const mrpt::math::TPoint2D &center, const double radius, const double zmin, const double zmax, CPointsMap *outMap)
Extracts the points in the map within a cylinder in 3D defined the provided radius and zmin/zmax valu...
void fuseWith(CPointsMap *anotherMap, float minDistForFuse=0.02f, std::vector< bool > *notFusedPoints=nullptr)
Insert the contents of another map into this one, fusing the previous content with the new one...
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:600
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
void extractPoints(const mrpt::math::TPoint3D &corner1, const mrpt::math::TPoint3D &corner2, CPointsMap *outMap, double R=1, double G=1, double B=1)
Extracts the points in the map within the area defined by two corners.
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
virtual void addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints)=0
Auxiliary method called from within addFrom() automatically, to finish the copying of class-specific ...
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
virtual void setPointWeight([[maybe_unused]] size_t index, [[maybe_unused]] unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
Definition: CPointsMap.h:524
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
Definition: CPointsMap.cpp:816
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CPointsMap.cpp:745
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:1124
void setPointFast(size_t index, float x, float y, float z)
Changes the coordinates of the given point (0-based index), without checking for out-of-bounds and wi...
Definition: CPointsMap.h:161
mrpt::maps::CPointsMap::Ptr pointcloud
The pointcloud.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void compute3DDistanceToMesh(const mrpt::maps::CMetricMap *otherMap2, const mrpt::poses::CPose3D &otherMapPose, float maxDistForCorrespondence, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio)
Computes the matchings between this and another 3D points map.
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
Undefined colormap [New in MRPT 2.0].
Definition: color_maps.h:33
void boundingBox(float &min_x, float &max_x, float &min_y, float &max_y, float &min_z, float &max_z) const
Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if there are no points.
Definition: CPointsMap.cpp:914
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
return_t square(const num_t x)
Inline function for the square of a number.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
mrpt::aligned_std_vector< float > m_z
Definition: CPointsMap.h:1116
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CPointsMap.cpp:687
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
virtual void resize(size_t newLength)=0
Resizes all point buffers so they can hold the given number of points: newly created points are set t...
void mark_as_modified() const
Users normally don&#39;t need to call this.
Definition: CPointsMap.h:1107
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
bool addToExistingPointsMap
Applicable to "loadFromRangeScan" only! If set to false, the points from the scan are loaded...
Definition: CPointsMap.h:237
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
bool fuseWithExisting
If set to true (default=false), inserted points are "fused" with previously existent ones...
Definition: CPointsMap.h:248
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1116
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
TLikelihoodOptions likelihoodOptions
Definition: CPointsMap.h:308
void filterUniqueRobustPairs(const size_t num_elements_this_map, TMatchingPairList &out_filtered_list) const
Creates a filtered list of pairings with those ones which have a single correspondence which coincide...
void getPointsBuffer(size_t &outPointsCount, const float *&xs, const float *&ys, const float *&zs) const
Provides a direct access to points buffer, or nullptr if there is no points in the map...
Definition: CPointsMap.cpp:216
void setPoint(size_t index, float x, float y, float z)
Changes a given point from map, with Z defaulting to 0 if not provided.
Definition: CPointsMap.h:492
TRenderOptions renderOptions
Definition: CPointsMap.h:331
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Definition: CPointsMap.cpp:795
void kdtree_mark_as_outdated() const
To be called by child classes when KD tree data changes.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
Lightweight 2D pose.
Definition: TPose2D.h:22
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
Definition: CPointsMap.h:256
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CPointsMap.cpp:275
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:612
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose) override
This is a common version of CMetricMap::insertObservation() for point maps (actually, CMetricMap::internal_insertObservation), so derived classes don&#39;t need to worry implementing that method unless something special is really necesary.
void(*)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps) scan2pts_functor
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:272
MRPT_TODO("toPointCloud / calibration")
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
Definition: CPointsMap.h:303
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
Definition: CMetricMap.h:295
static TAuxLoadFunctor dummy_loader
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
size_t kdTreeClosestPoint2D(float x0, float y0, float &out_x, float &out_y, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 2D coordinates.
virtual void insertPointFast(float x, float y, float z=0)=0
The virtual method for insertPoint() without calling mark_as_modified()
void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
virtual void setPointRGB(size_t index, float x, float y, float z, [[maybe_unused]] float R, [[maybe_unused]] float G, [[maybe_unused]] float B)
overload (RGB data is ignored in classes without color information)
Definition: CPointsMap.h:514
float squareDistanceToClosestCorrespondence(float x0, float y0) const override
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map...
Definition: CPointsMap.cpp:871
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
Definition: CPointsMap.h:306
Parameters for the determination of matchings between point clouds, etc.
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1116
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:440
bool disableDeletion
If set to false (default=true) points in the same plane as the inserted scan and inside the free spac...
Definition: CPointsMap.h:244
Functions for estimating the optimal transformation between two frames of references given measuremen...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPose3D.cpp:781
double phi
Orientation (rads)
Definition: TPose2D.h:32
void PLY_export_get_vertex(const size_t idx, mrpt::math::TPoint3Df &pt, bool &pt_has_color, mrpt::img::TColorf &pt_color) const override
In a base class, will be called after PLY_export_get_vertex_count() once for each exported point...
float kdTreeClosestPoint2DsqrError(float x0, float y0) const
Like kdTreeClosestPoint2D, but just return the square error from some point to its closest neighbor...
void kdTreeTwoClosestPoint2D(float x0, float y0, float &out_x1, float &out_y1, float &out_x2, float &out_y2, float &out_dist_sqr1, float &out_dist_sqr2) const
KD Tree-based search for the TWO closest point to some given 2D coordinates.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".
size_t kdTreeClosestPoint3D(float x0, float y0, float z0, float &out_x, float &out_y, float &out_z, float &out_dist_sqr) const
KD Tree-based search for the closest point (only ONE) to some given 3D coordinates.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:233
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:644
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.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020