icp_odometry.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap_ros/OdometryROS.h>
29 
30 #include <pluginlib/class_list_macros.hpp>
31 #include <pluginlib/class_loader.hpp>
32 
33 #include <nodelet/nodelet.h>
34 
35 #include <laser_geometry/laser_geometry.h>
36 
37 #include <sensor_msgs/LaserScan.h>
38 #include <sensor_msgs/PointCloud2.h>
39 #include <pcl_conversions/pcl_conversions.h>
40 #include <pcl_ros/transforms.h>
41 
42 #include "rtabmap_ros/MsgConversion.h"
43 #include "rtabmap_ros/PluginInterface.h"
44 
45 #include <rtabmap/core/util3d.h>
46 #include <rtabmap/core/util3d_surface.h>
47 #include <rtabmap/core/util3d_transforms.h>
48 #include <rtabmap/core/util3d_filtering.h>
49 #include <rtabmap/core/util2d.h>
50 #include <rtabmap/utilite/ULogger.h>
51 #include <rtabmap/utilite/UConversion.h>
52 #include <rtabmap/utilite/UStl.h>
53 
54 using namespace rtabmap;
55 
56 namespace rtabmap_ros
57 {
58 
59 class ICPOdometry : public rtabmap_ros::OdometryROS
60 {
61 public:
62  ICPOdometry() :
63  OdometryROS(false, false, true),
64  scanCloudMaxPoints_(0),
65  scanCloudIs2d_(false),
66  scanDownsamplingStep_(1),
67  scanRangeMin_(0),
68  scanRangeMax_(0),
69  scanVoxelSize_(0.0),
70  scanNormalK_(0),
71  scanNormalRadius_(0.0),
72  scanNormalGroundUp_(0.0),
73  deskewing_(false),
74  deskewingSlerp_(false),
75  plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
76  scanReceived_(false),
77  cloudReceived_(false)
78  {
79  }
80 
81  virtual ~ICPOdometry()
82  {
83  plugins_.clear();
84  }
85 
86 private:
87 
88  virtual void onOdomInit()
89  {
90  ros::NodeHandle & nh = getNodeHandle();
91  ros::NodeHandle & pnh = getPrivateNodeHandle();
92 
93  int queueSize = 1;
94  pnh.param("queue_size", queueSize, queueSize);
95  pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
96  pnh.param("scan_cloud_is_2d", scanCloudIs2d_, scanCloudIs2d_);
97  pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
98  pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
99  pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
100  pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
101  pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
102  pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
103  pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
104  pnh.param("deskewing", deskewing_, deskewing_);
105  pnh.param("deskewing_slerp", deskewingSlerp_, deskewingSlerp_);
106 
107  if (pnh.hasParam("plugins"))
108  {
109  XmlRpc::XmlRpcValue pluginsList;
110  pnh.getParam("plugins", pluginsList);
111 
112  for (int32_t i = 0; i < pluginsList.size(); ++i)
113  {
114  std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
115  std::string type = static_cast<std::string>(pluginsList[i]["type"]);
116  NODELET_INFO("IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
117  try {
118  boost::shared_ptr<rtabmap_ros::PluginInterface> plugin = plugin_loader_.createInstance(type);
119  plugins_.push_back(plugin);
120  plugin->initialize(pluginName, pnh);
121  if(!plugin->isEnabled())
122  {
123  NODELET_WARN("Plugin: %s is not enabled, filtering will not occur. \"enabled_\" member "
124  "should be managed in subclasses. This can be ignored if the "
125  "plugin should really be initialized as disabled.",
126  plugin->getName().c_str());
127  }
128  }
129  catch(pluginlib::PluginlibException & ex) {
130  ROS_ERROR("Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
131  }
132 
133  }
134  }
135 
136  if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
137  {
138  ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
139  "The value is still used. Use \"scan_normal_k\" to avoid this warning.");
140  pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
141  }
142 
143  NODELET_INFO("IcpOdometry: queue_size = %d", queueSize);
144  NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
145  NODELET_INFO("IcpOdometry: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false");
146  NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
147  NODELET_INFO("IcpOdometry: scan_range_min = %f m", scanRangeMin_);
148  NODELET_INFO("IcpOdometry: scan_range_max = %f m", scanRangeMax_);
149  NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
150  NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
151  NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
152  NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
153  NODELET_INFO("IcpOdometry: deskewing = %s", deskewing_?"true":"false");
154  NODELET_INFO("IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");
155 
156  scan_sub_ = nh.subscribe("scan", queueSize, &ICPOdometry::callbackScan, this);
157  cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
158 
159  filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
160  }
161 
162  virtual void updateParameters(ParametersMap & parameters)
163  {
164  //make sure we are using Reg/Strategy=0
165  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
166  if(iter != parameters.end() && iter->second.compare("1") != 0)
167  {
168  ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
169  }
170  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
171 
172  ros::NodeHandle & pnh = getPrivateNodeHandle();
173  iter = parameters.find(Parameters::kIcpDownsamplingStep());
174  if(iter != parameters.end())
175  {
176  int value = uStr2Int(iter->second);
177  if(value > 1)
178  {
179  if(!pnh.hasParam("scan_downsampling_step"))
180  {
181  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 1.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
182  scanDownsamplingStep_ = value;
183  iter->second = "1";
184  }
185  else
186  {
187  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
188  }
189  }
190  }
191  iter = parameters.find(Parameters::kIcpRangeMin());
192  if(iter != parameters.end())
193  {
194  float value = uStr2Float(iter->second);
195  if(value != 0.0f)
196  {
197  if(!pnh.hasParam("scan_range_min"))
198  {
199  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_min\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
200  scanRangeMin_ = value;
201  iter->second = "0";
202  }
203  else
204  {
205  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
206  }
207  }
208  }
209  iter = parameters.find(Parameters::kIcpRangeMax());
210  if(iter != parameters.end())
211  {
212  float value = uStr2Float(iter->second);
213  if(value != 0.0f)
214  {
215  if(!pnh.hasParam("scan_range_max"))
216  {
217  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_range_max\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
218  scanRangeMax_ = value;
219  iter->second = "0";
220  }
221  else
222  {
223  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
224  }
225  }
226  }
227  iter = parameters.find(Parameters::kIcpVoxelSize());
228  if(iter != parameters.end())
229  {
230  float value = uStr2Float(iter->second);
231  if(value != 0.0f)
232  {
233  if(!pnh.hasParam("scan_voxel_size"))
234  {
235  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_voxel_size\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
236  scanVoxelSize_ = value;
237  iter->second = "0";
238  }
239  else
240  {
241  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
242  }
243  }
244  }
245  else if(pnh.hasParam("scan_voxel_size"))
246  {
247  NODELET_INFO("IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
248  parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
249  }
250  iter = parameters.find(Parameters::kIcpPointToPlaneK());
251  if(iter != parameters.end())
252  {
253  int value = uStr2Int(iter->second);
254  if(value != 0)
255  {
256  if(!pnh.hasParam("scan_normal_k"))
257  {
258  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
259  scanNormalK_ = value;
260  }
261  else
262  {
263  NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
264  iter->second = uNumber2Str(scanNormalK_);
265  }
266  }
267  }
268  else if(pnh.hasParam("scan_normal_k"))
269  {
270  NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
271  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
272  }
273  iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
274  if(iter != parameters.end())
275  {
276  float value = uStr2Float(iter->second);
277  if(value != 0.0f)
278  {
279  if(!pnh.hasParam("scan_normal_radius"))
280  {
281  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
282  scanNormalRadius_ = value;
283  }
284  else
285  {
286  NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
287  iter->second = uNumber2Str(scanNormalK_);
288  }
289  }
290  }
291  else if(pnh.hasParam("scan_normal_radius"))
292  {
293  NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
294  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
295  }
296  iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
297  if(iter != parameters.end())
298  {
299  float value = uStr2Float(iter->second);
300  if(value != 0.0f)
301  {
302  if(!pnh.hasParam("scan_normal_ground_up"))
303  {
304  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_ground_up\" for convenience.", iter->second.c_str(), iter->first.c_str());
305  scanNormalGroundUp_ = value;
306  }
307  else
308  {
309  NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
310  iter->second = uNumber2Str(scanNormalK_);
311  }
312  }
313  }
314  else if(pnh.hasParam("scan_normal_ground_up"))
315  {
316  NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
317  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
318  }
319  }
320 
321  void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
322  {
323  if(cloudReceived_)
324  {
325  ROS_ERROR("%s is already receiving clouds on \"%s\", but also "
326  "just received a scan on \"%s\". Both subscribers cannot be "
327  "used at the same time! Disabling scan subscriber.",
328  this->getName().c_str(), cloud_sub_.getTopic().c_str(), scan_sub_.getTopic().c_str());
329  scan_sub_.shutdown();
330  return;
331  }
332  scanReceived_ = true;
333  if(this->isPaused())
334  {
335  return;
336  }
337 
338  // make sure the frame of the laser is updated
339  Transform localScanTransform = getTransform(this->frameId(),
340  scanMsg->header.frame_id,
341  scanMsg->header.stamp,
342  this->tfListener(),
343  this->waitForTransformDuration());
344  if(localScanTransform.isNull())
345  {
346  ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
347  return;
348  }
349 
350  //transform in scan frame
351  sensor_msgs::PointCloud2 scanOut;
352  laser_geometry::LaserProjection projection;
353 
354  if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
355  {
356  // make sure the frame of the laser is updated during the whole scan time
357  rtabmap::Transform tmpT = getTransform(
358  scanMsg->header.frame_id,
359  guessFrameId().empty()?frameId():guessFrameId(),
360  scanMsg->header.stamp,
361  scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
362  this->tfListener(),
363  this->waitForTransformDuration());
364  if(tmpT.isNull())
365  {
366  return;
367  }
368 
369  projection.transformLaserScanToPointCloud(
370  guessFrameId().empty()?frameId():guessFrameId(),
371  *scanMsg,
372  scanOut,
373  this->tfListener(),
374  laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
375 
376  if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
377  {
378  // deskew with constant velocity model (we are in frameId)
379  sensor_msgs::PointCloud2 scanOutDeskewed;
380  if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
381  {
382  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
383  return;
384  }
385  scanOut = scanOutDeskewed;
386  }
387 
388  sensor_msgs::PointCloud2 scanOutDeskewed;
389  if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
390  {
391  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
392  (guessFrameId().empty()?frameId():guessFrameId()).c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
393  return;
394  }
395  scanOut = scanOutDeskewed;
396  }
397  else
398  {
399  projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
400 
401  if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
402  {
403  // deskew with constant velocity model
404  sensor_msgs::PointCloud2 scanOutDeskewed;
405  if(!deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
406  {
407  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
408  return;
409  }
410  scanOut = scanOutDeskewed;
411  }
412  }
413 
414  bool hasIntensity = false;
415  for(unsigned int i=0; i<scanOut.fields.size(); ++i)
416  {
417  if(scanOut.fields[i].name.compare("intensity") == 0)
418  {
419  if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
420  {
421  hasIntensity = true;
422  }
423  else
424  {
425  static bool warningShown = false;
426  if(!warningShown)
427  {
428  ROS_WARN("The input scan cloud has an \"intensity\" field "
429  "but the datatype (%d) is not supported. Intensity will be ignored. "
430  "This message is only shown once.", scanOut.fields[i].datatype);
431  warningShown = true;
432  }
433  }
434  }
435  }
436 
437  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
438  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
439 
440  if(hasIntensity)
441  {
442  pcl::fromROSMsg(scanOut, *pclScanI);
443  pclScanI->is_dense = true;
444  }
445  else
446  {
447  pcl::fromROSMsg(scanOut, *pclScan);
448  pclScan->is_dense = true;
449  }
450 
451  LaserScan scan;
452  int maxLaserScans = (int)scanMsg->ranges.size();
453  if(!pclScan->empty() || !pclScanI->empty())
454  {
455  if(scanDownsamplingStep_ > 1)
456  {
457  if(hasIntensity)
458  {
459  pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
460  }
461  else
462  {
463  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
464  }
465  maxLaserScans /= scanDownsamplingStep_;
466  }
467  if(scanVoxelSize_ > 0.0f)
468  {
469  float pointsBeforeFiltering;
470  float pointsAfterFiltering;
471  if(hasIntensity)
472  {
473  pointsBeforeFiltering = (float)pclScanI->size();
474  pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
475  pointsAfterFiltering = (float)pclScanI->size();
476  }
477  else
478  {
479  pointsBeforeFiltering = (float)pclScan->size();
480  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
481  pointsAfterFiltering = (float)pclScan->size();
482  }
483  float ratio = pointsAfterFiltering / pointsBeforeFiltering;
484  maxLaserScans = int(float(maxLaserScans) * ratio);
485  }
486  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
487  {
488  //compute normals
489  pcl::PointCloud<pcl::Normal>::Ptr normals;
490  if(scanVoxelSize_ > 0.0f)
491  {
492  if(hasIntensity)
493  {
494  normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
495  }
496  else
497  {
498  normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
499  }
500  }
501  else
502  {
503  if(hasIntensity)
504  {
505  normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
506  }
507  else
508  {
509  normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
510  }
511  }
512  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
513  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
514  if(hasIntensity)
515  {
516  pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
517  pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
518  scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
519  }
520  else
521  {
522  pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
523  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
524  scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
525  }
526  }
527  else
528  {
529  if(hasIntensity)
530  {
531  scan = util3d::laserScan2dFromPointCloud(*pclScanI);
532  }
533  else
534  {
535  scan = util3d::laserScan2dFromPointCloud(*pclScan);
536  }
537  }
538  }
539 
540  if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
541  {
542  scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
543  }
544 
545  rtabmap::SensorData data(
546  LaserScan(scan,
547  maxLaserScans,
548  scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
549  localScanTransform),
550  cv::Mat(),
551  cv::Mat(),
552  rtabmap::CameraModel(),
553  0,
554  rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
555 
556  this->processData(data, scanMsg->header);
557  }
558 
559  void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
560  {
561  UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
562  uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
563 
564  if(scanReceived_)
565  {
566  ROS_ERROR("%s is already receiving scans on \"%s\", but also "
567  "just received a cloud on \"%s\". Both subscribers cannot be "
568  "used at the same time! Disabling cloud subscriber.",
569  this->getName().c_str(), scan_sub_.getTopic().c_str(), cloud_sub_.getTopic().c_str());
570  cloud_sub_.shutdown();
571  return;
572  }
573  cloudReceived_ = true;
574  if(this->isPaused())
575  {
576  return;
577  }
578 
579  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
580  if (!plugins_.empty())
581  {
582  if (plugins_[0]->isEnabled())
583  {
584  *cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
585  }
586  else
587  {
588  *cloudMsg = *pointCloudMsg;
589  }
590 
591  if (plugins_.size() > 1)
592  {
593  for (int i = 1; i < plugins_.size(); i++) {
594  if (plugins_[i]->isEnabled()) {
595  *cloudMsg = plugins_[i]->filterPointCloud(*cloudMsg);
596  }
597  }
598  }
599  }
600  else
601  {
602  *cloudMsg = *pointCloudMsg;
603  }
604 
605  Transform localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
606  if(localScanTransform.isNull())
607  {
608  ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
609  return;
610  }
611 
612  if(deskewing_)
613  {
614  if(!guessFrameId().empty())
615  {
616  // deskew with TF
617  if(!deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
618  {
619  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
620  return;
621  }
622  }
623  else if(previousStamp() > 0 && !velocityGuess().isNull())
624  {
625  // deskew with constant velocity model
626  bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
627  sensor_msgs::PointCloud2Ptr cloudInBaseFrame;
628  sensor_msgs::PointCloud2Ptr cloudPtr = cloudMsg;
629  if(!alreadyInBaseFrame)
630  {
631  // transform in base frame
632  cloudInBaseFrame.reset(new sensor_msgs::PointCloud2);
633  if(!pcl_ros::transformPointCloud(frameId(), *pointCloudMsg, *cloudInBaseFrame, this->tfListener()))
634  {
635  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
636  pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), pointCloudMsg->header.stamp.toSec());
637  return;
638  }
639  cloudPtr = cloudInBaseFrame;
640  }
641 
642  sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2);
643  if(!deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
644  {
645  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
646  return;
647  }
648 
649  if(!alreadyInBaseFrame)
650  {
651  // put back in scan frame
652  if(!pcl_ros::transformPointCloud(pointCloudMsg->header.frame_id.c_str(), *cloudDeskewed, *cloudMsg, this->tfListener()))
653  {
654  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
655  frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), pointCloudMsg->header.stamp.toSec());
656  return;
657  }
658  }
659  else
660  {
661  cloudMsg = cloudDeskewed;
662  }
663  }
664  }
665 
666  LaserScan scan;
667  bool hasNormals = false;
668  bool hasIntensity = false;
669  bool is3D = false;
670  for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
671  {
672  if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
673  {
674  hasNormals = true;
675  }
676  if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
677  {
678  is3D = true;
679  }
680  if(cloudMsg->fields[i].name.compare("intensity") == 0)
681  {
682  if(cloudMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
683  {
684  hasIntensity = true;
685  }
686  else
687  {
688  static bool warningShown = false;
689  if(!warningShown)
690  {
691  ROS_WARN("The input scan cloud has an \"intensity\" field "
692  "but the datatype (%d) is not supported. Intensity will be ignored. "
693  "This message is only shown once.", cloudMsg->fields[i].datatype);
694  warningShown = true;
695  }
696  }
697  }
698  }
699 
700  if(scanCloudMaxPoints_ == 0 && cloudMsg->height > 1)
701  {
702  scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
703  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
704  "cloud is not dense, for convenience it will be set to %d (%dx%d)",
705  scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
706  }
707  else if(cloudMsg->height > 1 && scanCloudMaxPoints_ < cloudMsg->height * cloudMsg->width)
708  {
709  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
710  "cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
711  scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
712  scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
713  }
714  int maxLaserScans = scanCloudMaxPoints_;
715 
716  if(hasNormals && hasIntensity)
717  {
718  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
719  pcl::fromROSMsg(*cloudMsg, *pclScan);
720  if(pclScan->size() && scanDownsamplingStep_ > 1)
721  {
722  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
723  if(pclScan->height>1)
724  {
725  maxLaserScans = pclScan->height * pclScan->width;
726  }
727  else
728  {
729  maxLaserScans /= scanDownsamplingStep_;
730  }
731  }
732  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
733  }
734  else if(hasNormals)
735  {
736  pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
737  pcl::fromROSMsg(*cloudMsg, *pclScan);
738  if(pclScan->size() && scanDownsamplingStep_ > 1)
739  {
740  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
741  if(pclScan->height>1)
742  {
743  maxLaserScans = pclScan->height * pclScan->width;
744  }
745  else
746  {
747  maxLaserScans /= scanDownsamplingStep_;
748  }
749  }
750  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
751  }
752  else if(hasIntensity)
753  {
754  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
755  pcl::fromROSMsg(*cloudMsg, *pclScan);
756  if(pclScan->size() && scanDownsamplingStep_ > 1)
757  {
758  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
759  if(pclScan->height>1)
760  {
761  maxLaserScans = pclScan->height * pclScan->width;
762  }
763  else
764  {
765  maxLaserScans /= scanDownsamplingStep_;
766  }
767  }
768  if(!pclScan->is_dense)
769  {
770  pclScan = util3d::removeNaNFromPointCloud(pclScan);
771  }
772 
773  if(pclScan->size())
774  {
775  if(scanVoxelSize_ > 0.0f)
776  {
777  float pointsBeforeFiltering = (float)pclScan->size();
778  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
779  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
780  maxLaserScans = int(float(maxLaserScans) * ratio);
781  }
782  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
783  {
784  //compute normals
785  pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
786  util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
787  util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
788  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
789  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
790  scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
791  }
792  else
793  {
794  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
795  }
796  }
797  }
798  else
799  {
800  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
801  pcl::fromROSMsg(*cloudMsg, *pclScan);
802  if(pclScan->size() && scanDownsamplingStep_ > 1)
803  {
804  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
805  if(pclScan->height>1)
806  {
807  maxLaserScans = pclScan->height * pclScan->width;
808  }
809  else
810  {
811  maxLaserScans /= scanDownsamplingStep_;
812  }
813  }
814  if(!pclScan->is_dense)
815  {
816  pclScan = util3d::removeNaNFromPointCloud(pclScan);
817  }
818 
819  if(pclScan->size())
820  {
821  if(scanVoxelSize_ > 0.0f)
822  {
823  float pointsBeforeFiltering = (float)pclScan->size();
824  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
825  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
826  maxLaserScans = int(float(maxLaserScans) * ratio);
827  }
828  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
829  {
830  //compute normals
831  pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
832  util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
833  util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
834  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
835  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
836  scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
837  }
838  else
839  {
840  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
841  }
842  }
843  }
844 
845  LaserScan laserScan(scan,
846  maxLaserScans,
847  0,
848  localScanTransform);
849  if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
850  {
851  laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
852  }
853  if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
854  {
855  laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
856  }
857 
858  rtabmap::SensorData data(
859  laserScan,
860  cv::Mat(),
861  cv::Mat(),
862  rtabmap::CameraModel(),
863  0,
864  rtabmap_ros::timestampFromROS(cloudMsg->header.stamp));
865 
866  this->processData(data, cloudMsg->header);
867  }
868 
869 protected:
870  virtual void flushCallbacks()
871  {
872  // flush callbacks
873  }
874 
875  void postProcessData(const SensorData & data, const std_msgs::Header & header) const
876  {
877  if(filtered_scan_pub_.getNumSubscribers())
878  {
879  sensor_msgs::PointCloud2 msg;
880  pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), msg);
881  msg.header = header;
882  filtered_scan_pub_.publish(msg);
883  }
884  }
885 
886 private:
887  ros::Subscriber scan_sub_;
888  ros::Subscriber cloud_sub_;
889  ros::Publisher filtered_scan_pub_;
890  int scanCloudMaxPoints_;
891  bool scanCloudIs2d_;
892  int scanDownsamplingStep_;
893  double scanRangeMin_;
894  double scanRangeMax_;
895  double scanVoxelSize_;
896  int scanNormalK_;
897  double scanNormalRadius_;
898  double scanNormalGroundUp_;
899  bool deskewing_;
900  bool deskewingSlerp_;
901  std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
902  pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
903  bool scanReceived_ = false;
904  bool cloudReceived_ = false;
905 
906 };
907 
908 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
909 
910 }
rtabmap_ros::ICPOdometry::scanRangeMin_
double scanRangeMin_
Definition: icp_odometry.cpp:893
laser_geometry::LaserProjection::projectLaser
void projectLaser(const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default)
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
rtabmap_ros::ICPOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: icp_odometry.cpp:870
laser_geometry::LaserProjection
rtabmap::util3d::computeNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
NODELET_WARN
#define NODELET_WARN(...)
rtabmap_ros::ICPOdometry::scanDownsamplingStep_
int scanDownsamplingStep_
Definition: icp_odometry.cpp:892
f
f
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
rtabmap_ros::ICPOdometry::scanNormalGroundUp_
double scanNormalGroundUp_
Definition: icp_odometry.cpp:898
data
data
rtabmap_ros::ICPOdometry::filtered_scan_pub_
ros::Publisher filtered_scan_pub_
Definition: icp_odometry.cpp:889
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
rtabmap_ros::ICPOdometry::scan_sub_
ros::Subscriber scan_sub_
Definition: icp_odometry.cpp:887
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
rtabmap_ros::ICPOdometry::callbackCloud
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr &pointCloudMsg)
Definition: icp_odometry.cpp:559
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
rtabmap_ros::ICPOdometry::deskewing_
bool deskewing_
Definition: icp_odometry.cpp:899
rtabmap_ros::ICPOdometry::scanRangeMax_
double scanRangeMax_
Definition: icp_odometry.cpp:894
rtabmap::LaserScan::isEmpty
bool isEmpty() const
rtabmap::LaserScan::is2d
bool is2d() const
rtabmap_ros::ICPOdometry
Definition: icp_odometry.cpp:59
rtabmap_ros::deskew
bool deskew(const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud2 &output, const std::string &fixedFrameId, tf::TransformListener &listener, double waitForTransform, bool slerp=false)
Definition: MsgConversion.cpp:2927
rtabmap_ros
Definition: CommonDataSubscriber.h:57
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1733
rtabmap_ros::ICPOdometry::cloud_sub_
ros::Subscriber cloud_sub_
Definition: icp_odometry.cpp:888
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
rtabmap_ros::ICPOdometry::plugin_loader_
pluginlib::ClassLoader< rtabmap_ros::PluginInterface > plugin_loader_
Definition: icp_odometry.cpp:902
rtabmap_ros::ICPOdometry::callbackScan
void callbackScan(const sensor_msgs::LaserScanConstPtr &scanMsg)
Definition: icp_odometry.cpp:321
boost::shared_ptr
isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
rtabmap_ros::ICPOdometry::scanCloudMaxPoints_
int scanCloudMaxPoints_
Definition: icp_odometry.cpp:890
class_list_macros.hpp
rtabmap::LaserScan::hasNormals
bool hasNormals() const
uStr2Int
int uStr2Int(const std::string &str)
uNumber2Str
std::string uNumber2Str(unsigned int number)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
true
#define true
rtabmap_ros::ICPOdometry::scanCloudIs2d_
bool scanCloudIs2d_
Definition: icp_odometry.cpp:891
class_loader.hpp
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
util3d_filtering.h
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
rtabmap_ros::ICPOdometry::scanNormalK_
int scanNormalK_
Definition: icp_odometry.cpp:896
pcl_conversions.h
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
laser_geometry::channel_option::Timestamp
Timestamp
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
ParametersMap
QMap< QString, QVariant > ParametersMap
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
util3d_transforms.h
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap::Transform::isNull
bool isNull() const
patrol.frameId
string frameId
Definition: patrol.py:11
false
#define false
rtabmap_ros::ICPOdometry::onOdomInit
virtual void onOdomInit()
Definition: icp_odometry.cpp:88
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_ros::ICPOdometry::~ICPOdometry
virtual ~ICPOdometry()
Definition: icp_odometry.cpp:81
int32_t
detail::int32 int32_t
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
laser_geometry::channel_option::Intensity
Intensity
pcl_ros::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
laser_geometry.h
rtabmap_ros::ICPOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: icp_odometry.cpp:162
rtabmap_ros::ICPOdometry::scanNormalRadius_
double scanNormalRadius_
Definition: icp_odometry.cpp:897
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
rtabmap_ros::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
Definition: MsgConversion.h:188
uStr2Float
float uStr2Float(const std::string &str)
laser_geometry::LaserProjection::transformLaserScanToPointCloud
void transformLaserScanToPointCloud(const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, double range_cutoff, int channel_options=channel_option::Default)
pcl_conversions::fromPCL
void fromPCL(const std::uint64_t &pcl_stamp, ros::Time &stamp)
rtabmap_ros::ICPOdometry::plugins_
std::vector< boost::shared_ptr< rtabmap_ros::PluginInterface > > plugins_
Definition: icp_odometry.cpp:901
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_ros::OdometryROS
Definition: OdometryROS.h:55
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(apriltag_ros::ContinuousDetector, nodelet::Nodelet)
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
rtabmap_ros::ICPOdometry::ICPOdometry
ICPOdometry()
Definition: icp_odometry.cpp:62
rtabmap::LaserScan::size
int size() const
rtabmap_ros::ICPOdometry::deskewingSlerp_
bool deskewingSlerp_
Definition: icp_odometry.cpp:900
rtabmap_ros::ICPOdometry::scanVoxelSize_
double scanVoxelSize_
Definition: icp_odometry.cpp:895
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
util3d_surface.h
rtabmap::util3d::adjustNormalsToViewPoint
LaserScan RTABMAP_EXP adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0), float groundNormalsUp=0.0f)
rtabmap_ros::ICPOdometry::postProcessData
void postProcessData(const SensorData &data, const std_msgs::Header &header) const
Definition: icp_odometry.cpp:875

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40