ros-pkg: added all remaining data contained in Signature into NodeData msg

map_optimizer doesn't need anymore to explicitly disable optimization in rtabmap node, thought it is still recommended to disable it to avoid double optimization...

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

CMakeLists.txt

@@ -5,7 +5,7 @@ project(rtabmap)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS 
-             cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs 
+             cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
              image_transport tf tf_conversions laser_geometry pcl_conversions 
              pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
              genmsg stereo_msgs
@@ -59,6 +59,7 @@ generate_messages(
    DEPENDENCIES
    std_msgs
    geometry_msgs
+   sensor_msgs
 )
 
 #add dynamic reconfigure api
@@ -76,7 +77,7 @@ generate_dynamic_reconfigure_options(cfg/Camera.cfg)
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES rtabmap_ros
-  CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs 
+  CATKIN_DEPENDS cv_bridge roscpp rospy sensor_msgs std_msgs std_srvs nav_msgs geometry_msgs
                  image_transport tf tf_conversions laser_geometry pcl_conversions 
                  pcl_ros nodelet dynamic_reconfigure rviz message_filters class_loader
                  stereo_msgs

msg/NodeData.msg

@@ -1,5 +1,9 @@
 
 int32 id
+int32 mapId
+
+# Pose from odometry not corrected
+geometry_msgs/Pose pose
 
 # compressed image in /camera_link frame
 # use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
@@ -23,5 +27,7 @@ geometry_msgs/Transform localTransform
 
 
 # std::multimap<int, cv::Keypoint> words
+# std::multimap<int, pcl::PointXYZ> words3D
 int32[] wordsKeys
-rtabmap/KeyPoint[] wordsValues
+rtabmap/KeyPoint[] wordsValues
+sensor_msgs/PointCloud2 words3DValues

package.xml

@@ -20,6 +20,7 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>nav_msgs</build_depend>
   <build_depend>stereo_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
@@ -31,7 +32,6 @@
   <build_depend>rviz</build_depend>
   <build_depend>message_filters</build_depend>
   <build_depend>class_loader</build_depend>
-  <build_depend>stereo_msgs</build_depend>
   
   <run_depend>cv_bridge</run_depend>
   <run_depend>roscpp</run_depend>
@@ -41,6 +41,7 @@
   <run_depend>std_srvs</run_depend>
   <run_depend>nav_msgs</run_depend>
   <run_depend>stereo_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>tf_conversions</run_depend>
@@ -52,7 +53,6 @@
   <run_depend>rviz</run_depend>
   <run_depend>message_filters</run_depend>
   <run_depend>class_loader</run_depend>
-  <run_depend>stereo_msgs</run_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
   

src/CoreWrapper.cpp

@@ -79,6 +79,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	bool subscribeDepth = true;
 	bool subscribeStereo = false;
 	int queueSize = 10;
+	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
 
 	// ROS related parameters (private)
@@ -106,6 +107,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
 	pnh.param("queue_size", queueSize, queueSize);
 
+	pnh.param("publish_tf", publishTf, publishTf);
 	pnh.param("tf_delay", tfDelay, tfDelay);
 
 	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
@@ -252,11 +254,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	int toroIterations = 0;
 	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
-	if(toroIterations != 0)
+	if(publishTf && toroIterations != 0)
 	{
 		transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
 	}
-	else
+	else if(publishTf)
 	{
 		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());
