libpointmatcher.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 #ifndef CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
28 #define CORELIB_SRC_ICP_LIBPOINTMATCHER_H_
29 
30 #include <fstream>
31 #include "pointmatcher/PointMatcher.h"
32 #include "nabo/nabo.h"
33 typedef PointMatcher<float> PM;
34 typedef PM::DataPoints DP;
35 
36 namespace rtabmap {
37 
38 DP pclToDP(const pcl::PointCloud<pcl::PointXYZI>::Ptr & pclCloud, bool is2D)
39 {
40  UDEBUG("");
41  typedef DP::Label Label;
42  typedef DP::Labels Labels;
43  typedef DP::View View;
44 
45  if (pclCloud->empty())
46  return DP();
47 
48  // fill labels
49  // conversions of descriptor fields from pcl
50  // see http://www.ros.org/wiki/pcl/Overview
51  Labels featLabels;
52  Labels descLabels;
53  featLabels.push_back(Label("x", 1));
54  featLabels.push_back(Label("y", 1));
55  if(!is2D)
56  {
57  featLabels.push_back(Label("z", 1));
58  }
59  featLabels.push_back(Label("pad", 1));
60 
61  descLabels.push_back(Label("intensity", 1));
62 
63  // create cloud
64  DP cloud(featLabels, descLabels, pclCloud->size());
65  cloud.getFeatureViewByName("pad").setConstant(1);
66 
67  // fill cloud
68  View view(cloud.getFeatureViewByName("x"));
69  View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
70  for(unsigned int i=0; i<pclCloud->size(); ++i)
71  {
72  view(0, i) = pclCloud->at(i).x;
73  view(1, i) = pclCloud->at(i).y;
74  if(!is2D)
75  {
76  view(2, i) = pclCloud->at(i).z;
77  }
78  viewIntensity(0, i) = pclCloud->at(i).intensity;
79  }
80 
81  return cloud;
82 }
83 
84 DP pclToDP(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud, bool is2D)
85 {
86  UDEBUG("");
87  typedef DP::Label Label;
88  typedef DP::Labels Labels;
89  typedef DP::View View;
90 
91  if (pclCloud->empty())
92  return DP();
93 
94  // fill labels
95  // conversions of descriptor fields from pcl
96  // see http://www.ros.org/wiki/pcl/Overview
97  Labels featLabels;
98  Labels descLabels;
99  featLabels.push_back(Label("x", 1));
100  featLabels.push_back(Label("y", 1));
101  if(!is2D)
102  {
103  featLabels.push_back(Label("z", 1));
104  }
105  featLabels.push_back(Label("pad", 1));
106 
107  descLabels.push_back(Label("normals", 3));
108  descLabels.push_back(Label("intensity", 1));
109 
110  // create cloud
111  DP cloud(featLabels, descLabels, pclCloud->size());
112  cloud.getFeatureViewByName("pad").setConstant(1);
113 
114  // fill cloud
115  View view(cloud.getFeatureViewByName("x"));
116  View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
117  View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
118  View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
119  View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
120  for(unsigned int i=0; i<pclCloud->size(); ++i)
121  {
122  view(0, i) = pclCloud->at(i).x;
123  view(1, i) = pclCloud->at(i).y;
124  if(!is2D)
125  {
126  view(2, i) = pclCloud->at(i).z;
127  }
128  viewNormalX(0, i) = pclCloud->at(i).normal_x;
129  viewNormalY(0, i) = pclCloud->at(i).normal_y;
130  viewNormalZ(0, i) = pclCloud->at(i).normal_z;
131  viewIntensity(0, i) = pclCloud->at(i).intensity;
132  }
133 
134  return cloud;
135 }
136 
137 DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = false)
138 {
139  UDEBUG("");
140  typedef DP::Label Label;
141  typedef DP::Labels Labels;
142  typedef DP::View View;
143 
144  if (scan.isEmpty())
145  return DP();
146 
147  // fill labels
148  // conversions of descriptor fields from pcl
149  // see http://www.ros.org/wiki/pcl/Overview
150  Labels featLabels;
151  Labels descLabels;
152  featLabels.push_back(Label("x", 1));
153  featLabels.push_back(Label("y", 1));
154  if(!scan.is2d())
155  {
156  featLabels.push_back(Label("z", 1));
157  }
158  featLabels.push_back(Label("pad", 1));
159 
160  if(scan.hasNormals())
161  {
162  descLabels.push_back(Label("normals", 3));
163  }
164  if(scan.hasIntensity())
165  {
166  descLabels.push_back(Label("intensity", 1));
167  }
168 
169 
170  // create cloud
171  DP cloud(featLabels, descLabels, scan.size());
172  cloud.getFeatureViewByName("pad").setConstant(1);
173 
174  // fill cloud
175  int nx = scan.getNormalsOffset();
176  int ny = nx+1;
177  int nz = ny+1;
178  int offsetI = scan.getIntensityOffset();
179  bool hasLocalTransform = !ignoreLocalTransform && !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
180  View view(cloud.getFeatureViewByName("x"));
181  View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
182  View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
183  View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
184  View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
185  int oi = 0;
186  for(int i=0; i<scan.size(); ++i)
187  {
188  const float * ptr = scan.data().ptr<float>(0, i);
189 
190  if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
191  {
192  if(hasLocalTransform)
193  {
194  if(nx == -1)
195  {
196  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
197  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
198  view(0, oi) = pt.x;
199  view(1, oi) = pt.y;
200  if(!scan.is2d())
201  {
202  view(2, oi) = pt.z;
203  }
204  if(offsetI!=-1)
205  {
206  viewIntensity(0, oi) = ptr[offsetI];
207  }
208  ++oi;
209  }
210  else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
211  {
212  pcl::PointNormal pt;
213  pt.x=ptr[0];
214  pt.y=ptr[1];
215  pt.z=scan.is2d()?0:ptr[2];
216  pt.normal_x=ptr[nx];
217  pt.normal_y=ptr[ny];
218  pt.normal_z=ptr[nz];
219  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
220  view(0, oi) = pt.x;
221  view(1, oi) = pt.y;
222  if(!scan.is2d())
223  {
224  view(2, oi) = pt.z;
225  }
226  viewNormalX(0, oi) = pt.normal_x;
227  viewNormalY(0, oi) = pt.normal_y;
228  viewNormalZ(0, oi) = pt.normal_z;
229 
230  if(offsetI!=-1)
231  {
232  viewIntensity(0, oi) = ptr[offsetI];
233  }
234 
235  ++oi;
236  }
237  else
238  {
239  UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
240  }
241  }
242  else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
243  {
244  view(0, oi) = ptr[0];
245  view(1, oi) = ptr[1];
246  if(!scan.is2d())
247  {
248  view(2, oi) = ptr[2];
249  }
250  if(nx!=-1)
251  {
252  viewNormalX(0, oi) = ptr[nx];
253  viewNormalY(0, oi) = ptr[ny];
254  viewNormalZ(0, oi) = ptr[nz];
255  }
256  if(offsetI!=-1)
257  {
258  viewIntensity(0, oi) = ptr[offsetI];
259  }
260  ++oi;
261  }
262  else
263  {
264  UWARN("Ignoring point %d with invalid data: pos=%f %f %f, normal=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3], ptr[nx], ptr[ny], ptr[nz]);
265  }
266  }
267  else
268  {
269  UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
270  }
271 
272  }
273  if(oi != scan.size())
274  {
275  cloud.conservativeResize(oi);
276  }
277 
278  return cloud;
279 }
280 
281 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZI> & pclCloud)
282 {
283  UDEBUG("");
284  typedef DP::ConstView ConstView;
285 
286  if (cloud.features.cols() == 0)
287  return;
288 
289  pclCloud.resize(cloud.features.cols());
290  pclCloud.is_dense = true;
291 
292  bool hasIntensity = cloud.descriptorExists("intensity");
293 
294  // fill cloud
295  ConstView view(cloud.getFeatureViewByName("x"));
296  ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
297  bool is3D = cloud.featureExists("z");
298  for(unsigned int i=0; i<pclCloud.size(); ++i)
299  {
300  pclCloud.at(i).x = view(0, i);
301  pclCloud.at(i).y = view(1, i);
302  pclCloud.at(i).z = is3D?view(2, i):0;
303  if(hasIntensity)
304  pclCloud.at(i).intensity = viewIntensity(0, i);
305  }
306 }
307 
308 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZINormal> & pclCloud)
309 {
310  UDEBUG("");
311  typedef DP::ConstView ConstView;
312 
313  if (cloud.features.cols() == 0)
314  return;
315 
316  pclCloud.resize(cloud.features.cols());
317  pclCloud.is_dense = true;
318 
319  bool hasIntensity = cloud.descriptorExists("intensity");
320 
321  // fill cloud
322  ConstView view(cloud.getFeatureViewByName("x"));
323  bool is3D = cloud.featureExists("z");
324  ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
325  ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
326  ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
327  ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
328  for(unsigned int i=0; i<pclCloud.size(); ++i)
329  {
330  pclCloud.at(i).x = view(0, i);
331  pclCloud.at(i).y = view(1, i);
332  pclCloud.at(i).z = is3D?view(2, i):0;
333  pclCloud.at(i).normal_x = viewNormalX(0, i);
334  pclCloud.at(i).normal_y = viewNormalY(0, i);
335  pclCloud.at(i).normal_z = viewNormalZ(0, i);
336  if(hasIntensity)
337  pclCloud.at(i).intensity = viewIntensity(0, i);
338  }
339 }
340 
341 rtabmap::LaserScan laserScanFromDP(const DP & cloud, const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity())
342 {
343  UDEBUG("");
344  typedef DP::ConstView ConstView;
345 
346  rtabmap::LaserScan scan;
347 
348  if (cloud.features.cols() == 0)
349  return rtabmap::LaserScan();
350 
351  // fill cloud
352  bool transformValid = !localTransform.isNull() && !localTransform.isIdentity();
353  rtabmap::Transform localTransformInv;
354  if(transformValid)
355  localTransformInv = localTransform.inverse();
356  bool is3D = cloud.featureExists("z");
357  bool hasNormals = cloud.descriptorExists("normals");
358  bool hasIntensity = cloud.descriptorExists("intensity");
359  ConstView view(cloud.getFeatureViewByName("x"));
360  ConstView viewNormalX(hasNormals?cloud.getDescriptorRowViewByName("normals",0):view);
361  ConstView viewNormalY(hasNormals?cloud.getDescriptorRowViewByName("normals",1):view);
362  ConstView viewNormalZ(hasNormals?cloud.getDescriptorRowViewByName("normals",2):view);
363  ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
364  int channels = 2+(is3D?1:0) + (hasNormals?3:0) + (hasIntensity?1:0);
365  cv::Mat data(1, cloud.features.cols(), CV_32FC(channels));
366  for(unsigned int i=0; i<cloud.features.cols(); ++i)
367  {
368  pcl::PointXYZINormal pt;
369  pt.x = view(0, i);
370  pt.y = view(1, i);
371  if(is3D)
372  pt.z = view(2, i);
373  if(hasIntensity)
374  pt.intensity = viewIntensity(0, i);
375  if(hasNormals) {
376  pt.normal_x = viewNormalX(0, i);
377  pt.normal_y = viewNormalY(0, i);
378  pt.normal_z = viewNormalZ(0, i);
379  }
380  if(transformValid)
381  pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
382 
383  float * value = data.ptr<float>(0, i);
384  int index = 0;
385  value[index++] = pt.x;
386  value[index++] = pt.y;
387  if(is3D)
388  value[index++] = pt.z;
389  if(hasIntensity)
390  value[index++] = pt.intensity;
391  if(hasNormals) {
392  value[index++] = pt.normal_x;
393  value[index++] = pt.normal_y;
394  value[index++] = pt.normal_z;
395  }
396  }
397 
398  UASSERT(data.channels() >= 2 && data.channels() <=7);
399  return rtabmap::LaserScan(data, 0, 0,
400  data.channels()==2?rtabmap::LaserScan::kXY:
401  data.channels()==3?(hasIntensity?rtabmap::LaserScan::kXYI:rtabmap::LaserScan::kXYZ):
402  data.channels()==4?rtabmap::LaserScan::kXYZI:
403  data.channels()==5?rtabmap::LaserScan::kXYINormal:
404  data.channels()==6?rtabmap::LaserScan::kXYZNormal:
405  rtabmap::LaserScan::kXYZINormal,
406  localTransform);
407 }
408 
409 template<typename T>
410 typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
411 {
412  typedef typename PointMatcher<T>::TransformationParameters M;
413  assert(matrix.rows() == matrix.cols());
414  assert((matrix.rows() == 3) || (matrix.rows() == 4));
415  assert((dimp1 == 3) || (dimp1 == 4));
416 
417  if (matrix.rows() == dimp1)
418  return matrix;
419 
420  M out(M::Identity(dimp1,dimp1));
421  out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
422  out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
423  return out;
424 }
425 
426 } // namespace rtabmap
427 
428 template<typename T>
429 struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
430 {
431  typedef PointMatcherSupport::Parametrizable Parametrizable;
432  typedef PointMatcherSupport::Parametrizable P;
433  typedef Parametrizable::Parameters Parameters;
434  typedef Parametrizable::ParameterDoc ParameterDoc;
435  typedef Parametrizable::ParametersDoc ParametersDoc;
436 
437  typedef typename Nabo::NearestNeighbourSearch<T> NNS;
438  typedef typename NNS::SearchType NNSearchType;
439 
440  typedef typename PointMatcher<T>::DataPoints DataPoints;
441  typedef typename PointMatcher<T>::Matcher Matcher;
442  typedef typename PointMatcher<T>::Matches Matches;
443  typedef typename PointMatcher<T>::Matrix Matrix;
444 
445  inline static const std::string description()
446  {
447  return "This matcher matches a point from the reading to its closest neighbors in the reference.";
448  }
449  inline static const ParametersDoc availableParameters()
450  {
451  return {
452  {"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
453  {"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
454  {"searchType", "Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)", "1", "0", "2", &P::Comp<unsigned>},
455  {"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
456  };
457  }
458 
459  const int knn;
460  const T epsilon;
461  const NNSearchType searchType;
462  const T maxDist;
463 
464 protected:
465  std::shared_ptr<NNS> featureNNS;
466  Matrix filteredReferenceIntensity;
467 
468 public:
469  KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
470  PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
471  knn(Parametrizable::get<int>("knn")),
472  epsilon(Parametrizable::get<T>("epsilon")),
473  searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
474  maxDist(Parametrizable::get<T>("maxDist"))
475  {
476  UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
477  }
478  virtual ~KDTreeMatcherIntensity() {}
479  virtual void init(const DataPoints& filteredReference)
480  {
481  // build and populate NNS
482  if(knn>1)
483  {
484  filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
485  }
486  else
487  {
488  UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
489  }
490  featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
491  }
492  virtual PM::Matches findClosests(const DP& filteredReading)
493  {
494  const int pointsCount(filteredReading.features.cols());
495  Matches matches(
496  typename Matches::Dists(knn, pointsCount),
497  typename Matches::Ids(knn, pointsCount)
498  );
499 
500  const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
501 
502  static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
503  static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
504  this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
505 
506  if(knn > 1)
507  {
508  Matches matchesOrderedByIntensity(
509  typename Matches::Dists(1, pointsCount),
510  typename Matches::Ids(1, pointsCount)
511  );
512  #pragma omp parallel for
513  for (int i = 0; i < pointsCount; ++i)
514  {
515  float minDistance = std::numeric_limits<float>::max();
516  for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
517  {
518  float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
519  if(distIntensity < minDistance)
520  {
521  matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
522  matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
523  minDistance = distIntensity;
524  }
525  }
526  }
527  matches = matchesOrderedByIntensity;
528  }
529  return matches;
530  }
531 };
532 
533 
534 #endif /* CORELIB_SRC_ICP_LIBPOINTMATCHER_H_ */
KDTreeMatcherIntensity::KDTreeMatcherIntensity
KDTreeMatcherIntensity(const Parameters &params=Parameters())
Definition: libpointmatcher.h:469
Label
DataPoints::Label Label
KDTreeMatcherIntensity::NNS
Nabo::NearestNeighbourSearch< T > NNS
Definition: libpointmatcher.h:437
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
T
T
KDTreeMatcherIntensity::NNSearchType
NNS::SearchType NNSearchType
Definition: libpointmatcher.h:438
KDTreeMatcherIntensity::Matches
PointMatcher< T >::Matches Matches
Definition: libpointmatcher.h:442
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
KDTreeMatcherIntensity::Matrix
PointMatcher< T >::Matrix Matrix
Definition: libpointmatcher.h:443
KDTreeMatcherIntensity::description
static const std::string description()
Definition: libpointmatcher.h:445
data
data
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
KDTreeMatcherIntensity::DataPoints
PointMatcher< T >::DataPoints DataPoints
Definition: libpointmatcher.h:440
KDTreeMatcherIntensity::init
virtual void init(const DataPoints &filteredReference)
Definition: libpointmatcher.h:479
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:137
KDTreeMatcherIntensity::P
PointMatcherSupport::Parametrizable P
Definition: libpointmatcher.h:432
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
DP
PM::DataPoints DP
Definition: libpointmatcher.h:34
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
KDTreeMatcherIntensity::findClosests
virtual PM::Matches findClosests(const DP &filteredReading)
Definition: libpointmatcher.h:492
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
KDTreeMatcherIntensity::Parametrizable
PointMatcherSupport::Parametrizable Parametrizable
Definition: libpointmatcher.h:431
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
PM
PointMatcher< float > PM
Definition: libpointmatcher.h:33
Labels
DataPoints::Labels Labels
rtabmap::eigenMatrixToDim
PointMatcher< T >::TransformationParameters eigenMatrixToDim(const typename PointMatcher< T >::TransformationParameters &matrix, int dimp1)
Definition: libpointmatcher.h:410
KDTreeMatcherIntensity::availableParameters
static const ParametersDoc availableParameters()
Definition: libpointmatcher.h:449
get
const M::mapped_type & get(const M &m, const typename M::key_type &k)
params
params
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:135
KDTreeMatcherIntensity::ParameterDoc
Parametrizable::ParameterDoc ParameterDoc
Definition: libpointmatcher.h:434
KDTreeMatcherIntensity::Parameters
Parametrizable::Parameters Parameters
Definition: libpointmatcher.h:433
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
matches
matches
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UWARN
#define UWARN(...)
KDTreeMatcherIntensity::Matcher
PointMatcher< T >::Matcher Matcher
Definition: libpointmatcher.h:441
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:131
KDTreeMatcherIntensity::searchType
const NNSearchType searchType
Definition: libpointmatcher.h:461
rtabmap::laserScanFromDP
rtabmap::LaserScan laserScanFromDP(const DP &cloud, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
Definition: libpointmatcher.h:341
KDTreeMatcherIntensity::featureNNS
std::shared_ptr< NNS > featureNNS
Definition: libpointmatcher.h:465
rtabmap::laserScanToDP
DP laserScanToDP(const rtabmap::LaserScan &scan, bool ignoreLocalTransform=false)
Definition: libpointmatcher.h:137
KDTreeMatcherIntensity::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: libpointmatcher.h:435
DP
PM::DataPoints DP
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
PointMatcher::TransformationParameters
Matrix TransformationParameters
rtabmap::pclToDP
DP pclToDP(const pcl::PointCloud< pcl::PointXYZI >::Ptr &pclCloud, bool is2D)
Definition: libpointmatcher.h:38
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::pclFromDP
void pclFromDP(const DP &cloud, pcl::PointCloud< pcl::PointXYZI > &pclCloud)
Definition: libpointmatcher.h:281
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28