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_odom/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_conversions/MsgConversion.h"
43 #include "rtabmap_odom/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_odom
57 {
58 
59 class ICPOdometry : public OdometryROS
60 {
61 public:
62  ICPOdometry() :
63  OdometryROS(false, false, true),
64  scanCloudMaxPoints_(-1),
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_odom", "rtabmap_odom::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_odom::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  initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
162  getName().c_str(),
163  scan_sub_.getTopic().c_str(),
164  cloud_sub_.getTopic().c_str()), true);
165  }
166 
167  virtual void updateParameters(ParametersMap & parameters)
168  {
169  //make sure we are using Reg/Strategy=0
170  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
171  if(iter != parameters.end() && iter->second.compare("1") != 0)
172  {
173  ROS_WARN("ICP odometry works only with \"Reg/Strategy\"=1. Ignoring value %s.", iter->second.c_str());
174  }
175  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
176 
177  ros::NodeHandle & pnh = getPrivateNodeHandle();
178  iter = parameters.find(Parameters::kIcpDownsamplingStep());
179  if(iter != parameters.end())
180  {
181  int value = uStr2Int(iter->second);
182  if(value > 1)
183  {
184  if(!pnh.hasParam("scan_downsampling_step"))
185  {
186  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());
187  scanDownsamplingStep_ = value;
188  iter->second = "1";
189  }
190  else
191  {
192  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
193  }
194  }
195  }
196  iter = parameters.find(Parameters::kIcpRangeMin());
197  if(iter != parameters.end())
198  {
199  float value = uStr2Float(iter->second);
200  if(value != 0.0f)
201  {
202  if(!pnh.hasParam("scan_range_min"))
203  {
204  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());
205  scanRangeMin_ = value;
206  iter->second = "0";
207  }
208  else
209  {
210  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_min\" are set.", iter->first.c_str());
211  }
212  }
213  }
214  iter = parameters.find(Parameters::kIcpRangeMax());
215  if(iter != parameters.end())
216  {
217  float value = uStr2Float(iter->second);
218  if(value != 0.0f)
219  {
220  if(!pnh.hasParam("scan_range_max"))
221  {
222  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());
223  scanRangeMax_ = value;
224  iter->second = "0";
225  }
226  else
227  {
228  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_range_max\" are set.", iter->first.c_str());
229  }
230  }
231  }
232  iter = parameters.find(Parameters::kIcpVoxelSize());
233  if(iter != parameters.end())
234  {
235  float value = uStr2Float(iter->second);
236  if(value != 0.0f)
237  {
238  if(!pnh.hasParam("scan_voxel_size"))
239  {
240  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());
241  scanVoxelSize_ = value;
242  iter->second = "0";
243  }
244  else
245  {
246  ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_voxel_size\" are set.", iter->first.c_str());
247  }
248  }
249  }
250  else if(pnh.hasParam("scan_voxel_size"))
251  {
252  NODELET_INFO("IcpOdometry: scan_voxel_size is set (%f), setting %s to 0", scanVoxelSize_, Parameters::kIcpVoxelSize().c_str());
253  parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0"));
254  }
255  iter = parameters.find(Parameters::kIcpPointToPlaneK());
256  if(iter != parameters.end())
257  {
258  int value = uStr2Int(iter->second);
259  if(value != 0)
260  {
261  if(!pnh.hasParam("scan_normal_k"))
262  {
263  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_k\" for convenience.", iter->second.c_str(), iter->first.c_str());
264  scanNormalK_ = value;
265  }
266  else
267  {
268  NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
269  iter->second = uNumber2Str(scanNormalK_);
270  }
271  }
272  }
273  else if(pnh.hasParam("scan_normal_k"))
274  {
275  NODELET_INFO("IcpOdometry: scan_normal_k is set (%d), setting %s to same value.", scanNormalK_, Parameters::kIcpPointToPlaneK().c_str());
276  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), uNumber2Str(scanNormalK_)));
277  }
278  iter = parameters.find(Parameters::kIcpPointToPlaneRadius());
279  if(iter != parameters.end())
280  {
281  float value = uStr2Float(iter->second);
282  if(value != 0.0f)
283  {
284  if(!pnh.hasParam("scan_normal_radius"))
285  {
286  ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_normal_radius\" for convenience.", iter->second.c_str(), iter->first.c_str());
287  scanNormalRadius_ = value;
288  }
289  else
290  {
291  NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
292  iter->second = uNumber2Str(scanNormalK_);
293  }
294  }
295  }
296  else if(pnh.hasParam("scan_normal_radius"))
297  {
298  NODELET_INFO("IcpOdometry: scan_normal_radius is set (%f), setting %s to same value.", scanNormalRadius_, Parameters::kIcpPointToPlaneRadius().c_str());
299  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneRadius(), uNumber2Str(scanNormalRadius_)));
300  }
301  iter = parameters.find(Parameters::kIcpPointToPlaneGroundNormalsUp());
302  if(iter != parameters.end())
303  {
304  float value = uStr2Float(iter->second);
305  if(value != 0.0f)
306  {
307  if(!pnh.hasParam("scan_normal_ground_up"))
308  {
309  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());
310  scanNormalGroundUp_ = value;
311  }
312  else
313  {
314  NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
315  iter->second = uNumber2Str(scanNormalK_);
316  }
317  }
318  }
319  else if(pnh.hasParam("scan_normal_ground_up"))
320  {
321  NODELET_INFO("IcpOdometry: scan_normal_ground_up is set (%f), setting %s to same value.", scanNormalGroundUp_, Parameters::kIcpPointToPlaneGroundNormalsUp().c_str());
322  parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneGroundNormalsUp(), uNumber2Str(scanNormalGroundUp_)));
323  }
324  }
325 
326  void callbackScan(const sensor_msgs::LaserScanConstPtr& scanMsg)
327  {
328  if(cloudReceived_)
329  {
330  ROS_ERROR("%s is already receiving clouds on \"%s\", but also "
331  "just received a scan on \"%s\". Both subscribers cannot be "
332  "used at the same time! Disabling scan subscriber.",
333  this->getName().c_str(), cloud_sub_.getTopic().c_str(), scan_sub_.getTopic().c_str());
334  scan_sub_.shutdown();
335  return;
336  }
337 
338  scanReceived_ = true;
339  if(this->isPaused())
340  {
341  return;
342  }
343 
344  // make sure the frame of the laser is updated
345  Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(),
346  scanMsg->header.frame_id,
347  scanMsg->header.stamp,
348  this->tfListener(),
349  this->waitForTransformDuration());
350  if(localScanTransform.isNull())
351  {
352  ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
353  return;
354  }
355 
356  //transform in scan frame
357  sensor_msgs::PointCloud2 scanOut;
358  laser_geometry::LaserProjection projection;
359 
360  if(deskewing_ && (!guessFrameId().empty() || (frameId().compare(scanMsg->header.frame_id) != 0)))
361  {
362  // make sure the frame of the laser is updated during the whole scan time
363  rtabmap::Transform tmpT = rtabmap_conversions::getMovingTransform(
364  scanMsg->header.frame_id,
365  guessFrameId().empty()?frameId():guessFrameId(),
366  scanMsg->header.stamp,
367  scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment),
368  this->tfListener(),
369  this->waitForTransformDuration());
370  if(tmpT.isNull())
371  {
372  return;
373  }
374 
375  projection.transformLaserScanToPointCloud(
376  guessFrameId().empty()?frameId():guessFrameId(),
377  *scanMsg,
378  scanOut,
379  this->tfListener(),
380  -1.0,
381  laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
382 
383  if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
384  {
385  // deskew with constant velocity model (we are in frameId)
386  sensor_msgs::PointCloud2 scanOutDeskewed;
387  if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
388  {
389  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
390  return;
391  }
392  scanOut = scanOutDeskewed;
393  }
394 
395  sensor_msgs::PointCloud2 scanOutDeskewed;
396  if(!pcl_ros::transformPointCloud(scanMsg->header.frame_id, scanOut, scanOutDeskewed, this->tfListener()))
397  {
398  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
399  (guessFrameId().empty()?frameId():guessFrameId()).c_str(), scanMsg->header.frame_id.c_str(), scanMsg->header.stamp.toSec());
400  return;
401  }
402  scanOut = scanOutDeskewed;
403  }
404  else
405  {
406  projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
407 
408  if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
409  {
410  // deskew with constant velocity model
411  sensor_msgs::PointCloud2 scanOutDeskewed;
412  if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
413  {
414  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
415  return;
416  }
417  scanOut = scanOutDeskewed;
418  }
419  }
420 
421  bool hasIntensity = false;
422  for(unsigned int i=0; i<scanOut.fields.size(); ++i)
423  {
424  if(scanOut.fields[i].name.compare("intensity") == 0)
425  {
426  if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
427  {
428  hasIntensity = true;
429  }
430  else
431  {
432  static bool warningShown = false;
433  if(!warningShown)
434  {
435  ROS_WARN("The input scan cloud has an \"intensity\" field "
436  "but the datatype (%d) is not supported. Intensity will be ignored. "
437  "This message is only shown once.", scanOut.fields[i].datatype);
438  warningShown = true;
439  }
440  }
441  }
442  }
443 
444  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScanI(new pcl::PointCloud<pcl::PointXYZI>);
445  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
446 
447  if(hasIntensity)
448  {
449  pcl::fromROSMsg(scanOut, *pclScanI);
450  pclScanI->is_dense = true;
451  }
452  else
453  {
454  pcl::fromROSMsg(scanOut, *pclScan);
455  pclScan->is_dense = true;
456  }
457 
458  LaserScan scan;
459  int maxLaserScans = (int)scanMsg->ranges.size();
460  if(!pclScan->empty() || !pclScanI->empty())
461  {
462  if(scanDownsamplingStep_ > 1)
463  {
464  if(hasIntensity)
465  {
466  pclScanI = util3d::downsample(pclScanI, scanDownsamplingStep_);
467  }
468  else
469  {
470  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
471  }
472  maxLaserScans /= scanDownsamplingStep_;
473  }
474  if(scanVoxelSize_ > 0.0f)
475  {
476  float pointsBeforeFiltering;
477  float pointsAfterFiltering;
478  if(hasIntensity)
479  {
480  pointsBeforeFiltering = (float)pclScanI->size();
481  pclScanI = util3d::voxelize(pclScanI, scanVoxelSize_);
482  pointsAfterFiltering = (float)pclScanI->size();
483  }
484  else
485  {
486  pointsBeforeFiltering = (float)pclScan->size();
487  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
488  pointsAfterFiltering = (float)pclScan->size();
489  }
490  float ratio = pointsAfterFiltering / pointsBeforeFiltering;
491  maxLaserScans = int(float(maxLaserScans) * ratio);
492  }
493  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
494  {
495  //compute normals
496  pcl::PointCloud<pcl::Normal>::Ptr normals;
497  if(scanVoxelSize_ > 0.0f)
498  {
499  if(hasIntensity)
500  {
501  normals = util3d::computeNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
502  }
503  else
504  {
505  normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
506  }
507  }
508  else
509  {
510  if(hasIntensity)
511  {
512  normals = util3d::computeFastOrganizedNormals2D(pclScanI, scanNormalK_, scanNormalRadius_);
513  }
514  else
515  {
516  normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
517  }
518  }
519  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
520  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
521  if(hasIntensity)
522  {
523  pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
524  pcl::concatenateFields(*pclScanI, *normals, *pclScanINormal);
525  scan = util3d::laserScan2dFromPointCloud(*pclScanINormal);
526  }
527  else
528  {
529  pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
530  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
531  scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
532  }
533  }
534  else
535  {
536  if(hasIntensity)
537  {
538  scan = util3d::laserScan2dFromPointCloud(*pclScanI);
539  }
540  else
541  {
542  scan = util3d::laserScan2dFromPointCloud(*pclScan);
543  }
544  }
545  }
546 
547  if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
548  {
549  scan = util3d::rangeFiltering(scan, scanRangeMin_, scanRangeMax_);
550  }
551 
552  rtabmap::SensorData data(
553  LaserScan(scan,
554  maxLaserScans,
555  scanRangeMax_>0&&scanRangeMax_<scanMsg->range_max?scanRangeMax_:scanMsg->range_max,
556  localScanTransform),
557  cv::Mat(),
558  cv::Mat(),
559  rtabmap::CameraModel(),
560  0,
561  rtabmap_conversions::timestampFromROS(scanMsg->header.stamp));
562 
563  this->processData(data, scanMsg->header);
564  }
565 
566  void callbackCloud(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg)
567  {
568  UASSERT_MSG(pointCloudMsg->data.size() == pointCloudMsg->row_step*pointCloudMsg->height,
569  uFormat("data=%d row_step=%d height=%d", pointCloudMsg->data.size(), pointCloudMsg->row_step, pointCloudMsg->height).c_str());
570 
571  if(scanReceived_)
572  {
573  ROS_ERROR("%s is already receiving scans on \"%s\", but also "
574  "just received a cloud on \"%s\". Both subscribers cannot be "
575  "used at the same time! Disabling cloud subscriber.",
576  this->getName().c_str(), scan_sub_.getTopic().c_str(), cloud_sub_.getTopic().c_str());
577  cloud_sub_.shutdown();
578  return;
579  }
580 
581  cloudReceived_ = true;
582  if(this->isPaused())
583  {
584  return;
585  }
586 
587  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
588  if (!plugins_.empty())
589  {
590  if (plugins_[0]->isEnabled())
591  {
592  *cloudMsg = plugins_[0]->filterPointCloud(*pointCloudMsg);
593  }
594  else
595  {
596  *cloudMsg = *pointCloudMsg;
597  }
598 
599  if (plugins_.size() > 1)
600  {
601  for (int i = 1; i < plugins_.size(); i++) {
602  if (plugins_[i]->isEnabled()) {
603  *cloudMsg = plugins_[i]->filterPointCloud(*cloudMsg);
604  }
605  }
606  }
607  }
608  else
609  {
610  *cloudMsg = *pointCloudMsg;
611  }
612 
613  Transform localScanTransform = rtabmap_conversions::getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp, this->tfListener(), this->waitForTransformDuration());
614  if(localScanTransform.isNull())
615  {
616  ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
617  return;
618  }
619 
620  if(deskewing_)
621  {
622  if(!guessFrameId().empty())
623  {
624  // deskew with TF
625  if(!rtabmap_conversions::deskew(*pointCloudMsg, *cloudMsg, guessFrameId(), tfListener(), waitForTransformDuration(), deskewingSlerp_))
626  {
627  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
628  return;
629  }
630  }
631  else if(previousStamp() > 0 && !velocityGuess().isNull())
632  {
633  // deskew with constant velocity model
634  bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
635  sensor_msgs::PointCloud2Ptr cloudInBaseFrame;
636  sensor_msgs::PointCloud2Ptr cloudPtr = cloudMsg;
637  if(!alreadyInBaseFrame)
638  {
639  // transform in base frame
640  cloudInBaseFrame.reset(new sensor_msgs::PointCloud2);
641  if(!pcl_ros::transformPointCloud(frameId(), *pointCloudMsg, *cloudInBaseFrame, this->tfListener()))
642  {
643  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
644  pointCloudMsg->header.frame_id.c_str(), frameId().c_str(), pointCloudMsg->header.stamp.toSec());
645  return;
646  }
647  cloudPtr = cloudInBaseFrame;
648  }
649 
650  sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2);
651  if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
652  {
653  ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
654  return;
655  }
656 
657  if(!alreadyInBaseFrame)
658  {
659  // put back in scan frame
660  if(!pcl_ros::transformPointCloud(pointCloudMsg->header.frame_id.c_str(), *cloudDeskewed, *cloudMsg, this->tfListener()))
661  {
662  ROS_ERROR("Cannot transform back projected scan from \"%s\" frame to \"%s\" frame at time %fs.",
663  frameId().c_str(), pointCloudMsg->header.frame_id.c_str(), pointCloudMsg->header.stamp.toSec());
664  return;
665  }
666  }
667  else
668  {
669  cloudMsg = cloudDeskewed;
670  }
671  }
672  }
673 
674  LaserScan scan;
675  bool hasNormals = false;
676  bool hasIntensity = false;
677  bool is3D = false;
678  for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
679  {
680  if(scanVoxelSize_ == 0.0f && cloudMsg->fields[i].name.compare("normal_x") == 0)
681  {
682  hasNormals = true;
683  }
684  if(cloudMsg->fields[i].name.compare("z") == 0 && !scanCloudIs2d_)
685  {
686  is3D = true;
687  }
688  if(cloudMsg->fields[i].name.compare("intensity") == 0)
689  {
690  if(cloudMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
691  {
692  hasIntensity = true;
693  }
694  else
695  {
696  static bool warningShown = false;
697  if(!warningShown)
698  {
699  ROS_WARN("The input scan cloud has an \"intensity\" field "
700  "but the datatype (%d) is not supported. Intensity will be ignored. "
701  "This message is only shown once.", cloudMsg->fields[i].datatype);
702  warningShown = true;
703  }
704  }
705  }
706  }
707 
708  if(cloudMsg->height > 1) // organized cloud
709  {
710  if(scanCloudMaxPoints_ == -1)
711  {
712  scanCloudMaxPoints_ = cloudMsg->height * cloudMsg->width;
713  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is not set but input "
714  "cloud is not dense, for convenience it will be set to %d (%dx%d)",
715  scanCloudMaxPoints_, cloudMsg->width, cloudMsg->height);
716  }
717  else if(scanCloudMaxPoints_ > 0 && scanCloudMaxPoints_ < cloudMsg->height * cloudMsg->width)
718  {
719  NODELET_WARN("IcpOdometry: \"scan_cloud_max_points\" is set to %d but input "
720  "cloud is not dense and has a size of %d (%dx%d), setting to this later size.",
721  scanCloudMaxPoints_, cloudMsg->width *cloudMsg->height, cloudMsg->width, cloudMsg->height);
722  scanCloudMaxPoints_ = cloudMsg->width *cloudMsg->height;
723  }
724  }
725  if(scanCloudMaxPoints_ == -1)
726  {
727  scanCloudMaxPoints_ = 0;
728  }
729  int maxLaserScans = scanCloudMaxPoints_;
730 
731  if(hasNormals && hasIntensity)
732  {
733  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
734  pcl::fromROSMsg(*cloudMsg, *pclScan);
735  if(pclScan->size() && scanDownsamplingStep_ > 1)
736  {
737  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
738  if(pclScan->height>1)
739  {
740  maxLaserScans = pclScan->height * pclScan->width;
741  }
742  else
743  {
744  maxLaserScans /= scanDownsamplingStep_;
745  }
746  }
747  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
748  }
749  else if(hasNormals)
750  {
751  pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
752  pcl::fromROSMsg(*cloudMsg, *pclScan);
753  if(pclScan->size() && scanDownsamplingStep_ > 1)
754  {
755  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
756  if(pclScan->height>1)
757  {
758  maxLaserScans = pclScan->height * pclScan->width;
759  }
760  else
761  {
762  maxLaserScans /= scanDownsamplingStep_;
763  }
764  }
765  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
766  }
767  else if(hasIntensity)
768  {
769  pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
770  pcl::fromROSMsg(*cloudMsg, *pclScan);
771  if(pclScan->size() && scanDownsamplingStep_ > 1)
772  {
773  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
774  if(pclScan->height>1)
775  {
776  maxLaserScans = pclScan->height * pclScan->width;
777  }
778  else
779  {
780  maxLaserScans /= scanDownsamplingStep_;
781  }
782  }
783  if(!pclScan->is_dense)
784  {
785  pclScan = util3d::removeNaNFromPointCloud(pclScan);
786  }
787 
788  if(pclScan->size())
789  {
790  if(scanVoxelSize_ > 0.0f)
791  {
792  float pointsBeforeFiltering = (float)pclScan->size();
793  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
794  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
795  maxLaserScans = int(float(maxLaserScans) * ratio);
796  }
797  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
798  {
799  //compute normals
800  pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
801  util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
802  util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
803  pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
804  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
805  scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
806  }
807  else
808  {
809  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
810  }
811  }
812  }
813  else
814  {
815  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
816  pcl::fromROSMsg(*cloudMsg, *pclScan);
817  if(pclScan->size() && scanDownsamplingStep_ > 1)
818  {
819  pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
820  if(pclScan->height>1)
821  {
822  maxLaserScans = pclScan->height * pclScan->width;
823  }
824  else
825  {
826  maxLaserScans /= scanDownsamplingStep_;
827  }
828  }
829  if(!pclScan->is_dense)
830  {
831  pclScan = util3d::removeNaNFromPointCloud(pclScan);
832  }
833 
834  if(pclScan->size())
835  {
836  if(scanVoxelSize_ > 0.0f)
837  {
838  float pointsBeforeFiltering = (float)pclScan->size();
839  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
840  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
841  maxLaserScans = int(float(maxLaserScans) * ratio);
842  }
843  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
844  {
845  //compute normals
846  pcl::PointCloud<pcl::Normal>::Ptr normals = is3D?
847  util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_):
848  util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
849  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
850  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
851  scan = is3D?util3d::laserScanFromPointCloud(*pclScanNormal):util3d::laserScan2dFromPointCloud(*pclScanNormal);
852  }
853  else
854  {
855  scan = is3D?util3d::laserScanFromPointCloud(*pclScan):util3d::laserScan2dFromPointCloud(*pclScan);
856  }
857  }
858  }
859 
860  LaserScan laserScan(scan,
861  maxLaserScans,
862  0,
863  localScanTransform);
864  if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
865  {
866  laserScan = util3d::rangeFiltering(laserScan, scanRangeMin_, scanRangeMax_);
867  }
868  if(!laserScan.isEmpty() && laserScan.hasNormals() && !laserScan.is2d() && scanNormalGroundUp_)
869  {
870  laserScan = util3d::adjustNormalsToViewPoint(laserScan, Eigen::Vector3f(0,0,10), (float)scanNormalGroundUp_);
871  }
872 
873  rtabmap::SensorData data(
874  laserScan,
875  cv::Mat(),
876  cv::Mat(),
877  rtabmap::CameraModel(),
878  0,
879  rtabmap_conversions::timestampFromROS(cloudMsg->header.stamp));
880 
881  this->processData(data, cloudMsg->header);
882  }
883 
884 protected:
885  virtual void flushCallbacks()
886  {
887  // flush callbacks
888  }
889 
890  void postProcessData(const SensorData & data, const std_msgs::Header & header) const
891  {
892  if(filtered_scan_pub_.getNumSubscribers())
893  {
894  sensor_msgs::PointCloud2 msg;
895  pcl_conversions::fromPCL(*rtabmap::util3d::laserScanToPointCloud2(data.laserScanRaw()), msg);
896  msg.header = header;
897  filtered_scan_pub_.publish(msg);
898  }
899  }
900 
901 private:
902  ros::Subscriber scan_sub_;
903  ros::Subscriber cloud_sub_;
904  ros::Publisher filtered_scan_pub_;
905  int scanCloudMaxPoints_;
906  bool scanCloudIs2d_;
907  int scanDownsamplingStep_;
908  double scanRangeMin_;
909  double scanRangeMax_;
910  double scanVoxelSize_;
911  int scanNormalK_;
912  double scanNormalRadius_;
913  double scanNormalGroundUp_;
914  bool deskewing_;
915  bool deskewingSlerp_;
916  std::vector<boost::shared_ptr<rtabmap_odom::PluginInterface> > plugins_;
917  pluginlib::ClassLoader<rtabmap_odom::PluginInterface> plugin_loader_;
918  bool scanReceived_ = false;
919  bool cloudReceived_ = false;
920 
921 };
922 
923 PLUGINLIB_EXPORT_CLASS(rtabmap_odom::ICPOdometry, nodelet::Nodelet);
924 
925 }
rtabmap::SensorData
XmlRpc::XmlRpcValue::size
int size() const
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
int
int
rtabmap_odom
Definition: OdometryROS.h:57
compare
bool compare
rtabmap_odom::ICPOdometry::scan_sub_
ros::Subscriber scan_sub_
Definition: icp_odometry.cpp:902
rtabmap_conversions::deskew
bool deskew(const sensor_msgs::PointCloud2 &input, sensor_msgs::PointCloud2 &output, const std::string &fixedFrameId, tf::TransformListener &listener, double waitForTransform, bool slerp=false)
rtabmap_odom::ICPOdometry::scanNormalGroundUp_
double scanNormalGroundUp_
Definition: icp_odometry.cpp:913
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
msg
msg
ros::Publisher
DurationBase< Duration >::fromSec
Duration & fromSec(double t)
rtabmap_odom::ICPOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: icp_odometry.cpp:167
height
height
boost::shared_ptr
rtabmap_conversions::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
laser_geometry::LaserProjection
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
uFormat
std::string uFormat(const char *fmt,...)
rtabmap_odom::ICPOdometry::onOdomInit
virtual void onOdomInit()
Definition: icp_odometry.cpp:88
this
this
rtabmap::CameraModel
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
rtabmap_odom::ICPOdometry::scanCloudIs2d_
bool scanCloudIs2d_
Definition: icp_odometry.cpp:906
rtabmap_odom::ICPOdometry::scanDownsamplingStep_
int scanDownsamplingStep_
Definition: icp_odometry.cpp:907
projection
Expression< Point2 > projection(f, p_cam)
rtabmap_odom::ICPOdometry::filtered_scan_pub_
ros::Publisher filtered_scan_pub_
Definition: icp_odometry.cpp:904
frameId
string frameId
type
UStl.h
NODELET_WARN
#define NODELET_WARN(...)
rtabmap::util3d::adjustNormalsToViewPoint
RTABMAP_DEPRECATED LaserScan RTABMAP_CORE_EXPORT adjustNormalsToViewPoint(const LaserScan &scan, const Eigen::Vector3f &viewpoint, bool forceGroundNormalsUp)
true
#define true
rtabmap::LaserScan::hasNormals
bool hasNormals() const
rtabmap::LaserScan
ParametersMap
std::map< std::string, std::string > ParametersMap
pcl::fromROSMsg
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
f
Vector3 f(-6, 1, 0.5)
uNumber2Str
std::string uNumber2Str(double number, int precision, bool fixed)
rtabmap::LaserScan::size
int size() const
rtabmap::Transform::isNull
bool isNull() const
rtabmap_odom::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::ICPOdometry, nodelet::Nodelet)
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
rtabmap_odom::ICPOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: icp_odometry.cpp:885
data
data
rtabmap_odom::ICPOdometry::deskewing_
bool deskewing_
Definition: icp_odometry.cpp:914
transforms.h
rtabmap_odom::ICPOdometry::~ICPOdometry
virtual ~ICPOdometry()
Definition: icp_odometry.cpp:81
isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
pluginlib::PluginlibException
laser_geometry.h
pcl_conversions::fromPCL
void fromPCL(const pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
uInsert
void uInsert(std::map< K, V > &map, const std::map< K, V > &items)
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
rtabmap::LaserScan::is2d
bool is2d() const
rtabmap_odom::ICPOdometry::scanVoxelSize_
double scanVoxelSize_
Definition: icp_odometry.cpp:910
rtabmap_odom::ICPOdometry::postProcessData
void postProcessData(const SensorData &data, const std_msgs::Header &header) const
Definition: icp_odometry.cpp:890
rtabmap_odom::ICPOdometry::plugins_
std::vector< boost::shared_ptr< rtabmap_odom::PluginInterface > > plugins_
Definition: icp_odometry.cpp:916
PluginInterface.h
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScanFromPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, const pcl::IndicesPtr &indices, const Transform &transform=Transform(), bool filterNaNs=true)
rtabmap_odom::ICPOdometry::callbackScan
void callbackScan(const sensor_msgs::LaserScanConstPtr &scanMsg)
Definition: icp_odometry.cpp:326
rtabmap_odom::ICPOdometry::plugin_loader_
pluginlib::ClassLoader< rtabmap_odom::PluginInterface > plugin_loader_
Definition: icp_odometry.cpp:917
rtabmap::util3d::downsample
LaserScan RTABMAP_CORE_EXPORT downsample(const LaserScan &cloud, int step)
rtabmap::util3d::removeNaNFromPointCloud
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud(const pcl::PCLPointCloud2::Ptr &cloud)
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
ROS_WARN
#define ROS_WARN(...)
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
pluginlib::ClassLoader< rtabmap_odom::PluginInterface >
class_loader.hpp
rtabmap::util3d::computeNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_CORE_EXPORT computeNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
rtabmap_odom::ICPOdometry
Definition: icp_odometry.cpp:59
rtabmap_odom::ICPOdometry::callbackCloud
void callbackCloud(const sensor_msgs::PointCloud2ConstPtr &pointCloudMsg)
Definition: icp_odometry.cpp:566
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
uStr2Int
int uStr2Int(const std::string &str)
util3d_filtering.h
rtabmap_odom::OdometryROS
Definition: OdometryROS.h:59
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_odom::ICPOdometry::deskewingSlerp_
bool deskewingSlerp_
Definition: icp_odometry.cpp:915
rtabmap::Transform
empty
pcl_ros::transformPointCloud
void transformPointCloud(const Eigen::Matrix4f &transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out)
rtabmap_conversions::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
rtabmap_conversions::getMovingTransform
rtabmap::Transform getMovingTransform(const std::string &movingFrame, const std::string &fixedFrame, const ros::Time &stampFrom, const ros::Time &stampTo, tf::TransformListener &listener, double waitForTransform)
nodelet::Nodelet
laser_geometry::channel_option::Timestamp
Timestamp
iter
iterator iter(handle obj)
rtabmap_odom::ICPOdometry::scanRangeMin_
double scanRangeMin_
Definition: icp_odometry.cpp:908
nodelet.h
rtabmap_odom::ICPOdometry::scanRangeMax_
double scanRangeMax_
Definition: icp_odometry.cpp:909
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
ROS_ERROR
#define ROS_ERROR(...)
class_list_macros.hpp
rtabmap_odom::ICPOdometry::scanNormalK_
int scanNormalK_
Definition: icp_odometry.cpp:911
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
float
float
ULogger.h
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointNormal > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
rtabmap::util3d::rangeFiltering
LaserScan RTABMAP_CORE_EXPORT rangeFiltering(const LaserScan &scan, float rangeMin, float rangeMax)
false
#define false
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
util3d_surface.h
rtabmap_odom::ICPOdometry::scanNormalRadius_
double scanNormalRadius_
Definition: icp_odometry.cpp:912
header
const std::string header
uStr2Float
float uStr2Float(const std::string &str)
rtabmap_odom::ICPOdometry::cloud_sub_
ros::Subscriber cloud_sub_
Definition: icp_odometry.cpp:903
laser_geometry::channel_option::Intensity
Intensity
UConversion.h
OdometryROS.h
MsgConversion.h
rtabmap
util3d_transforms.h
rtabmap::util3d::computeFastOrganizedNormals2D
pcl::PointCloud< pcl::Normal >::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals2D(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, int searchK=5, float searchRadius=0.0f, const Eigen::Vector3f &viewPoint=Eigen::Vector3f(0, 0, 0))
ros::Duration
value
value
i
int i
rtabmap_odom::ICPOdometry::ICPOdometry
ICPOdometry()
Definition: icp_odometry.cpp:62
util3d.h
XmlRpc::XmlRpcValue
rtabmap_odom::ICPOdometry::scanCloudMaxPoints_
int scanCloudMaxPoints_
Definition: icp_odometry.cpp:905
int32_t
::int32_t int32_t
ros::NodeHandle
ros::Subscriber
util2d.h
pcl_conversions.h

rtabmap_odom
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:42:24