ros-pkg: added map_optimizer node

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1897 f169173b-cf89-36c8-b27e-44dbe73f0c83

CMakeLists.txt

@@ -152,6 +152,9 @@ target_link_libraries(visual_odometry ${Libraries})
 add_executable(stereo_odometry src/StereoOdometryNode.cpp)
 target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
 
+add_executable(map_optimizer src/MapOptimizerNode.cpp)
+target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
+
 add_executable(map_assembler src/MapAssemblerNode.cpp)
 target_link_libraries(map_assembler rtabmap_ros ${Libraries})
 

launch/tests/test_stereo_mapping_bag.launch

@@ -67,11 +67,13 @@
       <param name="frame_id" type="string" value="base_link"/>
       <param name="queue_size" type="int" value="30"/>
       
+      <param name="RGBD/ToroIterations" type="string" value="0"/>
       <param name="Rtabmap/TimeThr" type="string" value="700"/>
       <param name="Rtabmap/DetectionRate" type="string" value="1"/>
-      <param name="Kp/DetectorStrategy" type="string" value="6"/>
+      <param name="Kp/DetectorStrategy" type="string" value="0"/>
-      <param name="NN/NNStrategy" type="string" value="3"/>
+      <param name="Kp/WordsPerImage" type="string" value="200"/>
-	  <param name="GFTT/MaxCorners" type="string" value="200"/>
+      <param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
+      <param name="NN/NNStrategy" type="string" value="1"/>
       
       <param name="LccBow/MinInliers" type="string" value="10"/>
       <param name="LccBow/InlierDistance" type="string" value="0.02"/>
@@ -79,9 +81,9 @@
       
       <param name="LccReextract/FeatureType" type="string" value="4"/>
 	  <param name="LccReextract/Activated" type="string" value="true"/>
-	  <param name="LccReextract/MaxDepth" type="string" value="3"/>
+	  <param name="LccReextract/MaxDepth" type="string" value="10"/>
 	  <param name="LccReextract/NNDR" type="string" value="0.8"/>
-	  <param name="LccReextract/NNType" type="string" value="3"/>  		  	
+	  <param name="LccReextract/NNType" type="string" value="3"/> 	  	
     </node>
   
     <!-- Visualisation (client side) -->
@@ -103,10 +105,15 @@
     </node>
     -->
     
+    <node pkg="rtabmap" type="map_optimizer" name="map_optimizer" output="screen"/>
+  	<node pkg="rtabmap" type="map_assembler" name="map_assembler" output="screen">
+  		<remap from="mapData" to="mapData_optimized"/>
+ 	</node>
+    
   </group>
   
   <!-- RVIZ -->
-  
+    
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap)/launch/config/rgbd.rviz"/>
   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
   <node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">

src/CoreWrapper.cpp

@@ -67,6 +67,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		odomFrameId_(""),
 		configPath_(""),
 		mapToOdom_(tf::Transform::getIdentity()),
+		transformThread_(0),
 		rate_(Parameters::defaultRtabmapDetectionRate()),
 		time_(ros::Time::now())
 {
@@ -244,7 +245,17 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize);
 
-	transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
+	int toroIterations = 0;
+	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
+	if(toroIterations != 0)
+	{
+		transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
+	}
+	else
+	{
+		UWARN("Graph optimization is disabled (%s=0), the tf between frame \"%s\" and odometry frame will not be published. You can safely ignore this warning if you are using map_optimizer node.",
+				Parameters::kRGBDToroIterations().c_str(), mapFrameId_.c_str());
+	}
 }
 
 CoreWrapper::~CoreWrapper()

src/MapOptimizerNode.cpp

