i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 14:50: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 ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.8.12 REQUIRED) find_package(RTABMap 0.9.0 REQUIRED)
#Qt stuff #Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED) FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>rtabmap_ros</name> <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> <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> <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <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/Camera.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
#include <rtabmap/core/util3d.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/Graph.h>
#include <rtabmap/core/Memory.h> #include <rtabmap/core/Memory.h>
#include <rtabmap/core/VWDictionary.h> #include <rtabmap/core/VWDictionary.h>
@@ -762,7 +767,7 @@ void CoreWrapper::commonDepthCallback(
return; return;
} }
Transform t = odomT.inverse() * sensorT; 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; return;
} }
Transform t = odomT.inverse() * sensorT; 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) 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) if(cloudRGB->size() && cloudVoxelSize_ > 0)
{ {
cloudRGB = util3d::voxelize<pcl::PointXYZRGB>(cloudRGB, cloudVoxelSize_); cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
} }
if(cloudRGB->size()) if(cloudRGB->size())
{ {
cloudRGB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudRGB, localTransform); cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
} }
} }
else else
@@ -2200,7 +2205,7 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
leftMono = image; leftMono = image;
} }
cloudXYZ = rtabmap::util3d::cloudFromDisparity( cloudXYZ = rtabmap::util3d::cloudFromDisparity(
util3d::disparityFromStereoImages(leftMono, depth), util2d::disparityFromStereoImages(leftMono, depth),
cx, cy, cx, cy,
fx, fy, fx, fy,
cloudDecimation_); cloudDecimation_);
@@ -2215,16 +2220,16 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
{ {
if(cloudXYZ->size() && cloudMaxDepth_ > 0) 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) if(cloudXYZ->size() && gridCellSize_ > 0)
{ {
// use gridCellSize since this cloud is only for the projection map // 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()) if(cloudXYZ->size())
{ {
cloudXYZ = util3d::transformPointCloud<pcl::PointXYZ>(cloudXYZ, localTransform); cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
} }
} }
else else
@@ -2251,11 +2256,11 @@ std::map<int, rtabmap::Transform> CoreWrapper::updateMapCaches(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
if(cloudClipped->size() && projMaxHeight_ > 0) 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()) 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_); 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; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxHeight_ > 0) 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()) if(cloudClipped->size())
{ {
@@ -2353,7 +2358,7 @@ void CoreWrapper::publishMaps(
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first); std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
if(jter != clouds_.end()) 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; *assembledCloud+=*transformed;
++count; ++count;
} }
@@ -2363,7 +2368,7 @@ void CoreWrapper::publishMaps(
{ {
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_) 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()); 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 <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h> #include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/DBReader.h> #include <rtabmap/core/DBReader.h>
bool paused = false; 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 <ros/ros.h>
#include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.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/Graph.h>
#include <rtabmap/core/Compression.h> #include <rtabmap/core/Compression.h>
#include <rtabmap/utilite/ULogger.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/Parameters.h>
#include <rtabmap/core/ParamEvent.h> #include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/OdometryEvent.h> #include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include "rtabmap_ros/MsgConversion.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 <ros/ros.h>
#include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.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/Compression.h>
#include <rtabmap/core/Graph.h> #include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
@@ -143,23 +147,23 @@ public:
if(cloud->size() && cloudMaxDepth_ > 0) 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) 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::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indices, *tmp); pcl::copyPointCloud(*cloud, *indices, *tmp);
cloud = tmp; cloud = tmp;
} }
if(cloud->size() && cloudVoxelSize_ > 0) if(cloud->size() && cloudVoxelSize_ > 0)
{ {
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloudVoxelSize_); cloud = util3d::voxelize(cloud, cloudVoxelSize_);
} }
if(cloud->size()) if(cloud->size())
{ {
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform); cloud = util3d::transformPointCloud(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud)); rgbClouds_.insert(std::make_pair(id, cloud));
@@ -169,11 +173,11 @@ public:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(cloudClipped->size() && maxHeight_ > 0) 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()) if(cloudClipped->size())
{ {
cloudClipped = util3d::voxelize<pcl::PointXYZRGB>(cloudClipped, gridCellSize_); cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_); util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_);
@@ -197,7 +201,7 @@ public:
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
if(cloud->size() && scanVoxelSize_ > 0) if(cloud->size() && scanVoxelSize_ > 0)
{ {
cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_); cloud = util3d::voxelize(cloud, scanVoxelSize_);
} }
if(cloud->size()) if(cloud->size())
{ {
@@ -228,7 +232,7 @@ public:
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = rgbClouds_.find(iter->first); std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = rgbClouds_.find(iter->first);
if(jter != rgbClouds_.end()) 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; *assembledCloud+=*transformed;
} }
} }
@@ -237,7 +241,7 @@ public:
{ {
if(cloudVoxelSize_ > 0) if(cloudVoxelSize_ > 0)
{ {
assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud,cloudVoxelSize_); assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
} }
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); 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); std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
if(jter != scans_.end()) 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; *assembledCloud+=*transformed;
} }
} }
@@ -267,7 +271,7 @@ public:
{ {
if(scanVoxelSize_ > 0) if(scanVoxelSize_ > 0)
{ {
assembledCloud = util3d::voxelize<pcl::PointXYZ>(assembledCloud, scanVoxelSize_); assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
} }
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); 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 <cv_bridge/cv_bridge.h>
#include <rtabmap/core/Odometry.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/Memory.h>
#include <rtabmap/core/Signature.h> #include <rtabmap/core/Signature.h>
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
@@ -396,7 +396,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
if(cloud->size()) if(cloud->size())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose); cloudTransformed = util3d::transformPointCloud(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg); pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image 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 <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util2d.h>
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -148,7 +148,7 @@ private:
cv_bridge::CvImage out; cv_bridge::CvImage out;
out.header = imagePtr->header; out.header = imagePtr->header;
out.encoding = imagePtr->encoding; out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_); out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg()); imagePub_.publish(out.toImageMsg());
} }
else else
@@ -164,7 +164,7 @@ private:
cv_bridge::CvImage out; cv_bridge::CvImage out;
out.header = imagePtr->header; out.header = imagePtr->header;
out.encoding = imagePtr->encoding; out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_); out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg()); imageDepthPub_.publish(out.toImageMsg());
} }
else 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_ros/MsgConversion.h>
#include "rtabmap/core/util3d.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 namespace rtabmap_ros
{ {
@@ -128,11 +131,11 @@ private:
pcl::IndicesPtr ground, obstacles; pcl::IndicesPtr ground, obstacles;
if(cloud->size()) if(cloud->size())
{ {
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform); cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
if(maxObstaclesHeight_ > 0) 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()) 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 <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -212,12 +213,12 @@ private:
{ {
if(pclCloud->size() && maxDepth_ > 0) 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) 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::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp); pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp; pclCloud = tmp;
@@ -225,7 +226,7 @@ private:
if(pclCloud->size() && voxelSize_ > 0.0) if(pclCloud->size() && voxelSize_ > 0.0)
{ {
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
} }
sensor_msgs::PointCloud2 rosCloud; 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 <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -252,12 +253,12 @@ private:
{ {
if(pclCloud->size() && maxDepth_ > 0) 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) 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::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp); pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp; pclCloud = tmp;
@@ -265,7 +266,7 @@ private:
if(pclCloud->size() && voxelSize_ > 0.0) if(pclCloud->size() && voxelSize_ > 0.0)
{ {
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(pclCloud, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
} }
sensor_msgs::PointCloud2 rosCloud; 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 <cv_bridge/cv_bridge.h>
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util2d.h>
namespace rtabmap_ros namespace rtabmap_ros
{ {
@@ -146,7 +146,7 @@ private:
cv_bridge::CvImage out; cv_bridge::CvImage out;
out.header = imagePtr->header; out.header = imagePtr->header;
out.encoding = imagePtr->encoding; out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_); out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageLeftPub_.publish(out.toImageMsg()); imageLeftPub_.publish(out.toImageMsg());
} }
else else
@@ -162,7 +162,7 @@ private:
cv_bridge::CvImage out; cv_bridge::CvImage out;
out.header = imagePtr->header; out.header = imagePtr->header;
out.encoding = imagePtr->encoding; out.encoding = imagePtr->encoding;
out.image = rtabmap::util3d::decimate(imagePtr->image, decimation_); out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageRightPub_.publish(out.toImageMsg()); imageRightPub_.publish(out.toImageMsg());
} }
else 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 "MapCloudDisplay.h"
#include <rtabmap/core/Transform.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/util3d.h>
#include <rtabmap/core/Compression.h> #include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.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) 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) 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 // do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f) 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); sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);