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  }
1533  {
1534  const auto* o = dynamic_cast<const CObservationVelodyneScan*>(obs);
1535  ASSERT_(o != nullptr);
1536 
1537  // Automatically generate pointcloud if needed:
1538  if (!o->point_cloud.size())
1539  const_cast<CObservationVelodyneScan*>(o)->generatePointCloud();
1540 
1541  const size_t N = o->point_cloud.size();
1542  if (!N || !this->size()) return -100;
1543 
1544  const CPose3D sensorAbsPose = takenFrom + o->sensorPose;
1545 
1546  const float* xs = &o->point_cloud.x[0];
1547  const float* ys = &o->point_cloud.y[0];
1548  const float* zs = &o->point_cloud.z[0];
1549 
1551  sensorAbsPose, xs, ys, zs, N);
1552  }
1553  else if (obs->GetRuntimeClass() == CLASS_ID(CObservationPointCloud))
1554  {
1555  const auto* o = dynamic_cast<const CObservationPointCloud*>(obs);
1556  ASSERT_(o != nullptr);
1557 
1558  const size_t N = o->pointcloud->size();
1559  if (!N || !this->size()) return -100;
1560 
1561  const CPose3D sensorAbsPose = takenFrom + o->sensorPose;
1562 
1563  auto xs = o->pointcloud->getPointsBufferRef_x();
1564  auto ys = o->pointcloud->getPointsBufferRef_y();
1565  auto zs = o->pointcloud->getPointsBufferRef_z();
1566 
1568  sensorAbsPose, &xs[0], &ys[0], &zs[0], N);
1569  }
1570 
1571  return .0;
1572 }
1573 
1574 namespace mrpt::obs
1575 {
1576 // Tricky way to call to a library that depends on us, a sort of "run-time"
1577 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
1578 // "mrpt-obs", set by "mrpt-maps" at its startup.
1579 using scan2pts_functor = void (*)(
1581  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
1582 
1584 } // namespace mrpt::obs
1585 
1588  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps)
1589 {
1590  // Create on first call:
1591  if (out_map) return; // Already done!
1592 
1593  out_map = std::make_shared<CSimplePointsMap>();
1594 
1595  if (insertOps)
1596  static_cast<CSimplePointsMap*>(out_map.get())->insertionOptions =
1597  *static_cast<const CPointsMap::TInsertionOptions*>(insertOps);
1598 
1599  out_map->insertObservation(&obs, nullptr);
1600 }
1601 
1603 {
1605  {
1608  }
1609 };
1610 
1611 TAuxLoadFunctor dummy_loader; // used just to set
1612 // "ptr_internal_build_points_map_from_scan2D"
1613 
1614 // ================================ PLY files import & export virtual
1615 // methods
1616 // ================================
1617 
1618 /** In a base class, will be called after PLY_import_set_vertex_count() once
1619  * for each loaded point. \param pt_color Will be nullptr if the loaded file
1620  * does not provide color info.
1621  */
1623  const size_t idx, const mrpt::math::TPoint3Df& pt,
1624  const mrpt::img::TColorf* pt_color)
1625 {
1626  MRPT_UNUSED_PARAM(pt_color);
1627  this->setPoint(idx, pt.x, pt.y, pt.z);
1628 }
1629 
1630 /** In a base class, return the number of vertices */
1631 size_t CPointsMap::PLY_export_get_vertex_count() const { return this->size(); }
1632 /** In a base class, will be called after PLY_export_get_vertex_count() once
1633  * for each exported point. \param pt_color Will be nullptr if the loaded
1634  * file does not provide color info.
1635  */
1637  const size_t idx, mrpt::math::TPoint3Df& pt, bool& pt_has_color,
1638  mrpt::img::TColorf& pt_color) const
1639 {
1640  MRPT_UNUSED_PARAM(pt_color);
1641  pt_has_color = false;
1642 
1643  pt.x = m_x[idx];
1644  pt.y = m_y[idx];
1645  pt.z = m_z[idx];
1646 }
1647 
1648 // Generic implementation (a more optimized one should exist in derived
1649 // classes):
1650 void CPointsMap::addFrom(const CPointsMap& anotherMap)
1651 {
1652  const size_t nThis = this->size();
1653  const size_t nOther = anotherMap.size();
1654 
1655  const size_t nTot = nThis + nOther;
1656 
1657  this->resize(nTot);
1658 
1659  for (size_t i = 0, j = nThis; i < nOther; i++, j++)
1660  {
1661  m_x[j] = anotherMap.m_x[i];
1662  m_y[j] = anotherMap.m_y[i];
1663  m_z[j] = anotherMap.m_z[i];
1664  }
1665 
1666  // Also copy other data fields (color, ...)
1667  addFrom_classSpecific(anotherMap, nThis);
1668 
1669  mark_as_modified();
1670 }
1671 
1672 /** Save the point cloud as a PCL PCD file, in either ASCII or binary format
1673  * \return false on any error */
1675  const std::string& filename, bool save_as_binary) const
1676 {
1677 #if MRPT_HAS_PCL
1678  pcl::PointCloud<pcl::PointXYZ> cloud;
1679  this->getPCLPointCloud(cloud);
1680 
1681  return 0 == pcl::io::savePCDFile(filename, cloud, save_as_binary);
1682 
1683 #else
1684  MRPT_UNUSED_PARAM(filename);
1685  MRPT_UNUSED_PARAM(save_as_binary);
1686  THROW_EXCEPTION("Operation not available: MRPT was built without PCL");
1687 #endif
1688 }
1689 
1690 /** Load the point cloud from a PCL PCD file (requires MRPT built against
1691  * PCL) \return false on any error */
1693 {
1694 #if MRPT_HAS_PCL
1695  pcl::PointCloud<pcl::PointXYZ> cloud;
1696  if (0 != pcl::io::loadPCDFile(filename, cloud)) return false;
1697 
1698  this->getPCLPointCloud(cloud);
1699 
1700  return true;
1701 #else
1702  MRPT_UNUSED_PARAM(filename);
1703  THROW_EXCEPTION("Operation not available: MRPT was built without PCL");
1704 #endif
1705 }
1706 
1707 /*---------------------------------------------------------------
1708  applyDeletionMask
1709  ---------------------------------------------------------------*/
1710 void CPointsMap::applyDeletionMask(const std::vector<bool>& mask)
1711 {
1712  ASSERT_EQUAL_(size(), mask.size());
1713  // Remove marked points:
1714  const size_t n = mask.size();
1715  vector<float> Pt;
1716  size_t i, j;
1717  for (i = 0, j = 0; i < n; i++)
1718  {
1719  if (!mask[i])
1720  {
1721  // Pt[j] <---- Pt[i]
1722  this->getPointAllFieldsFast(i, Pt);
1723  this->setPointAllFieldsFast(j++, Pt);
1724  }
1725  }
1726 
1727  // Set new correct size:
1728  this->resize(j);
1729 
1730  mark_as_modified();
1731 }
1732 
1733 /*---------------------------------------------------------------
1734  insertAnotherMap
1735  ---------------------------------------------------------------*/
1737  const CPointsMap* otherMap, const CPose3D& otherPose)
1738 {
1739  const size_t N_this = size();
1740  const size_t N_other = otherMap->size();
1741 
1742  // Set the new size:
1743  this->resize(N_this + N_other);
1744 
1746  size_t src, dst;
1747  for (src = 0, dst = N_this; src < N_other; src++, dst++)
1748  {
1749  // Load the next point:
1750  otherMap->getPointFast(src, pt.x, pt.y, pt.z);
1751 
1752  // Translation:
1753  double gx, gy, gz;
1754  otherPose.composePoint(pt.x, pt.y, pt.z, gx, gy, gz);
1755 
1756  // Add to this map:
1757  this->setPointFast(dst, gx, gy, gz);
1758  }
1759 
1760  // Also copy other data fields (color, ...)
1761  addFrom_classSpecific(*otherMap, N_this);
1762 
1763  mark_as_modified();
1764 }
1765 
1766 /** Helper method for ::copyFrom() */
1768 {
1769  MRPT_START
1770 
1771  if (this == &obj) return;
1772 
1773  m_x = obj.m_x;
1774  m_y = obj.m_y;
1775  m_z = obj.m_z;
1776 
1778  obj.m_largestDistanceFromOriginIsUpdated;
1779  m_largestDistanceFromOrigin = obj.m_largestDistanceFromOrigin;
1780 
1781  // Fill missing fields (R,G,B,min_dist) with default values.
1782  this->resize(m_x.size());
1783 
1785 
1786  MRPT_END
1787 }
1788 
1789 /*---------------------------------------------------------------
1790  internal_insertObservation
1791 
1792  Insert the observation information into this map.
1793  ---------------------------------------------------------------*/
1795  const CObservation* obs, const CPose3D* robotPose)
1796 {
1797  MRPT_START
1798 
1799  CPose2D robotPose2D;
1800  CPose3D robotPose3D;
1801 
1802  if (robotPose)
1803  {
1804  robotPose2D = CPose2D(*robotPose);
1805  robotPose3D = (*robotPose);
1806  }
1807  else
1808  {
1809  // Default values are (0,0,0)
1810  }
1811 
1813  {
1814  /********************************************************************
1815  OBSERVATION TYPE: CObservation2DRangeScan
1816  ********************************************************************/
1817  mark_as_modified();
1818 
1819  const auto* o = static_cast<const CObservation2DRangeScan*>(obs);
1820  // Insert only HORIZONTAL scans??
1821  bool reallyInsertIt;
1822 
1824  reallyInsertIt =
1826  else
1827  reallyInsertIt = true;
1828 
1829  if (reallyInsertIt)
1830  {
1831  std::vector<bool> checkForDeletion;
1832 
1833  // 1) Fuse into the points map or add directly?
1834  // ----------------------------------------------
1836  {
1837  CSimplePointsMap auxMap;
1838  // Fuse:
1841 
1842  auxMap.loadFromRangeScan(
1843  *o, // The laser range scan observation
1844  &robotPose3D // The robot pose
1845  );
1846 
1847  fuseWith(
1848  &auxMap, // Fuse with this map
1850  &checkForDeletion // Set to "false" if a point in "map"
1851  // has
1852  // been fused.
1853  );
1854 
1856  {
1857  // 2) Delete points in newly added free
1858  // region, thus dynamic areas:
1859  // --------------------------------------
1860  // Load scan as a polygon:
1861  CPolygon pol;
1862  const float *xs, *ys, *zs;
1863  size_t n;
1864  auxMap.getPointsBuffer(n, xs, ys, zs);
1865  pol.setAllVertices(n, xs, ys);
1866 
1867  // Check for deletion of points in "map"
1868  n = size();
1869  for (size_t i = 0; i < n; i++)
1870  {
1871  if (checkForDeletion[i]) // Default to true, unless
1872  // a
1873  // fused point, which must be
1874  // kept.
1875  {
1876  float x, y;
1877  getPoint(i, x, y);
1878  if (!pol.PointIntoPolygon(x, y))
1879  checkForDeletion[i] =
1880  false; // Out of polygon, don't delete
1881  }
1882  }
1883 
1884  // Create a new points list just with non-deleted
1885  // points.
1886  // ----------------------------------------------------------
1887  applyDeletionMask(checkForDeletion);
1888  }
1889  }
1890  else
1891  {
1892  // Don't fuse: Simply add
1895  *o, // The laser range scan observation
1896  &robotPose3D // The robot pose
1897  );
1898  }
1899 
1900  return true;
1901  }
1902  // A planar map and a non-horizontal scan.
1903  else
1904  return false;
1905  }
1906  else if (IS_CLASS(obs, CObservation3DRangeScan))
1907  {
1908  /********************************************************************
1909  OBSERVATION TYPE: CObservation3DRangeScan
1910  ********************************************************************/
1911  mark_as_modified();
1912 
1913  const auto* o = static_cast<const CObservation3DRangeScan*>(obs);
1914  // Insert only HORIZONTAL scans??
1915  bool reallyInsertIt;
1916 
1918  reallyInsertIt =
1919  false; // Don't insert 3D range observation into planar map
1920  else
1921  reallyInsertIt = true;
1922 
1923  if (reallyInsertIt)
1924  {
1925  // 1) Fuse into the points map or add directly?
1926  // ----------------------------------------------
1928  {
1929  // Fuse:
1930  CSimplePointsMap auxMap;
1933 
1934  auxMap.loadFromRangeScan(
1935  *o, // The laser range scan observation
1936  &robotPose3D // The robot pose
1937  );
1938 
1939  fuseWith(
1940  &auxMap, // Fuse with this map
1942  nullptr // rather than &checkForDeletion which we don't
1943  // need
1944  // for 3D observations
1945  );
1946  }
1947  else
1948  {
1949  // Don't fuse: Simply add
1952  *o, // The laser range scan observation
1953  &robotPose3D // The robot pose
1954  );
1955  }
1956 
1957  return true;
1958  }
1959  // A planar map and a non-horizontal scan.
1960  else
1961  return false;
1962  }
1963  else if (IS_CLASS(obs, CObservationRange))
1964  {
1965  /********************************************************************
1966  OBSERVATION TYPE: CObservationRange (IRs, Sonars, etc.)
1967  ********************************************************************/
1968  mark_as_modified();
1969 
1970  const auto* o = static_cast<const CObservationRange*>(obs);
1971 
1972  const double aper_2 = 0.5 * o->sensorConeApperture;
1973 
1974  this->reserve(
1975  this->size() + o->sensedData.size() * 30); // faster push_back's.
1976 
1977  for (auto it = o->begin(); it != o->end(); ++it)
1978  {
1979  const CPose3D sensorPose = robotPose3D + CPose3D(it->sensorPose);
1980  const double rang = it->sensedDistance;
1981 
1982  if (rang <= 0 || rang < o->minSensorDistance ||
1983  rang > o->maxSensorDistance)
1984  continue;
1985 
1986  // Insert a few points with a given maximum separation between
1987  // them:
1988  const double arc_len = o->sensorConeApperture * rang;
1989  const unsigned int nSteps = round(1 + arc_len / 0.05);
1990  const double Aa = o->sensorConeApperture / double(nSteps);
1991  TPoint3D loc, glob;
1992 
1993  for (double a1 = -aper_2; a1 < aper_2; a1 += Aa)
1994  {
1995  for (double a2 = -aper_2; a2 < aper_2; a2 += Aa)
1996  {
1997  loc.x = cos(a1) * cos(a2) * rang;
1998  loc.y = cos(a1) * sin(a2) * rang;
1999  loc.z = sin(a1) * rang;
2000  sensorPose.composePoint(loc, glob);
2001 
2002  this->insertPointFast(glob.x, glob.y, glob.z);
2003  }
2004  }
2005  }
2006  return true;
2007  }
2008  else if (IS_CLASS(obs, CObservationVelodyneScan))
2009  {
2010  /********************************************************************
2011  OBSERVATION TYPE: CObservationVelodyneScan
2012  ********************************************************************/
2013  mark_as_modified();
2014 
2015  const auto* o = static_cast<const CObservationVelodyneScan*>(obs);
2016 
2017  // Automatically generate pointcloud if needed:
2018  if (!o->point_cloud.size())
2019  const_cast<CObservationVelodyneScan*>(o)->generatePointCloud();
2020 
2022  {
2023  // Fuse:
2024  CSimplePointsMap auxMap;
2027  auxMap.loadFromVelodyneScan(*o, &robotPose3D);
2028  fuseWith(
2029  &auxMap, insertionOptions.minDistBetweenLaserPoints, nullptr /* rather than &checkForDeletion which we don't need for 3D observations */);
2030  }
2031  else
2032  {
2033  // Don't fuse: Simply add
2035  loadFromVelodyneScan(*o, &robotPose3D);
2036  }
2037  return true;
2038  }
2039  else if (IS_CLASS(obs, CObservationPointCloud))
2040  {
2041  mark_as_modified();
2042 
2043  const auto* o = static_cast<const CObservationPointCloud*>(obs);
2044  ASSERT_(o->pointcloud);
2045 
2047  {
2048  fuseWith(
2049  o->pointcloud.get(), insertionOptions.minDistBetweenLaserPoints, nullptr /* rather than &checkForDeletion which we don't need for 3D observations */);
2050  }
2051  else
2052  {
2053  // Don't fuse: Simply add
2055  this->insertAnotherMap(o->pointcloud.get(), o->sensorPose);
2056  }
2057  return true;
2058  }
2059  else
2060  {
2061  /********************************************************************
2062  OBSERVATION TYPE: Unknown
2063  ********************************************************************/
2064  return false;
2065  }
2066 
2067  MRPT_END
2068 }
2069 
2070 /*---------------------------------------------------------------
2071 Insert the contents of another map into this one, fusing the previous
2072 content with the new one. This means that points very close to existing ones
2073 will be "fused", rather than "added". This prevents the unbounded increase
2074 in size of these class of maps.
2075  ---------------------------------------------------------------*/
2077  CPointsMap* otherMap, float minDistForFuse,
2078  std::vector<bool>* notFusedPoints)
2079 {
2080  TMatchingPairList correspondences;
2081  TPoint3D a, b;
2082  const CPose2D nullPose(0, 0, 0);
2083 
2084  mark_as_modified();
2085 
2086  // const size_t nThis = this->size();
2087  const size_t nOther = otherMap->size();
2088 
2089  // Find correspondences between this map and the other one:
2090  // ------------------------------------------------------------
2092  TMatchingExtraResults extraResults;
2093 
2094  params.maxAngularDistForCorrespondence = 0;
2095  params.maxDistForCorrespondence = minDistForFuse;
2096 
2098  otherMap, // The other map
2099  nullPose, // The other map's pose
2100  correspondences, params, extraResults);
2101 
2102  // Initially, all set to "true" -> "not fused".
2103  if (notFusedPoints)
2104  {
2105  notFusedPoints->clear();
2106  notFusedPoints->reserve(m_x.size() + nOther);
2107  notFusedPoints->resize(m_x.size(), true);
2108  }
2109 
2110  // Speeds-up possible memory reallocations:
2111  reserve(m_x.size() + nOther);
2112 
2113  // Merge matched points from both maps:
2114  // AND add new points which have been not matched:
2115  // -------------------------------------------------
2116  for (size_t i = 0; i < nOther; i++)
2117  {
2118  otherMap->getPoint(i, a); // Get "local" point into "a"
2119  const unsigned long w_a = otherMap->getPointWeight(i);
2120 
2121  // Find closest correspondence of "a":
2122  int closestCorr = -1;
2123  float minDist = std::numeric_limits<float>::max();
2124  for (auto corrsIt = correspondences.begin();
2125  corrsIt != correspondences.end(); ++corrsIt)
2126  {
2127  if (corrsIt->other_idx == i)
2128  {
2129  float dist = square(corrsIt->other_x - corrsIt->this_x) +
2130  square(corrsIt->other_y - corrsIt->this_y) +
2131  square(corrsIt->other_z - corrsIt->this_z);
2132  if (dist < minDist)
2133  {
2134  minDist = dist;
2135  closestCorr = corrsIt->this_idx;
2136  }
2137  }
2138  } // End of for each correspondence...
2139 
2140  if (closestCorr != -1)
2141  { // Merge: FUSION
2142  getPoint(closestCorr, b);
2143  unsigned long w_b = getPointWeight(closestCorr);
2144 
2145  ASSERT_((w_a + w_b) > 0);
2146 
2147  const float F = 1.0f / (w_a + w_b);
2148 
2149  m_x[closestCorr] = F * (w_a * a.x + w_b * b.x);
2150  m_y[closestCorr] = F * (w_a * a.y + w_b * b.y);
2151  m_z[closestCorr] = F * (w_a * a.z + w_b * b.z);
2152 
2153  this->setPointWeight(closestCorr, w_a + w_b);
2154 
2155  // Append to fused points list
2156  if (notFusedPoints) (*notFusedPoints)[closestCorr] = false;
2157  }
2158  else
2159  { // New point: ADDITION
2160  this->insertPointFast(a.x, a.y, a.z);
2161  if (notFusedPoints) (*notFusedPoints).push_back(false);
2162  }
2163  }
2164 }
2165 
2168  const mrpt::poses::CPose3D* robotPose)
2169 {
2170  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.y.size());
2171  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.z.size());
2172  ASSERT_EQUAL_(scan.point_cloud.x.size(), scan.point_cloud.intensity.size());
2173 
2174  if (scan.point_cloud.x.empty()) return;
2175 
2176  this->mark_as_modified();
2177 
2178  // Insert vs. load and replace:
2180  resize(0); // Resize to 0 instead of clear() so the std::vector<>
2181  // memory is not actually deallocated and can be reused.
2182 
2183  // Alloc space:
2184  const size_t nOldPtsCount = this->size();
2185  const size_t nScanPts = scan.point_cloud.size();
2186  const size_t nNewPtsCount = nOldPtsCount + nScanPts;
2187  this->resize(nNewPtsCount);
2188 
2189  const float K = 1.0f / 255; // Intensity scale.
2190 
2191  // global 3D pose:
2192  CPose3D sensorGlobalPose;
2193  if (robotPose)
2194  sensorGlobalPose = *robotPose + scan.sensorPose;
2195  else
2196  sensorGlobalPose = scan.sensorPose;
2197 
2199  sensorGlobalPose.getHomogeneousMatrix(HM);
2200 
2201  const double m00 = HM(0, 0), m01 = HM(0, 1), m02 = HM(0, 2), m03 = HM(0, 3);
2202  const double m10 = HM(1, 0), m11 = HM(1, 1), m12 = HM(1, 2), m13 = HM(1, 3);
2203  const double m20 = HM(2, 0), m21 = HM(2, 1), m22 = HM(2, 2), m23 = HM(2, 3);
2204 
2205  // Copy points:
2206  for (size_t i = 0; i < nScanPts; i++)
2207  {
2208  const float inten = scan.point_cloud.intensity[i] * K;
2209  const double lx = scan.point_cloud.x[i];
2210  const double ly = scan.point_cloud.y[i];
2211  const double lz = scan.point_cloud.z[i];
2212 
2213  const double gx = m00 * lx + m01 * ly + m02 * lz + m03;
2214  const double gy = m10 * lx + m11 * ly + m12 * lz + m13;
2215  const double gz = m20 * lx + m21 * ly + m22 * lz + m23;
2216 
2217  this->setPointRGB(
2218  nOldPtsCount + i, gx, gy, gz, // XYZ
2219  inten, inten, inten // RGB
2220  );
2221  }
2222 }
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
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.
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:368
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:606
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.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
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:279
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...
#define IS_DERIVED(ptrObj, class_name)
Evaluates to true if a pointer to an object (derived from mrpt::rtti::CObject) is an instance of the ...
Definition: CObject.h:139
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:141
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
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)
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
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
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 const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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:52
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:42
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
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:133
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
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:300
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:787
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: 606c7e360 Mon Jun 24 22:58:07 2019 +0200 at lun jun 24 23:00:15 CEST 2019