GTSAM: added optimizer approach option #172

corelib/include/rtabmap/core/OptimizerGTSAM.h

@@ -40,20 +40,18 @@ public:
 	static bool available();
 
 public:
-	OptimizerGTSAM(
+	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
-			int iterations         = Parameters::defaultOptimizerIterations(),
+		Optimizer(parameters),
-			bool slam2d            = Parameters::defaultRegForce3DoF(),
+		optimizer_(Parameters::defaultGTSAMOptimizer())
-			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+	{
-			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+		parseParameters(parameters);
-			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 void parseParameters(const ParametersMap & parameters);
+
 	virtual std::map<int, Transform> optimize(
 			int rootId,
 			const std::map<int, Transform> & poses,
@@ -61,6 +59,9 @@ public:
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
 			double * finalError = 0,
 			int * iterationsDone = 0);
+
+private:
+	int optimizer_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -358,6 +358,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(g2o, RobustKernelDelta,       double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
 	RTABMAP_PARAM(g2o, Baseline,                double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 
+	RTABMAP_PARAM(GTSAM, Optimizer,             int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
+
 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");

corelib/src/OptimizerGTSAM.cpp

@@ -69,6 +69,12 @@ bool OptimizerGTSAM::available()
 #endif
 }
 
+void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
+{
+	Optimizer::parseParameters(parameters);
+	Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
+}
+
 std::map<int, Transform> OptimizerGTSAM::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -215,18 +221,29 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		}
 
 		UDEBUG("create optimizer");
+		gtsam::NonlinearOptimizer * optimizer;
+
+		if(optimizer_ == 2)
+		{
+			gtsam::DoglegParams parameters;
+			parameters.relativeErrorTol = epsilon();
+			parameters.maxIterations = iterations();
+			optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
+		}
+		else if(optimizer_ == 1)
+		{
 			gtsam::GaussNewtonParams parameters;
 			parameters.relativeErrorTol = epsilon();
 			parameters.maxIterations = iterations();
-		gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
+			optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
-		//gtsam::LevenbergMarquardtParams parametersLev;
+		}
-		//parametersLev.relativeErrorTol = epsilon();
+		else
-		//parametersLev.maxIterations = iterations();
+		{
-		//gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev);
+			gtsam::LevenbergMarquardtParams parameters;
-		//gtsam::DoglegParams parametersDogleg;
+			parameters.relativeErrorTol = epsilon();
-		//parametersDogleg.relativeErrorTol = epsilon();
+			parameters.maxIterations = iterations();
-		//parametersDogleg.maxIterations = iterations();
+			optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
-		//gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg);
+		}
 
 		UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
 		UTimer timer;
@@ -237,7 +254,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			if(intermediateGraphes && i > 0)
 			{
 				std::map<int, Transform> tmpPoses;
-				for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
+				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 				{
 					if(iter->value.dim() > 1)
 					{
@@ -257,17 +274,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			}
 			try
 			{
-				optimizer.iterate();
+				optimizer->iterate();
 				++it;
 			}
 			catch(gtsam::IndeterminantLinearSystemException & e)
 			{
 				UERROR("GTSAM exception caught: %s", e.what());
+				delete optimizer;
 				return optimizedPoses;
 			}
 
 			// early stop condition
-			double error = optimizer.error();
+			double error = optimizer->error();
 			UDEBUG("iteration %d error =%f", i+1, error);
 			double errorDelta = lastError - error;
 			if(i>0 && errorDelta < this->epsilon())
@@ -297,9 +315,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			*iterationsDone = it;
 		}
-		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks());
+		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
 
-		for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
+		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
 		{
 			if(iter->value.dim() > 1)
 			{
@@ -315,6 +333,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				}
 			}
 		}
+		delete optimizer;
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{

guilib/src/PreferencesDialog.cpp

@@ -752,6 +752,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_g2o_robustKernelDelta->setObjectName(Parameters::kg2oRobustKernelDelta().c_str());
 	_ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str());
 
+	_ui->comboBox_gtsam_optimizer->setObjectName(Parameters::kGTSAMOptimizer().c_str());
+
 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
 	_ui->graphPlan_stuckIterations->setObjectName(Parameters::kRGBDPlanStuckIterations().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-483</y>
+            <y>0</y>
             <width>673</width>
-            <height>2649</height>
+            <height>2718</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>15</number>
+                <number>21</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -12245,6 +12245,44 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </layout>
                      </widget>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_19">
+                      <property name="title">
+                       <string>GTSAM</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_83" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QComboBox" name="comboBox_gtsam_optimizer">
+                         <item>
+                          <property name="text">
+                           <string>Levenberg</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Gauss Newton</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Dogleg</string>
+                          </property>
+                         </item>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_364">
+                         <property name="text">
+                          <string>Optimization algorithm.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>