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