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.h>
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 
41 #include "rtabmap_ros/MsgConversion.h"
42 #include "rtabmap_ros/PluginInterface.h"
43 
44 #include <rtabmap/core/util3d.h>
45 #include <rtabmap/core/util3d_surface.h>
46 #include <rtabmap/core/util3d_transforms.h>
47 #include <rtabmap/core/util3d_filtering.h>
48 #include <rtabmap/core/util2d.h>
49 #include <rtabmap/utilite/ULogger.h>
50 #include <rtabmap/utilite/UConversion.h>
51 #include <rtabmap/utilite/UStl.h>
52 
53 using namespace rtabmap;
54 
55 namespace rtabmap_ros
56 {
57 
58 class ICPOdometry : public rtabmap_ros::OdometryROS
59 {
60 public:
61  ICPOdometry() :
62  OdometryROS(false, false, true),
63  scanCloudMaxPoints_(0),
64  scanDownsamplingStep_(1),
65  scanRangeMin_(0),
66  scanRangeMax_(0),
67  scanVoxelSize_(0.0),
68  scanNormalK_(0),
69  scanNormalRadius_(0.0),
70  scanNormalGroundUp_(0.0),
71  plugin_loader_("rtabmap_ros", "rtabmap_ros::PluginInterface"),
72  scanReceived_(false),
73  cloudReceived_(false)
74  {
75  }
76 
77  virtual ~ICPOdometry()
78  {
79  plugins_.clear();
80  }
81 
82 private:
83 
84  virtual void onOdomInit()
85  {
86  ros::NodeHandle & nh = getNodeHandle();
87  ros::NodeHandle & pnh = getPrivateNodeHandle();
88 
89  pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
90  pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
91  pnh.param("scan_range_min", scanRangeMin_, scanRangeMin_);
92  pnh.param("scan_range_max", scanRangeMax_, scanRangeMax_);
93  pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
94  pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
95  pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
96  pnh.param("scan_normal_ground_up", scanNormalGroundUp_, scanNormalGroundUp_);
97 
98  if (pnh.hasParam("plugins"))
99  {
100  XmlRpc::XmlRpcValue pluginsList;
101  pnh.getParam("plugins", pluginsList);
102 
103  for (int32_t i = 0; i < pluginsList.size(); ++i)
104  {
105  std::string pluginName = static_cast<std::string>(pluginsList[i]["name"]);
106  std::string type = static_cast<std::string>(pluginsList[i]["type"]);
107  NODELET_INFO("IcpOdometry: Using plugin %s of type \"%s\"", pluginName.c_str(), type.c_str());
108  try {
109  boost::shared_ptr<rtabmap_ros::PluginInterface> plugin = plugin_loader_.createInstance(type);
110  plugins_.push_back(plugin);
111  plugin->initialize(pluginName, pnh);
112  if(!plugin->isEnabled())
113  {
114  NODELET_WARN("Plugin: %s is not enabled, filtering will not occur. \"enabled_\" member "
115  "should be managed in subclasses. This can be ignored if the "
116  "plugin should really be initialized as disabled.",
117  plugin->getName().c_str());
118  }
119  }
120  catch(pluginlib::PluginlibException & ex) {
121  ROS_ERROR("Failed to load plugin %s. Error: %s", pluginName.c_str(), ex.what());
122  }
123 
124  }
125  }
126 
127  if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
128  {
129  ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
130  "The value is still used. Use \"scan_normal_k\" to avoid this warning.");
131  pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
132  }
133 
134  NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
135  NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
136  NODELET_INFO("IcpOdometry: scan_range_min = %f m", scanRangeMin_);
137  NODELET_INFO("IcpOdometry: scan_range_max = %f m", scanRangeMax_);
138  NODELET_INFO("IcpOdometry: scan_voxel_size = %f m", scanVoxelSize_);
139  NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
140  NODELET_INFO("IcpOdometry: scan_normal_radius = %f m", scanNormalRadius_);
141  NODELET_INFO("IcpOdometry: scan_normal_ground_up = %f", scanNormalGroundUp_);
142 
143  scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
144  cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
145 
146  filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
147  }
148 
149  virtual void updateParameters(ParametersMap & parameters)
150  {
151  //make sure we are using Reg/Strategy=0
152  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
153  if(iter != parameters.end() && iter->second.compare("1") != 0)
154  {
155  ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
156  }
157  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
158 
159  ros::NodeHandle & pnh = getPrivateNodeHandle();
160  iter = parameters.find(Parameters::kIcpDownsamplingStep());
161  if(iter != parameters.end())
162  {
163  int value = uStr2Int(iter->second);
164  if(value > 1)
165  {
166  if(!pnh.hasParam("scan_downsampling_step"))
167  {
168  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());
169  scanDownsamplingStep_ = value;
170  iter->second = "1";
171  }
172  else
173  {
174  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
175  }
176  }
177  }
178  iter = parameters.find(Parameters::kIcpRangeMin());
179  if(iter != parameters.end())
180  {
181  int value = uStr2Int(iter->second);
182  if(value > 1)
183  {
184  if(!pnh.hasParam("scan_range_min"))
185  {
186  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());
187  scanRangeMin_ = value;
188  iter->second = "0";
189  }
190  else
191  {
192  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
193  }
194  }
195  }
196  iter = parameters.find(Parameters::kIcpRangeMax());
197  if(iter != parameters.end())
198  {
199  int value = uStr2Int(iter->second);
200  if(value > 1)
201  {
202  if(!pnh.hasParam("scan_range_max"))
203  {
204  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());
205  scanRangeMax_ = value;
206  iter->second = "0";
207  }
208  else
209  {
210  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
211  }
212  }
213  }
214  iter = parameters.find(Parameters::kIcpVoxelSize());
215  if(iter != parameters.end())
216  {
217  float value = uStr2Float(iter->second);
218  if(value != 0.0f)
219  {
220  if(!pnh.hasParam("scan_voxel_size"))
221  {
222  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());
223  scanVoxelSize_ = value;
224  iter->second = "0";
225  }
226  else
227  {
228  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
229  }
230  }
231  }
232  iter = parameters.find(Parameters::kIcpPointToPlaneK());
233  if(iter != parameters.end())
234  {
235  int value = uStr2Int(iter->second);
236  if(value != 0)
237  {
238  if(!pnh.hasParam("scan_normal_k"))
239  {
240  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
241  scanNormalK_ = value;
242  }
243  }
244  }
245  iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
246  if(iter != parameters.end())
247  {
248  float value = uStr2Float(iter->second);
249  if(value != 0.0f)
250  {
251  if(!pnh.hasParam("scan_normal_radius"))
252  {
253  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
254  scanNormalRadius_ = value;
255  }
256  }
257  }
258  iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
259  if(iter != parameters.end())
260  {
261  float value = uStr2Float(iter->second);
262  if(value != 0.0f)
263  {
264  if(!pnh.hasParam("scan_normal_ground_up"))
265  {
266  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());
267  scanNormalGroundUp_ = value;
268  }
269  }
270  }
271  }
272 
273  void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
274  {
275  if(cloudReceived_)
276  {
277  ROS_ERROR("%s is already receiving clouds on \"%s\", but also "
278  "just received a scan on \"%s\". Both subscribers cannot be "
279  "used at the same time! Disabling scan subscriber.",
280  this->getName().c_str(), cloud_sub_.getTopic().c_str(), scan_sub_.getTopic().c_str());
281  scan_sub_.shutdown();
282  return;
283  }
284  scanReceived_ = true;
285  if(this->isPaused())
286  {
287  return;
288  }
289 
290  // make sure the frame of the laser is updated too
291  Transform localScanTransform = getTransform(this->frameId(),
292  scanMsg->header.frame_id,
293  scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
294  if(localScanTransform.isNull())
295  {
296  ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
297  return;
298  }
299 
300  //transform in frameId_ frame
301  sensor_msgs::PointCloud2 scanOut;
302  laser_geometry::LaserProjection projection;
303  projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
304 
305  bool hasIntensity = false;
306  for(unsigned int i=0; i<scanOut.fields.size(); ++i)
307  {
308  if(scanOut.fields[i].name.compare("intensity") == 0)
309  {
310  if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
311  {
312  hasIntensity = true;
313  }
314  else
315  {
316  static bool warningShown = false;
317  if(!warningShown)
318  {
319  ROS_WARN("The input scan cloud has an \"intensity\" field "
320  "but the datatype (%d) is not supported. Intensity will be ignored. "
321  "This message is only shown once.", scanOut.fields[i].datatype);
322  warningShown = true;
323  }
324  }
325  }
326  }
327 
328  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
329  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
330 
331  if(hasIntensity)
332  {
333  pcl::fromROSMsg(scanOut, *pclScanI);
334  pclScanI->is_dense = true;
335  }
336  else
337  {
338  pcl::fromROSMsg(scanOut, *pclScan);
339  pclScan->is_dense = true;
340  }
341 
342  cv::Mat scan;
343  int maxLaserScans = (int)scanMsg->ranges.size();
344  if(!pclScan->empty() || !pclScanI->empty())
345  {
346  if(scanDownsamplingStep_ > 1)
347  {
348  if(hasIntensity)
349  {
350  pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
351  }
352  else
353  {
354  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
355  }
356  maxLaserScans /= scanDownsamplingStep_;
357  }
358  if(scanVoxelSize_ > 0.0f)
359  {
360  float pointsBeforeFiltering;
361  float pointsAfterFiltering;
362  if(hasIntensity)
363  {
364  pointsBeforeFiltering = (float)pclScanI->size();
365  pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
366  pointsAfterFiltering = (float)pclScanI->size();
367  }
368  else
369  {
370  pointsBeforeFiltering = (float)pclScan->size();
371  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
372  pointsAfterFiltering = (float)pclScan->size();
373  }
374  float ratio = pointsAfterFiltering / pointsBeforeFiltering;
375  maxLaserScans = int(float(maxLaserScans) * ratio);
376  }
377  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
378  {
379  //compute normals
380  pcl::PointCloud<pcl::Normal>::Ptr normals;
381  if(scanVoxelSize_ > 0.0f)
382  {
383  if(hasIntensity)
384  {
385  normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
386  }
387  else
388  {
389  normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
390  }
391  }
392  else
393  {
394  if(hasIntensity)
395  {
396  normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
397  }
398  else
399  {
400  normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
401  }
402  }
403  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
404  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal;
405  if(hasIntensity)
406  {
407  pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
408  pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
409  scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
410  }
411  else
412  {
413  pclScanNormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
414  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
415  scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
416  }
417 
418  if(filtered_scan_pub_.getNumSubscribers())
419  {
420  sensor_msgs::PointCloud2 msg;
421  if(hasIntensity)
422  {
423  pcl::toROSMsg(*pclScanINormal, msg);
424  }
425  else
426  {
427  pcl::toROSMsg(*pclScanNormal, msg);
428  }
429  msg.header = scanMsg->header;
430  filtered_scan_pub_.publish(msg);
431  }
432  }
433  else
434  {
435  if(hasIntensity)
436  {
437  scan = util3d::laserScan2dFromPointCloud(*pclScanI);
438  }
439  else
440  {
441  scan = util3d::laserScan2dFromPointCloud(*pclScan);
442  }
443 
444  if(filtered_scan_pub_.getNumSubscribers())
445  {
446  sensor_msgs::PointCloud2 msg;
447  if(hasIntensity)
448  {
449  pcl::toROSMsg(*pclScanI, msg);
450  }
451  else
452  {
453  pcl::toROSMsg(*pclScan, msg);
454  }
455  msg.header = scanMsg->header;
456  filtered_scan_pub_.publish(msg);
457  }
458  }
459  }
460 
461  rtabmap::SensorData data(
462  LaserScan(scan, maxLaserScans, scanMsg->range_max,
463  scan.channels()==6?LaserScan::kXYINormal:
464  scan.channels()==5?LaserScan::kXYNormal:
465  scan.channels()==3?LaserScan::kXYI:LaserScan::kXY,
466  localScanTransform),
467  cv::Mat(),
468  cv::Mat(),
469  CameraModel(),
470  0,
471  rtabmap_ros::timestampFromROS(scanMsg->header.stamp));
472 
473  this->processData(data, scanMsg->header.stamp, "");
474  }
475 
476  void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
477  {
478  if(scanReceived_)
479  {
480  ROS_ERROR("%s is already receiving scans on \"%s\", but also "
481  "just received a cloud on \"%s\". Both subscribers cannot be "
482  "used at the same time! Disabling cloud subscriber.",
483  this->getName().c_str(), scan_sub_.getTopic().c_str(), cloud_sub_.getTopic().c_str());
484  cloud_sub_.shutdown();
485  return;
486  }
487  cloudReceived_ = true;
488  if(this->isPaused())
489  {
490  return;
491  }
492 
493  sensor_msgs::PointCloud2 cloudMsg;
494  if (!plugins_.empty())
495  {
496  if (plugins_[0]->isEnabled())
497  {
498  cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
499  }
500  else
501  {
502  cloudMsg = *pointCloudMsg;
503  }
504 
505  if (plugins_.size() > 1)
506  {
507  for (int i = 1; i < plugins_.size(); i++) {
508  if (plugins_[i]->isEnabled()) {
509  cloudMsg = plugins_[i]->filterPointCloud(cloudMsg);
510  }
511  }
512  }
513  }
514  else
515  {
516  cloudMsg = *pointCloudMsg;
517  }
518 
519  cv::Mat scan;
520  bool hasNormals = false;
521  bool hasIntensity = false;
522  for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
523  {
524  if(scanVoxelSize_ == 0.0f && cloudMsg.fields[i].name.compare("normal_x") == 0)
525  {
526  hasNormals = true;
527  }
528  if(cloudMsg.fields[i].name.compare("intensity") == 0)
529  {
530  if(cloudMsg.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
531  {
532  hasIntensity = true;
533  }
534  else
535  {
536  static bool warningShown = false;
537  if(!warningShown)
538  {
539  ROS_WARN("The input scan cloud has an \"intensity\" field "
540  "but the datatype (%d) is not supported. Intensity will be ignored. "
541  "This message is only shown once.", cloudMsg.fields[i].datatype);
542  warningShown = true;
543  }
544  }
545  }
546  }
547 
548  Transform localScanTransform = getTransform(this->frameId(), cloudMsg.header.frame_id, cloudMsg.header.stamp);
549  if(localScanTransform.isNull())
550  {
551  ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg.header.stamp.toSec());
552  return;
553  }
554  if(scanCloudMaxPoints_ == 0 && cloudMsg.height > 1)
555  {
556  scanCloudMaxPoints_ = cloudMsg.height * cloudMsg.width;
557  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
558  "cloud is not dense, for convenience it will be set to %d (%dx%d)",
559  scanCloudMaxPoints_, cloudMsg.width, cloudMsg.height);
560  }
561  else if(cloudMsg.height > 1 && scanCloudMaxPoints_ != cloudMsg.height * cloudMsg.width)
562  {
563  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
564  "cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
565  scanCloudMaxPoints_, cloudMsg.width *cloudMsg.height, cloudMsg.width, cloudMsg.height);
566  scanCloudMaxPoints_ = cloudMsg.width *cloudMsg.height;
567  }
568  int maxLaserScans = scanCloudMaxPoints_;
569 
570  if(hasNormals && hasIntensity)
571  {
572  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
573  pcl::fromROSMsg(cloudMsg, *pclScan);
574  if(pclScan->size() && scanDownsamplingStep_ > 1)
575  {
576  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
577  if(pclScan->height>1)
578  {
579  maxLaserScans = pclScan->height * pclScan->width;
580  }
581  else
582  {
583  maxLaserScans /= scanDownsamplingStep_;
584  }
585  }
586  scan = util3d::laserScanFromPointCloud(*pclScan);
587  if(filtered_scan_pub_.getNumSubscribers())
588  {
589  sensor_msgs::PointCloud2 msg;
590  pcl::toROSMsg(*pclScan, msg);
591  msg.header = cloudMsg.header;
592  filtered_scan_pub_.publish(msg);
593  }
594  }
595  else if(hasNormals)
596  {
597  pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
598  pcl::fromROSMsg(cloudMsg, *pclScan);
599  if(pclScan->size() && scanDownsamplingStep_ > 1)
600  {
601  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
602  if(pclScan->height>1)
603  {
604  maxLaserScans = pclScan->height * pclScan->width;
605  }
606  else
607  {
608  maxLaserScans /= scanDownsamplingStep_;
609  }
610  }
611  scan = util3d::laserScanFromPointCloud(*pclScan);
612  if(filtered_scan_pub_.getNumSubscribers())
613  {
614  sensor_msgs::PointCloud2 msg;
615  pcl::toROSMsg(*pclScan, msg);
616  msg.header = cloudMsg.header;
617  filtered_scan_pub_.publish(msg);
618  }
619  }
620  else if(hasIntensity)
621  {
622  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
623  pcl::fromROSMsg(cloudMsg, *pclScan);
624  if(pclScan->size() && scanDownsamplingStep_ > 1)
625  {
626  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
627  if(pclScan->height>1)
628  {
629  maxLaserScans = pclScan->height * pclScan->width;
630  }
631  else
632  {
633  maxLaserScans /= scanDownsamplingStep_;
634  }
635  }
636  if(!pclScan->is_dense)
637  {
638  pclScan = util3d::removeNaNFromPointCloud(pclScan);
639  }
640 
641  if(pclScan->size())
642  {
643  if(scanVoxelSize_ > 0.0f)
644  {
645  float pointsBeforeFiltering = (float)pclScan->size();
646  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
647  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
648  maxLaserScans = int(float(maxLaserScans) * ratio);
649  }
650  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
651  {
652  //compute normals
653  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
654  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
655  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
656  scan = util3d::laserScanFromPointCloud(*pclScanNormal);
657 
658  if(filtered_scan_pub_.getNumSubscribers())
659  {
660  sensor_msgs::PointCloud2 msg;
661  pcl::toROSMsg(*pclScanNormal, msg);
662  msg.header = cloudMsg.header;
663  filtered_scan_pub_.publish(msg);
664  }
665  }
666  else
667  {
668  scan = util3d::laserScanFromPointCloud(*pclScan);
669 
670  if(filtered_scan_pub_.getNumSubscribers())
671  {
672  sensor_msgs::PointCloud2 msg;
673  pcl::toROSMsg(*pclScan, msg);
674  msg.header = cloudMsg.header;
675  filtered_scan_pub_.publish(msg);
676  }
677  }
678  }
679  }
680  else
681  {
682  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
683  pcl::fromROSMsg(cloudMsg, *pclScan);
684  if(pclScan->size() && scanDownsamplingStep_ > 1)
685  {
686  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
687  if(pclScan->height>1)
688  {
689  maxLaserScans = pclScan->height * pclScan->width;
690  }
691  else
692  {
693  maxLaserScans /= scanDownsamplingStep_;
694  }
695  }
696  if(!pclScan->is_dense)
697  {
698  pclScan = util3d::removeNaNFromPointCloud(pclScan);
699  }
700 
701  if(pclScan->size())
702  {
703  if(scanVoxelSize_ > 0.0f)
704  {
705  float pointsBeforeFiltering = (float)pclScan->size();
706  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
707  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
708  maxLaserScans = int(float(maxLaserScans) * ratio);
709  }
710  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
711  {
712  //compute normals
713  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
714  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
715  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
716  scan = util3d::laserScanFromPointCloud(*pclScanNormal);
717 
718  if(filtered_scan_pub_.getNumSubscribers())
719  {
720  sensor_msgs::PointCloud2 msg;
721  pcl::toROSMsg(*pclScanNormal, msg);
722  msg.header = cloudMsg.header;
723  filtered_scan_pub_.publish(msg);
724  }
725  }
726  else
727  {
728  scan = util3d::laserScanFromPointCloud(*pclScan);
729 
730  if(filtered_scan_pub_.getNumSubscribers())
731  {
732  sensor_msgs::PointCloud2 msg;
733  pcl::toROSMsg(*pclScan, msg);
734  msg.header = cloudMsg.header;
735  filtered_scan_pub_.publish(msg);
736  }
737  }
738  }
739  }
740 
741  LaserScan laserScan(scan, maxLaserScans, 0,
742  scan.channels()==7?LaserScan::kXYZINormal:
743  scan.channels()==6?LaserScan::kXYZNormal:
744  scan.channels()==4?LaserScan::kXYZI:LaserScan::kXYZ,
745  localScanTransform);
746  if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
747  {
748  laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
749  }
750  if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
751  {
752  laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
753  }
754 
755  rtabmap::SensorData data(
756  laserScan,
757  cv::Mat(),
758  cv::Mat(),
759  CameraModel(),
760  0,
761  rtabmap_ros::timestampFromROS(cloudMsg.header.stamp));
762 
763  this->processData(data, cloudMsg.header.stamp, cloudMsg.header.frame_id);
764  }
765 
766 protected:
767  virtual void flushCallbacks()
768  {
769  // flush callbacks
770  }
771 
772 private:
773  ros::Subscriber scan_sub_;
774  ros::Subscriber cloud_sub_;
775  ros::Publisher filtered_scan_pub_;
776  int scanCloudMaxPoints_;
777  int scanDownsamplingStep_;
778  double scanRangeMin_;
779  double scanRangeMax_;
780  double scanVoxelSize_;
781  int scanNormalK_;
782  double scanNormalRadius_;
783  double scanNormalGroundUp_;
784  std::vector<boost::shared_ptr<rtabmap_ros::PluginInterface> > plugins_;
785  pluginlib::ClassLoader<rtabmap_ros::PluginInterface> plugin_loader_;
786  bool scanReceived_ = false;
787  bool cloudReceived_ = false;
788 
789 };
790 
791 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
792 
793 }
rtabmap_ros::ICPOdometry::scanRangeMin_
double scanRangeMin_
Definition: icp_odometry.cpp:778
ros::NodeHandle
pluginName
string pluginName()
rtabmap_ros::ICPOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: icp_odometry.cpp:767
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:777
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())
XmlRpc::XmlRpcValue::size
int size() const
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
rtabmap_ros::ICPOdometry::scanNormalGroundUp_
double scanNormalGroundUp_
Definition: icp_odometry.cpp:783
rtabmap_ros::ICPOdometry::filtered_scan_pub_
ros::Publisher filtered_scan_pub_
Definition: icp_odometry.cpp:775
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
rtabmap_ros::ICPOdometry::scan_sub_
ros::Subscriber scan_sub_
Definition: icp_odometry.cpp:773
getName
std::string getName(void *handle)
rtabmap_ros::ICPOdometry::callbackCloud
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr &pointCloudMsg)
Definition: icp_odometry.cpp:476
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
rtabmap_ros::ICPOdometry::scanRangeMax_
double scanRangeMax_
Definition: icp_odometry.cpp:779
rtabmap_ros::ICPOdometry
Definition: icp_odometry.cpp:58
rtabmap_ros
Definition: CommonDataSubscriber.h:56
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:1614
rtabmap_ros::ICPOdometry::cloud_sub_
ros::Subscriber cloud_sub_
Definition: icp_odometry.cpp:774
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:785
rtabmap_ros::ICPOdometry::callbackScan
void callbackScan(const sensor_msgs::LaserScanConstPtr &scanMsg)
Definition: icp_odometry.cpp:273
boost::shared_ptr
rtabmap::util3d::downsample
LaserScan RTABMAP_EXP downsample(const LaserScan &cloud, int step)
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet)
rtabmap_ros::ICPOdometry::scanCloudMaxPoints_
int scanCloudMaxPoints_
Definition: icp_odometry.cpp:776
uStr2Int
int uStr2Int(const std::string &str)
true
#define true
rtabmap::Transform::isNull
bool isNull() const
class_loader.hpp
class_list_macros.h
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
util3d_filtering.h
rtabmap_ros::ICPOdometry::scanNormalK_
int scanNormalK_
Definition: icp_odometry.cpp:781
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pcl_conversions.h
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
ParametersMap
QMap< QString, QVariant > ParametersMap
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
util3d_transforms.h
rtabmap::SensorData::stamp
double stamp() const
patrol.frameId
string frameId
Definition: patrol.py:11
false
#define false
rtabmap_ros::ICPOdometry::onOdomInit
virtual void onOdomInit()
Definition: icp_odometry.cpp:84
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_ros::ICPOdometry::~ICPOdometry
virtual ~ICPOdometry()
Definition: icp_odometry.cpp:77
int32_t
detail::int32 int32_t
laser_geometry.h
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
rtabmap_ros::ICPOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: icp_odometry.cpp:149
rtabmap_ros::ICPOdometry::scanNormalRadius_
double scanNormalRadius_
Definition: icp_odometry.cpp:782
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_EXP rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
pcl::toROSMsg
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
rtabmap_ros::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
Definition: MsgConversion.h:187
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)
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap_ros::ICPOdometry::plugins_
std::vector< boost::shared_ptr< rtabmap_ros::PluginInterface > > plugins_
Definition: icp_odometry.cpp:784
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:53
rtabmap_ros::ICPOdometry::ICPOdometry
ICPOdometry()
Definition: icp_odometry.cpp:61
rtabmap_ros::ICPOdometry::scanVoxelSize_
double scanVoxelSize_
Definition: icp_odometry.cpp:780
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::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true, bool is2D=false, const Transform &transform=Transform())

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19