@@ -0,0 +1,240 @@
+/*
+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/MapData.h"
+#include "rtabmap/MsgConversion.h"
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <ros/subscriber.h>
+#include <ros/publisher.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <boost/thread.hpp>
+
+using namespace rtabmap;
+
+class MapOptimizer
+{
+
+public:
+	MapOptimizer() :
+		mapFrameId_("map"),
+		odomFrameId_("odom"),
+		iterations_(100),
+		globalOptimization_(true),
+		condChecked_(false),
+		mapToOdom_(tf::Transform::getIdentity()),
+		transformThread_(0)
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+
+		pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+		pnh.param("iterations", iterations_, iterations_);
+		pnh.param("global_optimization", globalOptimization_, globalOptimization_);
+
+		UASSERT(iterations_ > 0);
+
+		double tfDelay = 0.05; // 20 Hz
+		pnh.param("tf_delay", tfDelay, tfDelay);
+
+		// Verify that rtabmap is not sending optimized poses!
+		std::string toroIterations;
+		if(nh.getParam(Parameters::kRGBDToroIterations(), toroIterations))
+		{
+			if(std::atoi(toroIterations.c_str()) != 0)
+			{
+				ROS_WARN("map_optimizer: Parameter \"%s\" of rtabmap node is not 0 (value=%s), it should be 0 (optimization desactivated).",
+						Parameters::kRGBDToroIterations().c_str(), toroIterations.c_str());
+				exit(-1);
+			}
+		}
+
+		mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
+		mapDataPub_ = nh.advertise<rtabmap::MapData>(nh.resolveName("mapData")+"_optimized", 1);
+
+		ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
+		ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
+		ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
+		ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
+		transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
+	}
+
+	~MapOptimizer()
+	{
+		if(transformThread_)
+		{
+			transformThread_->join();
+			delete transformThread_;
+		}
+	}
+
+	void publishLoop(double tfDelay)
+	{
+		if(tfDelay == 0)
+			return;
+		ros::Rate r(1.0 / tfDelay);
+		while(ros::ok())
+		{
+			mapToOdomMutex_.lock();
+			ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
+			tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
+			mapToOdomMutex_.unlock();
+			r.sleep();
+		}
+	}
+
+	void mapDataReceivedCallback(const rtabmap::MapDataConstPtr & msg)
+	{
+		if(globalOptimization_ && !condChecked_)
+		{
+			// Verify that rtabmap is not sending optimized poses!
+			ros::NodeHandle nh;
+			std::string toroIterations;
+			if(nh.getParam(Parameters::kRGBDToroIterations(), toroIterations))
+			{
+				if(std::atoi(toroIterations.c_str()) != 0)
+				{
+					ROS_ERROR("map_optimizer: Parameter \"%s\" of rtabmap node is not 0 (value=%s), it should be 0 (optimization desactivated).",
+							Parameters::kRGBDToroIterations().c_str(), toroIterations.c_str());
+					exit(-1);
+				}
+			}
+			else
+			{
+				ROS_ERROR("map_optimizer: Could not get parameter \"%s\" of rtabmap node, it should be 0 (optimization desactivated). Is rtabmap node started? and in the same namespace that map_assembler?", Parameters::kRGBDToroIterations().c_str());
+				exit(-1);
+			}
+			condChecked_ = true;
+		}
+		else if(!globalOptimization_)
+		{
+			// optimize only local map
+			poses_.clear();
+			constraints_.clear();
+		}
+
+		// save new poses and constraints
+		// Assuming that nodes/constraints are all linked together
+		UASSERT(msg->poseIDs.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poseIDs.size(); ++i)
+		{
+			poses_.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i])));
+		}
+		UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() &&
+				msg->constraints.size() == msg->constraintToIDs.size() &&
+				msg->constraints.size() == msg->constraintTypes.size());
+		for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i)
+		{
+			bool edgeAlreadyAdded = false;
+			for(std::multimap<int, Link>::iterator iter = constraints_.lower_bound(msg->constraintFromIDs[i]);
+					iter != constraints_.end() && iter->first == msg->constraintFromIDs[i];
+					++iter)
+			{
+				if(iter->second.to() == msg->constraintToIDs[i])
+				{
+					edgeAlreadyAdded = true;
+				}
+			}
+			if(!edgeAlreadyAdded)
+			{
+				constraints_.insert(std::make_pair(msg->constraintFromIDs[i], Link(msg->constraintFromIDs[i], msg->constraintToIDs[i], transformFromGeometryMsg(msg->constraints[i]), (Link::Type)msg->constraintTypes[i])));
+			}
+		}
+
+		// Optimize only if there is a subscriber
+		if(mapDataPub_.getNumSubscribers())
+		{
+			std::map<int, Transform> optimizedPoses;
+			if(poses_.size() > 1 && constraints_.size() > 0)
+			{
+				Transform mapCorrectionToro;
+				util3d::optimizeTOROGraph(poses_, constraints_, optimizedPoses, mapCorrectionToro, iterations_, true);
+
+				mapToOdomMutex_.lock();
+				Transform mapCorrection = optimizedPoses.at(poses_.rbegin()->first) * poses_.rbegin()->second.inverse();
+				rtabmap::transformToTF(mapCorrection, mapToOdom_);
+				mapToOdomMutex_.unlock();
+			}
+			else if(poses_.size() == 1 && constraints_.size() == 0)
+			{
+				optimizedPoses = poses_;
+			}
+			else if(poses_.size() || constraints_.size())
+			{
+				ROS_ERROR("map_optimizer: Poses=%zu and edges=%zu (poses must "
+					   "not be null if there are edges, and edges must be null if poses <= 1)",
+					   poses_.size(), constraints_.size());
+			}
+
+			rtabmap::MapData outputMsg = *msg;
+			outputMsg.poseIDs.resize(optimizedPoses.size());
+			outputMsg.poses.resize(optimizedPoses.size());
+			int i=0;
+			for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+			{
+				outputMsg.poseIDs[i] = iter->first;
+				transformToPoseMsg(iter->second, outputMsg.poses[i]);
+				++i;
+			}
+			mapDataPub_.publish(outputMsg);
+		}
+	}
+
+private:
+	std::string mapFrameId_;
+	std::string odomFrameId_;
+	int iterations_;
+	bool globalOptimization_;
+
+	bool condChecked_;
+
+	tf::Transform mapToOdom_;
+	boost::mutex mapToOdomMutex_;
+
+	ros::Subscriber mapDataTopic_;
+
+	ros::Publisher mapDataPub_;
+
+	std::map<int, Transform> poses_;
+	std::multimap<int, Link> constraints_;
+
+	tf::TransformBroadcaster tfBroadcaster_;
+	boost::thread* transformThread_;
+};
+
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "map_optimizer");
+	MapOptimizer optimizer;
+	ros::spin();
+	return 0;
+}