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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019