Skip to content

Commit

Permalink
updated with upstream changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Dec 18, 2023
1 parent 596f637 commit be0736c
Show file tree
Hide file tree
Showing 4 changed files with 32 additions and 135 deletions.
2 changes: 1 addition & 1 deletion rtabmap_slam/src/CoreWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/core/LocalMapMaker.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Registration.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/LocalGridMaker.h>
#include <rtabmap/core/Optimizer.h>

#ifdef WITH_OCTOMAP_MSGS
Expand Down
10 changes: 5 additions & 5 deletions rtabmap_util/include/rtabmap_util/MapsManager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/FlannIndex.h>
#include <rtabmap/core/LocalGrid.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <ros/time.h>
Expand All @@ -40,7 +41,7 @@ namespace rtabmap {
class OctoMap;
class Memory;
class OccupancyGrid;
class LocalMapMaker;
class LocalGridMaker;
class GridMap;

} // namespace rtabmap
Expand Down Expand Up @@ -87,7 +88,7 @@ class MapsManager {

const rtabmap::OctoMap * getOctomap() const {return octomap_;}
const rtabmap::OccupancyGrid * getOccupancyGrid() const {return occupancyGrid_;}
const rtabmap::LocalMapMaker * getLocalMapMaker() const {return localMapMaker_;}
const rtabmap::LocalGridMaker * getLocalMapMaker() const {return localMapMaker_;}

private:
// mapping stuff
Expand Down Expand Up @@ -126,11 +127,10 @@ class MapsManager {
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > groundClouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > obstacleClouds_;

std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps_; // < <ground, obstacles>, empty cells >
std::map<int, cv::Point3f> localMapsViewpoints_;
rtabmap::LocalGridCache localMaps_;

rtabmap::OccupancyGrid * occupancyGrid_;
rtabmap::LocalMapMaker * localMapMaker_;
rtabmap::LocalGridMaker * localMapMaker_;
bool gridUpdated_;

rtabmap::OctoMap * octomap_;
Expand Down
151 changes: 24 additions & 127 deletions rtabmap_util/src/MapsManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Version.h>
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/core/LocalMapMaker.h>

#include <pcl/search/kdtree.h>

#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>

#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap/core/LocalGridMaker.h>

#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
#include <octomap_msgs/conversions.h>
Expand Down Expand Up @@ -73,18 +72,18 @@ MapsManager::MapsManager() :
scanEmptyRayTracing_(true),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
occupancyGrid_(new OccupancyGrid),
localMapMaker_(new LocalMapMaker),
occupancyGrid_(new OccupancyGrid(&localMaps_)),
localMapMaker_(new LocalGridMaker),
gridUpdated_(true),
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
octomap_(new OctoMap),
octomap_(new OctoMap(&localMaps_)),
#else
octomap_(0),
#endif
octomapTreeDepth_(16),
octomapUpdated_(true),
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_(new GridMap),
elevationMap_(new GridMap(&localMaps_)),
#else
elevationMap_(0),
#endif
Expand Down Expand Up @@ -330,16 +329,16 @@ void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, Paramet
void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
{
parameters_ = parameters;
occupancyGrid_ = new OccupancyGrid(parameters_);
localMapMaker_ = new LocalMapMaker(parameters_);
occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
localMapMaker_ = new LocalGridMaker(parameters_);

#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
octomap_ = new OctoMap(parameters_);
octomap_ = new OctoMap(&localMaps_, parameters_);
#endif

#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
Expand All @@ -348,7 +347,7 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
delete elevationMap_;
elevationMap_ = 0;
}
elevationMap_ = new GridMap(parameters_);
elevationMap_ = new GridMap(&localMaps_, parameters_);
#endif
}

Expand All @@ -366,8 +365,8 @@ void MapsManager::set2DMap(
{
for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
{
std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = localMaps_.find(iter->first);
if(!uContains(localMaps_, iter->first))
std::map<int, LocalGrid>::const_iterator jter = localMaps_.find(iter->first);
if(!uContains(localMaps_.localGrids(), iter->first))
{
rtabmap::SensorData data;
data = memory->getNodeData(iter->first, false, false, false, true);
Expand All @@ -387,23 +386,16 @@ void MapsManager::set2DMap(
&obstacles,
&emptyCells);

uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(localMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint()));
occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells);
localMaps_.add(iter->first, ground, obstacles, emptyCells, data.gridCellSize(), data.gridViewPoint());
}
}
else
{
occupancyGrid_->addToCache(iter->first, jter->second.first.first, jter->second.first.second, jter->second.second);
}
}
}
}

void MapsManager::clear()
{
localMaps_.clear();
localMapsViewpoints_.clear();
assembledGround_->clear();
assembledObstacles_->clear();
assembledGroundPoses_.clear();
Expand Down Expand Up @@ -584,7 +576,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(!iter->second.isNull())
{
rtabmap::SensorData data;
if(updateGridCache && (iter->first == 0 || !uContains(localMaps_, iter->first)))
if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first)))
{
ROS_DEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
Expand Down Expand Up @@ -639,14 +631,12 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
Signature tmp(data);
tmp.setPose(iter->second);
localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint));
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}
else
{
viewPoint = data.gridViewPoint();
uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint));
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}
}
else
Expand Down Expand Up @@ -680,14 +670,12 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
Signature tmp(data);
tmp.setPose(iter->second);
localMapMaker_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint));
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}
else
{
viewPoint = data.gridViewPoint();
uInsert(localMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
uInsert(localMapsViewpoints_, std::make_pair(iter->first, viewPoint));
localMaps_.add(iter->first, ground, obstacles, emptyCells, localMapMaker_->getCellSize(), viewPoint);
}

// put back
Expand All @@ -699,72 +687,6 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
}
}

