mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Removed grid_map_assembler, map_assembler is now using MapsManager (now use same published topics and mapping parameters than rtabmap), added all optimization parameters to map_optimizer (g2o and GTSAM can be selected)
This commit is contained in:
matlabbe
+2
-6
@@ -145,6 +145,7 @@ SET(rtabmap_ros_lib_src
|
||||
src/rviz/MapGraphDisplay.cpp
|
||||
src/rviz/InfoDisplay.cpp
|
||||
src/rviz/OrbitOrientedViewController.cpp
|
||||
src/MapsManager.cpp
|
||||
${MOC_FILES}
|
||||
)
|
||||
|
||||
@@ -186,7 +187,7 @@ SET(Libraries
|
||||
add_definitions(-DWITH_OCTOMAP)
|
||||
ENDIF(octomap_ros_FOUND)
|
||||
|
||||
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp src/MapsManager.cpp)
|
||||
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
|
||||
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
|
||||
|
||||
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
|
||||
@@ -201,9 +202,6 @@ target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
|
||||
add_executable(map_assembler src/MapAssemblerNode.cpp)
|
||||
target_link_libraries(map_assembler rtabmap_ros ${Libraries})
|
||||
|
||||
add_executable(grid_map_assembler src/GridMapAssemblerNode.cpp)
|
||||
target_link_libraries(grid_map_assembler rtabmap_ros ${Libraries})
|
||||
|
||||
add_executable(camera src/CameraNode.cpp)
|
||||
add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
|
||||
target_link_libraries(camera ${Libraries})
|
||||
@@ -244,7 +242,6 @@ install(TARGETS
|
||||
rgbd_odometry
|
||||
stereo_odometry
|
||||
map_assembler
|
||||
grid_map_assembler
|
||||
map_optimizer
|
||||
data_player
|
||||
camera
|
||||
@@ -259,7 +256,6 @@ install(TARGETS
|
||||
rgbd_odometry
|
||||
stereo_odometry
|
||||
map_assembler
|
||||
grid_map_assembler
|
||||
map_optimizer
|
||||
data_player
|
||||
camera
|
||||
|
||||
+8
-3
@@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
|
||||
genScan_(false),
|
||||
genScanMaxDepth_(4.0),
|
||||
mapToOdom_(rtabmap::Transform::getIdentity()),
|
||||
mapsManager_(true),
|
||||
depthSync_(0),
|
||||
depthScanSync_(0),
|
||||
stereoScanSync_(0),
|
||||
@@ -1244,6 +1245,7 @@ void CoreWrapper::process(
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
tmpSignature);
|
||||
|
||||
mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
|
||||
@@ -1663,6 +1665,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
|
||||
rtabmap_.getMemory(),
|
||||
false,
|
||||
true,
|
||||
false,
|
||||
false);
|
||||
if(filteredPoses.size())
|
||||
{
|
||||
@@ -1706,7 +1709,8 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
|
||||
rtabmap_.getMemory(),
|
||||
false,
|
||||
false,
|
||||
true);
|
||||
true,
|
||||
false);
|
||||
if(filteredPoses.size())
|
||||
{
|
||||
// create the grid map
|
||||
@@ -1818,6 +1822,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
signatures);
|
||||
}
|
||||
else
|
||||
@@ -2282,7 +2287,7 @@ bool CoreWrapper::octomapBinaryCallback(
|
||||
res.map.header.stamp = ros::Time::now();
|
||||
|
||||
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false);
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
|
||||
|
||||
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
|
||||
bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
|
||||
@@ -2302,7 +2307,7 @@ bool CoreWrapper::octomapFullCallback(
|
||||
res.map.header.stamp = ros::Time::now();
|
||||
|
||||
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false);
|
||||
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
|
||||
|
||||
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
|
||||
bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
|
||||
|
||||
@@ -1,199 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* 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.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
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_mapping.h>
|
||||
#include <rtabmap/core/Graph.h>
|
||||
#include <rtabmap/core/Compression.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UStl.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <nav_msgs/GetMap.h>
|
||||
#include <std_srvs/Empty.h>
|
||||
#include <pcl_ros/transforms.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
class GridMapAssembler
|
||||
{
|
||||
|
||||
public:
|
||||
GridMapAssembler() :
|
||||
gridCellSize_(0.05), // meters
|
||||
mapSize_(0), // meters
|
||||
eroded_(false),
|
||||
filterRadius_(0.5),
|
||||
filterAngle_(30.0) // degrees
|
||||
{
|
||||
ros::NodeHandle pnh("~");
|
||||
pnh.param("cell_size", gridCellSize_, gridCellSize_); // m
|
||||
pnh.param("map_size", mapSize_, mapSize_); // m
|
||||
pnh.param("filter_radius", filterRadius_, filterRadius_);
|
||||
pnh.param("filter_angle", filterAngle_, filterAngle_);
|
||||
pnh.param("eroded", eroded_, eroded_);
|
||||
|
||||
UASSERT(gridCellSize_ > 0.0);
|
||||
UASSERT(mapSize_ >= 0.0);
|
||||
|
||||
ros::NodeHandle nh;
|
||||
mapDataTopic_ = nh.subscribe("mapData", 1, &GridMapAssembler::mapDataReceivedCallback, this);
|
||||
|
||||
gridMap_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
|
||||
|
||||
//private service
|
||||
getMapService_ = pnh.advertiseService("get_map", &GridMapAssembler::getGridMapCallback, this);
|
||||
resetService_ = pnh.advertiseService("reset", &GridMapAssembler::reset, this);
|
||||
}
|
||||
|
||||
~GridMapAssembler()
|
||||
{
|
||||
}
|
||||
|
||||
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
|
||||
{
|
||||
UTimer timer;
|
||||
for(unsigned int i=0; i<msg->nodes.size(); ++i)
|
||||
{
|
||||
if(!uContains(gridMaps_, msg->nodes[i].id) && msg->nodes[i].laserScan.size())
|
||||
{
|
||||
cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
|
||||
if(!laserScan.empty())
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, gridCellSize_);
|
||||
|
||||
if(!ground.empty() || !obstacles.empty())
|
||||
{
|
||||
gridMaps_.insert(std::make_pair(msg->nodes[i].id, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
|
||||
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
|
||||
{
|
||||
poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
|
||||
}
|
||||
|
||||
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
|
||||
{
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses, filterRadius_, filterAngle_*CV_PI/180.0);
|
||||
}
|
||||
|
||||
if(gridMap_.getNumSubscribers())
|
||||
{
|
||||
// create the map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
//cv::Mat pixels = util3d::create2DMap(poses, scans_, gridCellSize_, gridUnknownSpaceFilled_, xMin, yMin, mapSize_);
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
gridMaps_,
|
||||
gridCellSize_,
|
||||
xMin, yMin,
|
||||
mapSize_,
|
||||
eroded_);
|
||||
|
||||
if(!pixels.empty())
|
||||
{
|
||||
//init
|
||||
map_.info.resolution = gridCellSize_;
|
||||
map_.info.origin.position.x = 0.0;
|
||||
map_.info.origin.position.y = 0.0;
|
||||
map_.info.origin.position.z = 0.0;
|
||||
map_.info.origin.orientation.x = 0.0;
|
||||
map_.info.origin.orientation.y = 0.0;
|
||||
map_.info.origin.orientation.z = 0.0;
|
||||
map_.info.origin.orientation.w = 1.0;
|
||||
|
||||
map_.info.width = pixels.cols;
|
||||
map_.info.height = pixels.rows;
|
||||
map_.info.origin.position.x = xMin;
|
||||
map_.info.origin.position.y = yMin;
|
||||
map_.data.resize(map_.info.width * map_.info.height);
|
||||
|
||||
memcpy(map_.data.data(), pixels.data, map_.info.width * map_.info.height);
|
||||
|
||||
map_.header.frame_id = msg->header.frame_id;
|
||||
map_.header.stamp = ros::Time::now();
|
||||
|
||||
gridMap_.publish(map_);
|
||||
ROS_INFO("Grid Map published [%d,%d] (%fs)", pixels.cols, pixels.rows, timer.ticks());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res)
|
||||
{
|
||||
if(map_.data.size())
|
||||
{
|
||||
res.map = map_;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("grid_map_assembler: reset!");
|
||||
gridMaps_.clear();
|
||||
map_ = nav_msgs::OccupancyGrid();
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
double gridCellSize_;
|
||||
double mapSize_;
|
||||
bool eroded_;
|
||||
double filterRadius_;
|
||||
double filterAngle_;
|
||||
|
||||
ros::Subscriber mapDataTopic_;
|
||||
|
||||
ros::Publisher gridMap_;
|
||||
|
||||
ros::ServiceServer getMapService_;
|
||||
ros::ServiceServer resetService_;
|
||||
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; //<ground,obstacles>
|
||||
|
||||
nav_msgs::OccupancyGrid map_;
|
||||
};
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "grid_map_assembler");
|
||||
GridMapAssembler assembler;
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
+32
-251
@@ -28,6 +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 "MapsManager.h"
|
||||
#include <rtabmap/core/util3d_transforms.h>
|
||||
#include <rtabmap/core/util3d.h>
|
||||
#include <rtabmap/core/util3d_filtering.h>
|
||||
@@ -49,54 +50,13 @@ class MapAssembler
|
||||
|
||||
public:
|
||||
MapAssembler() :
|
||||
cloudDecimation_(4),
|
||||
cloudMaxDepth_(4.0),
|
||||
cloudVoxelSize_(0.02),
|
||||
scanVoxelSize_(0.01),
|
||||
nodeFilteringAngle_(30), // degrees
|
||||
nodeFilteringRadius_(0.5),
|
||||
noiseFilterRadius_(0.0),
|
||||
noiseFilterMinNeighbors_(5),
|
||||
computeOccupancyGrid_(false),
|
||||
gridCellSize_(0.05),
|
||||
groundMaxAngle_(M_PI_4),
|
||||
clusterMinSize_(20),
|
||||
maxHeight_(0),
|
||||
occupancyMapSize_(0.0)
|
||||
mapsManager_(false)
|
||||
{
|
||||
ros::NodeHandle pnh("~");
|
||||
pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
|
||||
pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
|
||||
pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
|
||||
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
|
||||
|
||||
pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_);
|
||||
pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_);
|
||||
|
||||
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
|
||||
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
|
||||
|
||||
pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_);
|
||||
pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_);
|
||||
pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_);
|
||||
pnh.param("occupancy_cluster_min_size", clusterMinSize_, clusterMinSize_);
|
||||
pnh.param("occupancy_max_height", maxHeight_, maxHeight_);
|
||||
pnh.param("occupancy_map_size", occupancyMapSize_, occupancyMapSize_);
|
||||
|
||||
UASSERT(gridCellSize_ > 0);
|
||||
UASSERT(maxHeight_ >= 0);
|
||||
UASSERT(occupancyMapSize_ >=0.0);
|
||||
|
||||
ros::NodeHandle nh;
|
||||
mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);
|
||||
|
||||
assembledMapClouds_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_clouds", 1);
|
||||
assembledMapScans_ = nh.advertise<sensor_msgs::PointCloud2>("assembled_scans", 1);
|
||||
if(computeOccupancyGrid_)
|
||||
{
|
||||
occupancyMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_projection_map", 1);
|
||||
}
|
||||
|
||||
// private service
|
||||
resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
|
||||
}
|
||||
@@ -108,239 +68,60 @@ public:
|
||||
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
|
||||
{
|
||||
UTimer timer;
|
||||
|
||||
std::map<int, Transform> poses;
|
||||
std::multimap<int, Link> constraints;
|
||||
Transform mapOdom;
|
||||
rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
|
||||
for(unsigned int i=0; i<msg->nodes.size(); ++i)
|
||||
{
|
||||
int id = msg->nodes[i].id;
|
||||
if(!uContains(rgbClouds_, id))
|
||||
if(msg->nodes[i].image.size() ||
|
||||
msg->nodes[i].depth.size() ||
|
||||
msg->nodes[i].laserScan.size())
|
||||
{
|
||||
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
|
||||
if(!s.sensorData().imageCompressed().empty() &&
|
||||
!s.sensorData().depthOrRightCompressed().empty() &&
|
||||
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
|
||||
{
|
||||
cv::Mat image, depth;
|
||||
s.sensorData().uncompressData(&image, &depth, 0);
|
||||
|
||||
|
||||
if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
|
||||
cloud = rtabmap::util3d::cloudRGBFromSensorData(
|
||||
s.sensorData(),
|
||||
cloudDecimation_,
|
||||
cloudMaxDepth_);
|
||||
|
||||
if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
|
||||
{
|
||||
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(cloud, cloudVoxelSize_);
|
||||
}
|
||||
|
||||
if(cloud->size())
|
||||
{
|
||||
rgbClouds_.insert(std::make_pair(id, cloud));
|
||||
|
||||
if(computeOccupancyGrid_)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
|
||||
if(cloudClipped->size() && maxHeight_ > 0)
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
{
|
||||
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
|
||||
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_);
|
||||
if(!ground.empty() || !obstacles.empty())
|
||||
{
|
||||
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(!uContains(scans_, id) && msg->nodes[i].laserScan.size())
|
||||
{
|
||||
cv::Mat laserScan = rtabmap::uncompressData(msg->nodes[i].laserScan);
|
||||
if(!laserScan.empty())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
|
||||
if(cloud->size() && scanVoxelSize_ > 0)
|
||||
{
|
||||
cloud = util3d::voxelize(cloud, scanVoxelSize_);
|
||||
}
|
||||
if(cloud->size())
|
||||
{
|
||||
scans_.insert(std::make_pair(id, cloud));
|
||||
}
|
||||
}
|
||||
uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
|
||||
}
|
||||
}
|
||||
|
||||
// filter poses
|
||||
std::map<int, Transform> poses;
|
||||
UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
|
||||
for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
|
||||
// create a tmp signature with latest sensory data
|
||||
if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
|
||||
{
|
||||
poses.insert(std::make_pair(msg->graph.posesId[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
|
||||
}
|
||||
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
|
||||
{
|
||||
poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
|
||||
Signature tmpS = nodes_.at(poses.rbegin()->first);
|
||||
SensorData tmpData = tmpS.sensorData();
|
||||
tmpData.setId(-1);
|
||||
uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpData)));
|
||||
poses.insert(std::make_pair(-1, poses.rbegin()->second));
|
||||
}
|
||||
|
||||
if(assembledMapClouds_.getNumSubscribers())
|
||||
{
|
||||
// generate the assembled cloud!
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
// Update maps
|
||||
poses = mapsManager_.updateMapCaches(
|
||||
poses,
|
||||
0,
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
nodes_);
|
||||
|
||||
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
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(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
}
|
||||
}
|
||||
mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
|
||||
|
||||
if(assembledCloud->size())
|
||||
{
|
||||
if(cloudVoxelSize_ > 0)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud,cloudVoxelSize_);
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
|
||||
pcl::toROSMsg(*assembledCloud, *cloudMsg);
|
||||
cloudMsg->header.stamp = ros::Time::now();
|
||||
cloudMsg->header.frame_id = msg->header.frame_id;
|
||||
assembledMapClouds_.publish(cloudMsg);
|
||||
}
|
||||
}
|
||||
|
||||
if(assembledMapScans_.getNumSubscribers())
|
||||
{
|
||||
// generate the assembled scan!
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
|
||||
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
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(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
}
|
||||
}
|
||||
|
||||
if(assembledCloud->size())
|
||||
{
|
||||
if(scanVoxelSize_ > 0)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
|
||||
pcl::toROSMsg(*assembledCloud, *cloudMsg);
|
||||
cloudMsg->header.stamp = ros::Time::now();
|
||||
cloudMsg->header.frame_id = msg->header.frame_id;
|
||||
assembledMapScans_.publish(cloudMsg);
|
||||
}
|
||||
}
|
||||
|
||||
if(occupancyMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the map
|
||||
float xMin=0.0f, yMin=0.0f;
|
||||
cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
|
||||
poses,
|
||||
occupancyLocalMaps_,
|
||||
gridCellSize_, xMin, yMin,
|
||||
occupancyMapSize_);
|
||||
|
||||
if(!pixels.empty())
|
||||
{
|
||||
//init
|
||||
nav_msgs::OccupancyGrid map;
|
||||
map.info.resolution = gridCellSize_;
|
||||
map.info.origin.position.x = 0.0;
|
||||
map.info.origin.position.y = 0.0;
|
||||
map.info.origin.position.z = 0.0;
|
||||
map.info.origin.orientation.x = 0.0;
|
||||
map.info.origin.orientation.y = 0.0;
|
||||
map.info.origin.orientation.z = 0.0;
|
||||
map.info.origin.orientation.w = 1.0;
|
||||
|
||||
map.info.width = pixels.cols;
|
||||
map.info.height = pixels.rows;
|
||||
map.info.origin.position.x = xMin;
|
||||
map.info.origin.position.y = yMin;
|
||||
map.data.resize(map.info.width * map.info.height);
|
||||
|
||||
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
|
||||
|
||||
map.header.frame_id = msg->header.frame_id;
|
||||
map.header.stamp = ros::Time::now();
|
||||
|
||||
occupancyMapPub_.publish(map);
|
||||
}
|
||||
}
|
||||
ROS_INFO("Processing data %fs", timer.ticks());
|
||||
ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
|
||||
}
|
||||
|
||||
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
{
|
||||
ROS_INFO("map_assembler: reset!");
|
||||
occupancyLocalMaps_.clear();
|
||||
rgbClouds_.clear();
|
||||
scans_.clear();
|
||||
mapsManager_.clear();
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
int cloudDecimation_;
|
||||
double cloudMaxDepth_;
|
||||
double cloudVoxelSize_;
|
||||
double scanVoxelSize_;
|
||||
|
||||
double nodeFilteringAngle_;
|
||||
double nodeFilteringRadius_;
|
||||
|
||||
double noiseFilterRadius_;
|
||||
double noiseFilterMinNeighbors_;
|
||||
|
||||
bool computeOccupancyGrid_;
|
||||
double gridCellSize_;
|
||||
double groundMaxAngle_;
|
||||
int clusterMinSize_;
|
||||
double maxHeight_;
|
||||
double occupancyMapSize_;
|
||||
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > occupancyLocalMaps_; // <ground, obstacles>
|
||||
MapsManager mapsManager_;
|
||||
std::map<int, Signature> nodes_;
|
||||
|
||||
ros::Subscriber mapDataTopic_;
|
||||
|
||||
ros::Publisher assembledMapClouds_;
|
||||
ros::Publisher assembledMapScans_;
|
||||
ros::Publisher occupancyMapPub_;
|
||||
|
||||
ros::ServiceServer resetService_;
|
||||
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > rgbClouds_;
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
|
||||
};
|
||||
|
||||
|
||||
|
||||
+28
-10
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
#include <rtabmap/utilite/UConversion.h>
|
||||
#include <ros/subscriber.h>
|
||||
#include <ros/publisher.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
@@ -48,8 +49,6 @@ public:
|
||||
MapOptimizer() :
|
||||
mapFrameId_("map"),
|
||||
odomFrameId_("odom"),
|
||||
iterations_(100),
|
||||
ignoreVariance_(false),
|
||||
globalOptimization_(true),
|
||||
optimizeFromLastNode_(false),
|
||||
mapToOdom_(rtabmap::Transform::getIdentity()),
|
||||
@@ -58,14 +57,35 @@ public:
|
||||
ros::NodeHandle nh;
|
||||
ros::NodeHandle pnh("~");
|
||||
|
||||
double epsilon = 0.0;
|
||||
bool robust = true;
|
||||
bool slam2d =false;
|
||||
int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
|
||||
int iterations = 100;
|
||||
bool ignoreVariance = false;
|
||||
|
||||
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
|
||||
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
|
||||
pnh.param("iterations", iterations_, iterations_);
|
||||
pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_);
|
||||
pnh.param("iterations", iterations, iterations);
|
||||
pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
|
||||
pnh.param("global_optimization", globalOptimization_, globalOptimization_);
|
||||
pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
|
||||
pnh.param("epsilon", epsilon, epsilon);
|
||||
pnh.param("robust", robust, robust);
|
||||
pnh.param("slam_2d", slam2d, slam2d);
|
||||
pnh.param("strategy", strategy, strategy);
|
||||
|
||||
UASSERT(iterations_ > 0);
|
||||
|
||||
UASSERT(iterations > 0);
|
||||
|
||||
ParametersMap parameters;
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeStrategy(), uNumber2Str(strategy)));
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeEpsilon(), uNumber2Str(epsilon)));
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(iterations)));
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeRobust(), uBool2Str(robust)));
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeSlam2D(), uBool2Str(slam2d)));
|
||||
parameters.insert(ParametersPair(Parameters::kRGBDOptimizeVarianceIgnored(), uBool2Str(ignoreVariance)));
|
||||
optimizer_ = graph::Optimizer::create(parameters);
|
||||
|
||||
double tfDelay = 0.05; // 20 Hz
|
||||
bool publishTf = true;
|
||||
@@ -216,15 +236,14 @@ public:
|
||||
std::multimap<int, rtabmap::Link> linksOut;
|
||||
if(poses.size() > 1 && constraints.size() > 0)
|
||||
{
|
||||
graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
|
||||
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
|
||||
optimizer.getConnectedGraph(
|
||||
optimizer_->getConnectedGraph(
|
||||
fromId,
|
||||
poses,
|
||||
constraints,
|
||||
posesOut,
|
||||
linksOut);
|
||||
optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut);
|
||||
optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
|
||||
mapToOdomMutex_.lock();
|
||||
mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
|
||||
mapToOdom_ = mapCorrection;
|
||||
@@ -296,10 +315,9 @@ public:
|
||||
private:
|
||||
std::string mapFrameId_;
|
||||
std::string odomFrameId_;
|
||||
int iterations_;
|
||||
bool ignoreVariance_;
|
||||
bool globalOptimization_;
|
||||
bool optimizeFromLastNode_;
|
||||
graph::Optimizer * optimizer_;
|
||||
|
||||
rtabmap::Transform mapToOdom_;
|
||||
boost::mutex mapToOdomMutex_;
|
||||
|
||||
+134
-21
@@ -29,13 +29,15 @@
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
MapsManager::MapsManager() :
|
||||
MapsManager::MapsManager(bool usePublicNamespace) :
|
||||
cloudDecimation_(4),
|
||||
cloudMaxDepth_(4.0), // meters
|
||||
cloudVoxelSize_(0.05), // meters
|
||||
cloudFloorCullingHeight_(0.0),
|
||||
cloudOutputVoxelized_(false),
|
||||
cloudFrustumCulling_(false),
|
||||
scanVoxelSize_(0.0),
|
||||
scanOutputVoxelized_(false),
|
||||
projMaxGroundAngle_(45.0), // degrees
|
||||
projMinClusterSize_(20),
|
||||
projMaxHeight_(2.0), // meters
|
||||
@@ -58,6 +60,12 @@ MapsManager::MapsManager() :
|
||||
pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
|
||||
pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
|
||||
pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
|
||||
pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
|
||||
pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_);
|
||||
|
||||
// scan map stuff
|
||||
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
|
||||
pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_);
|
||||
|
||||
//projection map stuff
|
||||
pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
|
||||
@@ -75,10 +83,27 @@ MapsManager::MapsManager() :
|
||||
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
|
||||
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
|
||||
|
||||
// If true, the last message published on
|
||||
// the map topics will be saved and sent to new subscribers when they
|
||||
// connect
|
||||
bool latch = true;
|
||||
pnh.param("latch", latch, latch);
|
||||
|
||||
// mapping topics
|
||||
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
|
||||
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
|
||||
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
|
||||
if(usePublicNamespace)
|
||||
{
|
||||
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
|
||||
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
|
||||
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
|
||||
scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
|
||||
}
|
||||
else
|
||||
{
|
||||
cloudMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
|
||||
projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
|
||||
gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
|
||||
scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
|
||||
}
|
||||
}
|
||||
|
||||
MapsManager::~MapsManager() {
|
||||
@@ -97,7 +122,8 @@ bool MapsManager::hasSubscribers() const
|
||||
{
|
||||
return cloudMapPub_.getNumSubscribers() != 0 ||
|
||||
projMapPub_.getNumSubscribers() != 0 ||
|
||||
gridMapPub_.getNumSubscribers() != 0;
|
||||
gridMapPub_.getNumSubscribers() != 0 ||
|
||||
scanMapPub_.getNumSubscribers() != 0;
|
||||
}
|
||||
|
||||
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
|
||||
@@ -117,28 +143,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
bool updateScan,
|
||||
const std::map<int, rtabmap::Signature> & signatures)
|
||||
{
|
||||
if(!updateCloud && !updateProj && !updateGrid)
|
||||
if(!updateCloud && !updateProj && !updateGrid && !updateScan)
|
||||
{
|
||||
// all false, udpate only those where we have subscribers
|
||||
updateCloud = cloudMapPub_.getNumSubscribers() != 0;
|
||||
updateProj = projMapPub_.getNumSubscribers() != 0;
|
||||
updateGrid = gridMapPub_.getNumSubscribers() != 0;
|
||||
updateScan = scanMapPub_.getNumSubscribers() != 0;
|
||||
}
|
||||
|
||||
UDEBUG("Updating map caches...");
|
||||
|
||||
if(!memory && signatures.size() == 0)
|
||||
{
|
||||
ROS_FATAL("Memory should not be null!?");
|
||||
ROS_ERROR("Memory and signatures should not be both null!?");
|
||||
return std::map<int, rtabmap::Transform>();
|
||||
}
|
||||
|
||||
std::map<int, rtabmap::Transform> filteredPoses;
|
||||
|
||||
// update cache
|
||||
if(updateCloud || updateProj || updateGrid)
|
||||
if(updateCloud || updateProj || updateGrid || updateScan)
|
||||
{
|
||||
// filter nodes
|
||||
if(mapFilterRadius_ > 0.0)
|
||||
@@ -170,11 +198,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
rtabmap::SensorData data;
|
||||
bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
|
||||
bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first));
|
||||
bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
|
||||
bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
|
||||
bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));
|
||||
|
||||
if(rgbDepthRequired ||
|
||||
depthRequired ||
|
||||
scanRequired)
|
||||
scanRequired ||
|
||||
gridRequired)
|
||||
{
|
||||
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
|
||||
if(findIter != signatures.end())
|
||||
@@ -198,7 +228,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
data.uncompressData(
|
||||
(rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0,
|
||||
(rgbDepthRequired||depthRequired) ? &depth:0,
|
||||
scanRequired?&scan:0);
|
||||
scanRequired||gridRequired?&scan:0);
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
|
||||
@@ -211,6 +241,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
cloudDecimation_,
|
||||
cloudMaxDepth_,
|
||||
cloudVoxelSize_);
|
||||
if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
|
||||
{
|
||||
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
pcl::copyPointCloud(*cloudRGB, *indices, *tmp);
|
||||
cloudRGB = tmp;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -226,6 +263,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
cloudDecimation_,
|
||||
cloudMaxDepth_,
|
||||
gridCellSize_); // use gridCellSize since this cloud is only for the projection map
|
||||
if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
|
||||
{
|
||||
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*cloudXYZ, *indices, *tmp);
|
||||
cloudXYZ = tmp;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -273,7 +317,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
{
|
||||
cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
|
||||
}
|
||||
if(cloudClipped->size())
|
||||
if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_)
|
||||
{
|
||||
cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
|
||||
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
|
||||
@@ -294,11 +338,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
|
||||
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
|
||||
if(scanRequired)
|
||||
if(scanRequired || gridRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
|
||||
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
|
||||
if(scanVoxelSize_ > 0.0)
|
||||
{
|
||||
scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
|
||||
if(gridRequired)
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*scanCloud);
|
||||
}
|
||||
}
|
||||
if(scanRequired)
|
||||
{
|
||||
uInsert(scans_, std::make_pair(iter->first, scanCloud));
|
||||
}
|
||||
}
|
||||
if(gridRequired)
|
||||
{
|
||||
cv::Mat ground, obstacles;
|
||||
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
|
||||
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -428,20 +491,23 @@ void MapsManager::publishMaps(
|
||||
cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
|
||||
true);
|
||||
//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
if(jter->second->size())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(cloudFloorCullingHeight_ > 0.0)
|
||||
if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0)
|
||||
{
|
||||
assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
|
||||
}
|
||||
|
||||
if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
|
||||
if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
|
||||
}
|
||||
@@ -456,7 +522,7 @@ void MapsManager::publishMaps(
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
|
||||
ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size());
|
||||
}
|
||||
}
|
||||
else if(mapCacheCleanup_)
|
||||
@@ -465,6 +531,53 @@ void MapsManager::publishMaps(
|
||||
cameraModels_.clear();
|
||||
}
|
||||
|
||||
if(scanMapPub_.getNumSubscribers())
|
||||
{
|
||||
// generate the assembled scan cloud!
|
||||
UTimer time;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
int count = 0;
|
||||
std::list<std::pair<int, Transform> > negativePoses;
|
||||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
|
||||
{
|
||||
if(iter->first > 0)
|
||||
{
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
|
||||
if(jter != scans_.end() && jter->second->size())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
|
||||
*assembledCloud+=*transformed;
|
||||
++count;
|
||||
}
|
||||
}
|
||||
// negative poses are not used
|
||||
}
|
||||
|
||||
if(assembledCloud->size())
|
||||
{
|
||||
if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_)
|
||||
{
|
||||
assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
|
||||
}
|
||||
|
||||
ROS_INFO("Assembled %d scans (%fs)", count, time.ticks());
|
||||
|
||||
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
|
||||
pcl::toROSMsg(*assembledCloud, *cloudMsg);
|
||||
cloudMsg->header.stamp = stamp;
|
||||
cloudMsg->header.frame_id = mapFrameId;
|
||||
scanMapPub_.publish(cloudMsg);
|
||||
}
|
||||
else if(poses.size())
|
||||
{
|
||||
ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size());
|
||||
}
|
||||
}
|
||||
else if(mapCacheCleanup_)
|
||||
{
|
||||
scans_.clear();
|
||||
}
|
||||
|
||||
if(projMapPub_.getNumSubscribers())
|
||||
{
|
||||
// create the projection map
|
||||
|
||||
+8
-1
@@ -26,7 +26,7 @@ class Memory;
|
||||
|
||||
class MapsManager {
|
||||
public:
|
||||
MapsManager();
|
||||
MapsManager(bool usePublicNamespace);
|
||||
virtual ~MapsManager();
|
||||
void clear();
|
||||
bool hasSubscribers() const;
|
||||
@@ -40,6 +40,7 @@ public:
|
||||
bool updateCloud,
|
||||
bool updateProj,
|
||||
bool updateGrid,
|
||||
bool updateScan,
|
||||
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
|
||||
|
||||
void publishMaps(
|
||||
@@ -71,6 +72,10 @@ private:
|
||||
double cloudFloorCullingHeight_;
|
||||
bool cloudOutputVoxelized_;
|
||||
bool cloudFrustumCulling_;
|
||||
double cloudNoiseFilteringRadius_;
|
||||
int cloudNoiseFilteringMinNeighbors_;
|
||||
double scanVoxelSize_;
|
||||
bool scanOutputVoxelized_;
|
||||
double projMaxGroundAngle_;
|
||||
int projMinClusterSize_;
|
||||
double projMaxHeight_;
|
||||
@@ -85,8 +90,10 @@ private:
|
||||
ros::Publisher cloudMapPub_;
|
||||
ros::Publisher projMapPub_;
|
||||
ros::Publisher gridMapPub_;
|
||||
ros::Publisher scanMapPub_;
|
||||
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
|
||||
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
|
||||
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
|
||||
|
||||
Reference in New Issue
Block a user