@@ -1000,6 +1002,8 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 	for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
 	{
 		rep.data.nodes[i].id = iter->second.id();
+		rep.data.nodes[i].mapId = iter->second.mapId();
+		transformToPoseMsg(iter->second.getPose(), rep.data.nodes[i].pose);
 		compressedMatToBytes(iter->second.getImageCompressed(), rep.data.nodes[i].image.bytes);
 		compressedMatToBytes(iter->second.getDepthCompressed(), rep.data.nodes[i].depth.bytes);
 		compressedMatToBytes(iter->second.getDepth2DCompressed(), rep.data.nodes[i].depth2D.bytes);
@@ -1027,6 +1031,26 @@ bool CoreWrapper::getMapCallback(rtabmap::GetMap::Request& req, rtabmap::GetMap:
 			++j;
 		}
 
+		if(iter->second.getWords3().size() && iter->second.getWords3().size() == iter->second.getWords().size())
+		{
+			pcl::PointCloud<pcl::PointXYZ> cloud;
+			cloud.resize(iter->second.getWords3().size());
+			j = 0;
+			for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->second.getWords3().begin();
+				jter!=iter->second.getWords3().end();
+				++jter)
+			{
+				cloud[j++] = jter->second;
+			}
+			pcl::toROSMsg(cloud, rep.data.nodes[i].words3DValues);
+		}
+		else if(iter->second.getWords3().size())
+		{
+			ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
+					(int)iter->second.getWords().size(),
+					(int)iter->second.getWords3().size());
+		}
+
 		++i;
 	}
 
@@ -1107,6 +1131,8 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
 		for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
 		{
 			msg->nodes[i].id = iter->second.id();
+			msg->nodes[i].mapId = iter->second.mapId();
+			transformToPoseMsg(iter->second.getPose(), msg->nodes[i].pose);
 			compressedMatToBytes(iter->second.getImageCompressed(), msg->nodes[i].image.bytes);
 			compressedMatToBytes(iter->second.getDepthCompressed(), msg->nodes[i].depth.bytes);
 			compressedMatToBytes(iter->second.getDepth2DCompressed(), msg->nodes[i].depth2D.bytes);
@@ -1134,6 +1160,26 @@ bool CoreWrapper::publishMapCallback(rtabmap::PublishMap::Request& req, rtabmap:
 				++j;
 			}
 
+			if(iter->second.getWords3().size() && iter->second.getWords3().size() == iter->second.getWords().size())
+			{
+				pcl::PointCloud<pcl::PointXYZ> cloud;
+				cloud.resize(iter->second.getWords3().size());
+				j = 0;
+				for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->second.getWords3().begin();
+					jter!=iter->second.getWords3().end();
+					++jter)
+				{
+					cloud[j++] = jter->second;
+				}
+				pcl::toROSMsg(cloud, msg->nodes[i].words3DValues);
+			}
+			else if(iter->second.getWords3().size())
+			{
+				ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
+						(int)iter->second.getWords().size(),
+						(int)iter->second.getWords3().size());
+			}
+
 			++i;
 		}
 
@@ -1235,6 +1281,8 @@ void CoreWrapper::publishStats(const Statistics & stats)
 		// add data
 		msg->nodes.resize(1);
 		msg->nodes[0].id = stats.getSignature().id();
+		msg->nodes[0].mapId = stats.getSignature().mapId();
+		transformToPoseMsg(stats.getSignature().getPose(), msg->nodes[0].pose);
 		compressedMatToBytes(stats.getSignature().getImageCompressed(), msg->nodes[0].image.bytes);
 		compressedMatToBytes(stats.getSignature().getDepthCompressed(), msg->nodes[0].depth.bytes);
 		compressedMatToBytes(stats.getSignature().getDepth2DCompressed(), msg->nodes[0].depth2D.bytes);
@@ -1262,6 +1310,26 @@ void CoreWrapper::publishStats(const Statistics & stats)
 			++index;
 		}
 
+		if(stats.getSignature().getWords3().size() && stats.getSignature().getWords3().size() == stats.getSignature().getWords().size())
+		{
+			pcl::PointCloud<pcl::PointXYZ> cloud;
+			cloud.resize(stats.getSignature().getWords3().size());
+			index = 0;
+			for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=stats.getSignature().getWords3().begin();
+				jter!=stats.getSignature().getWords3().end();
+				++jter)
+			{
+				cloud[index++] = jter->second;
+			}
+			pcl::toROSMsg(cloud, msg->nodes[0].words3DValues);
+		}
+		else if(stats.getSignature().getWords3().size())
+		{
+			ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
+					(int)stats.getSignature().getWords().size(),
+					(int)stats.getSignature().getWords3().size());
+		}
+
 		mapData_.publish(msg);
 	}
 }

src/GuiWrapper.cpp

