RegistrationIcp.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, 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 
28 
29 #include <rtabmap/core/RegistrationIcp.h>
30 #include <rtabmap/core/util3d_registration.h>
31 #include <rtabmap/core/util3d_surface.h>
32 #include <rtabmap/core/util3d.h>
33 #include <rtabmap/core/util3d_transforms.h>
34 #include <rtabmap/core/util3d_filtering.h>
35 #include <rtabmap/utilite/ULogger.h>
36 #include <rtabmap/utilite/UConversion.h>
37 #include <rtabmap/utilite/UMath.h>
38 #include <rtabmap/utilite/UTimer.h>
39 #include <pcl/conversions.h>
40 #include <pcl/common/pca.h>
41 #include <pcl/common/io.h>
42 
43 #ifdef RTABMAP_POINTMATCHER
44 #include <fstream>
45 #include "pointmatcher/PointMatcher.h"
46 #include "nabo/nabo.h"
47 typedef PointMatcher<float> PM;
48 typedef PM::DataPoints DP;
49 
50 DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
51 {
52  UDEBUG("");
53  typedef DP::Label Label;
54  typedef DP::Labels Labels;
55  typedef DP::View View;
56 
57  if (pclCloud->empty())
58  return DP();
59 
60  // fill labels
61  // conversions of descriptor fields from pcl
62  // see http://www.ros.org/wiki/pcl/Overview
63  Labels featLabels;
64  Labels descLabels;
65  std::vector<bool> isFeature;
66  featLabels.push_back(Label("x", 1));
67  isFeature.push_back(true);
68  featLabels.push_back(Label("y", 1));
69  isFeature.push_back(true);
70  if(!is2D)
71  {
72  featLabels.push_back(Label("z", 1));
73  isFeature.push_back(true);
74  }
75  featLabels.push_back(Label("pad", 1));
76 
77  // create cloud
78  DP cloud(featLabels, descLabels, pclCloud->size());
79  cloud.getFeatureViewByName("pad").setConstant(1);
80 
81  // fill cloud
82  View view(cloud.getFeatureViewByName("x"));
83  for(unsigned int i=0; i<pclCloud->size(); ++i)
84  {
85  view(0, i) = pclCloud->at(i).x;
86  view(1, i) = pclCloud->at(i).y;
87  if(!is2D)
88  {
89  view(2, i) = pclCloud->at(i).z;
90  }
91  }
92 
93  return cloud;
94 }
95 
96 DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
97 {
98  UDEBUG("");
99  typedef DP::Label Label;
100  typedef DP::Labels Labels;
101  typedef DP::View View;
102 
103  if (pclCloud->empty())
104  return DP();
105 
106  // fill labels
107  // conversions of descriptor fields from pcl
108  // see http://www.ros.org/wiki/pcl/Overview
109  Labels featLabels;
110  Labels descLabels;
111  std::vector<bool> isFeature;
112  featLabels.push_back(Label("x", 1));
113  isFeature.push_back(true);
114  featLabels.push_back(Label("y", 1));
115  isFeature.push_back(true);
116  if(!is2D)
117  {
118  featLabels.push_back(Label("z", 1));
119  isFeature.push_back(true);
120  }
121 
122  descLabels.push_back(Label("normals", 3));
123  isFeature.push_back(false);
124  isFeature.push_back(false);
125  isFeature.push_back(false);
126 
127  featLabels.push_back(Label("pad", 1));
128 
129  // create cloud
130  DP cloud(featLabels, descLabels, pclCloud->size());
131  cloud.getFeatureViewByName("pad").setConstant(1);
132 
133  // fill cloud
134  View view(cloud.getFeatureViewByName("x"));
135  View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
136  View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
137  View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
138  for(unsigned int i=0; i<pclCloud->size(); ++i)
139  {
140  view(0, i) = pclCloud->at(i).x;
141  view(1, i) = pclCloud->at(i).y;
142  if(!is2D)
143  {
144  view(2, i) = pclCloud->at(i).z;
145  }
146  viewNormalX(0, i) = pclCloud->at(i).normal_x;
147  viewNormalY(0, i) = pclCloud->at(i).normal_y;
148  viewNormalZ(0, i) = pclCloud->at(i).normal_z;
149  }
150 
151  return cloud;
152 }
153 
154 DP laserScanToDP(const rtabmap::LaserScan & scan)
155 {
156  UDEBUG("");
157  typedef DP::Label Label;
158  typedef DP::Labels Labels;
159  typedef DP::View View;
160 
161  if (scan.isEmpty())
162  return DP();
163 
164  // fill labels
165  // conversions of descriptor fields from pcl
166  // see http://www.ros.org/wiki/pcl/Overview
167  Labels featLabels;
168  Labels descLabels;
169  featLabels.push_back(Label("x", 1));
170  featLabels.push_back(Label("y", 1));
171  if(!scan.is2d())
172  {
173  featLabels.push_back(Label("z", 1));
174  }
175  featLabels.push_back(Label("pad", 1));
176 
177  if(scan.hasNormals())
178  {
179  descLabels.push_back(Label("normals", 3));
180  }
181  if(scan.hasIntensity())
182  {
183  descLabels.push_back(Label("intensity", 1));
184  }
185 
186 
187  // create cloud
188  DP cloud(featLabels, descLabels, scan.size());
189  cloud.getFeatureViewByName("pad").setConstant(1);
190 
191  // fill cloud
192  int nx = scan.getNormalsOffset();
193  int ny = nx+1;
194  int nz = ny+1;
195  int offsetI = scan.getIntensityOffset();
196  bool hasLocalTransform = !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
197  View view(cloud.getFeatureViewByName("x"));
198  View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
199  View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
200  View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
201  View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
202  int oi = 0;
203  for(int i=0; i<scan.size(); ++i)
204  {
205  const float * ptr = scan.data().ptr<float>(0, i);
206 
207  if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
208  {
209  if(hasLocalTransform)
210  {
211  if(nx == -1)
212  {
213  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
214  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
215  view(0, oi) = pt.x;
216  view(1, oi) = pt.y;
217  if(!scan.is2d())
218  {
219  view(2, oi) = pt.z;
220  }
221  if(offsetI!=-1)
222  {
223  viewIntensity(0, oi) = ptr[offsetI];
224  }
225  ++oi;
226  }
227  else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
228  {
229  pcl::PointNormal pt;
230  pt.x=ptr[0];
231  pt.y=ptr[1];
232  pt.z=scan.is2d()?0:ptr[2];
233  pt.normal_x=ptr[nx];
234  pt.normal_y=ptr[ny];
235  pt.normal_z=ptr[nz];
236  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
237  view(0, oi) = pt.x;
238  view(1, oi) = pt.y;
239  if(!scan.is2d())
240  {
241  view(2, oi) = pt.z;
242  }
243  viewNormalX(0, oi) = pt.normal_x;
244  viewNormalY(0, oi) = pt.normal_y;
245  viewNormalZ(0, oi) = pt.normal_z;
246 
247  if(offsetI!=-1)
248  {
249  viewIntensity(0, oi) = ptr[offsetI];
250  }
251 
252  ++oi;
253  }
254  else
255  {
256  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]);
257  }
258  }
259  else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
260  {
261  view(0, oi) = ptr[0];
262  view(1, oi) = ptr[1];
263  if(!scan.is2d())
264  {
265  view(2, oi) = ptr[2];
266  }
267  if(nx!=-1)
268  {
269  viewNormalX(0, oi) = ptr[nx];
270  viewNormalY(0, oi) = ptr[ny];
271  viewNormalZ(0, oi) = ptr[nz];
272  }
273  if(offsetI!=-1)
274  {
275  viewIntensity(0, oi) = ptr[offsetI];
276  }
277  ++oi;
278  }
279  else
280  {
281  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]);
282  }
283  }
284  else
285  {
286  UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
287  }
288 
289  }
290  if(oi != scan.size())
291  {
292  cloud.conservativeResize(oi);
293  }
294 
295  return cloud;
296 }
297 
298 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
299 {
300  UDEBUG("");
301  typedef DP::ConstView ConstView;
302 
303  if (cloud.features.cols() == 0)
304  return;
305 
306  pclCloud.resize(cloud.features.cols());
307  pclCloud.is_dense = true;
308 
309  // fill cloud
310  ConstView view(cloud.getFeatureViewByName("x"));
311  bool is3D = cloud.featureExists("z");
312  for(unsigned int i=0; i<pclCloud.size(); ++i)
313  {
314  pclCloud.at(i).x = view(0, i);
315  pclCloud.at(i).y = view(1, i);
316  pclCloud.at(i).z = is3D?view(2, i):0;
317  }
318 }
319 
320 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
321 {
322  UDEBUG("");
323  typedef DP::ConstView ConstView;
324 
325  if (cloud.features.cols() == 0)
326  return;
327 
328  pclCloud.resize(cloud.features.cols());
329  pclCloud.is_dense = true;
330 
331  // fill cloud
332  ConstView view(cloud.getFeatureViewByName("x"));
333  bool is3D = cloud.featureExists("z");
334  ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
335  ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
336  ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
337  for(unsigned int i=0; i<pclCloud.size(); ++i)
338  {
339  pclCloud.at(i).x = view(0, i);
340  pclCloud.at(i).y = view(1, i);
341  pclCloud.at(i).z = is3D?view(2, i):0;
342  pclCloud.at(i).normal_x = viewNormalX(0, i);
343  pclCloud.at(i).normal_y = viewNormalY(0, i);
344  pclCloud.at(i).normal_z = viewNormalZ(0, i);
345  }
346 }
347 
348 template<typename T>
349 typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
350 {
351  typedef typename PointMatcher<T>::TransformationParameters M;
352  assert(matrix.rows() == matrix.cols());
353  assert((matrix.rows() == 3) || (matrix.rows() == 4));
354  assert((dimp1 == 3) || (dimp1 == 4));
355 
356  if (matrix.rows() == dimp1)
357  return matrix;
358 
359  M out(M::Identity(dimp1,dimp1));
360  out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
361  out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
362  return out;
363 }
364 
365 template<typename T>
366 struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
367 {
368  typedef PointMatcherSupport::Parametrizable Parametrizable;
369  typedef PointMatcherSupport::Parametrizable P;
370  typedef Parametrizable::Parameters Parameters;
371  typedef Parametrizable::ParameterDoc ParameterDoc;
372  typedef Parametrizable::ParametersDoc ParametersDoc;
373 
374  typedef typename Nabo::NearestNeighbourSearch<T> NNS;
375  typedef typename NNS::SearchType NNSearchType;
376 
377  typedef typename PointMatcher<T>::DataPoints DataPoints;
378  typedef typename PointMatcher<T>::Matcher Matcher;
379  typedef typename PointMatcher<T>::Matches Matches;
380  typedef typename PointMatcher<T>::Matrix Matrix;
381 
382  inline static const std::string description()
383  {
384  return "This matcher matches a point from the reading to its closest neighbors in the reference.";
385  }
386  inline static const ParametersDoc availableParameters()
387  {
388  return {
389  {"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
390  {"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
391  {"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>},
392  {"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
393  };
394  }
395 
396  const int knn;
397  const T epsilon;
398  const NNSearchType searchType;
399  const T maxDist;
400 
401 protected:
402  std::shared_ptr<NNS> featureNNS;
403  Matrix filteredReferenceIntensity;
404 
405 public:
406  KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
407  PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
408  knn(Parametrizable::get<int>("knn")),
409  epsilon(Parametrizable::get<T>("epsilon")),
410  searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
411  maxDist(Parametrizable::get<T>("maxDist"))
412  {
413  UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
414  }
415  virtual ~KDTreeMatcherIntensity() {}
416  virtual void init(const DataPoints& filteredReference)
417  {
418  // build and populate NNS
419  if(knn>1)
420  {
421  filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
422  }
423  else
424  {
425  UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
426  }
427  featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
428  }
429  virtual PM::Matches findClosests(const DP& filteredReading)
430  {
431  const int pointsCount(filteredReading.features.cols());
432  Matches matches(
433  typename Matches::Dists(knn, pointsCount),
434  typename Matches::Ids(knn, pointsCount)
435  );
436 
437  const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
438 
439  static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
440  static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
441  this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
442 
443  if(knn > 1)
444  {
445  Matches matchesOrderedByIntensity(
446  typename Matches::Dists(1, pointsCount),
447  typename Matches::Ids(1, pointsCount)
448  );
449  #pragma omp parallel for
450  for (int i = 0; i < pointsCount; ++i)
451  {
452  float minDistance = std::numeric_limits<float>::max();
453  for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
454  {
455  float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
456  if(distIntensity < minDistance)
457  {
458  matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
459  matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
460  minDistance = distIntensity;
461  }
462  }
463  }
464  matches = matchesOrderedByIntensity;
465  }
466  return matches;
467  }
468 };
469 #endif
470 
471 namespace rtabmap {
472 
473 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
474  Registration(parameters, child),
475  _maxTranslation(Parameters::defaultIcpMaxTranslation()),
476  _maxRotation(Parameters::defaultIcpMaxRotation()),
477  _voxelSize(Parameters::defaultIcpVoxelSize()),
478  _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
479  _rangeMin(Parameters::defaultIcpRangeMin()),
480  _rangeMax(Parameters::defaultIcpRangeMax()),
481  _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
482  _maxIterations(Parameters::defaultIcpIterations()),
483  _epsilon(Parameters::defaultIcpEpsilon()),
484  _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
485  _pointToPlane(Parameters::defaultIcpPointToPlane()),
486  _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
487  _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
488  _pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
489  _pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
490  _pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
491  _libpointmatcher(Parameters::defaultIcpPM()),
492  _libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
493  _libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
494  _libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
495  _libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
496  _libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
497  _libpointmatcherICP(0)
498 {
499  this->parseParameters(parameters);
500 }
501 
502 RegistrationIcp::~RegistrationIcp()
503 {
504 #ifdef RTABMAP_POINTMATCHER
505  delete (PM::ICP*)_libpointmatcherICP;
506 #endif
507 }
508 
509 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
510 {
511  Registration::parseParameters(parameters);
512 
513  Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
514  Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
515  Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
516  Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
517  Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin);
518  Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax);
519  Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
520  Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
521  Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
522  Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
523  Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
524  Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
525  Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
526  Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
527  Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
528  Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
529  UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
530  UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
531 
532  Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
533  Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
534  Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
535  Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
536  Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
537  Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);
538 
539 #ifndef RTABMAP_POINTMATCHER
540  if(_libpointmatcher)
541  {
542  UWARN("Parameter %s is set to true but RTAB-Map has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
543  _libpointmatcher = false;
544  }
545 #else
546  if(_libpointmatcher)
547  {
548  UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
549  if(_libpointmatcherICP!=0)
550  {
551  delete (PM::ICP*)_libpointmatcherICP;
552  _libpointmatcherICP = 0;
553  }
554 
555  _libpointmatcherICP = new PM::ICP();
556 
557  PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
558 
559  bool useDefaults = true;
560  if(!_libpointmatcherConfig.empty())
561  {
562  // load YAML config
563  std::ifstream ifs(_libpointmatcherConfig.c_str());
564  if (ifs.good())
565  {
566  icp->loadFromYaml(ifs);
567  useDefaults = false;
568  }
569  else
570  {
571  UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str());
572  }
573  }
574  if(useDefaults)
575  {
576  // Create the default ICP algorithm
577  // See the implementation of setDefault() to create a custom ICP algorithm
578  icp->setDefault();
579 
580  icp->readingDataPointsFilters.clear();
581  icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
582 
583  icp->referenceDataPointsFilters.clear();
584  icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
585 
586  PM::Parameters params;
587  params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
588  params["knn"] = uNumber2Str(_libpointmatcherKnn);
589  params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
590 
591  if(_libpointmatcherIntensity)
592  {
593  icp->matcher.reset(new KDTreeMatcherIntensity<float>(params));
594  }
595  else
596  {
597 #if POINTMATCHER_VERSION_INT >= 10300
598  icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
599 #else
600  icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
601 #endif
602  }
603  params.clear();
604 
605  params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
606  icp->outlierFilters.clear();
607  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
608  params.clear();
609  if(_pointToPlane)
610  {
611  params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
612  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
613  params.clear();
614 
615  params["force2D"] = force3DoF()?"1":"0";
616 #if POINTMATCHER_VERSION_INT >= 10300
617  icp->errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params);
618 #else
619  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
620 #endif
621  params.clear();
622  }
623  else
624  {
625 #if POINTMATCHER_VERSION_INT >= 10300
626  icp->errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
627 #else
628  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
629 #endif
630  }
631 
632  icp->transformationCheckers.clear();
633  params["maxIterationCount"] = uNumber2Str(_maxIterations);
634  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params));
635  params.clear();
636 
637  params["minDiffRotErr"] = uNumber2Str(_epsilon*_epsilon*100.0f);
638  params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon);
639  params["smoothLength"] = uNumber2Str(4);
640  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
641  params.clear();
642 
643  params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
644  params["maxTranslationNorm"] = uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
645  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
646  params.clear();
647  }
648  }
649 #endif
650 
651  UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
652  UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
653  UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
654  UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
655  UASSERT(_epsilon >= 0.0f);
656  UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
657  UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
658 }
659 
660 Transform RegistrationIcp::computeTransformationImpl(
661  Signature & fromSignature,
662  Signature & toSignature,
663  Transform guess,
664  RegistrationInfo & info) const
665 {
666  UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
667  UDEBUG("Voxel size=%f", _voxelSize);
668  UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
669  UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
670  UDEBUG("Normal radius=%f", _pointToPlaneRadius);
671  UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
672  UDEBUG("Max Iterations=%d", _maxIterations);
673  UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
674  UDEBUG("Max translation=%f", _maxTranslation);
675  UDEBUG("Max rotation=%f", _maxRotation);
676  UDEBUG("Downsampling step=%d", _downsamplingStep);
677  UDEBUG("libpointmatcher=%d (knn=%d, outlier ratio=%f)", _libpointmatcher?1:0, _libpointmatcherKnn, _libpointmatcherOutlierRatio);
678 
679  UTimer timer;
680  std::string msg;
681  Transform transform;
682 
683  SensorData & dataFrom = fromSignature.sensorData();
684  SensorData & dataTo = toSignature.sensorData();
685 
686  UDEBUG("size from=%d (format=%d, max pts=%d) to=%d (format=%d, max pts=%d)",
687  dataFrom.laserScanRaw().size(),
688  (int)dataFrom.laserScanRaw().format(),
689  dataFrom.laserScanRaw().maxPoints(),
690  dataTo.laserScanRaw().size(),
691  (int)dataTo.laserScanRaw().format(),
692  dataTo.laserScanRaw().maxPoints());
693 
694  if(!guess.isNull() && !dataFrom.laserScanRaw().isEmpty() && !dataTo.laserScanRaw().isEmpty())
695  {
696  // ICP with guess transform
697  LaserScan fromScan = dataFrom.laserScanRaw();
698  LaserScan toScan = dataTo.laserScanRaw();
699  if(_downsamplingStep>1 || _rangeMin >0.0f || _rangeMax > 0.0f)
700  {
701  fromScan = util3d::commonFiltering(fromScan, _downsamplingStep, _rangeMin, _rangeMax);
702  toScan = util3d::commonFiltering(toScan, _downsamplingStep, _rangeMin, _rangeMax);
703  UDEBUG("Downsampling and/or range filtering time (step=%d, min=%fm, max=%fm) = %f s", _downsamplingStep, _rangeMin, _rangeMax, timer.ticks());
704  }
705 
706  if(fromScan.size() && toScan.size())
707  {
708  Transform icpT;
709  bool hasConverged = false;
710  float correspondencesRatio = 0.0f;
711  int correspondences = 0;
712  double variance = 1.0;
713  bool transformComputed = false;
714  bool tooLowComplexityForPlaneToPlane = false;
715  float secondEigenValue = 1.0f;
716  cv::Mat complexityVectors;
717 
718  if( _pointToPlane &&
719  _voxelSize == 0.0f &&
720  fromScan.hasNormals() &&
721  toScan.hasNormals() &&
722  !((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D)
723  {
724  //special case if we have already normals computed and there is no filtering
725 
726  cv::Mat complexityVectorsFrom, complexityVectorsTo;
727  cv::Mat complexityValuesFrom, complexityValuesTo;
728  double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
729  double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
730  float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
731  info.icpStructuralComplexity = complexity;
732  if(complexity < _pointToPlaneMinComplexity)
733  {
734  tooLowComplexityForPlaneToPlane = true;
735  if(complexity > 0.0f)
736  {
737  complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
738 
739  UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
740  (complexityVectors.rows == 3 && complexityVectors.cols == 3));
741  secondEigenValue = complexityValuesFrom.at<float>(1,0)<complexityValuesTo.at<float>(1,0)?complexityValuesFrom.at<float>(1,0):complexityValuesTo.at<float>(1,0);
742  UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
743  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
744  "direction of normals (%s: %s).",
745  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
746  secondEigenValue,
747  fromComplexity<toComplexity?"From":"To",
748  complexityVectors.rows==2?
749  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
750  secondEigenValue<_pointToPlaneMinComplexity?
751  uFormat("n=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2)).c_str():
752  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
753  }
754  else
755  {
756  UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
757  }
758  if(ULogger::level() == ULogger::kDebug)
759  {
760  std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
761  std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
762  std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
763  std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
764  }
765  }
766  else
767  {
768  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudINormal(fromScan, fromScan.localTransform());
769  pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());
770 
771  fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
772  toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
773 
774  if(fromCloudNormals->size() > 2 && toCloudNormals->size() > 2)
775  {
776  pcl::PCA<pcl::PointXYZINormal> pca;
777  pca.setInputCloud(fromCloudNormals);
778  Eigen::Vector3f valuesFrom = pca.getEigenValues();
779  pca.setInputCloud(toCloudNormals);
780  Eigen::Vector3f valuesTo = pca.getEigenValues();
781  if(valuesFrom[0]/fromCloudNormals->size() < valuesTo[0]/toCloudNormals->size())
782  {
783  info.icpStructuralDistribution = sqrt(valuesFrom[0]/fromCloudNormals->size());
784  }
785  else
786  {
787  info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloudNormals->size());
788  }
789  }
790 
791  UDEBUG("Conversion time = %f s", timer.ticks());
792  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
793 #ifdef RTABMAP_POINTMATCHER
794  if(_libpointmatcher)
795  {
796  // Load point clouds
797  DP data = laserScanToDP(fromScan);
798  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess * toScan.localTransform()));
799 
800  // Compute the transformation to express data in ref
801  PM::TransformationParameters T;
802  try
803  {
804  UASSERT(_libpointmatcherICP != 0);
805  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
806  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
807  T = icp(data, ref);
808  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
809  UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
810 
811  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
812  UDEBUG("match ratio: %f", matchRatio);
813 
814  if(!icpT.isNull())
815  {
816  fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
817  hasConverged = true;
818  }
819  }
820  catch(const std::exception & e)
821  {
822  msg = uFormat("libpointmatcher has failed: %s", e.what());
823  }
824  }
825  else
826 #endif
827  {
828  icpT = util3d::icpPointToPlane(
829  fromCloudNormals,
830  toCloudNormals,
831  _maxCorrespondenceDistance,
832  _maxIterations,
833  hasConverged,
834  *fromCloudNormalsRegistered,
835  _epsilon,
836  this->force3DoF());
837  }
838 
839  if(!icpT.isNull() && hasConverged)
840  {
841  util3d::computeVarianceAndCorrespondences(
842  fromCloudNormalsRegistered,
843  toCloudNormals,
844  _maxCorrespondenceDistance,
845  _maxRotation,
846  variance,
847  correspondences);
848  }
849  transformComputed = true;
850  }
851  }
852 
853  int maxLaserScansFrom = fromScan.maxPoints();
854  int maxLaserScansTo = toScan.maxPoints();
855  if(!transformComputed)
856  {
857  pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloud = util3d::laserScanToPointCloudI(fromScan, fromScan.localTransform());
858  pcl::PointCloud<pcl::PointXYZI>::Ptr toCloud = util3d::laserScanToPointCloudI(toScan, guess * toScan.localTransform());
859  UDEBUG("Conversion time = %f s", timer.ticks());
860 
861  if(fromCloud->size() > 2 && toCloud->size() > 2)
862  {
863  pcl::PCA<pcl::PointXYZI> pca;
864  pca.setInputCloud(fromCloud);
865  Eigen::Vector3f valuesFrom = pca.getEigenValues();
866  pca.setInputCloud(toCloud);
867  Eigen::Vector3f valuesTo = pca.getEigenValues();
868  if(valuesFrom[0]/fromCloud->size() < valuesTo[0]/toCloud->size())
869  {
870  info.icpStructuralDistribution = sqrt(valuesFrom[0]/fromCloud->size());
871  }
872  else
873  {
874  info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloud->size());
875  }
876  UDEBUG("Computed icpStructuralDistribution %f s",timer.ticks());
877  }
878 
879  pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudFiltered = fromCloud;
880  pcl::PointCloud<pcl::PointXYZI>::Ptr toCloudFiltered = toCloud;
881  if(_voxelSize > 0.0f)
882  {
883  float pointsBeforeFiltering = (float)fromCloudFiltered->size();
884  fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
885  float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
886  maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
887 
888  pointsBeforeFiltering = (float)toCloudFiltered->size();
889  toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
890  float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
891  maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
892 
893  UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
894  _voxelSize,
895  ratioFrom,
896  (int)fromCloudFiltered->size(),
897  maxLaserScansFrom,
898  ratioTo,
899  (int)toCloudFiltered->size(),
900  maxLaserScansTo,
901  timer.ticks());
902  }
903 
904  pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZI>());
905  if(_pointToPlane && // ICP Point To Plane
906  !tooLowComplexityForPlaneToPlane && // if previously rejected above
907  !((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
908  {
909  Eigen::Vector3f viewpointFrom(fromScan.localTransform().x(), fromScan.localTransform().y(), fromScan.localTransform().z());
910  pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
911  if(fromScan.is2d())
912  {
913  if(_voxelSize > 0.0f)
914  {
915  normalsFrom = util3d::computeNormals2D(
916  fromCloudFiltered,
917  _pointToPlaneK,
918  _pointToPlaneRadius,
919  viewpointFrom);
920  }
921  else
922  {
923  normalsFrom = util3d::computeFastOrganizedNormals2D(
924  fromCloudFiltered,
925  _pointToPlaneK,
926  _pointToPlaneRadius,
927  viewpointFrom);
928  }
929  }
930  else
931  {
932  normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
933  }
934 
935  Transform toT = guess * toScan.localTransform();
936  Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
937  pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
938  if(toScan.is2d())
939  {
940  if(_voxelSize > 0.0f)
941  {
942  normalsTo = util3d::computeNormals2D(
943  toCloudFiltered,
944  _pointToPlaneK,
945  _pointToPlaneRadius,
946  viewpointTo);
947  }
948  else
949  {
950  normalsTo = util3d::computeFastOrganizedNormals2D(
951  toCloudFiltered,
952  _pointToPlaneK,
953  _pointToPlaneRadius,
954  viewpointTo);
955  }
956  }
957  else
958  {
959  normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
960  }
961 
962  cv::Mat complexityVectorsFrom, complexityVectorsTo;
963  cv::Mat complexityValuesFrom, complexityValuesTo;
964  double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, Transform::getIdentity(), fromScan.is2d(), &complexityVectorsFrom, &complexityValuesFrom);
965  double toComplexity = util3d::computeNormalsComplexity(*normalsTo, Transform::getIdentity(), toScan.is2d(), &complexityVectorsTo, &complexityValuesTo);
966  float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
967  info.icpStructuralComplexity = complexity;
968  if(complexity < _pointToPlaneMinComplexity)
969  {
970  tooLowComplexityForPlaneToPlane = true;
971  if(complexity > 0.0f)
972  {
973  complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
974  UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
975  (complexityVectors.rows == 3 && complexityVectors.cols == 3));
976  UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). "
977  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
978  "direction of normals (%s: %s).",
979  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
980  fromComplexity<toComplexity?"From":"To",
981  complexityVectors.rows==2?
982  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
983  uFormat("n1=%f,%f,%f n2=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2), complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2)).c_str());
984  }
985  else
986  {
987  UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
988  }
989  if(ULogger::level() == ULogger::kDebug)
990  {
991  std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
992  std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
993  std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
994  std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
995  }
996  }
997  else
998  {
999  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
1000  pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
1001 
1002  pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
1003  pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
1004 
1005  std::vector<int> indices;
1006  toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
1007  fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
1008 
1009  if(!fromCloudNormals->empty() && !fromScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
1010  {
1011  util3d::adjustNormalsToViewPoint(fromCloudNormals,
1012  Eigen::Vector3f(fromScan.localTransform().x(),fromScan.localTransform().y(),fromScan.localTransform().z()+10),
1013  _pointToPlaneGroundNormalsUp);
1014  }
1015  if(!toCloudNormals->empty() && !toScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
1016  {
1017  Transform toT = guess * toScan.localTransform();
1018  Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()+10);
1019  util3d::adjustNormalsToViewPoint(toCloudNormals,
1020  viewpointTo,
1021  _pointToPlaneGroundNormalsUp);
1022  }
1023 
1024  // update output scans
1025  if(fromScan.is2d())
1026  {
1027  fromSignature.sensorData().setLaserScan(
1028  LaserScan(
1029  util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
1030  maxLaserScansFrom,
1031  fromScan.rangeMax(),
1032  LaserScan::kXYINormal,
1033  fromScan.localTransform()));
1034  }
1035  else
1036  {
1037  fromSignature.sensorData().setLaserScan(
1038  LaserScan(
1039  util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
1040  maxLaserScansFrom,
1041  fromScan.rangeMax(),
1042  LaserScan::kXYZINormal,
1043  fromScan.localTransform()));
1044  }
1045  if(toScan.is2d())
1046  {
1047  toSignature.sensorData().setLaserScan(
1048  LaserScan(
1049  util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
1050  maxLaserScansTo,
1051  toScan.rangeMax(),
1052  LaserScan::kXYINormal,
1053  toScan.localTransform()));
1054  }
1055  else
1056  {
1057  toSignature.sensorData().setLaserScan(
1058  LaserScan(
1059  util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
1060  maxLaserScansTo,
1061  toScan.rangeMax(),
1062  LaserScan::kXYZINormal,
1063  toScan.localTransform()));
1064  }
1065  UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
1066  fromScan = fromSignature.sensorData().laserScanRaw();
1067  toScan = toSignature.sensorData().laserScanRaw();
1068 
1069  if(toCloudNormals->size() && fromCloudNormals->size())
1070  {
1071  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
1072 
1073 #ifdef RTABMAP_POINTMATCHER
1074  if(_libpointmatcher)
1075  {
1076  // Load point clouds
1077  DP data = laserScanToDP(fromScan);
1078  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
1079 
1080  // Compute the transformation to express data in ref
1081  PM::TransformationParameters T;
1082  try
1083  {
1084  UASSERT(_libpointmatcherICP != 0);
1085  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
1086  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
1087  T = icp(data, ref);
1088  UDEBUG("libpointmatcher icp...done!");
1089  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
1090 
1091  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
1092  UDEBUG("match ratio: %f", matchRatio);
1093 
1094  if(!icpT.isNull())
1095  {
1096  fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
1097  hasConverged = true;
1098  }
1099  }
1100  catch(const std::exception & e)
1101  {
1102  msg = uFormat("libpointmatcher has failed: %s", e.what());
1103  }
1104  }
1105  else
1106 #endif
1107  {
1108  icpT = util3d::icpPointToPlane(
1109  fromCloudNormals,
1110  toCloudNormals,
1111  _maxCorrespondenceDistance,
1112  _maxIterations,
1113  hasConverged,
1114  *fromCloudNormalsRegistered,
1115  _epsilon,
1116  this->force3DoF());
1117  }
1118 
1119  if(!icpT.isNull() && hasConverged)
1120  {
1121  util3d::computeVarianceAndCorrespondences(
1122  fromCloudNormalsRegistered,
1123  toCloudNormals,
1124  _maxCorrespondenceDistance,
1125  _maxRotation,
1126  variance,
1127  correspondences);
1128  }
1129  }
1130  transformComputed = true;
1131  }
1132  }
1133 
1134  if(!transformComputed) // ICP Point to Point
1135  {
1136  if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher))
1137  {
1138  UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
1139  }
1140 
1141  if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
1142  {
1143  // update output scans
1144  if(fromScan.is2d())
1145  {
1146  fromSignature.sensorData().setLaserScan(
1147  LaserScan(
1148  util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
1149  maxLaserScansFrom,
1150  fromScan.rangeMax(),
1151  LaserScan::kXYI,
1152  fromScan.localTransform()));
1153  }
1154  else
1155  {
1156  fromSignature.sensorData().setLaserScan(
1157  LaserScan(
1158  util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
1159  maxLaserScansFrom,
1160  fromScan.rangeMax(),
1161  LaserScan::kXYZI,
1162  fromScan.localTransform()));
1163  }
1164  if(toScan.is2d())
1165  {
1166  toSignature.sensorData().setLaserScan(
1167  LaserScan(
1168  util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
1169  maxLaserScansTo,
1170  toScan.rangeMax(),
1171  LaserScan::kXYI,
1172  toScan.localTransform()));
1173  }
1174  else
1175  {
1176  toSignature.sensorData().setLaserScan(
1177  LaserScan(
1178  util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
1179  maxLaserScansTo,
1180  toScan.rangeMax(),
1181  LaserScan::kXYZI,
1182  toScan.localTransform()));
1183  }
1184  fromScan = fromSignature.sensorData().laserScanRaw();
1185  toScan = toSignature.sensorData().laserScanRaw();
1186  }
1187 
1188 #ifdef RTABMAP_POINTMATCHER
1189  if(_libpointmatcher)
1190  {
1191  // Load point clouds
1192  DP data = laserScanToDP(fromScan);
1193  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
1194 
1195  // Compute the transformation to express data in ref
1196  PM::TransformationParameters T;
1197  try
1198  {
1199  UASSERT(_libpointmatcherICP != 0);
1200  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
1201  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
1202  if(_pointToPlane)
1203  {
1204  // temporary set PointToPointErrorMinimizer
1205  PM::ICP icpTmp = icp;
1206 
1207  PM::Parameters params;
1208  params["maxDist"] = uNumber2Str(_voxelSize>0?_voxelSize:_maxCorrespondenceDistance/2.0f);
1209  UWARN("libpointmatcher icp...temporary maxDist=%s (%s=%f, %s=%f)", params["maxDist"].c_str(), Parameters::kIcpMaxCorrespondenceDistance().c_str(), _maxCorrespondenceDistance, Parameters::kIcpVoxelSize().c_str(), _voxelSize);
1210  params["knn"] = uNumber2Str(_libpointmatcherKnn);
1211  params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
1212 #if POINTMATCHER_VERSION_INT >= 10300
1213  icpTmp.matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
1214 #else
1215  icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
1216 #endif
1217 
1218 #if POINTMATCHER_VERSION_INT >= 10300
1219  icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
1220 #else
1221  icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
1222 #endif
1223  for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
1224  {
1225  if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
1226  {
1227  iter = icpTmp.outlierFilters.erase(iter);
1228  }
1229  else
1230  {
1231  ++iter;
1232  }
1233  }
1234 
1235  T = icpTmp(data, ref);
1236  }
1237  else
1238  {
1239  T = icp(data, ref);
1240  }
1241  UDEBUG("libpointmatcher icp...done!");
1242  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
1243 
1244  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
1245  UDEBUG("match ratio: %f", matchRatio);
1246 
1247  if(!icpT.isNull())
1248  {
1249  fromCloudRegistered = util3d::transformPointCloud(fromCloudFiltered, icpT);
1250  hasConverged = true;
1251  }
1252  }
1253  catch(const std::exception & e)
1254  {
1255  msg = uFormat("libpointmatcher has failed: %s", e.what());
1256  }
1257  }
1258  else
1259 #endif
1260  {
1261  icpT = util3d::icp(
1262  fromCloudFiltered,
1263  toCloudFiltered,
1264  _maxCorrespondenceDistance,
1265  _maxIterations,
1266  hasConverged,
1267  *fromCloudRegistered,
1268  _epsilon,
1269  this->force3DoF()); // icp2D
1270  }
1271 
1272  if(!icpT.isNull() && hasConverged)
1273  {
1274  if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2)
1275  {
1276  if(complexityVectors.empty() || _pointToPlaneLowComplexityStrategy == 0)
1277  {
1278  msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
1279  icpT.setNull();
1280  UWARN(msg.c_str());
1281  }
1282  else //if(_pointToPlaneLowComplexityStrategy == 1)
1283  {
1284  Transform guessInv = guess.inverse();
1285  Transform t = guessInv * icpT.inverse() * guess;
1286  Eigen::Vector3f v(t.x(), t.y(), t.z());
1287  if(complexityVectors.cols == 2)
1288  {
1289  // limit translation in direction of the first eigen vector
1290  Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
1291  float a = v.dot(n);
1292  Eigen::Vector3f vp = n*a;
1293  UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
1294  v[0], v[1], vp[0], vp[1]);
1295  v= vp;
1296  }
1297  else if(complexityVectors.rows == 3)
1298  {
1299  // limit translation in direction of the first and second eigen vectors
1300  Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
1301  Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
1302  float a = v.dot(n1);
1303  float b = v.dot(n2);
1304  Eigen::Vector3f vp = n1*a;
1305  if(secondEigenValue >= _pointToPlaneMinComplexity)
1306  {
1307  vp += n2*b;
1308  }
1309  UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
1310  v[0], v[1], v[2], vp[0], vp[1], vp[2]);
1311  v = vp;
1312  }
1313  else
1314  {
1315  UWARN("not supposed to be here!");
1316  v = Eigen::Vector3f(0,0,0);
1317  }
1318  float roll, pitch, yaw;
1319  t.getEulerAngles(roll, pitch, yaw);
1320  t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
1321  icpT = guess * t.inverse() * guessInv;
1322 
1323  if(fromScan.hasNormals() && toScan.hasNormals())
1324  {
1325  // we were using normals, so compute correspondences using normals
1326  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudINormal(fromScan, icpT * fromScan.localTransform());
1327  pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());
1328 
1329  util3d::computeVarianceAndCorrespondences(
1330  fromCloudNormalsRegistered,
1331  toCloudNormals,
1332  _maxCorrespondenceDistance,
1333  _maxRotation,
1334  variance,
1335  correspondences);
1336  }
1337  else
1338  {
1339  util3d::computeVarianceAndCorrespondences(
1340  fromCloudRegistered,
1341  toCloudFiltered,
1342  _maxCorrespondenceDistance,
1343  variance,
1344  correspondences);
1345  }
1346  }
1347  }
1348  else
1349  {
1350  if(tooLowComplexityForPlaneToPlane)
1351  {
1352  UWARN("Even if complexity is low , PointToPoint transformation is accepted \"as is\" (%s=2)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
1353  }
1354  util3d::computeVarianceAndCorrespondences(
1355  fromCloudRegistered,
1356  toCloudFiltered,
1357  _maxCorrespondenceDistance,
1358  variance,
1359  correspondences);
1360  }
1361  }
1362  }
1363  }
1364  UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
1365 
1366  if(!icpT.isNull() && hasConverged)
1367  {
1368  float ix,iy,iz, iroll,ipitch,iyaw;
1369  Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
1370  icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
1371  info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
1372  info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
1373  if((_maxTranslation>0.0f &&
1374  info.icpTranslation > _maxTranslation)
1375  ||
1376  (_maxRotation>0.0f &&
1377  info.icpRotation > _maxRotation))
1378  {
1379  msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
1380  info.icpTranslation,
1381  info.icpRotation,
1382  _maxTranslation,
1383  _maxRotation);
1384  UINFO(msg.c_str());
1385  }
1386  else
1387  {
1388  // verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
1389  int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
1390  UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
1391 
1392  if(maxLaserScans)
1393  {
1394  correspondencesRatio = float(correspondences)/float(maxLaserScans);
1395  }
1396  else
1397  {
1398  static bool warningShown = false;
1399  if(!warningShown)
1400  {
1401  UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
1402  dataTo.id());
1403  warningShown = true;
1404  }
1405  correspondencesRatio = float(correspondences)/float(toScan.size()>fromScan.size()?toScan.size():fromScan.size());
1406  }
1407 
1408  variance/=10.0;
1409 
1410  UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
1411  dataTo.id(), dataFrom.id(),
1412  hasConverged?"true":"false",
1413  variance,
1414  correspondences,
1415  maxLaserScans>0?maxLaserScans:(int)(toScan.size()>fromScan.size()?toScan.size():fromScan.size()),
1416  correspondencesRatio*100.0f,
1417  info.icpTranslation,
1418  info.icpRotation);
1419 
1420  if(correspondences == 0)
1421  {
1422  UWARN("Transform is found (%s) but no correspondences has been found!? Variance is unknown!",
1423  icpT.prettyPrint().c_str());
1424  }
1425  else
1426  {
1427  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
1428  info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
1429  }
1430  info.icpInliersRatio = correspondencesRatio;
1431  info.icpCorrespondences = correspondences;
1432 
1433  if(correspondencesRatio <= _correspondenceRatio)
1434  {
1435  msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
1436  correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
1437  UINFO(msg.c_str());
1438  }
1439  else
1440  {
1441  transform = icpT.inverse()*guess;
1442  }
1443  }
1444  }
1445  else
1446  {
1447  if(msg.empty())
1448  {
1449  msg = uFormat("Cannot compute transform (converged=%s var=%f)",
1450  hasConverged?"true":"false", variance);
1451  }
1452  UINFO(msg.c_str());
1453  }
1454  }
1455  else
1456  {
1457  msg = "Laser scans empty ?!?";
1458  UWARN(msg.c_str());
1459  }
1460  }
1461  else if(dataTo.isValid())
1462  {
1463  if(guess.isNull())
1464  {
1465  msg = "RegistrationIcp cannot do registration with a null guess.";
1466  }
1467  else
1468  {
1469  msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
1470  dataTo.id(), dataTo.laserScanRaw().size(),
1471  dataFrom.id(), dataFrom.laserScanRaw().size());
1472  }
1473  UERROR(msg.c_str());
1474  }
1475 
1476 
1477  info.rejectedMsg = msg;
1478 
1479  UDEBUG("New transform = %s", transform.prettyPrint().c_str());
1480  return transform;
1481 }
1482 
1483 }
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
epsilon
double epsilon
UTimer
Definition: UTimer.h:46
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:295
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:88
rtabmap::util3d::computeNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:2777
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:89
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:105
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:337
f
f
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap_oanet.init
def init(descriptorDim, matchThreshold, iterations, cuda, model_path)
Definition: rtabmap_oanet.py:22
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::util3d::icpPointToPlane
Transform RTABMAP_EXP icpPointToPlane(const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointNormal > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
Definition: util3d_registration.cpp:489
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:94
glm::e
GLM_FUNC_DECL genType e()
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:101
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::computeVarianceAndCorrespondences
void RTABMAP_EXP computeVarianceAndCorrespondences(const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudA, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, double &variance, int &correspondencesOut)
Definition: util3d_registration.cpp:302
rtabmap::Registration::force3DoF
bool force3DoF() const
Definition: Registration.h:68
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:92
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:113
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:104
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:102
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:2889
util3d_registration.h
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2105
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:137
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_EXP commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
description
description
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:232
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2470
rtabmap::util3d::icp
Transform RTABMAP_EXP icp(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointXYZ > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
Definition: util3d_registration.cpp:418
util3d_filtering.h
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:78
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:98
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:155
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
util3d_transforms.h
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:111
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:404
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::util3d::removeNaNNormalsFromPointCloud
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
Definition: util3d_filtering.cpp:901
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::RegistrationIcp::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
Definition: RegistrationIcp.cpp:660
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2435
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:519
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:227
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::RegistrationIcp::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: RegistrationIcp.cpp:509
rtabmap::RegistrationIcp::_libpointmatcherConfig
std::string _libpointmatcherConfig
Definition: RegistrationIcp.h:76
rtabmap::util3d::computeNormalsComplexity
float RTABMAP_EXP computeNormalsComplexity(const LaserScan &scan, const Transform &t=Transform::getIdentity(), cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
Definition: util3d_surface.cpp:2954
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:107
util3d_surface.h
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
rtabmap::util3d::adjustNormalsToViewPoint
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0), float groundNormalsUp=0.0f)
Definition: util3d_surface.cpp:3363
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())
Definition: util3d.cpp:1266
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00