MRPT  1.9.9
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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "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 #if MRPT_HAS_SSE2
30 #include <mrpt/core/SSE_macros.h>
31 #include <mrpt/core/SSE_types.h>
32 #endif
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  save2D_to_text_file
65  Save to a text file. In each line there are a point coordinates.
66  Returns false if any error occured, true elsewere.
67  ---------------------------------------------------------------*/
68 bool CPointsMap::save2D_to_text_file(const string& file) const
69 {
70  FILE* f = os::fopen(file.c_str(), "wt");
71  if (!f) return false;
72 
73  for (unsigned int i = 0; i < m_x.size(); i++)
74  os::fprintf(f, "%f %f\n", m_x[i], m_y[i]);
75 
76  os::fclose(f);
77  return true;
78 }
79 
80 /*---------------------------------------------------------------
81  save3D_to_text_file
82  Save to a text file. In each line there are a point coordinates.
83  Returns false if any error occured, true elsewere.
84  ---------------------------------------------------------------*/
85 bool CPointsMap::save3D_to_text_file(const string& file) const
86 {
87  FILE* f = os::fopen(file.c_str(), "wt");
88  if (!f) return false;
89 
90  for (unsigned int i = 0; i < m_x.size(); i++)
91  os::fprintf(f, "%f %f %f\n", m_x[i], m_y[i], m_z[i]);
92 
93  os::fclose(f);
94  return true;
95 }
96 
97 /*---------------------------------------------------------------
98  load2Dor3D_from_text_file
99  Load from a text file. In each line there are a point coordinates.
100  Returns false if any error occured, true elsewere.
101  ---------------------------------------------------------------*/
102 bool CPointsMap::load2Dor3D_from_text_file(
103  const std::string& file, const bool is_3D)
104 {
105  MRPT_START
106 
107  mark_as_modified();
108 
109  FILE* f = os::fopen(file.c_str(), "rt");
110  if (!f) return false;
111 
112  char str[1024];
113  char *ptr, *ptr1, *ptr2, *ptr3;
114 
115  // Clear current map:
116  this->clear();
117 
118  while (!feof(f))
119  {
120  // Read one string line:
121  str[0] = 0;
122  if (!fgets(str, sizeof(str), f)) break;
123 
124  // Find the first digit:
125  ptr = str;
126  while (ptr[0] && (ptr[0] == ' ' || ptr[0] == '\t' || ptr[0] == '\r' ||
127  ptr[0] == '\n'))
128  ptr++;
129 
130  // And try to parse it:
131  float xx = strtod(ptr, &ptr1);
132  if (ptr1 != str)
133  {
134  float yy = strtod(ptr1, &ptr2);
135  if (ptr2 != ptr1)
136  {
137  if (!is_3D)
138  {
139  this->insertPoint(xx, yy, 0);
140  }
141  else
142  {
143  float zz = strtod(ptr2, &ptr3);
144  if (ptr3 != ptr2)
145  {
146  this->insertPoint(xx, yy, zz);
147  }
148  }
149  }
150  }
151  }
152 
153  os::fclose(f);
154  return true;
155 
156  MRPT_END
157 }
158 
159 /*---------------------------------------------------------------
160  Implements the writing to a mxArray for Matlab
161  ---------------------------------------------------------------*/
162 #if MRPT_HAS_MATLAB
163 // Add to implement mexplus::from template specialization
165 #endif
166 
167 mxArray* CPointsMap::writeToMatlab() const
168 {
169 #if MRPT_HAS_MATLAB
170  MRPT_TODO("Create 3xN array xyz of points coordinates")
171  const char* fields[] = {"x", "y", "z"};
172  mexplus::MxArray map_struct(
173  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
174 
175  map_struct.set("x", m_x);
176  map_struct.set("y", m_y);
177  map_struct.set("z", m_z);
178  return map_struct.release();
179 #else
180  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
181 #endif
182 }
183 
184 /*---------------------------------------------------------------
185  getPoint
186  Access to stored points coordinates:
187  ---------------------------------------------------------------*/
188 void CPointsMap::getPoint(size_t index, float& x, float& y) const
189 {
190  ASSERT_BELOW_(index, m_x.size());
191  x = m_x[index];
192  y = m_y[index];
193 }
194 void CPointsMap::getPoint(size_t index, float& x, float& y, float& z) const
195 {
196  ASSERT_BELOW_(index, m_x.size());
197  x = m_x[index];
198  y = m_y[index];
199  z = m_z[index];
200 }
201 void CPointsMap::getPoint(size_t index, double& x, double& y) const
202 {
203  ASSERT_BELOW_(index, m_x.size());
204  x = m_x[index];
205  y = m_y[index];
206 }
207 void CPointsMap::getPoint(size_t index, double& x, double& y, double& z) const
208 {
209  ASSERT_BELOW_(index, m_x.size());
210  x = m_x[index];
211  y = m_y[index];
212  z = m_z[index];
213 }
214 
215 /*---------------------------------------------------------------
216  getPointsBuffer
217  ---------------------------------------------------------------*/
218 void CPointsMap::getPointsBuffer(
219  size_t& outPointsCount, const float*& xs, const float*& ys,
220  const float*& zs) const
221 {
222  outPointsCount = size();
223 
224  if (outPointsCount > 0)
225  {
226  xs = &m_x[0];
227  ys = &m_y[0];
228  zs = &m_z[0];
229  }
230  else
231  {
232  xs = ys = zs = nullptr;
233  }
234 }
235 
236 /*---------------------------------------------------------------
237  clipOutOfRangeInZ
238  ---------------------------------------------------------------*/
239 void CPointsMap::clipOutOfRangeInZ(float zMin, float zMax)
240 {
241  const size_t n = size();
242  vector<bool> deletionMask(n);
243 
244  // Compute it:
245  for (size_t i = 0; i < n; i++)
246  deletionMask[i] = (m_z[i] < zMin || m_z[i] > zMax);
247 
248  // Perform deletion:
249  applyDeletionMask(deletionMask);
250 
251  mark_as_modified();
252 }
253 
254 /*---------------------------------------------------------------
255  clipOutOfRange
256  ---------------------------------------------------------------*/
257 void CPointsMap::clipOutOfRange(const TPoint2D& p, float maxRange)
258 {
259  size_t i, n = size();
260  vector<bool> deletionMask;
261 
262  // The deletion mask:
263  deletionMask.resize(n);
264 
265  const auto max_sq = maxRange * maxRange;
266 
267  // Compute it:
268  for (i = 0; i < n; i++)
269  deletionMask[i] = square(p.x - m_x[i]) + square(p.y - m_y[i]) > max_sq;
270 
271  // Perform deletion:
272  applyDeletionMask(deletionMask);
273 
274  mark_as_modified();
275 }
276 
277 void CPointsMap::determineMatching2D(
278  const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
279  TMatchingPairList& correspondences, const TMatchingParams& params,
280  TMatchingExtraResults& extraResults) const
281 {
282  MRPT_START
283 
284  extraResults = TMatchingExtraResults(); // Clear output
285 
286  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
288  params.offset_other_map_points, params.decimation_other_map_points);
289  ASSERT_(IS_DERIVED(*otherMap2, CPointsMap));
290  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
291 
292  const TPose2D otherMapPose(
293  otherMapPose_.x(), otherMapPose_.y(), otherMapPose_.phi());
294 
295  const size_t nLocalPoints = otherMap->size();
296  const size_t nGlobalPoints = this->size();
297  float _sumSqrDist = 0;
298  size_t _sumSqrCount = 0;
299  size_t nOtherMapPointsWithCorrespondence =
300  0; // Number of points with one corrs. at least
301 
302  float local_x_min = std::numeric_limits<float>::max(),
303  local_x_max = -std::numeric_limits<float>::max();
304  float global_x_min = std::numeric_limits<float>::max(),
305  global_x_max = -std::numeric_limits<float>::max();
306  float local_y_min = std::numeric_limits<float>::max(),
307  local_y_max = -std::numeric_limits<float>::max();
308  float global_y_min = std::numeric_limits<float>::max(),
309  global_y_max = -std::numeric_limits<float>::max();
310 
311  double maxDistForCorrespondenceSquared;
312  float x_local, y_local;
313  unsigned int localIdx;
314 
315  const float *x_other_it, *y_other_it, *z_other_it;
316 
317  // Prepare output: no correspondences initially:
318  correspondences.clear();
319  correspondences.reserve(nLocalPoints);
320  extraResults.correspondencesRatio = 0;
321 
322  TMatchingPairList _correspondences;
323  _correspondences.reserve(nLocalPoints);
324 
325  // Nothing to do if we have an empty map!
326  if (!nGlobalPoints || !nLocalPoints) return;
327 
328  const double sin_phi = sin(otherMapPose.phi);
329  const double cos_phi = cos(otherMapPose.phi);
330 
331 // Do matching only there is any chance of the two maps to overlap:
332 // -----------------------------------------------------------
333 // Translate and rotate all local points, while simultaneously
334 // estimating the bounding box:
335 #if MRPT_HAS_SSE2
336  // Number of 4-floats:
337  size_t nPackets = nLocalPoints / 4;
338 
339  Eigen::ArrayXf x_locals(nLocalPoints), y_locals(nLocalPoints);
340 
341  // load 4 copies of the same value
342  const __m128 cos_4val = _mm_set1_ps(cos_phi);
343  const __m128 sin_4val = _mm_set1_ps(sin_phi);
344  const __m128 x0_4val = _mm_set1_ps(otherMapPose.x);
345  const __m128 y0_4val = _mm_set1_ps(otherMapPose.y);
346 
347  // For the bounding box:
348  __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
349  __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
350  __m128 y_mins = x_mins;
351  __m128 y_maxs = x_maxs;
352 
353  const float* ptr_in_x = &otherMap->m_x[0];
354  const float* ptr_in_y = &otherMap->m_y[0];
355  float* ptr_out_x = &x_locals[0];
356  float* ptr_out_y = &y_locals[0];
357 
358  for (; nPackets; nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_out_x += 4,
359  ptr_out_y += 4)
360  {
361  const __m128 xs = _mm_loadu_ps(ptr_in_x); // *Unaligned* load
362  const __m128 ys = _mm_loadu_ps(ptr_in_y);
363 
364  const __m128 lxs = _mm_add_ps(
365  x0_4val,
366  _mm_sub_ps(_mm_mul_ps(xs, cos_4val), _mm_mul_ps(ys, sin_4val)));
367  const __m128 lys = _mm_add_ps(
368  y0_4val,
369  _mm_add_ps(_mm_mul_ps(xs, sin_4val), _mm_mul_ps(ys, cos_4val)));
370  _mm_store_ps(ptr_out_x, lxs);
371  _mm_store_ps(ptr_out_y, lys);
372 
373  x_mins = _mm_min_ps(x_mins, lxs);
374  x_maxs = _mm_max_ps(x_maxs, lxs);
375  y_mins = _mm_min_ps(y_mins, lys);
376  y_maxs = _mm_max_ps(y_maxs, lys);
377  }
378 
379  // Recover the min/max:
380  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float temp_nums[4];
381 
382  _mm_store_ps(temp_nums, x_mins);
383  local_x_min =
384  min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
385  _mm_store_ps(temp_nums, y_mins);
386  local_y_min =
387  min(min(temp_nums[0], temp_nums[1]), min(temp_nums[2], temp_nums[3]));
388  _mm_store_ps(temp_nums, x_maxs);
389  local_x_max =
390  max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
391  _mm_store_ps(temp_nums, y_maxs);
392  local_y_max =
393  max(max(temp_nums[0], temp_nums[1]), max(temp_nums[2], temp_nums[3]));
394 
395  // transform extra pts & update BBox
396  for (size_t k = 0; k < nLocalPoints % 4; k++)
397  {
398  float x = ptr_in_x[k];
399  float y = ptr_in_y[k];
400  float out_x = otherMapPose.x + cos_phi * x - sin_phi * y;
401  float out_y = otherMapPose.y + sin_phi * x + cos_phi * y;
402 
403  local_x_min = std::min(local_x_min, out_x);
404  local_x_max = std::max(local_x_max, out_x);
405 
406  local_y_min = std::min(local_y_min, out_y);
407  local_y_max = std::max(local_y_max, out_y);
408 
409  ptr_out_x[k] = out_x;
410  ptr_out_y[k] = out_y;
411  }
412 
413 #else
414  // Non SSE2 version:
416  const_cast<float*>(&otherMap->m_x[0]), otherMap->m_x.size(), 1);
418  const_cast<float*>(&otherMap->m_y[0]), otherMap->m_y.size(), 1);
419 
420  Eigen::Array<float, Eigen::Dynamic, 1> x_locals =
421  otherMapPose.x + cos_phi * x_org.array() - sin_phi * y_org.array();
422  Eigen::Array<float, Eigen::Dynamic, 1> y_locals =
423  otherMapPose.y + sin_phi * x_org.array() + cos_phi * y_org.array();
424 
425  local_x_min = x_locals.minCoeff();
426  local_y_min = y_locals.minCoeff();
427  local_x_max = x_locals.maxCoeff();
428  local_y_max = y_locals.maxCoeff();
429 #endif
430 
431  // Find the bounding box:
432  float global_z_min, global_z_max;
433  this->boundingBox(
434  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
435  global_z_max);
436 
437  // Only try doing a matching if there exist any chance of both maps
438  // touching/overlaping:
439  if (local_x_min > global_x_max || local_x_max < global_x_min ||
440  local_y_min > global_y_max || local_y_max < global_y_min)
441  return; // We know for sure there is no matching at all
442 
443  // Loop for each point in local map:
444  // --------------------------------------------------
445  for (localIdx = params.offset_other_map_points,
446  x_other_it = &otherMap->m_x[params.offset_other_map_points],
447  y_other_it = &otherMap->m_y[params.offset_other_map_points],
448  z_other_it = &otherMap->m_z[params.offset_other_map_points];
449  localIdx < nLocalPoints;
450  x_other_it += params.decimation_other_map_points,
451  y_other_it += params.decimation_other_map_points,
452  z_other_it += params.decimation_other_map_points,
453  localIdx += params.decimation_other_map_points)
454  {
455  // For speed-up:
456  x_local = x_locals[localIdx]; // *x_locals_it;
457  y_local = y_locals[localIdx]; // *y_locals_it;
458 
459  // Find all the matchings in the requested distance:
460 
461  // KD-TREE implementation =================================
462  // Use a KD-tree to look for the nearnest neighbor of:
463  // (x_local, y_local, z_local)
464  // In "this" (global/reference) points map.
465 
466  float tentativ_err_sq;
467  unsigned int tentativ_this_idx = kdTreeClosestPoint2D(
468  x_local, y_local, // Look closest to this guy
469  tentativ_err_sq // save here the min. distance squared
470  );
471 
472  // Compute max. allowed distance:
473  maxDistForCorrespondenceSquared = square(
474  params.maxAngularDistForCorrespondence *
475  std::sqrt(
476  square(params.angularDistPivotPoint.x - x_local) +
477  square(params.angularDistPivotPoint.y - y_local)) +
478  params.maxDistForCorrespondence);
479 
480  // Distance below the threshold??
481  if (tentativ_err_sq < maxDistForCorrespondenceSquared)
482  {
483  // Save all the correspondences:
484  _correspondences.resize(_correspondences.size() + 1);
485 
486  TMatchingPair& p = _correspondences.back();
487 
488  p.this_idx = tentativ_this_idx;
489  p.this_x = m_x[tentativ_this_idx];
490  p.this_y = m_y[tentativ_this_idx];
491  p.this_z = m_z[tentativ_this_idx];
492 
493  p.other_idx = localIdx;
494  p.other_x = *x_other_it;
495  p.other_y = *y_other_it;
496  p.other_z = *z_other_it;
497 
498  p.errorSquareAfterTransformation = tentativ_err_sq;
499 
500  // At least one:
501  nOtherMapPointsWithCorrespondence++;
502 
503  // Accumulate the MSE:
504  _sumSqrDist += p.errorSquareAfterTransformation;
505  _sumSqrCount++;
506  }
507 
508  } // For each local point
509 
510  // Additional consistency filter: "onlyKeepTheClosest" up to now
511  // led to just one correspondence for each "local map" point, but
512  // many of them may have as corresponding pair the same "global point"!!
513  // -------------------------------------------------------------------------
514  if (params.onlyUniqueRobust)
515  {
516  ASSERTMSG_(
517  params.onlyKeepTheClosest,
518  "ERROR: onlyKeepTheClosest must be also set to true when "
519  "onlyUniqueRobust=true.");
520  _correspondences.filterUniqueRobustPairs(
521  nGlobalPoints, correspondences);
522  }
523  else
524  {
525  correspondences.swap(_correspondences);
526  }
527 
528  // If requested, copy sum of squared distances to output pointer:
529  // -------------------------------------------------------------------
530  if (_sumSqrCount)
531  extraResults.sumSqrDist =
532  _sumSqrDist / static_cast<double>(_sumSqrCount);
533  else
534  extraResults.sumSqrDist = 0;
535 
536  // The ratio of points in the other map with corrs:
537  extraResults.correspondencesRatio = params.decimation_other_map_points *
538  nOtherMapPointsWithCorrespondence /
539  static_cast<float>(nLocalPoints);
540 
541  MRPT_END
542 }
543 
544 /*---------------------------------------------------------------
545  changeCoordinatesReference
546  ---------------------------------------------------------------*/
547 void CPointsMap::changeCoordinatesReference(const CPose2D& newBase)
548 {
549  const size_t N = m_x.size();
550 
551  const CPose3D newBase3D(newBase);
552 
553  for (size_t i = 0; i < N; i++)
554  newBase3D.composePoint(
555  m_x[i], m_y[i], m_z[i], // In
556  m_x[i], m_y[i], m_z[i] // Out
557  );
558 
559  mark_as_modified();
560 }
561 
562 /*---------------------------------------------------------------
563  changeCoordinatesReference
564  ---------------------------------------------------------------*/
565 void CPointsMap::changeCoordinatesReference(const CPose3D& newBase)
566 {
567  const size_t N = m_x.size();
568 
569  for (size_t i = 0; i < N; i++)
570  newBase.composePoint(
571  m_x[i], m_y[i], m_z[i], // In
572  m_x[i], m_y[i], m_z[i] // Out
573  );
574 
575  mark_as_modified();
576 }
577 
578 /*---------------------------------------------------------------
579  changeCoordinatesReference
580  ---------------------------------------------------------------*/
581 void CPointsMap::changeCoordinatesReference(
582  const CPointsMap& other, const CPose3D& newBase)
583 {
584  *this = other;
585  changeCoordinatesReference(newBase);
586 }
587 
588 /*---------------------------------------------------------------
589  isEmpty
590  ---------------------------------------------------------------*/
591 bool CPointsMap::isEmpty() const { return m_x.empty(); }
592 /*---------------------------------------------------------------
593  TInsertionOptions
594  ---------------------------------------------------------------*/
595 CPointsMap::TInsertionOptions::TInsertionOptions()
596  : horizontalTolerance(0.05_deg)
597 
598 {
599 }
600 
601 // Binary dump to/read from stream - for usage in derived classes' serialization
604 {
605  const int8_t version = 0;
606  out << version;
607 
608  out << minDistBetweenLaserPoints << addToExistingPointsMap
609  << also_interpolate << disableDeletion << fuseWithExisting
610  << isPlanarMap << horizontalTolerance << maxDistForInterpolatePoints
611  << insertInvalidPoints; // v0
612 }
613 
616 {
617  int8_t version;
618  in >> version;
619  switch (version)
620  {
621  case 0:
622  {
623  in >> minDistBetweenLaserPoints >> addToExistingPointsMap >>
624  also_interpolate >> disableDeletion >> fuseWithExisting >>
625  isPlanarMap >> horizontalTolerance >>
626  maxDistForInterpolatePoints >> insertInvalidPoints; // v0
627  }
628  break;
629  default:
631  }
632 }
633 
635 
636  = default;
637 
640 {
641  const int8_t version = 0;
642  out << version;
643  out << sigma_dist << max_corr_distance << decimation;
644 }
645 
648 {
649  int8_t version;
650  in >> version;
651  switch (version)
652  {
653  case 0:
654  {
655  in >> sigma_dist >> max_corr_distance >> decimation;
656  }
657  break;
658  default:
660  }
661 }
662 
665 {
666  const int8_t version = 0;
667  out << version;
668  out << point_size << color << int8_t(colormap);
669 }
670 
673 {
674  int8_t version;
675  in >> version;
676  switch (version)
677  {
678  case 0:
679  {
680  in >> point_size >> this->color;
681  in.ReadAsAndCastTo<int8_t>(this->colormap);
682  }
683  break;
684  default:
686  }
687 }
688 
690 {
691  out << "\n----------- [CPointsMap::TInsertionOptions] ------------ \n\n";
692 
693  LOADABLEOPTS_DUMP_VAR(minDistBetweenLaserPoints, double);
694  LOADABLEOPTS_DUMP_VAR(maxDistForInterpolatePoints, double);
695  LOADABLEOPTS_DUMP_VAR_DEG(horizontalTolerance);
696 
697  LOADABLEOPTS_DUMP_VAR(addToExistingPointsMap, bool);
698  LOADABLEOPTS_DUMP_VAR(also_interpolate, bool);
699  LOADABLEOPTS_DUMP_VAR(disableDeletion, bool);
700  LOADABLEOPTS_DUMP_VAR(fuseWithExisting, bool);
701  LOADABLEOPTS_DUMP_VAR(isPlanarMap, bool);
702 
703  LOADABLEOPTS_DUMP_VAR(insertInvalidPoints, bool);
704 
705  out << endl;
706 }
707 
709 {
710  out << "\n----------- [CPointsMap::TLikelihoodOptions] ------------ \n\n";
711 
712  LOADABLEOPTS_DUMP_VAR(sigma_dist, double);
713  LOADABLEOPTS_DUMP_VAR(max_corr_distance, double);
714  LOADABLEOPTS_DUMP_VAR(decimation, int);
715 }
716 
718 {
719  out << "\n----------- [CPointsMap::TRenderOptions] ------------ \n\n";
720 
721  LOADABLEOPTS_DUMP_VAR(point_size, float);
722  LOADABLEOPTS_DUMP_VAR(color.R, float);
723  LOADABLEOPTS_DUMP_VAR(color.G, float);
724  LOADABLEOPTS_DUMP_VAR(color.B, float);
725  // LOADABLEOPTS_DUMP_VAR(colormap, int);
726 }
727 /*---------------------------------------------------------------
728  loadFromConfigFile
729  ---------------------------------------------------------------*/
731  const mrpt::config::CConfigFileBase& iniFile, const string& section)
732 {
733  MRPT_LOAD_CONFIG_VAR(minDistBetweenLaserPoints, float, iniFile, section);
734  MRPT_LOAD_CONFIG_VAR_DEGREES(horizontalTolerance, iniFile, section);
735 
736  MRPT_LOAD_CONFIG_VAR(addToExistingPointsMap, bool, iniFile, section);
737  MRPT_LOAD_CONFIG_VAR(also_interpolate, bool, iniFile, section);
738  MRPT_LOAD_CONFIG_VAR(disableDeletion, bool, iniFile, section);
739  MRPT_LOAD_CONFIG_VAR(fuseWithExisting, bool, iniFile, section);
740  MRPT_LOAD_CONFIG_VAR(isPlanarMap, bool, iniFile, section);
741 
742  MRPT_LOAD_CONFIG_VAR(maxDistForInterpolatePoints, float, iniFile, section);
743 
744  MRPT_LOAD_CONFIG_VAR(insertInvalidPoints, bool, iniFile, section);
745 }
746 
748  const mrpt::config::CConfigFileBase& iniFile, const string& section)
749 {
750  MRPT_LOAD_CONFIG_VAR(sigma_dist, double, iniFile, section);
751  MRPT_LOAD_CONFIG_VAR(max_corr_distance, double, iniFile, section);
752  MRPT_LOAD_CONFIG_VAR(decimation, int, iniFile, section);
753 }
754 
756  const mrpt::config::CConfigFileBase& iniFile, const string& section)
757 {
758  MRPT_LOAD_CONFIG_VAR(point_size, float, iniFile, section);
759  MRPT_LOAD_CONFIG_VAR(color.R, float, iniFile, section);
760  MRPT_LOAD_CONFIG_VAR(color.G, float, iniFile, section);
761  MRPT_LOAD_CONFIG_VAR(color.B, float, iniFile, section);
762  colormap = iniFile.read_enum(section, "colormap", this->colormap);
763 }
764 
765 /*---------------------------------------------------------------
766  getAs3DObject
767 ---------------------------------------------------------------*/
769 {
770  MRPT_START
772 
774  {
775  // Single color:
777  obj->loadFromPointsMap(this);
778  obj->setColor(renderOptions.color);
779  obj->setPointSize(renderOptions.point_size);
780  obj->enableColorFromZ(false);
781  outObj->insert(obj);
782  }
783  else
784  {
786  obj->loadFromPointsMap(this);
787  obj->setPointSize(renderOptions.point_size);
788  mrpt::math::TPoint3D pMin, pMax;
789  this->boundingBox(pMin, pMax);
790  obj->recolorizeByCoordinate(
791  pMin.z, pMax.z, 2 /*z*/, renderOptions.colormap);
792  outObj->insert(obj);
793  }
794  MRPT_END
795 }
796 
798  const mrpt::maps::CMetricMap* otherMap2,
799  const mrpt::poses::CPose3D& otherMapPose,
800  const TMatchingRatioParams& mrp) const
801 {
802  TMatchingPairList correspondences;
804  TMatchingExtraResults extraResults;
805 
806  params.maxDistForCorrespondence = mrp.maxDistForCorr;
807 
808  this->determineMatching3D(
809  otherMap2->getAsSimplePointsMap(), otherMapPose, correspondences,
810  params, extraResults);
811 
812  return extraResults.correspondencesRatio;
813 }
814 
815 /*---------------------------------------------------------------
816  getLargestDistanceFromOrigin
817 ---------------------------------------------------------------*/
819 {
820  // Updated?
822  {
823  // NO: Update it:
824  float maxDistSq = 0, d;
825  for (auto X = m_x.begin(), Y = m_y.begin(), Z = m_z.begin();
826  X != m_x.end(); ++X, ++Y, ++Z)
827  {
828  d = square(*X) + square(*Y) + square(*Z);
829  maxDistSq = max(d, maxDistSq);
830  }
831 
832  m_largestDistanceFromOrigin = sqrt(maxDistSq);
834  }
836 }
837 
838 /*---------------------------------------------------------------
839  getAllPoints
840 ---------------------------------------------------------------*/
842  vector<float>& xs, vector<float>& ys, size_t decimation) const
843 {
844  MRPT_START
845  ASSERT_(decimation > 0);
846  if (decimation == 1)
847  {
848  xs = vector<float>(m_x.begin(), m_x.end());
849  ys = vector<float>(m_y.begin(), m_y.end());
850  }
851  else
852  {
853  size_t N = m_x.size() / decimation;
854 
855  xs.resize(N);
856  ys.resize(N);
857 
858  auto X = m_x.begin();
859  auto Y = m_y.begin();
860  for (auto oX = xs.begin(), oY = ys.begin(); oX != xs.end();
861  X += decimation, Y += decimation, ++oX, ++oY)
862  {
863  *oX = *X;
864  *oY = *Y;
865  }
866  }
867  MRPT_END
868 }
869 
870 /*---------------------------------------------------------------
871  squareDistanceToClosestCorrespondence
872 ---------------------------------------------------------------*/
874  float x0, float y0) const
875 {
876  // Just the closest point:
877 
878 #if 1
879  return kdTreeClosestPoint2DsqrError(x0, y0);
880 #else
881  // The distance to the line that interpolates the TWO closest points:
882  float x1, y1, x2, y2, d1, d2;
884  x0, y0, // The query
885  x1, y1, // Closest point #1
886  x2, y2, // Closest point #2
887  d1, d2);
888 
889  ASSERT_(d2 >= d1);
890 
891  // If the two points are too far, do not interpolate:
892  float d12 = square(x1 - x2) + square(y1 - y2);
893  if (d12 > 0.20f * 0.20f || d12 < 0.03f * 0.03f)
894  {
895  return square(x1 - x0) + square(y1 - y0);
896  }
897  else
898  { // Interpolate
899  double interp_x, interp_y;
900 
901  // math::closestFromPointToSegment(
903  x0, y0, // the point
904  x1, y1, x2, y2, // The segment
905  interp_x, interp_y // out
906  );
907 
908  return square(interp_x - x0) + square(interp_y - y0);
909  }
910 #endif
911 }
912 
913 /*---------------------------------------------------------------
914  boundingBox
915 ---------------------------------------------------------------*/
917  float& min_x, float& max_x, float& min_y, float& max_y, float& min_z,
918  float& max_z) const
919 {
920  MRPT_START
921 
922  const size_t nPoints = m_x.size();
923 
925  {
926  if (!nPoints)
927  {
929  m_bb_max_z = 0;
930  }
931  else
932  {
933 #if MRPT_HAS_SSE2
934  // Vectorized version: ~ 9x times faster
935 
936  // Number of 4-floats:
937  size_t nPackets = nPoints / 4;
938 
939  // For the bounding box:
940  __m128 x_mins = _mm_set1_ps(std::numeric_limits<float>::max());
941  __m128 x_maxs = _mm_set1_ps(std::numeric_limits<float>::min());
942  __m128 y_mins = x_mins, y_maxs = x_maxs;
943  __m128 z_mins = x_mins, z_maxs = x_maxs;
944 
945  const float* ptr_in_x = &m_x[0];
946  const float* ptr_in_y = &m_y[0];
947  const float* ptr_in_z = &m_z[0];
948 
949  for (; nPackets;
950  nPackets--, ptr_in_x += 4, ptr_in_y += 4, ptr_in_z += 4)
951  {
952  const __m128 xs = _mm_loadu_ps(ptr_in_x); // *Unaligned* load
953  x_mins = _mm_min_ps(x_mins, xs);
954  x_maxs = _mm_max_ps(x_maxs, xs);
955 
956  const __m128 ys = _mm_loadu_ps(ptr_in_y);
957  y_mins = _mm_min_ps(y_mins, ys);
958  y_maxs = _mm_max_ps(y_maxs, ys);
959 
960  const __m128 zs = _mm_loadu_ps(ptr_in_z);
961  z_mins = _mm_min_ps(z_mins, zs);
962  z_maxs = _mm_max_ps(z_maxs, zs);
963  }
964 
965  // Recover the min/max:
966  alignas(MRPT_MAX_STATIC_ALIGN_BYTES) float temp_nums[4];
967 
968  _mm_store_ps(temp_nums, x_mins);
969  m_bb_min_x =
970  min(min(temp_nums[0], temp_nums[1]),
971  min(temp_nums[2], temp_nums[3]));
972  _mm_store_ps(temp_nums, y_mins);
973  m_bb_min_y =
974  min(min(temp_nums[0], temp_nums[1]),
975  min(temp_nums[2], temp_nums[3]));
976  _mm_store_ps(temp_nums, z_mins);
977  m_bb_min_z =
978  min(min(temp_nums[0], temp_nums[1]),
979  min(temp_nums[2], temp_nums[3]));
980  _mm_store_ps(temp_nums, x_maxs);
981  m_bb_max_x =
982  max(max(temp_nums[0], temp_nums[1]),
983  max(temp_nums[2], temp_nums[3]));
984  _mm_store_ps(temp_nums, y_maxs);
985  m_bb_max_y =
986  max(max(temp_nums[0], temp_nums[1]),
987  max(temp_nums[2], temp_nums[3]));
988  _mm_store_ps(temp_nums, z_maxs);
989  m_bb_max_z =
990  max(max(temp_nums[0], temp_nums[1]),
991  max(temp_nums[2], temp_nums[3]));
992 
993  // extra
994  for (size_t k = 0; k < nPoints % 4; k++)
995  {
996  m_bb_min_x = std::min(m_bb_min_x, ptr_in_x[k]);
997  m_bb_max_x = std::max(m_bb_max_x, ptr_in_x[k]);
998 
999  m_bb_min_y = std::min(m_bb_min_y, ptr_in_y[k]);
1000  m_bb_max_y = std::max(m_bb_max_y, ptr_in_y[k]);
1001 
1002  m_bb_min_z = std::min(m_bb_min_z, ptr_in_z[k]);
1003  m_bb_max_z = std::max(m_bb_max_z, ptr_in_z[k]);
1004  }
1005 #else
1006  // Non vectorized version:
1008  (std::numeric_limits<float>::max)();
1009 
1011  -(std::numeric_limits<float>::max)();
1012 
1013  for (auto xi = m_x.begin(), yi = m_y.begin(), zi = m_z.begin();
1014  xi != m_x.end(); xi++, yi++, zi++)
1015  {
1016  m_bb_min_x = min(m_bb_min_x, *xi);
1017  m_bb_max_x = max(m_bb_max_x, *xi);
1018  m_bb_min_y = min(m_bb_min_y, *yi);
1019  m_bb_max_y = max(m_bb_max_y, *yi);
1020  m_bb_min_z = min(m_bb_min_z, *zi);
1021  m_bb_max_z = max(m_bb_max_z, *zi);
1022  }
1023 #endif
1024  }
1025  m_boundingBoxIsUpdated = true;
1026  }
1027 
1028  min_x = m_bb_min_x;
1029  max_x = m_bb_max_x;
1030  min_y = m_bb_min_y;
1031  max_y = m_bb_max_y;
1032  min_z = m_bb_min_z;
1033  max_z = m_bb_max_z;
1034  MRPT_END
1035 }
1036 
1037 /*---------------------------------------------------------------
1038  computeMatchingWith3D
1039 ---------------------------------------------------------------*/
1041  const mrpt::maps::CMetricMap* otherMap2, const CPose3D& otherMapPose,
1042  TMatchingPairList& correspondences, const TMatchingParams& params,
1043  TMatchingExtraResults& extraResults) const
1044 {
1045  MRPT_START
1046 
1047  extraResults = TMatchingExtraResults();
1048 
1049  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
1050  ASSERT_BELOW_(
1051  params.offset_other_map_points, params.decimation_other_map_points);
1052 
1054  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
1055 
1056  const size_t nLocalPoints = otherMap->size();
1057  const size_t nGlobalPoints = this->size();
1058  float _sumSqrDist = 0;
1059  size_t _sumSqrCount = 0;
1060  size_t nOtherMapPointsWithCorrespondence =
1061  0; // Number of points with one corrs. at least
1062 
1063  float local_x_min = std::numeric_limits<float>::max(),
1064  local_x_max = -std::numeric_limits<float>::max();
1065  float local_y_min = std::numeric_limits<float>::max(),
1066  local_y_max = -std::numeric_limits<float>::max();
1067  float local_z_min = std::numeric_limits<float>::max(),
1068  local_z_max = -std::numeric_limits<float>::max();
1069 
1070  double maxDistForCorrespondenceSquared;
1071 
1072  // Prepare output: no correspondences initially:
1073  correspondences.clear();
1074  correspondences.reserve(nLocalPoints);
1075 
1076  TMatchingPairList _correspondences;
1077  _correspondences.reserve(nLocalPoints);
1078 
1079  // Empty maps? Nothing to do
1080  if (!nGlobalPoints || !nLocalPoints) return;
1081 
1082  // Try to do matching only if the bounding boxes have some overlap:
1083  // Transform all local points:
1084  vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
1085  z_locals(nLocalPoints);
1086 
1087  for (unsigned int localIdx = params.offset_other_map_points;
1088  localIdx < nLocalPoints;
1089  localIdx += params.decimation_other_map_points)
1090  {
1091  float x_local, y_local, z_local;
1092  otherMapPose.composePoint(
1093  otherMap->m_x[localIdx], otherMap->m_y[localIdx],
1094  otherMap->m_z[localIdx], x_local, y_local, z_local);
1095 
1096  x_locals[localIdx] = x_local;
1097  y_locals[localIdx] = y_local;
1098  z_locals[localIdx] = z_local;
1099 
1100  // Find the bounding box:
1101  local_x_min = min(local_x_min, x_local);
1102  local_x_max = max(local_x_max, x_local);
1103  local_y_min = min(local_y_min, y_local);
1104  local_y_max = max(local_y_max, y_local);
1105  local_z_min = min(local_z_min, z_local);
1106  local_z_max = max(local_z_max, z_local);
1107  }
1108 
1109  // Find the bounding box:
1110  float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1111  global_z_max;
1112  this->boundingBox(
1113  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1114  global_z_max);
1115 
1116  // Solo hacer matching si existe alguna posibilidad de que
1117  // los dos mapas se toquen:
1118  if (local_x_min > global_x_max || local_x_max < global_x_min ||
1119  local_y_min > global_y_max || local_y_max < global_y_min)
1120  return; // No need to compute: matching is ZERO.
1121 
1122  // Loop for each point in local map:
1123  // --------------------------------------------------
1124  for (unsigned int localIdx = params.offset_other_map_points;
1125  localIdx < nLocalPoints;
1126  localIdx += params.decimation_other_map_points)
1127  {
1128  // For speed-up:
1129  const float x_local = x_locals[localIdx];
1130  const float y_local = y_locals[localIdx];
1131  const float z_local = z_locals[localIdx];
1132 
1133  {
1134  // KD-TREE implementation
1135  // Use a KD-tree to look for the nearnest neighbor of:
1136  // (x_local, y_local, z_local)
1137  // In "this" (global/reference) points map.
1138 
1139  float tentativ_err_sq;
1140  const unsigned int tentativ_this_idx = kdTreeClosestPoint3D(
1141  x_local, y_local, z_local, // Look closest to this guy
1142  tentativ_err_sq // save here the min. distance squared
1143  );
1144 
1145  // Compute max. allowed distance:
1146  maxDistForCorrespondenceSquared = square(
1147  params.maxAngularDistForCorrespondence *
1148  params.angularDistPivotPoint.distanceTo(
1149  TPoint3D(x_local, y_local, z_local)) +
1150  params.maxDistForCorrespondence);
1151 
1152  // Distance below the threshold??
1153  if (tentativ_err_sq < maxDistForCorrespondenceSquared)
1154  {
1155  // Save all the correspondences:
1156  _correspondences.resize(_correspondences.size() + 1);
1157 
1158  TMatchingPair& p = _correspondences.back();
1159 
1160  p.this_idx = tentativ_this_idx;
1161  p.this_x = m_x[tentativ_this_idx];
1162  p.this_y = m_y[tentativ_this_idx];
1163  p.this_z = m_z[tentativ_this_idx];
1164 
1165  p.other_idx = localIdx;
1166  p.other_x = otherMap->m_x[localIdx];
1167  p.other_y = otherMap->m_y[localIdx];
1168  p.other_z = otherMap->m_z[localIdx];
1169 
1170  p.errorSquareAfterTransformation = tentativ_err_sq;
1171 
1172  // At least one:
1173  nOtherMapPointsWithCorrespondence++;
1174 
1175  // Accumulate the MSE:
1176  _sumSqrDist += p.errorSquareAfterTransformation;
1177  _sumSqrCount++;
1178  }
1179 
1180  } // End of test_match
1181  } // For each local point
1182 
1183  // Additional consistency filter: "onlyKeepTheClosest" up to now
1184  // led to just one correspondence for each "local map" point, but
1185  // many of them may have as corresponding pair the same "global point"!!
1186  // -------------------------------------------------------------------------
1187  if (params.onlyUniqueRobust)
1188  {
1189  ASSERTMSG_(
1190  params.onlyKeepTheClosest,
1191  "ERROR: onlyKeepTheClosest must be also set to true when "
1192  "onlyUniqueRobust=true.");
1193  _correspondences.filterUniqueRobustPairs(
1194  nGlobalPoints, correspondences);
1195  }
1196  else
1197  {
1198  correspondences.swap(_correspondences);
1199  }
1200 
1201  // If requested, copy sum of squared distances to output pointer:
1202  // -------------------------------------------------------------------
1203  extraResults.sumSqrDist =
1204  (_sumSqrCount) ? _sumSqrDist / static_cast<double>(_sumSqrCount) : 0;
1205  extraResults.correspondencesRatio = params.decimation_other_map_points *
1206  nOtherMapPointsWithCorrespondence /
1207  static_cast<float>(nLocalPoints);
1208 
1209  MRPT_END
1210 }
1211 
1212 /*---------------------------------------------------------------
1213  extractCylinder
1214 ---------------------------------------------------------------*/
1216  const TPoint2D& center, const double radius, const double zmin,
1217  const double zmax, CPointsMap* outMap)
1218 {
1219  outMap->clear();
1220  for (size_t k = 0; k < m_x.size(); k++)
1221  {
1222  if ((m_z[k] <= zmax && m_z[k] >= zmin) &&
1223  (sqrt(square(center.x - m_x[k]) + square(center.y - m_y[k])) <
1224  radius))
1225  outMap->insertPoint(m_x[k], m_y[k], m_z[k]);
1226  }
1227 }
1228 
1229 /*---------------------------------------------------------------
1230  extractPoints
1231 ---------------------------------------------------------------*/
1233  const TPoint3D& corner1, const TPoint3D& corner2, CPointsMap* outMap,
1234  double R, double G, double B)
1235 {
1236  outMap->clear();
1237  double minX, maxX, minY, maxY, minZ, maxZ;
1238  minX = min(corner1.x, corner2.x);
1239  maxX = max(corner1.x, corner2.x);
1240  minY = min(corner1.y, corner2.y);
1241  maxY = max(corner1.y, corner2.y);
1242  minZ = min(corner1.z, corner2.z);
1243  maxZ = max(corner1.z, corner2.z);
1244  for (size_t k = 0; k < m_x.size(); k++)
1245  {
1246  if ((m_x[k] >= minX && m_x[k] <= maxX) &&
1247  (m_y[k] >= minY && m_y[k] <= maxY) &&
1248  (m_z[k] >= minZ && m_z[k] <= maxZ))
1249  outMap->insertPointRGB(m_x[k], m_y[k], m_z[k], R, G, B);
1250  }
1251 }
1252 
1253 /*---------------------------------------------------------------
1254  compute3DDistanceToMesh
1255 ---------------------------------------------------------------*/
1257  const mrpt::maps::CMetricMap* otherMap2, const CPose3D& otherMapPose,
1258  float maxDistForCorrespondence, TMatchingPairList& correspondences,
1259  float& correspondencesRatio)
1260 {
1261  MRPT_START
1262  MRPT_UNUSED_PARAM(otherMapPose);
1263 
1264  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
1265 
1266  const size_t nLocalPoints = otherMap->size();
1267  const size_t nGlobalPoints = this->size();
1268  size_t nOtherMapPointsWithCorrespondence =
1269  0; // Number of points with one corrs. at least
1270 
1271  // Prepare output: no correspondences initially:
1272  correspondences.clear();
1273  correspondences.reserve(nLocalPoints);
1274  correspondencesRatio = 0;
1275 
1276  // aux correspondence vector
1277  TMatchingPairList _correspondences;
1278  _correspondences.reserve(nLocalPoints);
1279 
1280  // Hay mapa global?
1281  if (!nGlobalPoints) return; // No
1282 
1283  // Hay mapa local?
1284  if (!nLocalPoints) return; // No
1285 
1286  // we'll assume by now both reference systems are the same
1287  float local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1288  local_z_max;
1289  otherMap->boundingBox(
1290  local_x_min, local_x_max, local_y_min, local_y_max, local_z_min,
1291  local_z_max);
1292 
1293  // Find the bounding box:
1294  float global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1295  global_z_max;
1296  this->boundingBox(
1297  global_x_min, global_x_max, global_y_min, global_y_max, global_z_min,
1298  global_z_max);
1299 
1300  // Solo hacer matching si existe alguna posibilidad de que
1301  // los dos mapas se toquen:
1302  if (local_x_min > global_x_max || local_x_max < global_x_min ||
1303  local_y_min > global_y_max || local_y_max < global_y_min)
1304  return; // No hace falta hacer matching,
1305  // porque es de CERO.
1306 
1307  std::vector<std::vector<size_t>> vIdx;
1308 
1309  // Loop for each point in local map:
1310  // --------------------------------------------------
1311  std::vector<float> outX, outY, outZ, tentativeErrSq;
1312  std::vector<size_t> outIdx;
1313  for (unsigned int localIdx = 0; localIdx < nLocalPoints; ++localIdx)
1314  {
1315  // For speed-up:
1316  const float x_local = otherMap->m_x[localIdx];
1317  const float y_local = otherMap->m_y[localIdx];
1318  const float z_local = otherMap->m_z[localIdx];
1319 
1320  {
1321  // KD-TREE implementation
1322  // Use a KD-tree to look for the nearnest neighbor of:
1323  // (x_local, y_local, z_local)
1324  // In "this" (global/reference) points map.
1326  x_local, y_local, z_local, // Look closest to this guy
1327  3, // get the three closest points
1328  outX, outY, outZ, // output vectors
1329  outIdx, // output indexes
1330  tentativeErrSq // save here the min. distance squared
1331  );
1332 
1333  // get the centroid
1334  const float mX = (outX[0] + outX[1] + outX[2]) / 3.0;
1335  const float mY = (outY[0] + outY[1] + outY[2]) / 3.0;
1336  const float mZ = (outZ[0] + outZ[1] + outZ[2]) / 3.0;
1337 
1338  const float distanceForThisPoint = fabs(mrpt::math::distance(
1339  TPoint3D(x_local, y_local, z_local), TPoint3D(mX, mY, mZ)));
1340 
1341  // Distance below the threshold??
1342  if (distanceForThisPoint < maxDistForCorrespondence)
1343  {
1344  // Save all the correspondences:
1345  _correspondences.resize(_correspondences.size() + 1);
1346 
1347  TMatchingPair& p = _correspondences.back();
1348 
1349  p.this_idx = nOtherMapPointsWithCorrespondence++; // insert a
1350  // consecutive index
1351  // here
1352  p.this_x = mX;
1353  p.this_y = mY;
1354  p.this_z = mZ;
1355 
1356  p.other_idx = localIdx;
1357  p.other_x = otherMap->m_x[localIdx];
1358  p.other_y = otherMap->m_y[localIdx];
1359  p.other_z = otherMap->m_z[localIdx];
1360 
1361  p.errorSquareAfterTransformation = distanceForThisPoint;
1362 
1363  // save the indexes
1364  std::sort(outIdx.begin(), outIdx.end());
1365  vIdx.push_back(outIdx);
1366  }
1367  } // End of test_match
1368  } // For each local point
1369 
1370  // Additional consistency filter: "onlyKeepTheClosest" up to now
1371  // led to just one correspondence for each "local map" point, but
1372  // many of them may have as corresponding pair the same "global point"!!
1373  // -------------------------------------------------------------------------
1374  std::map<size_t, std::map<size_t, std::map<size_t, pair<size_t, float>>>>
1375  best; // 3D associative map
1376  TMatchingPairList::iterator it;
1377  for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1378  {
1379  const size_t i0 = vIdx[it->this_idx][0];
1380  const size_t i1 = vIdx[it->this_idx][1];
1381  const size_t i2 = vIdx[it->this_idx][2];
1382 
1383  if (best.find(i0) != best.end() &&
1384  best[i0].find(i1) != best[i0].end() &&
1385  best[i0][i1].find(i2) !=
1386  best[i0][i1]
1387  .end()) // if there is a match, check if it is better
1388  {
1389  if (best[i0][i1][i2].second > it->errorSquareAfterTransformation)
1390  {
1391  best[i0][i1][i2].first = it->this_idx;
1392  best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1393  }
1394  }
1395  else // if there is no match
1396  {
1397  best[i0][i1][i2].first = it->this_idx;
1398  best[i0][i1][i2].second = it->errorSquareAfterTransformation;
1399  }
1400  } // end it correspondences
1401 
1402  for (it = _correspondences.begin(); it != _correspondences.end(); ++it)
1403  {
1404  const size_t i0 = vIdx[it->this_idx][0];
1405  const size_t i1 = vIdx[it->this_idx][1];
1406  const size_t i2 = vIdx[it->this_idx][2];
1407 
1408  if (best[i0][i1][i2].first == it->this_idx)
1409  correspondences.push_back(*it);
1410  }
1411 
1412  // The ratio of points in the other map with corrs:
1413  correspondencesRatio =
1414  nOtherMapPointsWithCorrespondence / static_cast<float>(nLocalPoints);
1415 
1416  MRPT_END
1417 }
1418 
1420  const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys,
1421  const float* zs, const std::size_t num_pts)
1422 {
1424 
1425  float closest_x, closest_y, closest_z;
1426  float closest_err;
1427  const float max_sqr_err = square(likelihoodOptions.max_corr_distance);
1428  double sumSqrDist = 0;
1429 
1430  std::size_t nPtsForAverage = 0;
1431  for (std::size_t i = 0; i < num_pts;
1432  i += likelihoodOptions.decimation, nPtsForAverage++)
1433  {
1434  // Transform the point from the scan reference to its global 3D
1435  // position:
1436  float xg, yg, zg;
1437  pc_in_map.composePoint(xs[i], ys[i], zs[i], xg, yg, zg);
1438 
1440  xg, yg, zg, // Look for the closest to this guy
1441  closest_x, closest_y,
1442  closest_z, // save here the closest match
1443  closest_err // save here the min. distance squared
1444  );
1445 
1446  // Put a limit:
1447  mrpt::keep_min(closest_err, max_sqr_err);
1448 
1449  sumSqrDist += static_cast<double>(closest_err);
1450  }
1451  if (nPtsForAverage) sumSqrDist /= nPtsForAverage;
1452 
1453  // Log-likelihood:
1454  return -sumSqrDist / likelihoodOptions.sigma_dist;
1455 
1456  MRPT_TRY_END
1457 }
1458 
1460  const CObservation& obs, const CPose3D& takenFrom)
1461 {
1462  // This function depends on the observation type:
1463  // -----------------------------------------------------
1465  {
1466  // Observation is a laser range scan:
1467  // -------------------------------------------
1468  const auto& o = static_cast<const CObservation2DRangeScan&>(obs);
1469 
1470  // Build (if not done before) the points map representation of this
1471  // observation:
1472  const auto* scanPoints = o.buildAuxPointsMap<CPointsMap>();
1473 
1474  const size_t N = scanPoints->m_x.size();
1475  if (!N || !this->size()) return -100;
1476 
1477  const float* xs = &scanPoints->m_x[0];
1478  const float* ys = &scanPoints->m_y[0];
1479  const float* zs = &scanPoints->m_z[0];
1480 
1481  if (takenFrom.isHorizontal())
1482  {
1483  double sumSqrDist = 0;
1484  float closest_x, closest_y;
1485  float closest_err;
1486  const float max_sqr_err =
1488 
1489  // optimized 2D version ---------------------------
1490  TPose2D takenFrom2D = CPose2D(takenFrom).asTPose();
1491 
1492  const double ccos = cos(takenFrom2D.phi);
1493  const double csin = sin(takenFrom2D.phi);
1494  int nPtsForAverage = 0;
1495 
1496  for (size_t i = 0; i < N;
1497  i += likelihoodOptions.decimation, nPtsForAverage++)
1498  {
1499  // Transform the point from the scan reference to its global
1500  // 3D position:
1501  const float xg = takenFrom2D.x + ccos * xs[i] - csin * ys[i];
1502  const float yg = takenFrom2D.y + csin * xs[i] + ccos * ys[i];
1503 
1505  xg, yg, // Look for the closest to this guy
1506  closest_x, closest_y, // save here the closest match
1507  closest_err // save here the min. distance squared
1508  );
1509 
1510  // Put a limit:
1511  mrpt::keep_min(closest_err, max_sqr_err);
1512 
1513  sumSqrDist += static_cast<double>(closest_err);
1514  }
1515  sumSqrDist /= nPtsForAverage;
1516  // Log-likelihood:
1517  return -sumSqrDist / likelihoodOptions.sigma_dist;
1518  }
1519  else
1520  {
1521  // Generic 3D version ---------------------------
1523  takenFrom, xs, ys, zs, N);
1524  }
1525  }
1526  else if (IS_CLASS(obs, CObservationVelodyneScan))
1527  {
1528  const auto& o = dynamic_cast<const CObservationVelodyneScan&>(obs);
1529 
1530  // Automatically generate pointcloud if needed:
1531  if (!o.point_cloud.size())
1532  const_cast<CObservationVelodyneScan&>(o).generatePointCloud();
1533 
1534  const size_t N = o.point_cloud.size();
1535  if (!N || !this->size()) return -100;
1536 
1537  const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1538 
1539  const float* xs = &o.point_cloud.x[0];
1540  const float* ys = &o.point_cloud.y[0];
1541  const float* zs = &o.point_cloud.z[0];
1542 
1544  sensorAbsPose, xs, ys, zs, N);
1545  }
1546  else if (IS_CLASS(obs, CObservationPointCloud))
1547  {
1548  const auto& o = dynamic_cast<const CObservationPointCloud&>(obs);
1549 
1550  const size_t N = o.pointcloud->size();
1551  if (!N || !this->size()) return -100;
1552 
1553  const CPose3D sensorAbsPose = takenFrom + o.sensorPose;
1554 
1555  auto xs = o.pointcloud->getPointsBufferRef_x();
1556  auto ys = o.pointcloud->getPointsBufferRef_y();
1557  auto zs = o.pointcloud->getPointsBufferRef_z();
1558 
1560  sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1561  }
1562 
1563  return .0;
1564 }
1565 
1566 namespace mrpt::obs
1567 {
1568 // Tricky way to call to a library that depends on us, a sort of "run-time"
1569 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
1570 // "mrpt-obs", set by "mrpt-maps" at its startup.
1571 using scan2pts_functor = void (*)(
1573  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
1574 
1576 } // namespace mrpt::obs
1577 
1580  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps)
1581 {
1582  // Create on first call:
1583  if (out_map) return; // Already done!
1584 
1585  out_map = std::make_shared<CSimplePointsMap>();
1586 
1587  if (insertOps)
1588  static_cast<CSimplePointsMap*>(out_map.get())->insertionOptions =
1589  *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1590 
1591  out_map->insertObservation(obs, nullptr);
1592 }
1593 
1595 {
1597  {
1600  }
1601 };
1602 
1603 // used just to set "ptr_internal_build_points_map_from_scan2D"
1605 
1606 // ================================ PLY files import & export virtual
1607 // methods
1608 // ================================
1609 
1610 /** In a base class, will be called after PLY_import_set_vertex_count() once
1611  * for each loaded point. \param pt_color Will be nullptr if the loaded file
1612  * does not provide color info.
1613  */
1615  const size_t idx, const mrpt::math::TPoint3Df& pt,
1616  const mrpt::img::TColorf* pt_color)
1617 {
1618  MRPT_UNUSED_PARAM(pt_color);
1619  this->setPoint(idx, pt.x, pt.y, pt.z);
1620 }
1621 
1622 /** In a base class, return the number of vertices */
1623 size_t CPointsMap::PLY_export_get_vertex_count() const { return this->size(); }
1624 /** In a base class, will be called after PLY_export_get_vertex_count() once
1625  * for each exported point. \param pt_color Will be nullptr if the loaded
1626  * file does not provide color info.
1627  */
1629  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
1630  mrpt::img::TColorf& pt_color) const
1631 {
1632  MRPT_UNUSED_PARAM(pt_color);
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 
1735  obj.m_largestDistanceFromOriginIsUpdated;
1736  m_largestDistanceFromOrigin = obj.m_largestDistanceFromOrigin;
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:717
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:663
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:638
double x
X,Y coordinates.
Definition: TPoint2D.h:23
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:194
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
GLenum GLint GLuint mask
Definition: glext.h:4062
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:253
mrpt::img::TColormap colormap
Colormap for points (index is "z" coordinates)
Definition: CPointsMap.h:330
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:481
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:755
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
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)
GLuint GLenum outX
Definition: glext.h:7391
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
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 ...
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:615
mrpt::img::TColorf color
Color of points.
Definition: CPointsMap.h:328
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 ...
GLint * first
Definition: glext.h:3833
const double G
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CPointsMap.cpp:768
void base_copyFrom(const CPointsMap &obj)
Helper method for ::copyFrom()
GLenum GLsizei n
Definition: glext.h:5136
void setAllVertices(const std::vector< double > &x, const std::vector< double > &y)
Set all vertices at once.
Definition: CPolygon.cpp:121
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
void loadFromVelodyneScan(const mrpt::obs::CObservationVelodyneScan &scan, const mrpt::poses::CPose3D *robotPose=nullptr)
Like loadFromRangeScan() for Velodyne 3D scans.
A wrapper of a TPolygon2D class, implementing CSerializable.
Definition: CPolygon.h:19
GLuint GLenum GLenum outY
Definition: glext.h:7391
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:644
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:275
virtual unsigned int getPointWeight(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:535
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:730
void generatePointCloud(const TGeneratePointCloudParameters &params=TGeneratePointCloudParameters())
Generates the point cloud into the point cloud data fields in CObservationVelodyneScan::point_cloud w...
GLuint src
Definition: glext.h:7397
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...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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:708
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:48
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
GLuint color
Definition: glext.h:8459
#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
GLuint GLenum GLenum GLenum outZ
Definition: glext.h:7391
#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...
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
GLuint dst
Definition: glext.h:7249
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
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
T square(const T x)
Inline function for the square of a number.
double sigma_dist
Sigma squared (variance, in meters) of the exponential used to model the likelihood (default= 0...
Definition: CPointsMap.h:300
GLuint index
Definition: glext.h:4068
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:576
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:1136
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - used in derived classes&#39; serialization.
Definition: CPointsMap.cpp:671
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:602
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
GLubyte GLubyte b
Definition: glext.h:6372
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 ...
virtual void setPointWeight(size_t index, unsigned long w)
Sets the point weight, which is ignored in all classes but those which actually store that field (Not...
Definition: CPointsMap.h:527
#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:818
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:747
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Definition: CPointsMap.h:1131
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
virtual void insertPointRGB(float x, float y, float z, float R, float G, float B)
overload (RGB data is ignored in classes without color information)
Definition: CPointsMap.h:655
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
GLclampd zmax
Definition: glext.h:8064
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:916
#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:410
#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:1123
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:689
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:58
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:1114
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:238
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:249
#define MRPT_TODO(x)
Definition: common.h:129
mrpt::aligned_std_vector< float > m_y
Definition: CPointsMap.h:1123
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
TLikelihoodOptions likelihoodOptions
Definition: CPointsMap.h:309
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:218
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:493
TRenderOptions renderOptions
Definition: CPointsMap.h:332
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:797
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
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
#define LOADABLEOPTS_DUMP_VAR_DEG(variableName)
Macro for dumping a variable to a stream, transforming the argument from radians to degrees...
GLuint in
Definition: glext.h:7391
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:257
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:277
virtual void setPointRGB(size_t index, float x, float y, float z, float R, float G, float B)
overload (RGB data is ignored in classes without color information)
Definition: CPointsMap.h:515
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:614
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.
GLenum GLint GLint y
Definition: glext.h:3542
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:273
double max_corr_distance
Maximum distance in meters to consider for the numerator divided by "sigma_dist", so that each point ...
Definition: CPointsMap.h:304
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...
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
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:296
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.
GLsizeiptr size
Definition: glext.h:3934
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.
GLenum GLint x
Definition: glext.h:3542
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:873
uint32_t decimation
Speed up the likelihood computation by considering only one out of N rays (default=10) ...
Definition: CPointsMap.h:307
Lightweight 3D point.
Definition: TPoint3D.h:90
Parameters for the determination of matchings between point clouds, etc.
mrpt::aligned_std_vector< float > m_x
The point coordinates.
Definition: CPointsMap.h:1123
Lightweight 2D point.
Definition: TPoint2D.h:31
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
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:245
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
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:797
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:1887
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:234
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream - for usage in derived classes&#39; serialization.
Definition: CPointsMap.cpp:646
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 1.9.9 Git: d655b8ca0 Wed Dec 4 23:23:44 2019 +0100 at miƩ dic 4 23:30:15 CET 2019