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