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>
32 #include "nabo/nabo.h"
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]);
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];
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 
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:
406  localTransform);
407 }
408 
409 template<typename T>
411 {
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>
430 {
434  typedef Parametrizable::ParameterDoc ParameterDoc;
436 
438  typedef typename NNS::SearchType NNSearchType;
439 
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;
462  const T maxDist;
463 
464 protected:
465  std::shared_ptr<NNS> featureNNS;
467 
468 public:
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  }
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());
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  bool minDistFound = false;
517  for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
518  {
519  int matchesIdsCoeff = matches.ids.coeff(k, i);
520  if (matchesIdsCoeff!=-1)
521  {
522  float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matchesIdsCoeff));
523  if(distIntensity < minDistance)
524  {
525  matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
526  matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
527  minDistance = distIntensity;
528  minDistFound = true;
529  }
530  }
531  }
532 
533  if (!minDistFound)
534  {
535  matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(0, i);
536  matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(0, i);
537  }
538  }
539  matches = matchesOrderedByIntensity;
540  }
541  return matches;
542  }
543 };
544 
545 
546 #endif /* CORELIB_SRC_ICP_LIBPOINTMATCHER_H_ */
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
KDTreeMatcherIntensity::NNS
Nabo::NearestNeighbourSearch< T > NNS
Definition: libpointmatcher.h:437
KDTreeMatcherIntensity::searchType
const NNSearchType searchType
Definition: libpointmatcher.h:461
KDTreeMatcherIntensity::Matches
PointMatcher< T >::Matches Matches
Definition: libpointmatcher.h:442
UINFO
#define UINFO(...)
KDTreeMatcherIntensity::~KDTreeMatcherIntensity
virtual ~KDTreeMatcherIntensity()
Definition: libpointmatcher.h:478
rtabmap::LaserScan::kXYZ
@ kXYZ
Definition: LaserScan.h:45
PointMatcher.h
rtabmap::pclFromDP
void pclFromDP(const DP &cloud, pcl::PointCloud< pcl::PointXYZI > &pclCloud)
Definition: libpointmatcher.h:281
Nabo::NearestNeighbourSearch::TOUCH_STATISTICS
TOUCH_STATISTICS
KDTreeMatcherIntensity::Matrix
PointMatcher< T >::Matrix Matrix
Definition: libpointmatcher.h:443
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
Nabo::NearestNeighbourSearch::ALLOW_SELF_MATCH
ALLOW_SELF_MATCH
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
KDTreeMatcherIntensity::DataPoints
PointMatcher< T >::DataPoints DataPoints
Definition: libpointmatcher.h:440
KDTreeMatcherIntensity::filteredReferenceIntensity
Matrix filteredReferenceIntensity
Definition: libpointmatcher.h:466
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:145
KDTreeMatcherIntensity::NNSearchType
NNS::SearchType NNSearchType
Definition: libpointmatcher.h:438
rtabmap::LaserScan::kXYZI
@ kXYZI
Definition: LaserScan.h:46
DP
PM::DataPoints DP
Definition: libpointmatcher.h:34
PointMatcherSupport::Parametrizable::get
S get(const std::string &paramName)
KDTreeMatcherIntensity::findClosests
virtual PM::Matches findClosests(const DP &filteredReading)
Definition: libpointmatcher.h:492
KDTreeMatcherIntensity::epsilon
const T epsilon
Definition: libpointmatcher.h:460
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
PointMatcher< float >
Nabo::NearestNeighbourSearch::SearchType
SearchType
PointMatcher
KDTreeMatcherIntensity
Definition: libpointmatcher.h:429
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
KDTreeMatcherIntensity::P
PointMatcherSupport::Parametrizable P
Definition: libpointmatcher.h:432
KDTreeMatcherIntensity::description
static const std::string description()
Definition: libpointmatcher.h:445
fabs
Real fabs(const Real &a)
view
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set view
rtabmap::LaserScan::kXYI
@ kXYI
Definition: LaserScan.h:42
KDTreeMatcherIntensity::Parametrizable
PointMatcherSupport::Parametrizable Parametrizable
Definition: libpointmatcher.h:431
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
matches
matches
data
int data[]
KDTreeMatcherIntensity::init
virtual void init(const DataPoints &filteredReference)
Definition: libpointmatcher.h:479
nabo.h
Nabo::NearestNeighbourSearch
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
KDTreeMatcherIntensity::availableParameters
static const ParametersDoc availableParameters()
Definition: libpointmatcher.h:449
PointMatcherSupport::Parametrizable::ParametersDoc
std::vector< ParameterDoc > ParametersDoc
KDTreeMatcherIntensity::Parameters
Parametrizable::Parameters Parameters
Definition: libpointmatcher.h:433
rtabmap::eigenMatrixToDim
PointMatcher< T >::TransformationParameters eigenMatrixToDim(const typename PointMatcher< T >::TransformationParameters &matrix, int dimp1)
Definition: libpointmatcher.h:410
Nabo::NearestNeighbourSearch::create
static NearestNeighbourSearch * create(const CloudType &cloud, const Index dim=std::numeric_limits< Index >::max(), const SearchType preferedType=KDTREE_LINEAR_HEAP, const unsigned creationOptionFlags=0, const Parameters &additionalParameters=Parameters())
rtabmap::LaserScan::kXY
@ kXY
Definition: LaserScan.h:41
Nabo::NearestNeighbourSearch::InvalidIndex
static constexpr Index InvalidIndex
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
Eigen::Triplet< double >
Labels
DataPoints::Labels Labels
KDTreeMatcherIntensity::ParameterDoc
Parametrizable::ParameterDoc ParameterDoc
Definition: libpointmatcher.h:434
out
std::ofstream out("Result.txt")
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
PointMatcherSupport::Parametrizable
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:143
KDTreeMatcherIntensity::maxDist
const T maxDist
Definition: libpointmatcher.h:462
UWARN
#define UWARN(...)
KDTreeMatcherIntensity::knn
const int knn
Definition: libpointmatcher.h:459
Label
Label
rtabmap::Transform
Definition: Transform.h:41
rtabmap::laserScanFromDP
rtabmap::LaserScan laserScanFromDP(const DP &cloud, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
Definition: libpointmatcher.h:341
PointMatcherSupport::Parametrizable
rtabmap::LaserScan::kXYINormal
@ kXYINormal
Definition: LaserScan.h:44
PM
PointMatcher< float > PM
Definition: libpointmatcher.h:33
KDTreeMatcherIntensity::Matcher
PointMatcher< T >::Matcher Matcher
Definition: libpointmatcher.h:441
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
PointMatcher::Matcher::visitCounter
unsigned long visitCounter
UDEBUG
#define UDEBUG(...)
rtabmap::pclToDP
DP pclToDP(const pcl::PointCloud< pcl::PointXYZI >::Ptr &pclCloud, bool is2D)
Definition: libpointmatcher.h:38
Eigen.Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
DP
PM::DataPoints DP
matrix
Map< const Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(const T *data, int rows, int cols, int stride)
rtabmap
Definition: CameraARCore.cpp:35
KDTreeMatcherIntensity::featureNNS
std::shared_ptr< NNS > featureNNS
Definition: libpointmatcher.h:465
KDTreeMatcherIntensity::KDTreeMatcherIntensity
KDTreeMatcherIntensity(const Parameters &params=Parameters())
Definition: libpointmatcher.h:469
value
value
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
Nabo::NearestNeighbourSearch::InvalidValue
static constexpr T InvalidValue
rtabmap::laserScanToDP
DP laserScanToDP(const rtabmap::LaserScan &scan, bool ignoreLocalTransform=false)
Definition: libpointmatcher.h:137
KDTreeMatcherIntensity::ParametersDoc
Parametrizable::ParametersDoc ParametersDoc
Definition: libpointmatcher.h:435
PointMatcherSupport::Parametrizable::Parameters
std::map< std::string, Parameter > Parameters
rtabmap::LaserScan::kXYZINormal
@ kXYZINormal
Definition: LaserScan.h:49
rtabmap::LaserScan::kXYZNormal
@ kXYZNormal
Definition: LaserScan.h:48


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11