Refactoring: Split all graph optimization approaches in multiple files

corelib/include/rtabmap/core/Graph.h

@@ -33,8 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <map>
 #include <list>
 #include <rtabmap/core/Link.h>
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Signature.h>
 
 namespace rtabmap {
 class Memory;
@@ -42,213 +40,17 @@ class Memory;
 namespace graph {
 
 ////////////////////////////////////////////
-// Graph optimizers
+// Graph utilities
 ////////////////////////////////////////////
-class RTABMAP_EXP Optimizer
-{
-public:
-	enum Type {
-		kTypeUndef = -1,
-		kTypeTORO = 0,
-		kTypeG2O = 1,
-		kTypeGTSAM = 2,
-		kTypeCVSBA = 3
-	};
-	static Optimizer * create(const ParametersMap & parameters);
-	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
-
-	// Get connected poses and constraints from a set of links
-	static void getConnectedGraph(
-			int fromId,
-			const std::map<int, Transform> & posesIn,
-			const std::multimap<int, Link> & linksIn, // only one link between two poses
-			std::map<int, Transform> & posesOut,
-			std::multimap<int, Link> & linksOut,
-			int depth = 0);
-
-public:
-	virtual ~Optimizer() {}
-
-	virtual Type type() const = 0;
-
-	int iterations() const {return iterations_;}
-	bool isSlam2d() const {return slam2d_;}
-	bool isCovarianceIgnored() const {return covarianceIgnored_;}
-	double epsilon() const {return epsilon_;}
-	bool isRobust() const {return robust_;}
-
-	// inherited classes should implement one of these methods
-	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & constraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
-	virtual std::map<int, Transform> optimizeBA(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
-
-	virtual void parseParameters(const ParametersMap & parameters);
-
-protected:
-	Optimizer(
-			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
-			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
-			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
-			bool robust            = Parameters::defaultRGBDOptimizeRobust());
-	Optimizer(const ParametersMap & parameters);
-
-private:
-	int iterations_;
-	bool slam2d_;
-	bool covarianceIgnored_;
-	double epsilon_;
-	bool robust_;
-};
-
-class RTABMAP_EXP TOROOptimizer : public Optimizer
-{
-public:
-	static bool saveGraph(
-			const std::string & fileName,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints);
-	static bool loadGraph(
-			const std::string & fileName,
-			std::map<int, Transform> & poses,
-			std::multimap<int, Link> & edgeConstraints);
-
-public:
-	TOROOptimizer(
-			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
-			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
-			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon()) :
-		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
-	TOROOptimizer(const ParametersMap & parameters) :
-		Optimizer(parameters) {}
-	virtual ~TOROOptimizer() {}
-
-	virtual Type type() const {return kTypeTORO;}
-
-	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
-};
-
-class RTABMAP_EXP G2OOptimizer : public Optimizer
-{
-public:
-	static bool available();
-	static bool saveGraph(
-			const std::string & fileName,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			bool useRobustConstraints = false);
-
-public:
-	G2OOptimizer(
-			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
-			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
-			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
-			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
-		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
-
-	G2OOptimizer(const ParametersMap & parameters) :
-		Optimizer(parameters) {}
-	virtual ~G2OOptimizer() {}
-
-	virtual Type type() const {return kTypeG2O;}
-
-	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
-};
-
-class RTABMAP_EXP GTSAMOptimizer : public Optimizer
-{
-public:
-	static bool available();
-
-public:
-	GTSAMOptimizer(
-			int iterations         = Parameters::defaultRGBDOptimizeIterations(),
-			bool slam2d            = Parameters::defaultRGBDOptimizeSlam2D(),
-			bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
-			double epsilon         = Parameters::defaultRGBDOptimizeEpsilon(),
-			bool robust            = Parameters::defaultRGBDOptimizeRobust()) :
-		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
-
-	GTSAMOptimizer(const ParametersMap & parameters) :
-		Optimizer(parameters) {}
-	virtual ~GTSAMOptimizer() {}
-
-	virtual Type type() const {return kTypeGTSAM;}
-
-	virtual std::map<int, Transform> optimize(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & edgeConstraints,
-			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
-			double * finalError = 0,
-			int * iterationsDone = 0);
-};
-
-class RTABMAP_EXP CVSBAOptimizer : public Optimizer
-{
-public:
-	static bool available();
-
-public:
-	CVSBAOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
-		Optimizer(iterations, slam2d, covarianceIgnored),
-		inlierDistance_(0.02),
-		minInliers_(10){}
-	CVSBAOptimizer(const ParametersMap & parameters) :
-		Optimizer(parameters),
-		inlierDistance_(0.02),
-		minInliers_(10){}
-	virtual ~CVSBAOptimizer() {}
-
-	virtual Type type() const {return kTypeCVSBA;}
-
-	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
-	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
-
-	virtual std::map<int, Transform> optimizeBA(
-			int rootId,
-			const std::map<int, Transform> & poses,
-			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
-
-private:
-	float inlierDistance_;
-	float minInliers_;
-};
 
 bool RTABMAP_EXP exportPoses(
 		const std::string & filePath,
 		int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
 		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & constraints, // required for formats 3 and 4
-		const std::map<int, double> & stamps); // required for format 1
+		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
+		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
+		bool g2oRobust = false); // optional for format 4
 
-////////////////////////////////////////////
-// Graph utilities
-////////////////////////////////////////////
 std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
 		std::multimap<int, Link> & links,
 		int from,

corelib/include/rtabmap/core/Optimizer.h

@@ -0,0 +1,112 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZER_H_
+#define OPTIMIZER_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <map>
+#include <list>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+////////////////////////////////////////////
+// Graph optimizers
+////////////////////////////////////////////
+class RTABMAP_EXP Optimizer
+{
+public:
+	enum Type {
+		kTypeUndef = -1,
+		kTypeTORO = 0,
+		kTypeG2O = 1,
+		kTypeGTSAM = 2,
+		kTypeCVSBA = 3
+	};
+	static bool isAvailable(Optimizer::Type type);
+	static Optimizer * create(const ParametersMap & parameters);
+	static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
+
+	// Get connected poses and constraints from a set of links
+	static void getConnectedGraph(
+			int fromId,
+			const std::map<int, Transform> & posesIn,
+			const std::multimap<int, Link> & linksIn, // only one link between two poses
+			std::map<int, Transform> & posesOut,
+			std::multimap<int, Link> & linksOut,
+			int depth = 0);
+
+public:
+	virtual ~Optimizer() {}
+
+	virtual Type type() const = 0;
+
+	int iterations() const {return iterations_;}
+	bool isSlam2d() const {return slam2d_;}
+	bool isCovarianceIgnored() const {return covarianceIgnored_;}
+	double epsilon() const {return epsilon_;}
+	bool isRobust() const {return robust_;}
+
+	// inherited classes should implement one of these methods
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+protected:
+	Optimizer(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+			bool robust            = Parameters::defaultOptimizerRobust());
+	Optimizer(const ParametersMap & parameters);
+
+private:
+	int iterations_;
+	bool slam2d_;
+	bool covarianceIgnored_;
+	double epsilon_;
+	bool robust_;
+};
+
+} /* namespace rtabmap */
+#endif /* OPTIMIZER_H_ */

corelib/include/rtabmap/core/OptimizerCVSBA.h

@@ -0,0 +1,73 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZERCVSBA_H_
+#define OPTIMIZERCVSBA_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OptimizerCVSBA : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	OptimizerCVSBA(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
+		Optimizer(iterations, slam2d, covarianceIgnored),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	OptimizerCVSBA(const ParametersMap & parameters) :
+		Optimizer(parameters),
+		inlierDistance_(0.02),
+		minInliers_(10){}
+	virtual ~OptimizerCVSBA() {}
+
+	virtual Type type() const {return kTypeCVSBA;}
+
+	void setInlierDistance(float inlierDistance) {inlierDistance_ = inlierDistance;}
+	void setMinInliers(int minInliers) {minInliers_ = minInliers;}
+
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures);
+
+private:
+	float inlierDistance_;
+	float minInliers_;
+};
+
+} /* namespace rtabmap */
+#endif /* OPTIMIZERCVSBA_H_ */

