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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:32