@@ -204,6 +204,9 @@ void GuiWrapper::infoMapCallback(
 	{
 		//Features stuff...
 		std::multimap<int, cv::KeyPoint> words;
+		std::multimap<int, pcl::PointXYZ> words3D;
+		pcl::PointCloud<pcl::PointXYZ> cloud;
+		pcl::fromROSMsg(mapMsg->nodes[0].words3DValues, cloud);
 		for(unsigned int i=0; i<mapMsg->nodes[0].wordsKeys.size() && i<mapMsg->nodes[0].wordsValues.size(); ++i)
 		{
 			cv::KeyPoint pt;
@@ -212,14 +215,24 @@ void GuiWrapper::infoMapCallback(
 			pt.pt.x = mapMsg->nodes[0].wordsValues.at(i).ptx;
 			pt.pt.y = mapMsg->nodes[0].wordsValues.at(i).pty;
 			pt.size = mapMsg->nodes[0].wordsValues.at(i).size;
-			words.insert(std::pair<int, cv::KeyPoint>(mapMsg->nodes[0].wordsKeys.at(i), pt));
+			int wordId = mapMsg->nodes[0].wordsKeys.at(i);
+			words.insert(std::make_pair(wordId, pt));
+			if(i< cloud.size())
+			{
+				words3D.insert(std::make_pair(wordId, cloud[i]));
+			}
+		}
+
+		if(words3D.size() && words3D.size() != words.size())
+		{
+			ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
 		}
 
 		Signature signature(mapMsg->nodes[0].id,
-				-1, // not set, see maps above
+				mapMsg->nodes[0].mapId,
 				words,
-				std::multimap<int, pcl::PointXYZ>(),
-				Transform(), // not set, see poses above
+				words3D,
+				transformFromPoseMsg(mapMsg->nodes[0].pose),
 				compressedMatFromBytes(mapMsg->nodes[0].depth2D.bytes),
 				compressedMatFromBytes(mapMsg->nodes[0].image.bytes),
 				compressedMatFromBytes(mapMsg->nodes[0].depth.bytes),
@@ -287,6 +300,9 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 	{
 		//Features stuff...
 		std::multimap<int, cv::KeyPoint> words;
+		std::multimap<int, pcl::PointXYZ> words3D;
+		pcl::PointCloud<pcl::PointXYZ> cloud;
+		pcl::fromROSMsg(map.nodes[i].words3DValues, cloud);
 		for(unsigned int j=0; j<map.nodes[i].wordsKeys.size() && j<map.nodes[0].wordsValues.size(); ++j)
 		{
 			cv::KeyPoint pt;
@@ -295,15 +311,25 @@ void GuiWrapper::processRequestedMap(const rtabmap::MapData & map)
 			pt.pt.x = map.nodes[i].wordsValues.at(j).ptx;
 			pt.pt.y = map.nodes[i].wordsValues.at(j).pty;
 			pt.size = map.nodes[i].wordsValues.at(j).size;
-			words.insert(std::pair<int, cv::KeyPoint>(map.nodes[i].wordsKeys.at(j), pt));
+			int wordId = map.nodes[i].wordsKeys.at(j);
+			words.insert(std::make_pair(wordId, pt));
+			if(j < cloud.size())
+			{
+				words3D.insert(std::make_pair(wordId, cloud[j]));
+			}
+		}
+
+		if(words3D.size() && words3D.size() != words.size())
+		{
+			ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
 		}
 
 		signatures.insert(std::make_pair(map.nodes[i].id,
 				Signature(map.nodes[i].id,
-				-1, // not set, see maps above
+				map.nodes[i].mapId,
 				words,
-				std::multimap<int, pcl::PointXYZ>(),
-				Transform(), // not set, see poses above
+				words3D,
+				transformFromPoseMsg(map.nodes[i].pose),
 				compressedMatFromBytes(map.nodes[i].depth2D.bytes),
 				compressedMatFromBytes(map.nodes[i].image.bytes),
 				compressedMatFromBytes(map.nodes[i].depth.bytes),

src/MapOptimizerNode.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <ros/subscriber.h>
 #include <ros/publisher.h>
 #include <tf/tf.h>
@@ -48,7 +49,7 @@ public:
 		odomFrameId_("odom"),
 		iterations_(100),
 		globalOptimization_(true),
-		condChecked_(false),
+		optimizeFromLastNode_(false),
 		mapToOdom_(tf::Transform::getIdentity()),
 		transformThread_(0)
 	{
@@ -59,32 +60,26 @@ public:
 		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
 		pnh.param("iterations", iterations_, iterations_);
 		pnh.param("global_optimization", globalOptimization_, globalOptimization_);
+		pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
 
 		UASSERT(iterations_ > 0);
 
 		double tfDelay = 0.05; // 20 Hz
+		bool publishTf = true;
+		pnh.param("publish_tf", publishTf, publishTf);
 		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));
+		if(publishTf)
+		{
+			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()
@@ -113,60 +108,26 @@ public:
 
 	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();
-			mapIds_.clear();
-		}
-
 		// save new poses and constraints
 		// Assuming that nodes/constraints are all linked together
 		UASSERT(msg->poseIDs.size() == msg->poses.size());
 		UASSERT(msg->mapIDs.size() == msg->poseIDs.size());
 		UASSERT(msg->mapIDs.size() == msg->maps.size());
-		std::map<int, Transform> newPoses;
-		std::map<int, int> newMapIds;
-		for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poseIDs.size(); ++i)
-		{
-			newPoses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i])));
-			newMapIds.insert(std::make_pair(msg->mapIDs[i], msg->maps[i]));
-		}
 		UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() &&
 				msg->constraints.size() == msg->constraintToIDs.size() &&
 				msg->constraints.size() == msg->constraintTypes.size());
-		std::multimap<int, Link> allNewConstraints;
-		std::multimap<int, Link> filteredNewConstraints;
-		bool constraintsChanged = false;
+
+		bool dataChanged = false;
+
+		std::multimap<int, Link> newConstraints;
 		for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i)
 		{
 			Link link(msg->constraintFromIDs[i], msg->constraintToIDs[i], transformFromGeometryMsg(msg->constraints[i]), (Link::Type)msg->constraintTypes[i]);
-			allNewConstraints.insert(std::make_pair(link.from(), link));
+			newConstraints.insert(std::make_pair(link.from(), link));
+
 			bool edgeAlreadyAdded = false;
-			for(std::multimap<int, Link>::iterator iter = constraints_.lower_bound(link.from());
-					iter != constraints_.end() && iter->first == link.from();
+			for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
+					iter != cachedConstraints_.end() && iter->first == link.from();
 					++iter)
 			{
 				if(iter->second.to() == link.to())
@@ -174,59 +135,106 @@ public:
 					edgeAlreadyAdded = true;
 					if(iter->second.transform() != link.transform())
 					{
-						constraintsChanged = true;
+						dataChanged = true;
 					}
 				}
 			}
 			if(!edgeAlreadyAdded)
 			{
-				filteredNewConstraints.insert(std::make_pair(link.from(), link));
+				cachedConstraints_.insert(std::make_pair(link.from(), link));
 			}
 		}
 