corelib/include/rtabmap/core/OptimizerG2O.h

@@ -0,0 +1,72 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZERG2O_H_
+#define OPTIMIZERG2O_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OptimizerG2O : public Optimizer
+{
+public:
+	static bool available();
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			bool useRobustConstraints = false);
+
+public:
+	OptimizerG2O(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+			bool robust            = Parameters::defaultOptimizerRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	OptimizerG2O(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~OptimizerG2O() {}
+
+	virtual Type type() const {return kTypeG2O;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+};
+
+} /* namespace rtabmap */
+#endif /* OPTIMIZERG2O_H_ */

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -0,0 +1,67 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZERGTSAM_H_
+#define OPTIMIZERGTSAM_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OptimizerGTSAM : public Optimizer
+{
+public:
+	static bool available();
+
+public:
+	OptimizerGTSAM(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+			bool robust            = Parameters::defaultOptimizerRobust()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
+
+	OptimizerGTSAM(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~OptimizerGTSAM() {}
+
+	virtual Type type() const {return kTypeGTSAM;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+};
+
+} /* namespace rtabmap */
+#endif /* OPTIMIZERGTSAM_H_ */

corelib/include/rtabmap/core/OptimizerTORO.h

@@ -0,0 +1,72 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZERTORO_H_
+#define OPTIMIZERTORO_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Optimizer.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP OptimizerTORO : public Optimizer
+{
+public:
+	static bool saveGraph(
+			const std::string & fileName,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints);
+	static bool loadGraph(
+			const std::string & fileName,
+			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & edgeConstraints);
+
+public:
+	OptimizerTORO(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+			double epsilon         = Parameters::defaultOptimizerEpsilon()) :
+		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
+	OptimizerTORO(const ParametersMap & parameters) :
+		Optimizer(parameters) {}
+	virtual ~OptimizerTORO() {}
+
+	virtual Type type() const {return kTypeTORO;}
+
+	virtual std::map<int, Transform> optimize(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & edgeConstraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+};
+
+} /* namespace rtabmap */
+#endif /* GRAPH_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -316,12 +316,12 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
 
 	// Graph optimization
-	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,       "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
-	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,     "Optimization iterations.");
-	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
-	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(RGBD, OptimizeEpsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
-	RTABMAP_PARAM(RGBD, OptimizeRobust,            bool, true,  "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
+	RTABMAP_PARAM(Optimizer, Strategy,          int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.")
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 100,        "Optimization iterations.");
+	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
+	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(Optimizer, Robust,            bool, true,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
 
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Bag-of-words 1=Optical Flow");

corelib/include/rtabmap/core/Rtabmap.h

@@ -47,9 +47,7 @@ class EpipolarGeometry;
 class Memory;
 class BayesFilter;
 class Signature;
-namespace graph {
 class Optimizer;
-}
 
 class RTABMAP_EXP Rtabmap
 {
@@ -216,7 +214,7 @@ private:
 	// strategies for a type of signature or configuration.
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
-	graph::Optimizer * _graphOptimizer;
+	Optimizer * _graphOptimizer;
 	ParametersMap _modifiedParameters;
 
 	Memory * _memory;