i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated for rtabmap 0.9.0

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-13 19:55:18 -04:00
parent 433f9fa3ae
commit 2b46da2773
14 changed files with 66 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -17,7 +17,7 @@ find_package(octomap_ros)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.8.12 REQUIRED)
find_package(RTABMap 0.9.0 REQUIRED)
#Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.8.12</version>
<version>0.9.0</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/CoreWrapper.cpp
+19 -14
View File
@@ -40,6 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h>
@@ -762,7 +767,7 @@ void CoreWrapper::commonDepthCallback(
return;
}
Transform t = odomT.inverse() * sensorT;
pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
pclScan = util3d::transformPointCloud(pclScan, t);
}
}
@@ -882,7 +887,7 @@ void CoreWrapper::commonStereoCallback(
return;
}
Transform t = odomT.inverse() * sensorT;
pclScan = util3d::transformPointCloud<pcl::PointXYZ>(pclScan, t);
pclScan = util3d::transformPointCloud(pclScan, t);
}
}
@@ -2164,15 +2169,15 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
if(cloudRGB->size() && cloudMaxDepth_ > 0)
{
cloudRGB = util3d::passThrough<pcl::PointXYZRGB>(cloudRGB, "z", 0, cloudMaxDepth_);
cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
}
if(cloudRGB->size() && cloudVoxelSize_ > 0)
{
cloudRGB = util3d::voxelize<pcl::PointXYZRGB>(cloudRGB, cloudVoxelSize_);
cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
}
if(cloudRGB->size())
{
cloudRGB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudRGB, localTransform);
cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
}
}
else
@@ -2200,7 +2205,7 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
leftMono = image;
}
cloudXYZ = rtabmap::util3d::cloudFromDisparity(
util3d::disparityFromStereoImages(leftMono, depth),
util2d::disparityFromStereoImages(leftMono, depth),
cx, cy,
fx, fy,
cloudDecimation_);
@@ -2215,16 +2220,16 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
{
if(cloudXYZ->size() && cloudMaxDepth_ > 0)
{
cloudXYZ = util3d::passThrough<pcl::PointXYZ>(cloudXYZ, "z", 0, cloudMaxDepth_);
cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
}
if(cloudXYZ->size() && gridCellSize_ > 0)
{
// use gridCellSize since this cloud is only for the projection map
cloudXYZ = util3d::voxelize<pcl::PointXYZ>(cloudXYZ, gridCellSize_);
cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
}
if(cloudXYZ->size())
{
cloudXYZ = util3d::transformPointCloud<pcl::PointXYZ>(cloudXYZ, localTransform);
cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
}
}
else
@@ -2251,11 +2256,11 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
if(cloudClipped->size() && projMaxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_);
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
}
}
@@ -2264,7 +2269,7 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZ>(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
}
if(cloudClipped->size())
{
@@ -2353,7 +2358,7 @@ void CoreWrapper::publishMaps(
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
if(jter != clouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZRGB>(jter->second, iter->second);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
++count;
}
@@ -2363,7 +2368,7 @@ void CoreWrapper::publishMaps(
{
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
{
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, cloudVoxelSize_);
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
}
ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
src/DbPlayerNode.cpp
+1 -1
View File
@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/DBReader.h>
bool paused = false;
src/GridMapAssemblerNode.cpp
+1 -1
View File
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/ULogger.h>
src/GuiWrapper.cpp
+1 -1
View File
@@ -47,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/utilite/UTimer.h>
#include "rtabmap_ros/MsgConversion.h"
src/MapAssemblerNode.cpp
+15 -11
View File
@@ -28,7 +28,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h>
@@ -143,23 +147,23 @@ public:
if(cloud->size() && cloudMaxDepth_ > 0)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloudMaxDepth_);
cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
}
if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indices, *tmp);
cloud = tmp;
}
if(cloud->size() && cloudVoxelSize_ > 0)
{
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloudVoxelSize_);
cloud = util3d::voxelize(cloud, cloudVoxelSize_);
}
if(cloud->size())
{
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = util3d::transformPointCloud(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud));
@@ -169,11 +173,11 @@ public:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(cloudClipped->size() && maxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
}
if(cloudClipped->size())
{
cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_);
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
cv::Mat ground, obstacles;
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_);
@@ -197,7 +201,7 @@ public:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
if(cloud->size() && scanVoxelSize_ > 0)
{
cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_);
cloud = util3d::voxelize(cloud, scanVoxelSize_);
}
if(cloud->size())
{
@@ -228,7 +232,7 @@ public:
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = rgbClouds_.find(iter->first);
if(jter != rgbClouds_.end())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZRGB>(jter->second, iter->second);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
@@ -237,7 +241,7 @@ public:
{
if(cloudVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud,cloudVoxelSize_);
assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -258,7 +262,7 @@ public:
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
if(jter != scans_.end())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud<pcl::PointXYZ>(jter->second, iter->second);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
*assembledCloud+=*transformed;
}
}
@@ -267,7 +271,7 @@ public:
{
if(scanVoxelSize_ > 0)
{
assembledCloud = util3d::voxelize<pcl::PointXYZ>(assembledCloud, scanVoxelSize_);
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
src/OdometryROS.cpp
+2 -2
View File
@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include "rtabmap_ros/MsgConversion.h"
@@ -396,7 +396,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
cloudTransformed = util3d::transformPointCloud(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
src/nodelets/data_throttle.cpp
+3 -3
View File
@@ -41,7 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
namespace rtabmap_ros
{
@@ -148,7 +148,7 @@ private:
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg());
}
else
@@ -164,7 +164,7 @@ private:
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg());
}
else
src/nodelets/obstacles_detection.cpp
+5 -2
View File
@@ -56,6 +56,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/MsgConversion.h>
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_transforms.h"
namespace rtabmap_ros
{
@@ -128,11 +131,11 @@ private:
pcl::IndicesPtr ground, obstacles;
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
if(maxObstaclesHeight_ > 0)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
}
if(cloud->size())
{
src/nodelets/point_cloud_xyz.cpp
+4 -3
View File
@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
namespace rtabmap_ros
{
@@ -212,12 +213,12 @@ private:
{
if(pclCloud->size() && maxDepth_ > 0)
{
pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
@@ -225,7 +226,7 @@ private:
if(pclCloud->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
}
sensor_msgs::PointCloud2 rosCloud;
src/nodelets/point_cloud_xyzrgb.cpp
+4 -3
View File
@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
namespace rtabmap_ros
{
@@ -252,12 +253,12 @@ private:
{
if(pclCloud->size() && maxDepth_ > 0)
{
pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", 0, maxDepth_);
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
@@ -265,7 +266,7 @@ private:
if(pclCloud->size() && voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_);
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
}
sensor_msgs::PointCloud2 rosCloud;
src/nodelets/stereo_throttle.cpp
+3 -3
View File
@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
namespace rtabmap_ros
{
@@ -146,7 +146,7 @@ private:
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageLeftPub_.publish(out.toImageMsg());
}
else
@@ -162,7 +162,7 @@ private:
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_);
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageRightPub_.publish(out.toImageMsg());
}
else
src/rviz/MapCloudDisplay.cpp
+6 -4
View File
@@ -50,6 +50,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "MapCloudDisplay.h"
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h>
@@ -282,19 +284,19 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
}
if(cloud_max_depth_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());
}
if(cloud_voxel_size_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
cloud = rtabmap::util3d::voxelize(cloud, cloud_voxel_size_->getFloat());
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
// do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);