-		//If a transform has changed, clear all.
-		if(constraintsChanged)
+		std::map<int, Transform> newPoses;
+		std::map<int, int> newMapIds;
+		// add new odometry poses
+		for(unsigned int i=0; i<msg->nodes.size(); ++i)
 		{
-			UWARN("Some received constraints have changed from the cached "
-					"constraints. RTAB-Map is restarted? If yes, ignore this "
-					"warning. Clearing all cached constraints and restart with "
-					"the new ones...");
-			poses_ = newPoses;
-			mapIds_ = newMapIds;
-			constraints_ = allNewConstraints;
+			int id = msg->nodes[i].id;
+			Transform pose = transformFromPoseMsg(msg->nodes[i].pose);
+			newPoses.insert(std::make_pair(id, pose));
+			newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
+
+			std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
+			if(!p.second && pose != cachedPoses_.at(id))
+			{
+				dataChanged = true;
+			}
+			else if(p.second)
+			{
+				cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
+			}
+		}
+
+		if(dataChanged)
+		{
+			ROS_WARN("Graph data has changed! Reset cache...");
+			cachedPoses_ = newPoses;
+			cachedMapIds_ = newMapIds;
+			cachedConstraints_ = newConstraints;
+		}
+
+		//match poses in the graph
+		std::map<int, Transform> poses;
+		std::map<int, int> mapIds;
+		std::multimap<int, Link> constraints;
+		if(globalOptimization_)
+		{
+			poses = cachedPoses_;
+			mapIds = cachedMapIds_;
+			constraints = cachedConstraints_;
 		}
 		else
 		{
-			poses_.insert(newPoses.begin(), newPoses.end());
-			mapIds_.insert(newMapIds.begin(), newMapIds.end());
-			constraints_.insert(filteredNewConstraints.begin(), filteredNewConstraints.end());
+			constraints = newConstraints;
+			for(unsigned int i=0; i<msg->poseIDs.size(); ++i)
+			{
+				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->poseIDs[i]);
+				if(iter != cachedPoses_.end())
+				{
+					poses.insert(*iter);
+					mapIds.insert(*cachedMapIds_.find(iter->first));
+				}
+				else
+				{
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->poseIDs[i]);
+					return;
+				}
+			}
 		}
 
 		// Optimize only if there is a subscriber
 		if(mapDataPub_.getNumSubscribers())
 		{
+			UTimer timer;
 			std::map<int, Transform> optimizedPoses;
-			std::map<int, int> mapIds = mapIds_;
 			Transform mapCorrection = Transform::getIdentity();
-			if(poses_.size() > 1 && constraints_.size() > 0)
+			if(poses.size() > 1 && constraints.size() > 0)
 			{
-				Transform mapCorrectionToro;
-				util3d::optimizeTOROGraph(poses_, constraints_, optimizedPoses, mapCorrectionToro, iterations_, true);
+				if(optimizeFromLastNode_)
+				{
+					std::map<int, int> depthGraph = util3d::generateDepthGraph(constraints, poses.rbegin()->first);
+					util3d::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_);
+				}
+				else
+				{
+					util3d::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_);
+				}
 
 				mapToOdomMutex_.lock();
