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