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)

CMakeLists.txt

@@ -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

src/CoreWrapper.cpp

@@ -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);

src/GridMapAssemblerNode.cpp

@@ -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;
-}

src/MapAssemblerNode.cpp

@@ -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),
+		mapsManager_(false)
-		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)
 	{
 		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(msg->nodes[i].image.size() ||
-			if(!uContains(rgbClouds_, id))
+			   msg->nodes[i].depth.size() ||
+			   msg->nodes[i].laserScan.size())
 			{
-				rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
+				uInsert(nodes_, std::make_pair(msg->nodes[i].id, 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));
-					}
-				}
 			}
 		}
 
-		// filter poses
+		// create a tmp signature with latest sensory data
-		std::map<int, Transform> poses;
+		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
-		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])));
+			Signature tmpS = nodes_.at(poses.rbegin()->first);
-		}
+			SensorData tmpData = tmpS.sensorData();
-		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
+			tmpData.setId(-1);
-		{
+			uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), tmpData)));
-			poses = rtabmap::graph::radiusPosesFiltering(poses, nodeFilteringRadius_, nodeFilteringAngle_*CV_PI/180.0);
+			poses.insert(std::make_pair(-1, poses.rbegin()->second));
 		}
 
-		if(assembledMapClouds_.getNumSubscribers())
+		// Update maps
-		{
+		poses = mapsManager_.updateMapCaches(
-			// generate the assembled cloud!
+				poses,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+				0,
+				false,
+				false,
+				false,
+				false,
+				nodes_);
 
-			for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
-			{
-				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;
-				}
-			}
 
-			if(assembledCloud->size())
+		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
-			{
-				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());
 	}
 
 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 	{
 		ROS_INFO("map_assembler: reset!");
-		occupancyLocalMaps_.clear();
+		mapsManager_.clear();
-		rgbClouds_.clear();
-		scans_.clear();
 		return true;
 	}
 
 private:
-	int cloudDecimation_;
+	MapsManager mapsManager_;
-	double cloudMaxDepth_;
+	std::map<int, Signature> nodes_;
-	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>
 
 	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_;
 };
 
 

src/MapOptimizerNode.cpp

@@ -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("iterations", iterations, iterations);
-		pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_);
+		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_;

src/MapsManager.cpp

@@ -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);
+	if(usePublicNamespace)
-	projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
+	{
-	gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
+		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;
+							if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
-							util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
+							{
-							uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+								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);
+								if(jter->second->size())
-								*assembledCloud+=*transformed;
+								{
+									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

src/MapsManager.h

@@ -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>