-				mapCorrection = optimizedPoses.at(poses_.rbegin()->first) * poses_.rbegin()->second.inverse();
+				mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();
 				rtabmap::transformToTF(mapCorrection, mapToOdom_);
 				mapToOdomMutex_.unlock();
 			}
-			else if(poses_.size() == 1 && constraints_.size() == 0)
+			else if(poses.size() == 1 && constraints.size() == 0)
 			{
-				optimizedPoses = poses_;
+				optimizedPoses = poses;
 			}
-			else if(poses_.size() || constraints_.size())
+			else if(poses.size() || constraints.size())
 			{
-				ROS_ERROR("map_optimizer: Poses=%zu and edges=%zu (poses must "
+				ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
 					   "not be null if there are edges, and edges must be null if poses <= 1)",
-					   poses_.size(), constraints_.size());
+					  (int)poses.size(), (int)constraints.size());
 				mapIds.clear();
 			}
 
@@ -248,6 +256,8 @@ public:
 				++i;
 			}
 			mapDataPub_.publish(outputMsg);
+
+			ROS_INFO("Time graph optimization = %f s", timer.ticks());
 		}
 	}
 
@@ -256,8 +266,7 @@ private:
 	std::string odomFrameId_;
 	int iterations_;
 	bool globalOptimization_;
-
-	bool condChecked_;
+	bool optimizeFromLastNode_;
 
 	tf::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;
@@ -266,9 +275,9 @@ private:
 
 	ros::Publisher mapDataPub_;
 
-	std::map<int, Transform> poses_;
-	std::multimap<int, Link> constraints_;
-	std::map<int, int> mapIds_;
+	std::map<int, Transform> cachedPoses_;
+	std::map<int, int> cachedMapIds_;
+	std::multimap<int, Link> cachedConstraints_;
 
 	tf::TransformBroadcaster tfBroadcaster_;
 	boost::thread* transformThread_;