rgbdicp_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 
29 
31 #include <nodelet/nodelet.h>
32 
36 
39 
42 
43 #include <sensor_msgs/Image.h>
45 #include <cv_bridge/cv_bridge.h>
46 #include <sensor_msgs/LaserScan.h>
47 #include <sensor_msgs/PointCloud2.h>
49 
51 
52 #include <rtabmap/core/util3d.h>
56 #include <rtabmap/core/util2d.h>
59 #include <rtabmap/utilite/UStl.h>
60 
61 using namespace rtabmap;
62 
63 namespace rtabmap_ros
64 {
65 
67 {
68 public:
71  approxScanSync_(0),
72  exactScanSync_(0),
73  approxCloudSync_(0),
74  exactCloudSync_(0),
75  queueSize_(5),
76  keepColor_(false),
77  scanCloudMaxPoints_(0),
78  scanVoxelSize_(0.0),
79  scanNormalK_(0),
80  scanNormalRadius_(0.0)
81  {
82  }
83 
84  virtual ~RGBDICPOdometry()
85  {
86  if(approxScanSync_)
87  {
88  delete approxScanSync_;
89  }
90  if(exactScanSync_)
91  {
92  delete exactScanSync_;
93  }
94  if(approxCloudSync_)
95  {
96  delete approxCloudSync_;
97  }
98  if(exactCloudSync_)
99  {
100  delete exactCloudSync_;
101  }
102  }
103 
104 private:
105 
106  virtual void onOdomInit()
107  {
108  ros::NodeHandle & nh = getNodeHandle();
109  ros::NodeHandle & pnh = getPrivateNodeHandle();
110 
111  bool approxSync = true;
112  bool subscribeScanCloud = false;
113  pnh.param("approx_sync", approxSync, approxSync);
114  pnh.param("queue_size", queueSize_, queueSize_);
115  pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
116  pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
117  pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
118  pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
119  if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
120  {
121  ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
122  "The value is still used. Use \"scan_normal_k\" to avoid this warning.");
123  pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
124  }
125  pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
126  pnh.param("keep_color", keepColor_, keepColor_);
127 
128  NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
129  NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_);
130  NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
131  NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
132  NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
133  NODELET_INFO("RGBDIcpOdometry: scan_normal_k = %d", scanNormalK_);
134  NODELET_INFO("RGBDIcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
135  NODELET_INFO("RGBDIcpOdometry: keep_color = %s", keepColor_?"true":"false");
136 
137  ros::NodeHandle rgb_nh(nh, "rgb");
138  ros::NodeHandle depth_nh(nh, "depth");
139  ros::NodeHandle rgb_pnh(pnh, "rgb");
140  ros::NodeHandle depth_pnh(pnh, "depth");
141  image_transport::ImageTransport rgb_it(rgb_nh);
142  image_transport::ImageTransport depth_it(depth_nh);
143  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
144  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
145 
146  image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
147  image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
148  info_sub_.subscribe(rgb_nh, "camera_info", 1);
149 
150  std::string subscribedTopicsMsg;
151  if(subscribeScanCloud)
152  {
153  cloud_sub_.subscribe(nh, "scan_cloud", 1);
154  if(approxSync)
155  {
156  approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
157  approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
158  }
159  else
160  {
161  exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
162  exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
163  }
164 
165  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
166  getName().c_str(),
167  approxSync?"approx":"exact",
168  image_mono_sub_.getTopic().c_str(),
169  image_depth_sub_.getTopic().c_str(),
170  info_sub_.getTopic().c_str(),
171  cloud_sub_.getTopic().c_str());
172  }
173  else
174  {
175  scan_sub_.subscribe(nh, "scan", 1);
176  if(approxSync)
177  {
178  approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
179  approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
180  }
181  else
182  {
183  exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
184  exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
185  }
186 
187  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
188  getName().c_str(),
189  approxSync?"approx":"exact",
190  image_mono_sub_.getTopic().c_str(),
191  image_depth_sub_.getTopic().c_str(),
192  info_sub_.getTopic().c_str(),
193  scan_sub_.getTopic().c_str());
194  }
195  this->startWarningThread(subscribedTopicsMsg, approxSync);
196  }
197 
198  virtual void updateParameters(ParametersMap & parameters)
199  {
200  //make sure we are using Reg/Strategy=0
201  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
202  if(iter != parameters.end() && iter->second.compare("0") != 0)
203  {
204  ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str());
205  }
206  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
207  }
208 
210  const sensor_msgs::ImageConstPtr& image,
211  const sensor_msgs::ImageConstPtr& depth,
212  const sensor_msgs::CameraInfoConstPtr& cameraInfo,
213  const sensor_msgs::LaserScanConstPtr& scanMsg)
214  {
215  sensor_msgs::PointCloud2ConstPtr cloudMsg;
216  callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
217  }
218 
220  const sensor_msgs::ImageConstPtr& image,
221  const sensor_msgs::ImageConstPtr& depth,
222  const sensor_msgs::CameraInfoConstPtr& cameraInfo,
223  const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
224  {
225  sensor_msgs::LaserScanConstPtr scanMsg;
226  callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
227  }
228 
230  const sensor_msgs::ImageConstPtr& image,
231  const sensor_msgs::ImageConstPtr& depth,
232  const sensor_msgs::CameraInfoConstPtr& cameraInfo,
233  const sensor_msgs::LaserScanConstPtr& scanMsg,
234  const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
235  {
236  callbackCalled();
237  if(!this->isPaused())
238  {
239  if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
240  image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
241  image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
242  image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
243  image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
244  image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
245  image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
246  image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
247  !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
248  depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
249  depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
250  {
251  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 "
252  "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
253  image->encoding.c_str(), depth->encoding.c_str());
254  return;
255  }
256 
257  // use the highest stamp to make sure that there will be no future interpolation required when synchronized with another node
258  ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
259  if(scanMsg.get() != 0)
260  {
261  if(stamp < scanMsg->header.stamp)
262  {
263  stamp = scanMsg->header.stamp;
264  }
265  }
266  else if(cloudMsg.get() != 0)
267  {
268  if(stamp < cloudMsg->header.stamp)
269  {
270  stamp = cloudMsg->header.stamp;
271  }
272  }
273 
274  Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
275  if(localTransform.isNull())
276  {
277  return;
278  }
279 
280  if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
281  {
282  rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
284  image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
285  image->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
286  keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
287  cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
288 
289  cv::Mat scan;
290  Transform localScanTransform = Transform::getIdentity();
291  int maxLaserScans = 0;
292  if(scanMsg.get() != 0)
293  {
294  // make sure the frame of the laser is updated too
295  localScanTransform = getTransform(this->frameId(),
296  scanMsg->header.frame_id,
297  scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
298  if(localScanTransform.isNull())
299  {
300  ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
301  return;
302  }
303 
304  //transform in frameId_ frame
305  sensor_msgs::PointCloud2 scanOut;
307  projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
308  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
309  pcl::fromROSMsg(scanOut, *pclScan);
310  pclScan->is_dense = true;
311 
312  maxLaserScans = (int)scanMsg->ranges.size();
313  if(pclScan->size())
314  {
315  if(scanVoxelSize_ > 0.0f)
316  {
317  float pointsBeforeFiltering = (float)pclScan->size();
318  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
319  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
320  maxLaserScans = int(float(maxLaserScans) * ratio);
321  }
322  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
323  {
324  //compute normals
325  pcl::PointCloud<pcl::Normal>::Ptr normals;
326  if(scanVoxelSize_ > 0.0f)
327  {
328  normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
329  }
330  else
331  {
332  normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
333  }
334  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
335  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
336  scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
337  }
338  else
339  {
340  scan = util3d::laserScan2dFromPointCloud(*pclScan);
341  }
342  }
343  }
344  else if(cloudMsg.get() != 0)
345  {
346  bool containNormals = false;
347  if(scanVoxelSize_ == 0.0f)
348  {
349  for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
350  {
351  if(cloudMsg->fields[i].name.compare("normal_x") == 0)
352  {
353  containNormals = true;
354  break;
355  }
356  }
357  }
358  localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
359  if(localScanTransform.isNull())
360  {
361  ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
362  return;
363  }
364 
365  maxLaserScans = scanCloudMaxPoints_;
366  if(containNormals)
367  {
368  pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
369  pcl::fromROSMsg(*cloudMsg, *pclScan);
370  if(!pclScan->is_dense)
371  {
372  pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
373  }
374  scan = util3d::laserScanFromPointCloud(*pclScan);
375  }
376  else
377  {
378  pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
379  pcl::fromROSMsg(*cloudMsg, *pclScan);
380  if(!pclScan->is_dense)
381  {
382  pclScan = util3d::removeNaNFromPointCloud(pclScan);
383  }
384 
385  if(pclScan->size())
386  {
387  if(scanVoxelSize_ > 0.0f)
388  {
389  float pointsBeforeFiltering = (float)pclScan->size();
390  pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
391  float ratio = float(pclScan->size()) / pointsBeforeFiltering;
392  maxLaserScans = int(float(maxLaserScans) * ratio);
393  }
394  if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
395  {
396  //compute normals
397  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
398  pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
399  pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
400  scan = util3d::laserScanFromPointCloud(*pclScanNormal);
401  }
402  else
403  {
404  scan = util3d::laserScanFromPointCloud(*pclScan);
405  }
406  }
407  }
408  }
409 
410  rtabmap::SensorData data(
412  scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
413  scanMsg.get() != 0?scanMsg->range_max:0,
414  localScanTransform),
415  ptrImage->image,
416  ptrDepth->image,
417  rtabmapModel,
418  0,
420 
421  this->processData(data, stamp, image->header.frame_id);
422  }
423  }
424  }
425 
426 protected:
427  virtual void flushCallbacks()
428  {
429  // flush callbacks
430  if(approxScanSync_)
431  {
432  delete approxScanSync_;
433  approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
434  approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
435  }
436  if(exactScanSync_)
437  {
438  delete exactScanSync_;
439  exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
440  exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
441  }
442  if(approxCloudSync_)
443  {
444  delete approxCloudSync_;
445  approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
446  approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
447  }
448  if(exactCloudSync_)
449  {
450  delete exactCloudSync_;
451  exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
452  exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
453  }
454  }
455 
456 private:
476 };
477 
479 
480 }
const std::string BAYER_GRBG8
void callbackScan(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo, const sensor_msgs::LaserScanConstPtr &scanMsg)
message_filters::Subscriber< sensor_msgs::PointCloud2 > cloud_sub_
std::string uFormat(const char *fmt,...)
#define NODELET_ERROR(...)
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))
f
image_transport::SubscriberFilter image_mono_sub_
message_filters::Synchronizer< MyApproxScanSyncPolicy > * approxScanSync_
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2 > MyApproxCloudSyncPolicy
std::pair< std::string, std::string > ParametersPair
std::string resolveName(const std::string &name, bool remap=true) const
static Transform getIdentity()
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
image_transport::SubscriberFilter image_depth_sub_
message_filters::Subscriber< sensor_msgs::LaserScan > scan_sub_
std::string getName(void *handle)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan > MyExactScanSyncPolicy
message_filters::Subscriber< sensor_msgs::CameraInfo > info_sub_
const std::string TYPE_8UC1
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
message_filters::Synchronizer< MyExactCloudSyncPolicy > * exactCloudSync_
#define ROS_WARN(...)
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
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))
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet)
void callbackCloud(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo, const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
void callbackCommon(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo, const sensor_msgs::LaserScanConstPtr &scanMsg, const sensor_msgs::PointCloud2ConstPtr &cloudMsg)
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
#define true
bool isNull() const
Connection registerCallback(C &callback)
message_filters::Synchronizer< MyExactScanSyncPolicy > * exactScanSync_
message_filters::Synchronizer< MyApproxCloudSyncPolicy > * approxCloudSync_
Duration & fromSec(double t)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const std::string TYPE_32FC1
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2 > MyExactCloudSyncPolicy
rtabmap::CameraModel cameraModelFromROS(const sensor_msgs::CameraInfo &camInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
const std::string TYPE_16UC1
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
QMap< QString, QVariant > ParametersMap
const std::string MONO16
virtual void updateParameters(ParametersMap &parameters)
string frameId
Definition: patrol.py:11
#define false
#define NODELET_INFO(...)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan > MyApproxScanSyncPolicy
double timestampFromROS(const ros::Time &stamp)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
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)
bool hasParam(const std::string &key) const
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
#define ROS_ERROR(...)
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
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