if(updateGrid &&
(iter->first == 0 ||
occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = localMaps_.find(iter->first);
if(mter != localMaps_.end())
{
if(!mter->second.first.first.empty() || !mter->second.first.second.empty() || !mter->second.second.empty())
{
occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second);
}
}
}

#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
if(updateOctomap &&
(iter->first == 0 ||
octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = localMaps_.find(iter->first);
std::map<int, cv::Point3f>::iterator pter = localMapsViewpoints_.find(iter->first);
if(mter != localMaps_.end() && pter!=localMapsViewpoints_.end())
{
if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) &&
(mter->second.first.second.empty() || mter->second.first.second.channels() > 2) &&
(mter->second.second.empty() || mter->second.second.channels() > 2))
{
octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second);
}
else if(!mter->second.first.first.empty() && !mter->second.first.second.empty() && !mter->second.second.empty())
{
ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. "
"Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
"all occupancy grid parameters.",
iter->first);
}
}
}
#endif

#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if(updateElevation &&
(iter->first == 0 ||
elevationMap_->addedNodes().find(iter->first) == elevationMap_->addedNodes().end()))
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = localMaps_.find(iter->first);
std::map<int, cv::Point3f>::iterator pter = localMapsViewpoints_.find(iter->first);
if(mter != localMaps_.end() && pter!=localMapsViewpoints_.end())
{
if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) &&
(mter->second.first.second.empty() || mter->second.first.second.channels() > 2) &&
(mter->second.second.empty() || mter->second.second.channels() > 2))
{
elevationMap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second);
}
else if(!mter->second.first.first.empty() && !mter->second.first.second.empty() && !mter->second.second.empty())
{
ROS_WARN("Node %d: Cannot update elevation map with 2D occupancy grids. "
"Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
"all occupancy grid parameters.",
iter->first);
}
}
}
#endif
}
else
{
Expand Down Expand Up @@ -795,19 +717,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
#endif

for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin();
iter!=localMaps_.end();)
{
if(!uContains(poses, iter->first))
{
UASSERT(localMapsViewpoints_.erase(iter->first) != 0);
localMaps_.erase(iter++);
}
else
{
++iter;
}
}
localMaps_.clear(true);

for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
iter!=groundClouds_.end();)
Expand Down Expand Up @@ -1030,10 +940,6 @@ void MapsManager::publishMaps(
}
++countObstacles;
}
else
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = localMaps_.find(iter->first);
}
}
}
double addingPointsTime = t.ticks();
Expand All @@ -1056,16 +962,16 @@ void MapsManager::publishMaps(

for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = localMaps_.find(iter->first);
std::map<int, LocalGrid>::const_iterator jter = localMaps_.localGrids().find(iter->first);
if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())
{
if(iter->first > 0)
{
assembledGroundPoses_.insert(*iter);
}
if(jter!=localMaps_.end() && jter->second.first.first.cols)
if(jter!=localMaps_.end() && jter->second.groundCells.cols)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.groundCells), iter->second, 0, 255, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
Expand Down Expand Up @@ -1110,9 +1016,9 @@ void MapsManager::publishMaps(
{
assembledObstaclePoses_.insert(*iter);
}
if(jter!=localMaps_.end() && jter->second.first.second.cols)
if(jter!=localMaps_.end() && jter->second.obstacleCells.cols)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.obstacleCells), iter->second, 255, 0, 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
if(cloudSubtractFiltering_)
{
Expand Down Expand Up @@ -1605,19 +1511,10 @@ void MapsManager::publishMaps(
{
if(!localMaps_.empty())
{
size_t totalBytes = 0;
for(std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
{
totalBytes+= sizeof(int)+
iter->second.first.first.total()*iter->second.first.first.elemSize() +
iter->second.first.second.total()*iter->second.first.second.elemSize() +
iter->second.second.total()*iter->second.second.elemSize();
}
totalBytes += localMapsViewpoints_.size()*sizeof(int) + localMapsViewpoints_.size() * sizeof(cv::Point3f);
size_t totalBytes = localMaps_.getMemoryUsed();
ROS_INFO("MapsManager: cleanup %ld grid maps (~%ld MB)...", localMaps_.size(), totalBytes/1048576);
}
localMaps_.clear();
localMapsViewpoints_.clear();
}
}

Expand Down
4 changes: 2 additions & 2 deletions rtabmap_util/src/nodelets/obstacles_detection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
#include <rtabmap/core/LocalGridMaker.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>

#include <rtabmap_conversions/MsgConversion.h>

#include "rtabmap/core/LocalMapMaker.h"
#include "rtabmap/utilite/UStl.h"

namespace rtabmap_util
Expand Down Expand Up @@ -441,7 +441,7 @@ class ObstaclesDetection : public nodelet::Nodelet
std::string mapFrameId_;
bool waitForTransform_;

rtabmap::LocalMapMaker localMapMaker_;
rtabmap::LocalGridMaker localMapMaker_;
bool mapFrameProjection_;
bool warned_;

Expand Down

0 comments on commit be0736c

Please sign in to comment.