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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019