i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.16.0: updated for upstream 0.16

Browse Source
This commit is contained in:
matlabbe
2018-02-08 21:42:15 -05:00
parent 709e7ddc06
commit 825a556c1b
6 changed files with 33 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -18,7 +18,7 @@ find_package(rviz)
## 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.15.4 REQUIRED) find_package(RTABMap 0.16.0 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
include/rtabmap_ros/MapsManager.h
+1 -1
View File
@@ -111,7 +111,7 @@ private:
std::map<int, rtabmap::Transform> gridPoses_; std::map<int, rtabmap::Transform> gridPoses_;
cv::Mat gridMap_; cv::Mat gridMap_;
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles> std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > gridMaps_; // < <ground, obstacles>, empty cells >
std::map<int, cv::Point3f> gridMapsViewpoints_; std::map<int, cv::Point3f> gridMapsViewpoints_;
rtabmap::OccupancyGrid * occupancyGrid_; rtabmap::OccupancyGrid * occupancyGrid_;
msg/NodeData.msg
+1
View File
@@ -48,6 +48,7 @@ uint8[] userData
# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
uint8[] grid_ground uint8[] grid_ground
uint8[] grid_obstacles uint8[] grid_obstacles
uint8[] grid_empty_cells
float32 grid_cell_size float32 grid_cell_size
Point3f grid_view_point Point3f grid_view_point
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.15.4</version> <version>0.16.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="[email protected]">Mathieu Labbe</maintainer> <maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <author>Mathieu Labbe</author>
src/MapsManager.cpp
+27 -26
View File
@@ -7,7 +7,6 @@ modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright * Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer. notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright * Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution. documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the * Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products names of its contributors may be used to endorse or promote products
@@ -110,7 +109,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_); pnh.param("octomap_occupancy_thr", octomapOccupancyThr_, octomapOccupancyThr_);
UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0); UASSERT(octomapOccupancyThr_>=0.0 && octomapOccupancyThr_<=1.0);
octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate()); octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_); pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
if(octomapTreeDepth_ > 16) if(octomapTreeDepth_ > 16)
{ {
@@ -287,7 +286,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
delete octomap_; delete octomap_;
octomap_ = 0; octomap_ = 0;
} }
octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate()); octomap_ = new OctoMap(occupancyGrid_->getCellSize(), octomapOccupancyThr_, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
#endif #endif
#endif #endif
} }
@@ -469,7 +468,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{ {
UDEBUG("Adding grid map %d to cache...", iter->first); UDEBUG("Adding grid map %d to cache...", iter->first);
cv::Point3f viewPoint; cv::Point3f viewPoint;
cv::Mat ground, obstacles; cv::Mat ground, obstacles, emptyCells;
if(iter->first > 0) if(iter->first > 0)
{ {
cv::Mat rgb, depth, scan; cv::Mat rgb, depth, scan;
@@ -494,20 +493,21 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
0, 0,
generateGrid?0:&ground, generateGrid?0:&ground,
generateGrid?0:&obstacles); generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid) if(generateGrid)
{ {
Signature tmp(data); Signature tmp(data);
tmp.setPose(iter->second); tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, viewPoint); occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
} }
else else
{ {
viewPoint = data.gridViewPoint(); viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
} }
} }
@@ -533,20 +533,21 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
!occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0, !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
0, 0,
generateGrid?0:&ground, generateGrid?0:&ground,
generateGrid?0:&obstacles); generateGrid?0:&obstacles,
generateGrid?0:&emptyCells);
if(generateGrid) if(generateGrid)
{ {
Signature tmp(data); Signature tmp(data);
tmp.setPose(iter->second); tmp.setPose(iter->second);
occupancyGrid_->createLocalMap(tmp, ground, obstacles, viewPoint); occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
} }
else else
{ {
viewPoint = data.gridViewPoint(); viewPoint = data.gridViewPoint();
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint)); uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
} }
@@ -569,12 +570,12 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
(iter->first < 0 || (iter->first < 0 ||
occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end())) occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
{ {
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
if(mter != gridMaps_.end()) if(mter != gridMaps_.end())
{ {
if(!mter->second.first.empty() || !mter->second.second.empty()) if(!mter->second.first.first.empty() || !mter->second.first.second.empty())
{ {
occupancyGrid_->addToCache(iter->first, mter->second.first, mter->second.second); occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second);
} }
} }
} }
@@ -585,16 +586,16 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
(iter->first < 0 || (iter->first < 0 ||
octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end())) octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
{ {
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = gridMaps_.find(iter->first); std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
std::map<int, cv::Point3f>::iterator pter = gridMapsViewpoints_.find(iter->first); std::map<int, cv::Point3f>::iterator pter = gridMapsViewpoints_.find(iter->first);
if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end()) if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end())
{ {
if((mter->second.first.empty() || mter->second.first.channels() > 2) && if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) &&
(mter->second.second.empty() || mter->second.second.channels() > 2)) (mter->second.first.second.empty() || mter->second.first.second.channels() > 2))
{ {
octomap_->addToCache(iter->first, mter->second.first, mter->second.second, pter->second); octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second);
} }
else if(!mter->second.first.empty() && !mter->second.second.empty()) else if(!mter->second.first.first.empty() && !mter->second.first.second.empty())
{ {
ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. " ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. "
"Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see " "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
@@ -627,7 +628,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
} }
#endif #endif
#endif #endif
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=gridMaps_.begin(); for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=gridMaps_.begin();
iter!=gridMaps_.end();) iter!=gridMaps_.end();)
{ {
if(!uContains(poses, iter->first)) if(!uContains(poses, iter->first))
@@ -885,16 +886,16 @@ void MapsManager::publishMaps(
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator jter = gridMaps_.find(iter->first); std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()) if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())
{ {
if(iter->first > 0) if(iter->first > 0)
{ {
assembledGroundPoses_.insert(*iter); assembledGroundPoses_.insert(*iter);
} }
if(jter!=gridMaps_.end() && jter->second.first.cols) if(jter!=gridMaps_.end() && jter->second.first.first.cols)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first, iter->second, 0, 255, 0); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.first, iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed; pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_) if(cloudSubtractFiltering_)
{ {
@@ -939,9 +940,9 @@ void MapsManager::publishMaps(
{ {
assembledObstaclePoses_.insert(*iter); assembledObstaclePoses_.insert(*iter);
} }
if(jter!=gridMaps_.end() && jter->second.second.cols) if(jter!=gridMaps_.end() && jter->second.first.second.cols)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.second, iter->second, 255, 0, 0); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(jter->second.first.second, iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed; pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_) if(cloudSubtractFiltering_)
{ {
src/MsgConversion.cpp
+2
View File
@@ -735,6 +735,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
s.sensorData().setOccupancyGrid( s.sensorData().setOccupancyGrid(
compressedMatFromBytes(msg.grid_ground), compressedMatFromBytes(msg.grid_ground),
compressedMatFromBytes(msg.grid_obstacles), compressedMatFromBytes(msg.grid_obstacles),
compressedMatFromBytes(msg.grid_empty_cells),
msg.grid_cell_size, msg.grid_cell_size,
point3fFromROS(msg.grid_view_point)); point3fFromROS(msg.grid_view_point));
s.sensorData().setGPS(rtabmap::GPS(msg.gps.stamp, msg.gps.longitude, msg.gps.latitude, msg.gps.altitude, msg.gps.error, msg.gps.bearing)); s.sensorData().setGPS(rtabmap::GPS(msg.gps.stamp, msg.gps.longitude, msg.gps.latitude, msg.gps.altitude, msg.gps.error, msg.gps.bearing));
@@ -762,6 +763,7 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData); compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground); compressedMatToBytes(signature.sensorData().gridGroundCellsCompressed(), msg.grid_ground);
compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles); compressedMatToBytes(signature.sensorData().gridObstacleCellsCompressed(), msg.grid_obstacles);
compressedMatToBytes(signature.sensorData().gridEmptyCellsCompressed(), msg.grid_empty_cells);
point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point); point3fToROS(signature.sensorData().gridViewPoint(), msg.grid_view_point);
msg.grid_cell_size = signature.sensorData().gridCellSize(); msg.grid_cell_size = signature.sensorData().gridCellSize();
msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints(); msg.laserScanMaxPts = signature.sensorData().laserScanInfo().maxPoints();