RegistrationIcp.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
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 <rtabmap/utilite/UDirectory.h>
40 #include <pcl/conversions.h>
41 #include <pcl/common/pca.h>
42 #include <pcl/common/io.h>
43 #include <pcl/io/pcd_io.h>
44 #include <pcl/io/ply_io.h>
45 #include <pcl/io/vtk_io.h>
46 
47 #ifdef RTABMAP_CCCORELIB
48 #include "icp/cccorelib.h"
49 #endif
50 
51 #ifdef RTABMAP_POINTMATCHER
52 #include "icp/libpointmatcher.h"
53 #endif
54 
55 namespace rtabmap {
56 
57 RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
58  Registration(parameters, child),
59  _strategy(Parameters::defaultIcpStrategy()),
60  _maxTranslation(Parameters::defaultIcpMaxTranslation()),
61  _maxRotation(Parameters::defaultIcpMaxRotation()),
62  _voxelSize(Parameters::defaultIcpVoxelSize()),
63  _downsamplingStep(Parameters::defaultIcpDownsamplingStep()),
64  _rangeMin(Parameters::defaultIcpRangeMin()),
65  _rangeMax(Parameters::defaultIcpRangeMax()),
66  _maxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
67  _maxIterations(Parameters::defaultIcpIterations()),
68  _epsilon(Parameters::defaultIcpEpsilon()),
69  _correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
70  _force4DoF(Parameters::defaultIcpForce4DoF()),
71  _pointToPlane(Parameters::defaultIcpPointToPlane()),
72  _pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
73  _pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
74  _pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
75  _pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
76  _pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
77  _libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
78  _libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
79  _libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
80  _libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
81  _outlierRatio(Parameters::defaultIcpOutlierRatio()),
82  _ccSamplingLimit (Parameters::defaultIcpCCSamplingLimit()),
83  _ccFilterOutFarthestPoints (Parameters::defaultIcpCCFilterOutFarthestPoints()),
84  _ccMaxFinalRMS (Parameters::defaultIcpCCMaxFinalRMS()),
85  _debugExportFormat(Parameters::defaultIcpDebugExportFormat()),
86  _libpointmatcherICP(0),
87  _libpointmatcherICPFilters(0)
88 {
89  this->parseParameters(parameters);
90 }
91 
92 RegistrationIcp::~RegistrationIcp()
93 {
94 #ifdef RTABMAP_POINTMATCHER
95  delete (PM::ICP*)_libpointmatcherICP;
96  delete (PM::ICP*)_libpointmatcherICPFilters;
97 #endif
98 }
99 
100 void RegistrationIcp::parseParameters(const ParametersMap & parameters)
101 {
102  Registration::parseParameters(parameters);
103 
104  Parameters::parse(parameters, Parameters::kIcpStrategy(), _strategy);
105  Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
106  Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
107  Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
108  Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _downsamplingStep);
109  Parameters::parse(parameters, Parameters::kIcpRangeMin(), _rangeMin);
110  Parameters::parse(parameters, Parameters::kIcpRangeMax(), _rangeMax);
111  Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _maxCorrespondenceDistance);
112  Parameters::parse(parameters, Parameters::kIcpIterations(), _maxIterations);
113  Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
114  Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
115  Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF);
116  Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio);
117  Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
118  Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
119  Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
120  Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
121  Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
122  Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
123  UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
124  UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
125 
126  Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
127  Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
128  Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
129  Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);
130 
131  Parameters::parse(parameters, Parameters::kIcpCCSamplingLimit(), _ccSamplingLimit);
132  Parameters::parse(parameters, Parameters::kIcpCCFilterOutFarthestPoints(), _ccFilterOutFarthestPoints);
133  Parameters::parse(parameters, Parameters::kIcpCCMaxFinalRMS(), _ccMaxFinalRMS);
134 
135  Parameters::parse(parameters, Parameters::kIcpDebugExportFormat(), _debugExportFormat);
136  ParametersMap::const_iterator iter;
137  if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
138  {
139  _workingDir = uReplaceChar(iter->second, '~', UDirectory::homeDir());
140  }
141 
142  bool pointToPlane = _pointToPlane;
143 
144 #ifndef RTABMAP_POINTMATCHER
145  if(_strategy==1)
146  {
147  UWARN("Parameter %s is set to 1 but RTAB-Map has not been built with libpointmatcher support. Setting to 0.", Parameters::kIcpStrategy().c_str());
148  _strategy = 0;
149  }
150 #else
151  delete (PM::ICP*)_libpointmatcherICP;
152  delete (PM::ICP*)_libpointmatcherICPFilters;
153  _libpointmatcherICP = 0;
154  _libpointmatcherICPFilters = 0;
155  if(_strategy==1)
156  {
157  _libpointmatcherConfig = uReplaceChar(_libpointmatcherConfig, '~', UDirectory::homeDir());
158  UDEBUG("libpointmatcher enabled! config=\"%s\"", _libpointmatcherConfig.c_str());
159  _libpointmatcherICP = new PM::ICP();
160  PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
161 
162  bool useDefaults = true;
163  if(!_libpointmatcherConfig.empty())
164  {
165  // load YAML config
166  std::ifstream ifs(_libpointmatcherConfig.c_str());
167  if (ifs.good())
168  {
169  try {
170  icp->loadFromYaml(ifs);
171  useDefaults = false;
172 
173  _libpointmatcherICPFilters = new PM::ICP();
174  PM::ICP * icpFilters = (PM::ICP*)_libpointmatcherICPFilters;
175  icpFilters->readingDataPointsFilters = icp->readingDataPointsFilters;
176  icpFilters->referenceDataPointsFilters = icp->referenceDataPointsFilters;
177 
178  icp->readingDataPointsFilters.clear();
179  icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
180 
181  icp->referenceDataPointsFilters.clear();
182  icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
183  }
184  catch (const std::exception & e)
185  {
186  UFATAL("Error reading libpointmatcher config file \"%s\": %s", _libpointmatcherConfig.c_str(), e.what());
187  }
188  }
189  else
190  {
191  UERROR("Cannot open libpointmatcher config file \"%s\", using default values instead.", _libpointmatcherConfig.c_str());
192  }
193  }
194  if(useDefaults)
195  {
196  // Create the default ICP algorithm
197  // See the implementation of setDefault() to create a custom ICP algorithm
198  icp->setDefault();
199 
200  icp->readingDataPointsFilters.clear();
201  icp->readingDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
202 
203  icp->referenceDataPointsFilters.clear();
204  icp->referenceDataPointsFilters.push_back(PM::get().DataPointsFilterRegistrar.create("IdentityDataPointsFilter"));
205 
206  PM::Parameters params;
207  params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
208  params["knn"] = uNumber2Str(_libpointmatcherKnn);
209  params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
210 
211  if(_libpointmatcherIntensity)
212  {
213  icp->matcher.reset(new KDTreeMatcherIntensity<float>(params));
214  }
215  else
216  {
217 #if POINTMATCHER_VERSION_INT >= 10300
218  icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
219 #else
220  icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
221 #endif
222  }
223  params.clear();
224 
225  params["ratio"] = uNumber2Str(_outlierRatio);
226  icp->outlierFilters.clear();
227  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
228  params.clear();
229  if(_pointToPlane)
230  {
231  params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
232  icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
233  params.clear();
234 
235  params["force2D"] = force3DoF()?"1":"0";
236 
237  if(!force3DoF()&&_force4DoF)
238  {
239  params["force4DOF"] = "1";
240  }
241 #if POINTMATCHER_VERSION_INT >= 10300
242  icp->errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params);
243 #else
244  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
245 #endif
246  params.clear();
247  }
248  else
249  {
250 #if POINTMATCHER_VERSION_INT >= 10300
251  icp->errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
252 #else
253  icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
254 #endif
255  }
256 
257  icp->transformationCheckers.clear();
258  params["maxIterationCount"] = uNumber2Str(_maxIterations);
259  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker", params));
260  params.clear();
261 
262  params["minDiffRotErr"] = uNumber2Str(_epsilon*_epsilon*100.0f);
263  params["minDiffTransErr"] = uNumber2Str(_epsilon*_epsilon);
264  params["smoothLength"] = uNumber2Str(4);
265  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
266  params.clear();
267 
268  params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
269  params["maxTranslationNorm"] = uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
270  icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
271  params.clear();
272  }
273  pointToPlane = icp->errorMinimizer->className.compare("PointToPlaneErrorMinimizer")==0;
274  }
275 #endif
276 
277 #ifndef RTABMAP_CCCORELIB
278  if(_strategy==2)
279  {
280 #ifdef RTABMAP_POINTMATCHER
281  _strategy = 1;
282 #else
283  _strategy = 0;
284 #endif
285  UWARN("Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().c_str(), _strategy);
286  }
287 #else
288  if(_strategy==2 && _pointToPlane)
289  {
290  UWARN("%s cannot be used with %s=2 (CCCoreLib), setting %s to false", Parameters::kIcpPointToPlane().c_str(), Parameters::kIcpStrategy().c_str(), Parameters::kIcpPointToPlane().c_str());
291  _pointToPlane = false;
292  }
293 #endif
294 
295  if(_force4DoF && _strategy == 0)
296  {
297  UWARN("%s cannot be used with %s == 0.", Parameters::kIcpForce4DoF().c_str(), Parameters::kIcpStrategy().c_str());
298  _force4DoF = false;
299  }
300 
301  UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
302  UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
303  UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
304  UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
305  UASSERT(_epsilon >= 0.0f);
306  UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
307  UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f || pointToPlane)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
308 }
309 
310 Transform RegistrationIcp::computeTransformationImpl(
311  Signature & fromSignature,
312  Signature & toSignature,
313  Transform guess,
314  RegistrationInfo & info) const
315 {
316  bool pointToPlane = _pointToPlane;
317 #ifdef RTABMAP_POINTMATCHER
318  if(_strategy==1)
319  {
320  PM::ICP * icp = (PM::ICP*)_libpointmatcherICP;
321  pointToPlane = icp->errorMinimizer->className.compare("PointToPlaneErrorMinimizer")==0;
322  }
323 #endif
324 
325  UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
326  UDEBUG("Voxel size=%f", _voxelSize);
327  UDEBUG("PointToPlane=%d", pointToPlane?1:0);
328  UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
329  UDEBUG("Normal radius=%f", _pointToPlaneRadius);
330  UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
331  UDEBUG("Max Iterations=%d", _maxIterations);
332  UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
333  UDEBUG("Max translation=%f", _maxTranslation);
334  UDEBUG("Max rotation=%f", _maxRotation);
335  UDEBUG("Downsampling step=%d", _downsamplingStep);
336  UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
337  UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
338  UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
339  UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
340  UDEBUG("Strategy=%d", _strategy);
341 
342  UTimer timer;
343  std::string msg;
344  Transform transform;
345 
346  SensorData & dataFrom = fromSignature.sensorData();
347  SensorData & dataTo = toSignature.sensorData();
348 
349  UDEBUG("size before filtering, from=%d (format=%s, max pts=%d) to=%d (format=%s, max pts=%d)",
350  dataFrom.laserScanRaw().size(),
351  dataFrom.laserScanRaw().formatName().c_str(),
352  dataFrom.laserScanRaw().maxPoints(),
353  dataTo.laserScanRaw().size(),
354  dataTo.laserScanRaw().formatName().c_str(),
355  dataTo.laserScanRaw().maxPoints());
356 
357  // Do the filtering
358  int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size();
359  int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size();
360 
361  if(!dataFrom.laserScanRaw().empty())
362  {
363  int pointsBeforeFiltering = dataFrom.laserScanRaw().size();
364  LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(),
365  _downsamplingStep,
366  _rangeMin,
367  _rangeMax,
368  _voxelSize,
369  pointToPlane?_pointToPlaneK:0,
370  pointToPlane?_pointToPlaneRadius:0.0f,
371  pointToPlane?_pointToPlaneGroundNormalsUp:0.0f);
372 #ifdef RTABMAP_POINTMATCHER
373  if(_strategy==1 && _libpointmatcherICPFilters)
374  {
375  PM::ICP & filters = *((PM::ICP*)_libpointmatcherICPFilters);
376  UDEBUG("icp.referenceDataPointsFilters.size()=%d", (int)filters.referenceDataPointsFilters.size());
377  if(filters.referenceDataPointsFilters.size()>1 ||
378  (filters.referenceDataPointsFilters.size() == 1 && filters.referenceDataPointsFilters[0]->className.compare("IdentityDataPointsFilter")!=0))
379  {
380  try {
381  DP data = laserScanToDP(fromScan, true); // ignore local transform to make sure viewpoint is 0,0,0
382  filters.referenceDataPointsFilters.apply(data);
383  rtabmap::LaserScan pmFromScan = laserScanFromDP(data);
384  fromScan = LaserScan(
385  pmFromScan,
386  fromScan.maxPoints(),
387  fromScan.rangeMax(),
388  fromScan.localTransform());
389  }
390  catch(const std::exception & e)
391  {
392  msg = uFormat("libpointmatcher's data filters have failed: %s", e.what());
393  UERROR("%s", msg.c_str());
394  }
395  }
396  }
397 #endif
398  dataFrom.setLaserScan(fromScan);
399  float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering);
400  maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio);
401  }
402  if(!dataTo.laserScanRaw().empty())
403  {
404  int pointsBeforeFiltering = dataTo.laserScanRaw().size();
405  LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(),
406  _downsamplingStep,
407  _rangeMin,
408  _rangeMax,
409  _voxelSize,
410  pointToPlane?_pointToPlaneK:0,
411  pointToPlane?_pointToPlaneRadius:0.0f,
412  pointToPlane?_pointToPlaneGroundNormalsUp:0.0f);
413 #ifdef RTABMAP_POINTMATCHER
414  if(_strategy == 1 && _libpointmatcherICPFilters)
415  {
416  PM::ICP & filters = *((PM::ICP*)_libpointmatcherICPFilters);
417  UDEBUG("icp.readingDataPointsFilters.size()=%d", (int)filters.readingDataPointsFilters.size());
418  if(filters.readingDataPointsFilters.size()>1 ||
419  (filters.readingDataPointsFilters.size() == 1 && filters.readingDataPointsFilters[0]->className.compare("IdentityDataPointsFilter")!=0))
420  {
421  try {
422  DP data = laserScanToDP(toScan, true); // ignore local transform to make sure viewpoint is 0,0,0
423  filters.readingDataPointsFilters.apply(data);
424  rtabmap::LaserScan pmToScan = laserScanFromDP(data);
425  toScan = LaserScan(
426  pmToScan,
427  toScan.maxPoints(),
428  toScan.rangeMax(),
429  toScan.localTransform());
430  }
431  catch(const std::exception & e)
432  {
433  msg = uFormat("libpointmatcher's data filters have failed: %s", e.what());
434  UERROR("%s", msg.c_str());
435  }
436  }
437  }
438 #endif
439  dataTo.setLaserScan(toScan);
440  float ratio = float(dataTo.laserScanRaw().size()) / float(pointsBeforeFiltering);
441  maxLaserScansTo = int(float(maxLaserScansTo) * ratio);
442  }
443 
444  UDEBUG("size after filtering, from=%d (format=%s, max pts=%d) to=%d (format=%s, max pts=%d), filtering time=%fs",
445  dataFrom.laserScanRaw().size(),
446  dataFrom.laserScanRaw().formatName().c_str(),
447  dataFrom.laserScanRaw().maxPoints(),
448  dataTo.laserScanRaw().size(),
449  dataTo.laserScanRaw().formatName().c_str(),
450  dataTo.laserScanRaw().maxPoints(),
451  timer.ticks());
452 
453 
454  if(!guess.isNull() && !dataFrom.laserScanRaw().isEmpty() && !dataTo.laserScanRaw().isEmpty())
455  {
456  // ICP with guess transform
457  LaserScan fromScan = dataFrom.laserScanRaw();
458  LaserScan toScan = dataTo.laserScanRaw();
459 
460  if(fromScan.size() && toScan.size())
461  {
462  std::string toPrefix = "rtabmap_icp_scan";
463  double now = UTimer::now();
464  if(!_workingDir.empty() && !_debugExportFormat.empty())
465  {
466  std::string fromPrefix = "rtabmap_icp_scan";
467  if(ULogger::level() == ULogger::kDebug)
468  {
469  fromPrefix+=uReplaceChar(uFormat("_%.3f_from_%d", fromSignature.id(), now), '.', '_');
470  toPrefix+=uReplaceChar(uFormat("_%.3f_to_%d", toSignature.id(), now), '.', '_');
471  }
472  else
473  {
474  fromPrefix+="_from";
475  toPrefix+="_to";
476  }
477  if(_debugExportFormat.compare("vtk")==0)
478  {
479  pcl::io::saveVTKFile(_workingDir+"/"+fromPrefix+".vtk", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()));
480  pcl::io::saveVTKFile(_workingDir+"/"+toPrefix+".vtk", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()));
481  UWARN("Saved %s.vtk and %s.vtk (%s=\"%s\") to working directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(), _workingDir.c_str());
482  }
483  else if(_debugExportFormat.compare("ply")==0)
484  {
485  pcl::io::savePLYFile(_workingDir+"/"+fromPrefix+".ply", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
486  pcl::io::savePLYFile(_workingDir+"/"+toPrefix+".ply", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
487  UWARN("Saved %s.ply and %s.ply (%s=\"%s\") to directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(), _workingDir.c_str());
488  }
489  else //pcd
490  {
491  pcl::io::savePCDFile(_workingDir+"/"+fromPrefix+".pcd", *util3d::laserScanToPointCloud2(fromScan, fromScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
492  pcl::io::savePCDFile(_workingDir+"/"+toPrefix+".pcd", *util3d::laserScanToPointCloud2(toScan, guess*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
493  UWARN("Saved %s.pcd and %s.pcd (%s=\"%s\") to working directory (%s=%s)", fromPrefix.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str(), Parameters::kRtabmapWorkingDirectory().c_str(), _workingDir.c_str());
494  }
495  }
496  else if(!_debugExportFormat.empty())
497  {
498  UWARN("%s is enabled but %s is not, cannot export debug scans.", Parameters::kIcpDebugExportFormat().c_str(), Parameters::kRtabmapWorkingDirectory().c_str());
499  }
500 
501  bool tooLowComplexityForPlaneToPlane = false;
502  float secondEigenValue = 1.0f;
503  cv::Mat complexityVectors;
504 
505  if(pointToPlane && ((fromScan.is2d() || toScan.is2d()) && _strategy==0))
506  {
507  UWARN("ICP PointToPlane ignored for 2d scans with PCL registration "
508  "(some crash issues). Use libpointmatcher (%s=1) or disable %s "
509  "to avoid this warning.",
510  Parameters::kIcpStrategy().c_str(),
511  Parameters::kIcpPointToPlane().c_str());
512  pointToPlane = false;
513  }
514 
515  // If we do PointToPlane, determinate if there is
516  // enough structural complexity. If not, fall back to PointToPoint ICP.
517  if( pointToPlane &&
518  fromScan.hasNormals() &&
519  toScan.hasNormals())
520  {
521  cv::Mat complexityVectorsFrom, complexityVectorsTo;
522  cv::Mat complexityValuesFrom, complexityValuesTo;
523  double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
524  double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
525  float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
526  info.icpStructuralComplexity = complexity;
527  if(complexity < _pointToPlaneMinComplexity)
528  {
529  tooLowComplexityForPlaneToPlane = true;
530  pointToPlane = false;
531  if(complexity > 0.0f)
532  {
533  complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
534 
535  UASSERT((complexityVectors.rows == 2 && complexityVectors.cols == 2)||
536  (complexityVectors.rows == 3 && complexityVectors.cols == 3));
537  secondEigenValue = complexityValuesFrom.at<float>(1,0)<complexityValuesTo.at<float>(1,0)?complexityValuesFrom.at<float>(1,0):complexityValuesTo.at<float>(1,0);
538  UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): (from=%f || to=%f) < %f (%s). Second eigen value=%f. "
539  "PointToPoint is done instead, orientation is still optimized but translation will be limited to "
540  "direction of normals (%s: %s).",
541  fromComplexity, toComplexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str(),
542  secondEigenValue,
543  fromComplexity<toComplexity?"From":"To",
544  complexityVectors.rows==2?
545  uFormat("n=%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1)).c_str():
546  secondEigenValue<_pointToPlaneMinComplexity?
547  uFormat("n=%f,%f,%f", complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2)).c_str():
548  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());
549  }
550  else
551  {
552  UWARN("ICP PointToPlane ignored as structural complexity cannot be computed (from=%f to=%f)!? PointToPoint is done instead.", fromComplexity, toComplexity);
553  }
554  if(ULogger::level() == ULogger::kDebug)
555  {
556  std::cout << "complexityVectorsFrom = " << std::endl << complexityVectorsFrom << std::endl;
557  std::cout << "complexityValuesFrom = " << std::endl << complexityValuesFrom << std::endl;
558  std::cout << "complexityVectorsTo = " << std::endl << complexityVectorsTo << std::endl;
559  std::cout << "complexityValuesTo = " << std::endl << complexityValuesTo << std::endl;
560  }
561 
562  // If the complexity is too low and we don't want to compute transform in this case
563  if(_pointToPlaneLowComplexityStrategy == 0)
564  {
565  msg = uFormat("Rejecting transform because too low complexity %f (%s=0)",
566  info.icpStructuralComplexity,
567  Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
568  UWARN(msg.c_str());
569  info.rejectedMsg = msg;
570  return Transform();
571  }
572  }
573  }
574  else
575  {
576  pointToPlane = false;
577  }
578 
579  Transform icpT;
580  bool hasConverged = false;
581  float correspondencesRatio = 0.0f;
582  int correspondences = 0;
583  double variance = 1.0;
584 
586  // Point To Plane
588  if(pointToPlane)
589  {
590  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudINormal(fromScan, fromScan.localTransform());
591  pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());
592 
593  fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
594  toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
595 
596  UDEBUG("Conversion time = %f s", timer.ticks());
597  pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
598 #ifdef RTABMAP_POINTMATCHER
599  if(_strategy==1)
600  {
601  // Load point clouds
602  DP data = laserScanToDP(fromScan);
603  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess * toScan.localTransform()));
604 
605  // Compute the transformation to express data in ref
606  PM::TransformationParameters T;
607  try
608  {
609  UASSERT(_libpointmatcherICP != 0);
610  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
611  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
612  T = icp(data, ref);
613  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
614  UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
615 
616  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
617  UDEBUG("match ratio: %f", matchRatio);
618 
619  if(!icpT.isNull())
620  {
621  fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
622  hasConverged = true;
623  }
624  }
625  catch(const std::exception & e)
626  {
627  msg = uFormat("libpointmatcher has failed: %s", e.what());
628  }
629  }
630  else
631 #endif
632  {
633  icpT = util3d::icpPointToPlane(
634  fromCloudNormals,
635  toCloudNormals,
636  _maxCorrespondenceDistance,
637  _maxIterations,
638  hasConverged,
639  *fromCloudNormalsRegistered,
640  _epsilon,
641  this->force3DoF());
642  }
643 
644  if(!icpT.isNull() && hasConverged)
645  {
646  util3d::computeVarianceAndCorrespondences(
647  fromCloudNormalsRegistered,
648  toCloudNormals,
649  _maxCorrespondenceDistance,
650  _maxRotation,
651  variance,
652  correspondences);
653  }
654  }
656  // Point To Point
658  else
659  {
660  pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloud = util3d::laserScanToPointCloudI(fromScan, fromScan.localTransform());
661  pcl::PointCloud<pcl::PointXYZI>::Ptr toCloud = util3d::laserScanToPointCloudI(toScan, guess * toScan.localTransform());
662  UDEBUG("Conversion time = %f s", timer.ticks());
663 
664  pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZI>());
665 
666 #ifdef RTABMAP_POINTMATCHER
667  if(_strategy==1)
668  {
669  // Load point clouds
670  DP data = laserScanToDP(fromScan);
671  DP ref = laserScanToDP(LaserScan(toScan.data(), toScan.maxPoints(), toScan.rangeMax(), toScan.format(), guess*toScan.localTransform()));
672 
673  // Compute the transformation to express data in ref
674  PM::TransformationParameters T;
675  try
676  {
677  UASSERT(_libpointmatcherICP != 0);
678  PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
679  UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
680  if(tooLowComplexityForPlaneToPlane)
681  {
682  // temporary set PointToPointErrorMinimizer
683  PM::ICP icpTmp = icp;
684 
685  PM::Parameters params;
686  params["maxDist"] = uNumber2Str(_voxelSize>0?_voxelSize:_maxCorrespondenceDistance/2.0f);
687  UWARN("libpointmatcher icp...temporary maxDist=%s (%s=%f, %s=%f)", params["maxDist"].c_str(), Parameters::kIcpMaxCorrespondenceDistance().c_str(), _maxCorrespondenceDistance, Parameters::kIcpVoxelSize().c_str(), _voxelSize);
688  params["knn"] = uNumber2Str(_libpointmatcherKnn);
689  params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
690  if(_libpointmatcherIntensity)
691  {
692  icpTmp.matcher.reset(new KDTreeMatcherIntensity<float>(params));
693  }
694  else
695  {
696 #if POINTMATCHER_VERSION_INT >= 10300
697  icpTmp.matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
698 #else
699  icpTmp.matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
700 #endif
701  }
702 
703 #if POINTMATCHER_VERSION_INT >= 10300
704  icpTmp.errorMinimizer = PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer");
705 #else
706  icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
707 #endif
708  for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
709  {
710  if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
711  {
712  iter = icpTmp.outlierFilters.erase(iter);
713  }
714  else
715  {
716  ++iter;
717  }
718  }
719 
720  T = icpTmp(data, ref);
721  }
722  else
723  {
724  T = icp(data, ref);
725  }
726  UDEBUG("libpointmatcher icp...done!");
727  icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
728 
729  float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
730  UDEBUG("match ratio: %f", matchRatio);
731 
732  if(!icpT.isNull())
733  {
734  hasConverged = true;
735  }
736  }
737  catch(const std::exception & e)
738  {
739  msg = uFormat("libpointmatcher has failed: %s", e.what());
740  }
741  }
742  else
743 #endif
744  {
745 #ifdef RTABMAP_CCCORELIB
746  if(_strategy==2)
747  {
748  icpT = icpCC(
749  fromCloud,
750  toCloud,
751  _maxIterations,
752  _epsilon,
753  this->force3DoF(),
754  _force4DoF,
755  _ccSamplingLimit,
756  _outlierRatio,
757  _ccFilterOutFarthestPoints,
758  _ccMaxFinalRMS,
759  &info.icpRMS,
760  &msg);
761  hasConverged = !icpT.isNull();
762  }
763  else
764 #endif
765  {
766  icpT = util3d::icp(
767  fromCloud,
768  toCloud,
769  _maxCorrespondenceDistance,
770  _maxIterations,
771  hasConverged,
772  *fromCloudRegistered,
773  _epsilon,
774  this->force3DoF()); // icp2D
775  }
776  }
777 
778  if(!icpT.isNull() && hasConverged)
779  {
780  if(tooLowComplexityForPlaneToPlane)
781  {
782  if(_pointToPlaneLowComplexityStrategy == 1)
783  {
784  Transform guessInv = guess.inverse();
785  Transform t = guessInv * icpT.inverse() * guess;
786  Eigen::Vector3f v(t.x(), t.y(), t.z());
787  if(complexityVectors.cols == 2)
788  {
789  // limit translation in direction of the first eigen vector
790  Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
791  float a = v.dot(n);
792  Eigen::Vector3f vp = n*a;
793  UWARN("Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
794  info.icpStructuralComplexity,
795  v[0], v[1], vp[0], vp[1]);
796  v= vp;
797  }
798  else if(complexityVectors.rows == 3)
799  {
800  // limit translation in direction of the first and second eigen vectors
801  Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
802  Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
803  float a = v.dot(n1);
804  float b = v.dot(n2);
805  Eigen::Vector3f vp = n1*a;
806  if(secondEigenValue >= _pointToPlaneMinComplexity)
807  {
808  vp += n2*b;
809  }
810  UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
811  v[0], v[1], v[2], vp[0], vp[1], vp[2]);
812  v = vp;
813  }
814  else
815  {
816  UWARN("not supposed to be here!");
817  v = Eigen::Vector3f(0,0,0);
818  }
819  float roll, pitch, yaw;
820  t.getEulerAngles(roll, pitch, yaw);
821  t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
822  icpT = guess * t.inverse() * guessInv;
823 
824  fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
825  }
826  else
827  {
828  UWARN("Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
829  info.icpStructuralComplexity,
830  Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
831  if(fromCloudRegistered->empty())
832  fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
833  }
834  }
835  else if(fromCloudRegistered->empty())
836  fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
837 
838  util3d::computeVarianceAndCorrespondences(
839  fromCloudRegistered,
840  toCloud,
841  _maxCorrespondenceDistance,
842  variance,
843  correspondences);
844  }
845  } // END Registration PointToPLane to PointToPoint
846  UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
847 
848  if(!icpT.isNull() && hasConverged)
849  {
850  float ix,iy,iz, iroll,ipitch,iyaw;
851  Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
852  icpInTargetReferential.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
853  info.icpTranslation = uMax3(fabs(ix), fabs(iy), fabs(iz));
854  info.icpRotation = uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw));
855  if((_maxTranslation>0.0f &&
856  info.icpTranslation > _maxTranslation)
857  ||
858  (_maxRotation>0.0f &&
859  info.icpRotation > _maxRotation))
860  {
861  msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
862  info.icpTranslation,
863  info.icpRotation,
864  _maxTranslation,
865  _maxRotation);
866  UINFO(msg.c_str());
867  }
868  else
869  {
870  // verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
871  int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
872  UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
873 
874  if(maxLaserScans)
875  {
876  correspondencesRatio = float(correspondences)/float(maxLaserScans);
877  }
878  else
879  {
880  static bool warningShown = false;
881  if(!warningShown)
882  {
883  UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute! This message will only appear once.",
884  dataTo.id());
885  warningShown = true;
886  }
887  correspondencesRatio = float(correspondences)/float(toScan.size()>fromScan.size()?toScan.size():fromScan.size());
888  }
889 
890  variance/=10.0;
891 
892  UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%), from guess: trans=%f rot=%f",
893  dataTo.id(), dataFrom.id(),
894  hasConverged?"true":"false",
895  variance,
896  correspondences,
897  maxLaserScans>0?maxLaserScans:(int)(toScan.size()>fromScan.size()?toScan.size():fromScan.size()),
898  correspondencesRatio*100.0f,
899  info.icpTranslation,
900  info.icpRotation);
901 
902  if(correspondences == 0)
903  {
904  UWARN("Transform is found (%s) but no correspondences has been found!? Variance is unknown!",
905  icpT.prettyPrint().c_str());
906  }
907  else
908  {
909  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
910  info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
911  if( ((_strategy == 1 && pointToPlane) || _strategy==2) &&
912  _force4DoF &&
913  !force3DoF())
914  {
915  // Force4DoF: Assume roll and pitch more accurate (IMU)
916  info.covariance(cv::Range(3,5),cv::Range(3,5))/=10.0;
917  }
918  }
919  info.icpInliersRatio = correspondencesRatio;
920  info.icpCorrespondences = correspondences;
921 
922  if(correspondencesRatio <= _correspondenceRatio)
923  {
924  msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
925  correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
926  UINFO(msg.c_str());
927  }
928  else
929  {
930  transform = icpT.inverse()*guess;
931 
932  if(!_workingDir.empty() && !_debugExportFormat.empty())
933  {
934  toPrefix+="_registered";
935  if(_debugExportFormat.compare("vtk")==0)
936  {
937  pcl::io::saveVTKFile(_workingDir+"/"+toPrefix+".vtk", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()));
938  UWARN("Saved %s/%s.vtk (%s=\"%s\")", _workingDir.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
939  }
940  else if(_debugExportFormat.compare("ply")==0)
941  {
942  pcl::io::savePLYFile(_workingDir+"/"+toPrefix+".ply", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
943  UWARN("Saved %s/%s.ply (%s=\"%s\")", _workingDir.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
944  }
945  else //pcd
946  {
947  pcl::io::savePCDFile(_workingDir+"/"+toPrefix+".pcd", *util3d::laserScanToPointCloud2(toScan, transform*toScan.localTransform()), Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
948  UWARN("Saved %s/%s.pcd (%s=\"%s\")", _workingDir.c_str(), toPrefix.c_str(), Parameters::kIcpDebugExportFormat().c_str(), _debugExportFormat.c_str());
949  }
950  }
951  }
952  }
953  }
954  else
955  {
956  if(msg.empty())
957  {
958  msg = uFormat("Cannot compute transform (converged=%s var=%f)",
959  hasConverged?"true":"false", variance);
960  }
961  UINFO(msg.c_str());
962  }
963  }
964  else
965  {
966  msg = "Laser scans empty ?!?";
967  UWARN(msg.c_str());
968  }
969  }
970  else if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
971  {
972  msg = "RegistrationIcp cannot do registration with a null guess.";
973  UERROR(msg.c_str());
974  }
975 
976 
977  info.rejectedMsg = msg;
978 
979  UDEBUG("New transform = %s", transform.prettyPrint().c_str());
980  return transform;
981 }
982 
983 }
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
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:497
UTimer
Definition: UTimer.h:46
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:253
OutlierFilterRegistrar
pms::Registrar< OutlierFilter > OutlierFilterRegistrar
rtabmap::RegistrationIcp::RegistrationIcp
RegistrationIcp(const ParametersMap &parameters=ParametersMap(), Registration *child=0)
Definition: RegistrationIcp.cpp:57
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
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:381
f
f
T
T
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
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
data
data
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2158
glm::e
GLM_FUNC_DECL genType e()
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
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:69
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
MatcherRegistrar
pms::Registrar< Matcher > MatcherRegistrar
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:248
UFATAL
#define UFATAL(...)
util3d_registration.h
rtabmap::RegistrationIcp::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
Definition: RegistrationIcp.cpp:310
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
PointMatcher< float >::Parameters
Parametrizable::Parameters Parameters
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
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
ErrorMinimizerRegistrar
pms::Registrar< ErrorMinimizer > ErrorMinimizerRegistrar
ref
ref
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
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:2285
PointMatcher< float >::get
static const PointMatcher & get()
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::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:79
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:124
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
util3d_transforms.h
params
params
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:425
DataPointsFilterRegistrar
pms::Registrar< DataPointsFilter > DataPointsFilterRegistrar
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
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
rtabmap::icpCC
rtabmap::Transform icpCC(const pcl::PointCloud< pcl::PointXYZI >::Ptr &fromCloud, pcl::PointCloud< pcl::PointXYZI >::Ptr &toCloud, int maxIterations=150, double minRMSDecrease=0.00001, bool force3DoF=false, bool force4DoF=false, int samplingLimit=50000, double finalOverlapRatio=0.85, bool filterOutFarthestPoints=false, double maxFinalRMS=0.2, float *finalRMS=0, std::string *errorMsg=0)
Definition: cccorelib.h:35
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
UTimer::now
static double now()
Definition: UTimer.cpp:80
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:1060
rtabmap::LaserScan::formatName
static std::string formatName(const Format &format)
Definition: LaserScan.cpp:34
TransformationCheckerRegistrar
pms::Registrar< TransformationChecker > TransformationCheckerRegistrar
rtabmap::laserScanFromDP
rtabmap::LaserScan laserScanFromDP(const DP &cloud, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
Definition: libpointmatcher.h:341
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:2250
rtabmap::laserScanToDP
DP laserScanToDP(const rtabmap::LaserScan &scan, bool ignoreLocalTransform=false)
Definition: libpointmatcher.h:137
DP
PM::DataPoints DP
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::RegistrationIcp::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: RegistrationIcp.cpp:100
rtabmap::RegistrationIcp::_libpointmatcherConfig
std::string _libpointmatcherConfig
Definition: RegistrationIcp.h:77
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
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:3084
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
util3d_surface.h
UINFO
#define UINFO(...)

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