updated for rtabmap standalone 0.8.7

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.8.6</version>
+  <version>0.8.7</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -259,6 +259,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	oldParameterNames.push_back("Rtabmap/DetectorStrategy");
 	oldParameterNames.push_back("RGBD/ScanMatchingSize");
 	oldParameterNames.push_back("RGBD/LocalLoopDetectionRadius");
+	oldParameterNames.push_back("RGBD/ToroIterations");
 	for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
 	{
 		std::string vStr;
@@ -288,6 +289,12 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 						Parameters::kRGBDLocalRadius().c_str());
 				parameters.at(Parameters::kRGBDLocalRadius())= vStr;
 			}
+			else if(iter->compare("RGBD/ToroIterations") == 0)
+			{
+				ROS_WARN("Parameter name changed: RGBD/ToroIterations -> %s. Please update your launch file accordingly.",
+						Parameters::kRGBDOptimizeIterations().c_str());
+				parameters.at(Parameters::kRGBDOptimizeIterations())= vStr;
+			}
 		}
 	}
 
@@ -363,16 +370,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
 
-	int toroIterations = 0;
+	int optimizeIterations = 0;
-	Parameters::parse(parameters, Parameters::kRGBDToroIterations(), toroIterations);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), optimizeIterations);
-	if(publishTf && toroIterations != 0)
+	if(publishTf && optimizeIterations != 0)
 	{
 		transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
 	}
 	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());
+				Parameters::kRGBDOptimizeIterations().c_str(), mapFrameId_.c_str());
 	}
 }
 

src/MapOptimizerNode.cpp

@@ -218,15 +218,17 @@ public:
 			Transform mapCorrection = Transform::getIdentity();
 			if(poses.size() > 1 && constraints.size() > 0)
 			{
-				if(optimizeFromLastNode_)
+				graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
-				{
+				int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
-					std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(constraints, poses.rbegin()->first);
+				std::map<int, rtabmap::Transform> posesOut;
-					rtabmap::graph::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+				std::multimap<int, rtabmap::Link> linksOut;
-				}
+				optimizer.getConnectedGraph(
-				else
+						fromId,
-				{
+						poses,
-					rtabmap::graph::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
+						constraints,
-				}
+						posesOut,
+						linksOut);
+				optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut);
 
 				mapToOdomMutex_.lock();
 				mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();