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 
41 #ifdef RTABMAP_POINTMATCHER
42 #include <fstream>
43 #include "pointmatcher/PointMatcher.h"
44 typedef PointMatcher<float> PM;
45 typedef PM::DataPoints DP;
46 
47 DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
48 {
49  UDEBUG("");
50  typedef DP::Label Label;
51  typedef DP::Labels Labels;
52  typedef DP::View View;
53 
54  if (pclCloud->empty())
55  return DP();
56 
57  // fill labels
58  // conversions of descriptor fields from pcl
59  // see http://www.ros.org/wiki/pcl/Overview
60  Labels featLabels;
61  Labels descLabels;
62  std::vector<bool> isFeature;
63  featLabels.push_back(Label("x", 1));
64  isFeature.push_back(true);
65  featLabels.push_back(Label("y", 1));
66  isFeature.push_back(true);
67  if(!is2D)
68  {
69  featLabels.push_back(Label("z", 1));
70  isFeature.push_back(true);
71  }
72  featLabels.push_back(Label("pad", 1));
73 
74  // create cloud
75  DP cloud(featLabels, descLabels, pclCloud->size());
76  cloud.getFeatureViewByName("pad").setConstant(1);
77 
78  // fill cloud
79  View view(cloud.getFeatureViewByName("x"));
80  for(unsigned int i=0; i<pclCloud->size(); ++i)
81  {
82  view(0, i) = pclCloud->at(i).x;
83  view(1, i) = pclCloud->at(i).y;
84  if(!is2D)
85  {
86  view(2, i) = pclCloud->at(i).z;
87  }
88  }
89 
90  return cloud;
91 }
92 
93 DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
94 {
95  UDEBUG("");
96  typedef DP::Label Label;
97  typedef DP::Labels Labels;
98  typedef DP::View View;
99 
100  if (pclCloud->empty())
101  return DP();
102 
103  // fill labels
104  // conversions of descriptor fields from pcl
105  // see http://www.ros.org/wiki/pcl/Overview
106  Labels featLabels;
107  Labels descLabels;
108  std::vector<bool> isFeature;
109  featLabels.push_back(Label("x", 1));
110  isFeature.push_back(true);
111  featLabels.push_back(Label("y", 1));
112  isFeature.push_back(true);
113  if(!is2D)
114  {
115  featLabels.push_back(Label("z", 1));
116  isFeature.push_back(true);
117  }
118 
119  descLabels.push_back(Label("normals", 3));
120  isFeature.push_back(false);
121  isFeature.push_back(false);
122  isFeature.push_back(false);
123 
124  featLabels.push_back(Label("pad", 1));
125 
126  // create cloud
127  DP cloud(featLabels, descLabels, pclCloud->size());
128  cloud.getFeatureViewByName("pad").setConstant(1);
129 
130  // fill cloud
131  View view(cloud.getFeatureViewByName("x"));
132  View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
133  View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
134  View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
135  for(unsigned int i=0; i<pclCloud->size(); ++i)
136  {
137  view(0, i) = pclCloud->at(i).x;
138  view(1, i) = pclCloud->at(i).y;
139  if(!is2D)
140  {
141  view(2, i) = pclCloud->at(i).z;
142  }
143  viewNormalX(0, i) = pclCloud->at(i).normal_x;
144  viewNormalY(0, i) = pclCloud->at(i).normal_y;
145  viewNormalZ(0, i) = pclCloud->at(i).normal_z;
146  }
147 
148  return cloud;
149 }
150 
151 DP laserScanToDP(const rtabmap::LaserScan & scan)
152 {
153  UDEBUG("");
154  typedef DP::Label Label;
155  typedef DP::Labels Labels;
156  typedef DP::View View;
157 
158  if (scan.isEmpty())
159  return DP();
160 
161  // fill labels
162  // conversions of descriptor fields from pcl
163  // see http://www.ros.org/wiki/pcl/Overview
164  Labels featLabels;
165  Labels descLabels;
166  featLabels.push_back(Label("x", 1));
167  featLabels.push_back(Label("y", 1));
168  if(!scan.is2d())
169  {
170  featLabels.push_back(Label("z", 1));
171  }
172  featLabels.push_back(Label("pad", 1));
173 
174  if(scan.hasNormals())
175  {
176  descLabels.push_back(Label("normals", 3));
177  }
178  if(scan.hasIntensity())
179  {
180  descLabels.push_back(Label("intensity", 1));
181  }
182 
183  // create cloud
184  DP cloud(featLabels, descLabels, scan.size());
185  cloud.getFeatureViewByName("pad").setConstant(1);
186 
187  // fill cloud
188  int nx = scan.getNormalsOffset();
189  int ny = nx+1;
190  int nz = ny+1;
191  int offsetI = scan.getIntensityOffset();
192  bool hasLocalTransform = !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
193  View view(cloud.getFeatureViewByName("x"));
194  View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
195  View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
196  View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
197  View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
198  int oi = 0;
199  for(int i=0; i<scan.size(); ++i)
200  {
201  const float * ptr = scan.data().ptr<float>(0, i);
202 
203  if(uIsFinite(ptr[0]) && uIsFinite(ptr[1]) && (scan.is2d() || uIsFinite(ptr[2])))
204  {
205  if(hasLocalTransform)
206  {
207  if(nx == -1)
208  {
209  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
210  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
211  view(0, oi) = pt.x;
212  view(1, oi) = pt.y;
213  if(!scan.is2d())
214  {
215  view(2, oi) = pt.z;
216  }
217  if(offsetI!=-1)
218  {
219  viewIntensity(0, oi) = ptr[offsetI];
220  }
221  ++oi;
222  }
223  else if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
224  {
225  pcl::PointNormal pt;
226  pt.x=ptr[0];
227  pt.y=ptr[1];
228  pt.z=scan.is2d()?0:ptr[2];
229  pt.normal_x=ptr[nx];
230  pt.normal_y=ptr[ny];
231  pt.normal_z=ptr[nz];
232  pt = rtabmap::util3d::transformPoint(pt, scan.localTransform());
233  view(0, oi) = pt.x;
234  view(1, oi) = pt.y;
235  if(!scan.is2d())
236  {
237  view(2, oi) = pt.z;
238  }
239  viewNormalX(0, oi) = pt.normal_x;
240  viewNormalY(0, oi) = pt.normal_y;
241  viewNormalZ(0, oi) = pt.normal_z;
242 
243  if(offsetI!=-1)
244  {
245  viewIntensity(0, oi) = ptr[offsetI];
246  }
247 
248  ++oi;
249  }
250  else
251  {
252  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]);
253  }
254  }
255  else if(nx==-1 || (uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz])))
256  {
257  view(0, oi) = ptr[0];
258  view(1, oi) = ptr[1];
259  if(!scan.is2d())
260  {
261  view(2, oi) = ptr[2];
262  }
263  if(nx!=-1)
264  {
265  viewNormalX(0, oi) = ptr[nx];
266  viewNormalY(0, oi) = ptr[ny];
267  viewNormalZ(0, oi) = ptr[nz];
268  }
269  if(offsetI!=-1)
270  {
271  viewIntensity(0, oi) = ptr[offsetI];
272  }
273  ++oi;
274  }
275  else
276  {
277  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]);
278  }
279  }
280  else
281  {
282  UWARN("Ignoring point %d with invalid data: pos=%f %f %f", i, ptr[0], ptr[1], scan.is2d()?0:ptr[3]);
283  }
284 
285  }
286  if(oi != scan.size())
287  {
288  cloud.conservativeResize(oi);
289  }
290 
291  return cloud;
292 }
293 
294 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
295 {
296  UDEBUG("");
297  typedef DP::ConstView ConstView;
298 
299  if (cloud.features.cols() == 0)
300  return;
301 
302  pclCloud.resize(cloud.features.cols());
303  pclCloud.is_dense = true;
304 
305  // fill cloud
306  ConstView view(cloud.getFeatureViewByName("x"));
307  bool is3D = cloud.featureExists("z");
308  for(unsigned int i=0; i<pclCloud.size(); ++i)
309  {
310  pclCloud.at(i).x = view(0, i);
311  pclCloud.at(i).y = view(1, i);
312  pclCloud.at(i).z = is3D?view(2, i):0;
313  }
314 }
315 
316 void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
317 {
318  UDEBUG("");
319  typedef DP::ConstView ConstView;
320 
321  if (cloud.features.cols() == 0)
322  return;
323 
324  pclCloud.resize(cloud.features.cols());
325  pclCloud.is_dense = true;
326 
327  // fill cloud
328  ConstView view(cloud.getFeatureViewByName("x"));
329  bool is3D = cloud.featureExists("z");
330  ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
331  ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
332  ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
333  for(unsigned int i=0; i<pclCloud.size(); ++i)
334  {
335  pclCloud.at(i).x = view(0, i);
336  pclCloud.at(i).y = view(1, i);
337  pclCloud.at(i).z = is3D?view(2, i):0;
338  pclCloud.at(i).normal_x = viewNormalX(0, i);
339  pclCloud.at(i).normal_y = viewNormalY(0, i);
340  pclCloud.at(i).normal_z = viewNormalZ(0, i);
341  }
342 }
343 
344 template<typename T>
345 typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1)
346 {
347  typedef typename PointMatcher<T>::TransformationParameters M;
348  assert(matrix.rows() == matrix.cols());
349  assert((matrix.rows() == 3) || (matrix.rows() == 4));
350  assert((dimp1 == 3) || (dimp1 == 4));
351 
352  if (matrix.rows() == dimp1)
353  return matrix;
354 
355  M out(M::Identity(dimp1,dimp1));
356  out.topLeftCorner(2,2) = matrix.topLeftCorner(2,2);
357  out.topRightCorner(2,1) = matrix.topRightCorner(2,1);
358  return out;
359 }
360 
361 #endif
362 
363 namespace rtabmap {
364 
365 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
366  Registration(parameters, child),
367  _maxTranslation(Parameters::defaultIcpMaxTranslation()),
368  _maxRotation(Parameters::defaultIcpMaxRotation()),
369  _voxelSize(Parameters::defaultIcpVoxelSize()),
370  _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
371  _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
372  _maxIterations(Parameters::defaultIcpIterations()),
373  _epsilon(Parameters::defaultIcpEpsilon()),
374  _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
375  _pointToPlane(Parameters::defaultIcpPointToPlane()),
376  _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
377  _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
378  _pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
379  _libpointmatcher(Parameters::defaultIcpPM()),
380  _libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
381  _libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
382  _libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
383  _libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
384  _libpointmatcherICP(0)
385 {
386  this->parseParameters(parameters);
387 }
388 
389 RegistrationIcp::~RegistrationIcp()
390 {
391 #ifdef RTABMAP_POINTMATCHER
392  delete (PM::ICP*)_libpointmatcherICP;
393 #endif
394 }
395 
396 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
397 {
398  Registration::parseParameters(parameters);
399 
400  Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
401  Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
402  Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
403  Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
404  Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
405  Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
406  Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
407  Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
408  Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
409  Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
410  Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
411  Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
412  UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
413 
414  Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
415  Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
416  Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
417  Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
418  Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
419 
420 #ifndef RTABMAP_POINTMATCHER
421  if(_libpointmatcher)
422  {
423  UWARN("Parameter %s is set to true but RTAB-Map has not been built with libpointmatcher support. Setting to false.", Parameters::kIcpPM().c_str());
424  _libpointmatcher = false;
425  }
426 #else
427  if(_libpointmatcher)
428  {
429  UINFO("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
430  if(_libpointmatcherICP!=0)
431  {
432  delete (PM::ICP*)_libpointmatcherICP;
433  _libpointmatcherICP = 0;
434  }
435 
436  _libpointmatcherICP = new PM::ICP();
437 
438  PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
439 
440  bool useDefaults = true;
441  if(!_libpointmatcherConfig.empty())
442  {
443  // load YAML config
444  std::ifstream ifs(_libpointmatcherConfig.c_str());
445  if (ifs.good())
446  {
447  icp->loadFromYaml(ifs);
448  useDefaults = false;
449  }
450  else
451  {
452  UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str());
453  }
454  }
455  if(useDefaults)
456  {
457  // Create the default ICP algorithm
458  // See the implementation of setDefault() to create a custom ICP algorithm
459  icp->setDefault();
460 
461  icp->readingDataPointsFilters.clear();
462  icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
463 
464  icp->referenceDataPointsFilters.clear();
465  icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
466 
467  PM::Parameters params;
468  params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
469  params["knn"] = uNumber2Str(_libpointmatcherKnn);
470  params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
471  icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
472  params.clear();
473 
474  params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
475  icp->outlierFilters.clear();
476  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
477  params.clear();
478  if(_pointToPlane)
479  {
480  params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
481  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
482  params.clear();
483 
484  params["force2D"] = force3DoF()?"1":"0";
485  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
486  params.clear();
487  }
488  else
489  {
490  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
491  }
492 
493  icp->transformationCheckers.clear();
494  params["maxIterationCount"] = uNumber2Str(_maxIterations);
495  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params));
496  params.clear();
497 
498  params["minDiffRotErr"] = uNumber2Str(_epsilon*_epsilon*100.0f);
499  params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon);
500  params["smoothLength"] = uNumber2Str(4);
501  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
502  params.clear();
503 
504  params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
505  params["maxTranslationNorm"] = uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
506  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
507  params.clear();
508  }
509  }
510 #endif
511 
512  UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
513  UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
514  UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
515  UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
516  UASSERT(_epsilon >= 0.0f);
517  UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
518  UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
519 }
520 
521 Transform RegistrationIcp::computeTransformationImpl(
522  Signature & fromSignature,
523  Signature & toSignature,
524  Transform guess,
525  RegistrationInfo & info) const
526 {
527  UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
528  UDEBUG("Voxel size=%f", _voxelSize);
529  UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
530  UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
531  UDEBUG("Normal radius=%f", _pointToPlaneRadius);
532  UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
533  UDEBUG("Max Iterations=%d", _maxIterations);
534  UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
535  UDEBUG("Max translation=%f", _maxTranslation);
536  UDEBUG("Max rotation=%f", _maxRotation);
537  UDEBUG("Downsampling step=%d", _downsamplingStep);
538  UDEBUG("libpointmatcher=%d (knn=%d, outlier ratio=%f)", _libpointmatcher?1:0, _libpointmatcherKnn, _libpointmatcherOutlierRatio);
539 
540  UTimer timer;
541  std::string msg;
542  Transform transform;
543 
544  SensorData & dataFrom = fromSignature.sensorData();
545  SensorData & dataTo = toSignature.sensorData();
546 
547  UDEBUG("size from=%d (format=%d, max pts=%d) to=%d (format=%d, max pts=%d)",
548  dataFrom.laserScanRaw().size(),
549  (int)dataFrom.laserScanRaw().format(),
550  dataFrom.laserScanRaw().maxPoints(),
551  dataTo.laserScanRaw().size(),
552  (int)dataTo.laserScanRaw().format(),
553  dataTo.laserScanRaw().maxPoints());
554 
555  if(!guess.isNull() && !dataFrom.laserScanRaw().isEmpty() && !dataTo.laserScanRaw().isEmpty())
556  {
557  // ICP with guess transform
558  LaserScan fromScan = dataFrom.laserScanRaw();
559  LaserScan toScan = dataTo.laserScanRaw();
560  if(_downsamplingStep>1)
561  {
562  fromScan = util3d::downsample(fromScan, _downsamplingStep);
563  toScan = util3d::downsample(toScan, _downsamplingStep);
564  UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
565  }
566 
567  if(fromScan.size() && toScan.size())
568  {
569  Transform icpT;
570  bool hasConverged = false;
571  float correspondencesRatio = 0.0f;
572  int correspondences = 0;
573  double variance = 1.0;
574  bool transformComputed = false;
575  bool tooLowComplexityForPlaneToPlane = false;
576  cv::Mat complexityVectors;
577 
578  if( _pointToPlane &&
579  _voxelSize == 0.0f &&
580  fromScan.hasNormals() &&
581  toScan.hasNormals() &&
582  !((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D)
583  {
584  //special case if we have already normals computed and there is no filtering
585 
586  cv::Mat complexityVectorsFrom, complexityVectorsTo;
587  double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
588  double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
589  float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
590  info.icpStructuralComplexity = complexity;
591  if(complexity < _pointToPlaneMinComplexity)
592  {
593  tooLowComplexityForPlaneToPlane = true;
594  complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
595  UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
596  }
597  else
598  {
599  pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
600  pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
601 
602  fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
603  toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
604 
605 
606  UDEBUG("Conversion time = %f s", timer.ticks());
607  pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
608 #ifdef RTABMAP_POINTMATCHER
609  if(_libpointmatcher)
610  {
611  // Load point clouds
612  DP data = laserScanToDP(fromScan);
613  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess * toScan.localTransform()));
614 
615  // Compute the transformation to express data in ref
616  PM::TransformationParameters T;
617  try
618  {
619  UASSERT(_libpointmatcherICP != 0);
620  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
621  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
622  T = icp(data, ref);
623  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
624  UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
625 
626  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
627  UDEBUG("match ratio: %f", matchRatio);
628 
629  if(!icpT.isNull())
630  {
631  fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
632  hasConverged = true;
633  }
634  }
635  catch(const std::exception & e)
636  {
637  msg = uFormat("libpointmatcher has failed: %s", e.what());
638  }
639  }
640  else
641 #endif
642  {
643  icpT = util3d::icpPointToPlane(
644  fromCloudNormals,
645  toCloudNormals,
646  _maxCorrespondenceDistance,
647  _maxIterations,
648  hasConverged,
649  *fromCloudNormalsRegistered,
650  _epsilon,
651  this->force3DoF());
652  }
653 
654  if(!icpT.isNull() && hasConverged)
655  {
656  util3d::computeVarianceAndCorrespondences(
657  fromCloudNormalsRegistered,
658  toCloudNormals,
659  _maxCorrespondenceDistance,
660  _maxRotation,
661  variance,
662  correspondences);
663  }
664  transformComputed = true;
665  }
666  }
667 
668  int maxLaserScansFrom = fromScan.maxPoints();
669  int maxLaserScansTo = toScan.maxPoints();
670  if(!transformComputed)
671  {
672  pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
673  pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
674  UDEBUG("Conversion time = %f s", timer.ticks());
675 
676  pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
677  pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
678  if(_voxelSize > 0.0f)
679  {
680  float pointsBeforeFiltering = (float)fromCloudFiltered->size();
681  fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
682  float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
683  maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
684 
685  pointsBeforeFiltering = (float)toCloudFiltered->size();
686  toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
687  float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
688  maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
689 
690  UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
691  _voxelSize,
692  ratioFrom,
693  (int)fromCloudFiltered->size(),
694  maxLaserScansFrom,
695  ratioTo,
696  (int)toCloudFiltered->size(),
697  maxLaserScansTo,
698  timer.ticks());
699  }
700 
701  pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
702  if(_pointToPlane && // ICP Point To Plane
703  !tooLowComplexityForPlaneToPlane && // if previously rejected above
704  !((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
705  {
706  Eigen::Vector3f viewpointFrom(fromScan.localTransform().x(), fromScan.localTransform().y(), fromScan.localTransform().z());
707  pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
708  if(fromScan.is2d())
709  {
710  if(_voxelSize > 0.0f)
711  {
712  normalsFrom = util3d::computeNormals2D(
713  fromCloudFiltered,
714  _pointToPlaneK,
715  _pointToPlaneRadius,
716  viewpointFrom);
717  }
718  else
719  {
720  normalsFrom = util3d::computeFastOrganizedNormals2D(
721  fromCloudFiltered,
722  _pointToPlaneK,
723  _pointToPlaneRadius,
724  viewpointFrom);
725  }
726  }
727  else
728  {
729  normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
730  }
731 
732  Transform toT = guess * toScan.localTransform();
733  Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
734  pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
735  if(toScan.is2d())
736  {
737  if(_voxelSize > 0.0f)
738  {
739  normalsTo = util3d::computeNormals2D(
740  toCloudFiltered,
741  _pointToPlaneK,
742  _pointToPlaneRadius,
743  viewpointTo);
744  }
745  else
746  {
747  normalsTo = util3d::computeFastOrganizedNormals2D(
748  toCloudFiltered,
749  _pointToPlaneK,
750  _pointToPlaneRadius,
751  viewpointTo);
752  }
753  }
754  else
755  {
756  normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
757  }
758 
759  cv::Mat complexityVectorsFrom, complexityVectorsTo;
760  double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.is2d(), &complexityVectorsFrom);
761  double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.is2d(), &complexityVectorsTo);
762  float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
763  info.icpStructuralComplexity = complexity;
764  if(complexity < _pointToPlaneMinComplexity)
765  {
766  tooLowComplexityForPlaneToPlane = true;
767  complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
768  UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
769  }
770  else
771  {
772  pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
773  pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
774 
775  pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
776  pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
777 
778  std::vector<int> indices;
779  toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
780  fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
781 
782  // update output scans
783  if(fromScan.is2d())
784  {
785  fromSignature.sensorData().setLaserScanRaw(
786  LaserScan(
787  util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
788  maxLaserScansFrom,
789  fromScan.maxRange(),
790  LaserScan::kXYNormal,
791  fromScan.localTransform()));
792  }
793  else
794  {
795  fromSignature.sensorData().setLaserScanRaw(
796  LaserScan(
797  util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
798  maxLaserScansFrom,
799  fromScan.maxRange(),
800  LaserScan::kXYZNormal,
801  fromScan.localTransform()));
802  }
803  if(toScan.is2d())
804  {
805  toSignature.sensorData().setLaserScanRaw(
806  LaserScan(
807  util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
808  maxLaserScansTo,
809  toScan.maxRange(),
810  LaserScan::kXYNormal,
811  toScan.localTransform()));
812  }
813  else
814  {
815  toSignature.sensorData().setLaserScanRaw(
816  LaserScan(
817  util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
818  maxLaserScansTo,
819  toScan.maxRange(),
820  LaserScan::kXYZNormal,
821  toScan.localTransform()));
822  }
823  UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
824  fromScan = fromSignature.sensorData().laserScanRaw();
825  toScan = toSignature.sensorData().laserScanRaw();
826 
827  if(toCloudNormals->size() && fromCloudNormals->size())
828  {
829  pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
830 
831 #ifdef RTABMAP_POINTMATCHER
832  if(_libpointmatcher)
833  {
834  // Load point clouds
835  DP data = laserScanToDP(fromScan);
836  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
837 
838  // Compute the transformation to express data in ref
839  PM::TransformationParameters T;
840  try
841  {
842  UASSERT(_libpointmatcherICP != 0);
843  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
844  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
845  T = icp(data, ref);
846  UDEBUG("libpointmatcher icp...done!");
847  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
848 
849  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
850  UDEBUG("match ratio: %f", matchRatio);
851 
852  if(!icpT.isNull())
853  {
854  fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
855  hasConverged = true;
856  }
857  }
858  catch(const std::exception & e)
859  {
860  msg = uFormat("libpointmatcher has failed: %s", e.what());
861  }
862  }
863  else
864 #endif
865  {
866  icpT = util3d::icpPointToPlane(
867  fromCloudNormals,
868  toCloudNormals,
869  _maxCorrespondenceDistance,
870  _maxIterations,
871  hasConverged,
872  *fromCloudNormalsRegistered,
873  _epsilon,
874  this->force3DoF());
875  }
876 
877  if(!icpT.isNull() && hasConverged)
878  {
879  util3d::computeVarianceAndCorrespondences(
880  fromCloudNormalsRegistered,
881  toCloudNormals,
882  _maxCorrespondenceDistance,
883  _maxRotation,
884  variance,
885  correspondences);
886  }
887  }
888  transformComputed = true;
889  }
890  }
891 
892  if(!transformComputed) // ICP Point to Point
893  {
894  if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.is2d() || toScan.is2d()) && !_libpointmatcher))
895  {
896  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());
897  }
898 
899  if(_voxelSize > 0.0f || !tooLowComplexityForPlaneToPlane)
900  {
901  // update output scans
902  if(fromScan.is2d())
903  {
904  fromSignature.sensorData().setLaserScanRaw(
905  LaserScan(
906  util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
907  maxLaserScansFrom,
908  fromScan.maxRange(),
909  LaserScan::kXY,
910  fromScan.localTransform()));
911  }
912  else
913  {
914  fromSignature.sensorData().setLaserScanRaw(
915  LaserScan(
916  util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
917  maxLaserScansFrom,
918  fromScan.maxRange(),
919  LaserScan::kXYZ,
920  fromScan.localTransform()));
921  }
922  if(toScan.is2d())
923  {
924  toSignature.sensorData().setLaserScanRaw(
925  LaserScan(
926  util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
927  maxLaserScansTo,
928  toScan.maxRange(),
929  LaserScan::kXY,
930  toScan.localTransform()));
931  }
932  else
933  {
934  toSignature.sensorData().setLaserScanRaw(
935  LaserScan(
936  util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
937  maxLaserScansTo,
938  toScan.maxRange(),
939  LaserScan::kXYZ,
940  toScan.localTransform()));
941  }
942  fromScan = fromSignature.sensorData().laserScanRaw();
943  toScan = toSignature.sensorData().laserScanRaw();
944  }
945 
946 #ifdef RTABMAP_POINTMATCHER
947  if(_libpointmatcher)
948  {
949  // Load point clouds
950  DP data = laserScanToDP(fromScan);
951  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.maxRange(), toScan.format(), guess*toScan.localTransform()));
952 
953  // Compute the transformation to express data in ref
954  PM::TransformationParameters T;
955  try
956  {
957  UASSERT(_libpointmatcherICP != 0);
958  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
959  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
960  if(_pointToPlane)
961  {
962  // temporary set PointToPointErrorMinimizer
963  PM::ICP & icpTmp = icp;
964  icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
965 
966  for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
967  {
968  if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
969  {
970  iter = icpTmp.outlierFilters.erase(iter);
971  }
972  else
973  {
974  ++iter;
975  }
976  }
977 
978  T = icpTmp(data, ref);
979  }
980  else
981  {
982  T = icp(data, ref);
983  }
984  UDEBUG("libpointmatcher icp...done!");
985  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
986 
987  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
988  UDEBUG("match ratio: %f", matchRatio);
989 
990  if(!icpT.isNull())
991  {
992  fromCloudRegistered = util3d::transformPointCloud(fromCloudFiltered, icpT);
993  hasConverged = true;
994  }
995  }
996  catch(const std::exception & e)
997  {
998  msg = uFormat("libpointmatcher has failed: %s", e.what());
999  }
1000  }
1001  else
1002 #endif
1003  {
1004  icpT = util3d::icp(
1005  fromCloudFiltered,
1006  toCloudFiltered,
1007  _maxCorrespondenceDistance,
1008  _maxIterations,
1009  hasConverged,
1010  *fromCloudRegistered,
1011  _epsilon,
1012  this->force3DoF()); // icp2D
1013  }
1014 
1015  if(!icpT.isNull() && hasConverged)
1016  {
1017  if(tooLowComplexityForPlaneToPlane)
1018  {
1019  Transform guessInv = guess.inverse();
1020  Transform t = guessInv * icpT.inverse() * guess;
1021  Eigen::Vector3f v(t.x(), t.y(), t.z());
1022  if(complexityVectors.cols == 2)
1023  {
1024  // limit translation in direction of the first eigen vector
1025  Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
1026  float a = v.dot(n);
1027  v = n*a;
1028  }
1029  else if(complexityVectors.rows == 3)
1030  {
1031  // limit translation in direction of the first and second eigen vectors
1032  Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
1033  Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
1034  float a = v.dot(n1);
1035  float b = v.dot(n2);
1036  v = n1*a;
1037  v += n2*b;
1038  }
1039  else
1040  {
1041  UWARN("not supposed to be here!");
1042  v = Eigen::Vector3f(0,0,0);
1043  }
1044  float roll, pitch, yaw;
1045  t.getEulerAngles(roll, pitch, yaw);
1046  t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
1047  icpT = guess * t.inverse() * guessInv;
1048 
1049  if(fromScan.hasNormals() && toScan.hasNormals())
1050  {
1051  // we were using normals, so compute correspondences using normals
1052  pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
1053  pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
1054 
1055  util3d::computeVarianceAndCorrespondences(
1056  fromCloudNormalsRegistered,
1057  toCloudNormals,
1058  _maxCorrespondenceDistance,
1059  _maxRotation,
1060  variance,
1061  correspondences);
1062  }
1063  else
1064  {
1065  util3d::computeVarianceAndCorrespondences(
1066  fromCloudRegistered,
1067  toCloudFiltered,
1068  _maxCorrespondenceDistance,
1069  variance,
1070  correspondences);
1071  }
1072  }
1073  else
1074  {
1075  util3d::computeVarianceAndCorrespondences(
1076  fromCloudRegistered,
1077  toCloudFiltered,
1078  _maxCorrespondenceDistance,
1079  variance,
1080  correspondences);
1081  }
1082  }
1083  }
1084  }
1085  UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
1086 
1087  if(!icpT.isNull() && hasConverged)
1088  {
1089  float ix,iy,iz, iroll,ipitch,iyaw;
1090  Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
1091  icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
1092  info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
1093  info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
1094  if((_maxTranslation>0.0f &&
1095  info.icpTranslation > _maxTranslation)
1096  ||
1097  (_maxRotation>0.0f &&
1098  info.icpRotation > _maxRotation))
1099  {
1100  msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
1101  info.icpTranslation,
1102  info.icpRotation,
1103  _maxTranslation,
1104  _maxRotation);
1105  UINFO(msg.c_str());
1106  }
1107  else
1108  {
1109  // verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
1110  int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
1111  UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
1112 
1113  if(maxLaserScans)
1114  {
1115  correspondencesRatio = float(correspondences)/float(maxLaserScans);
1116  }
1117  else
1118  {
1119  static bool warningShown = false;
1120  if(!warningShown)
1121  {
1122  UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
1123  dataTo.id());
1124  warningShown = true;
1125  }
1126  correspondencesRatio = float(correspondences)/float(toScan.size()>fromScan.size()?toScan.size():fromScan.size());
1127  }
1128 
1129  variance/=10.0;
1130 
1131  UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
1132  dataTo.id(), dataFrom.id(),
1133  hasConverged?"true":"false",
1134  variance,
1135  correspondences,
1136  maxLaserScans>0?maxLaserScans:(int)(toScan.size()>fromScan.size()?toScan.size():fromScan.size()),
1137  correspondencesRatio*100.0f,
1138  info.icpTranslation,
1139  info.icpRotation);
1140 
1141  if(correspondences == 0)
1142  {
1143  UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
1144  }
1145  else
1146  {
1147  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
1148  }
1149  info.icpInliersRatio = correspondencesRatio;
1150 
1151  if(correspondencesRatio <= _correspondenceRatio)
1152  {
1153  msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
1154  correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
1155  UINFO(msg.c_str());
1156  }
1157  else
1158  {
1159  transform = icpT.inverse()*guess;
1160  }
1161  }
1162  }
1163  else
1164  {
1165  if(msg.empty())
1166  {
1167  msg = uFormat("Cannot compute transform (converged=%s var=%f)",
1168  hasConverged?"true":"false", variance);
1169  }
1170  UINFO(msg.c_str());
1171  }
1172  }
1173  else
1174  {
1175  msg = "Laser scans empty ?!?";
1176  UWARN(msg.c_str());
1177  }
1178  }
1179  else if(dataTo.isValid())
1180  {
1181  if(guess.isNull())
1182  {
1183  msg = "RegistrationIcp cannot do registration with a null guess.";
1184  }
1185  else
1186  {
1187  msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
1188  dataTo.id(), dataTo.laserScanRaw().size(),
1189  dataFrom.id(), dataFrom.laserScanRaw().size());
1190  }
1191  UERROR(msg.c_str());
1192  }
1193 
1194 
1195  info.rejectedMsg = msg;
1196 
1197  UDEBUG("New transform = %s", transform.prettyPrint().c_str());
1198  return transform;
1199 }
1200 
1201 }
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
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:446
UTimer
Definition: UTimer.h:46
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
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:2359
rtabmap::RegistrationIcp::RegistrationIcp
RegistrationIcp(const ParametersMap &parameters=ParametersMap(), Registration *child=0)
Definition: RegistrationIcp.cpp:365
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
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:89
f
f
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
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:373
glm::e
GLM_FUNC_DECL genType e()
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
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:240
rtabmap::Registration::force3DoF
bool force3DoF() const
Definition: Registration.h:68
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::LaserScan::getNormalsOffset
int getNormalsOffset() const
Definition: LaserScan.h:81
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
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:2471
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
util3d_registration.h
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:317
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:134
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
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:211
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:335
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:67
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
util3d_transforms.h
rtabmap::LaserScan::getIntensityOffset
int getIntensityOffset() const
Definition: LaserScan.h:79
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:388
rtabmap::util3d::computeNormalsComplexity
float RTABMAP_EXP computeNormalsComplexity(const LaserScan &scan, cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
Definition: util3d_surface.cpp:2536
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
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:763
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::RegistrationIcp::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
Definition: RegistrationIcp.cpp:521
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:407
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:206
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:396
rtabmap::RegistrationIcp::_libpointmatcherConfig
std::string _libpointmatcherConfig
Definition: RegistrationIcp.h:72
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
util3d_surface.h
rtabmap::SensorData::setLaserScanRaw
void setLaserScanRaw(const LaserScan &laserScanRaw)
Definition: SensorData.h:166
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32