OptmizerG2O: add prior edge export (#319)

* OptmizerG2O: add prior edge export

* add required conditions and structure to export prior edges

* for now, make the graph exportation of priors not dependent of parameters

* include <locale.h> so to use setlocale() in a windows build

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -44,14 +44,14 @@ public:
 			const std::string & fileName,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & edgeConstraints,
-			bool useRobustConstraints = false);
+			const bool useRobustConstraints = false);
 
 public:
 	OptimizerG2O(const ParametersMap & parameters = ParametersMap()) :
 		Optimizer(parameters),
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer()),
-        pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		pixelVariance_(Parameters::defaultg2oPixelVariance()),
 		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
 		baseline_(Parameters::defaultg2oBaseline())
 	{

corelib/src/Graph.cpp

@@ -56,7 +56,7 @@ bool exportPoses(
 		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
-		bool g2oRobust) // optional for format 4
+		const bool g2oRobust) // optional for format 4
 {
 	UDEBUG("%s", filePath.c_str());
 	std::string tmpPath = filePath;
@@ -366,8 +366,8 @@ bool importPoses(
 				std::vector<std::string> strList = uListToVector(uSplit(str));
 				if(strList.size() ==  3)
 				{
-					if( uIsNumber(uReplaceChar(strList[0], ' ', "")) && 
+					if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
-						uIsNumber(uReplaceChar(strList[1], ' ', "")) && 
+						uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
 						uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
 						(strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
 					{
@@ -379,7 +379,7 @@ bool importPoses(
 						{
 							stamps->insert(std::make_pair(id, stamp));
 						}
-						float yaw = 0.0f; 
+						float yaw = 0.0f;
 						if(uContains(poses, id-1))
 						{
 							// set yaw depending on successive poses

corelib/src/Rtabmap.cpp

@@ -2304,7 +2304,7 @@ bool Rtabmap::process(
 
 			UINFO("Update map correction");
 			std::map<int, Transform> poses = _optimizedPoses;
-			
+
 			// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
 			float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
 			if((normMapCorrection > 0.000001f && _optimizeFromGraphEnd) ||
@@ -4694,7 +4694,7 @@ void Rtabmap::updateGoalIndex()
 						this->clearPath(-1);
 						return;
 					}
-				}				
+				}
 			}
 			else if(!isStuck)
 			{

corelib/src/RtabmapThread.cpp

@@ -348,12 +348,12 @@ bool RtabmapThread::handleEvent(UEvent* event)
 					}
 				}
 				else
-				{ 
+				{
 					OdometryInfo infoCov;
 					infoCov.reg.covariance = e->info().odomCovariance;
 					this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 				}
-				
+
 			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)

corelib/src/optimizer/OptimizerG2O.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
+#include <locale.h>
 #include <set>
 
 #include <rtabmap/core/Version.h>
@@ -1363,7 +1364,7 @@ bool OptimizerG2O::saveGraph(
 		const std::string & fileName,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & edgeConstraints,
-		bool useRobustConstraints)
+		const bool useRobustConstraints)
 {
 	FILE * file = 0;
 
@@ -1375,6 +1376,22 @@ bool OptimizerG2O::saveGraph(
 
 	if(file)
 	{
+		// force periods to be used instead of commas
+		setlocale(LC_ALL, "en_US.UTF-8");
+
+		// PARAMS_SE3OFFSET id x y z qw qx qy qz (set for priors)
+		Eigen::Vector3f v = Eigen::Vector3f::Zero();
+		Eigen::Quaternionf q = Eigen::Quaternionf::Identity();
+		fprintf(file, "PARAMS_SE3OFFSET %d %f %f %f %f %f %f %f\n",
+			PARAM_OFFSET,
+			v.x(),
+			v.y(),
+			v.z(),
+			q.x(),
+			q.y(),
+			q.z(),
+			q.w());
+
 		// VERTEX_SE3 id x y z qw qx qy qz
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -1390,12 +1407,14 @@ bool OptimizerG2O::saveGraph(
 					q.w());
 		}
 
-		//EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
+		// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
+		// EDGE_SE3_PRIOR observed_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
 		int virtualVertexId = poses.size()?poses.rbegin()->first+1:0;
 		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
 			std::string prefix = "EDGE_SE3:QUAT";
 			std::string suffix = "";
+			std::string to = uFormat(" %d", iter->second.to());
 
 			if(useRobustConstraints &&
 			   iter->second.type() != Link::kNeighbor &&
@@ -1406,12 +1425,17 @@ bool OptimizerG2O::saveGraph(
 				fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId);
 				suffix = uFormat(" %d", virtualVertexId++);
 			}
+			else if(iter->second.type() == Link::kPosePrior)
+			{
+				prefix = "EDGE_SE3_PRIOR";
+				to = uFormat(" %d", PARAM_OFFSET);
+			}
 
 			Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
-			fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+			fprintf(file, "%s %d%s%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					prefix.c_str(),
 					iter->second.from(),
-					iter->second.to(),
+					to.c_str(),
 					suffix.c_str(),
 					iter->second.transform().x(),
 					iter->second.transform().y(),
@@ -1443,6 +1467,7 @@ bool OptimizerG2O::saveGraph(
 					iter->second.infMatrix().at<double>(5,5));
 		}
 		UINFO("Graph saved to %s", fileName.c_str());
+
 		fclose(file);
 	}
 	else

guilib/src/MainWindow.cpp

@@ -208,9 +208,9 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 
 	// Add cloud viewers
 	// Note that we add them here manually because there is a crash issue
-	// when adding them in a DockWidget of the *.ui file. The cloud viewer is 
+	// when adding them in a DockWidget of the *.ui file. The cloud viewer is
 	// created in a widget which is not yet linked to main window when the CloudViewer constructor
-	// is called (see order in generated ui file). VTK needs to get the top 
+	// is called (see order in generated ui file). VTK needs to get the top
 	// level window at the time CloudViewer is created, otherwise it may crash on some systems.
 	_cloudViewer = new CloudViewer(_ui->layout_cloudViewer);
 	_cloudViewer->setObjectName("widget_cloudViewer");
@@ -394,7 +394,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 
 	_ui->actionPause->setShortcut(Qt::Key_Space);
 	_ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
-	// Qt5 issue, we should explicitly add actions not in 
+	// Qt5 issue, we should explicitly add actions not in
 	// menu bar to have shortcut working
 	this->addAction(_ui->actionSave_GUI_config);
 	_ui->actionReset_Odometry->setEnabled(false);
@@ -670,23 +670,23 @@ void MainWindow::setupMainLayout(bool vertical)
 }
 
 void MainWindow::setCloudViewer(rtabmap::CloudViewer * cloudViewer)
-{ 
+{
-	UASSERT(cloudViewer); 
+	UASSERT(cloudViewer);
 	delete _cloudViewer;
-	_cloudViewer = cloudViewer; 
+	_cloudViewer = cloudViewer;
 	_cloudViewer->setParent(_ui->layout_cloudViewer);
 	_cloudViewer->setObjectName("widget_cloudViewer");
 	_ui->layout_cloudViewer->layout()->addWidget(_cloudViewer);
-	
+
 	_cloudViewer->setBackfaceCulling(true, false);
 	_preferencesDialog->loadWidgetState(_cloudViewer);
 
 	connect(_cloudViewer, SIGNAL(configChanged()), this, SLOT(configGUIModified()));
 }
 void MainWindow::setLoopClosureViewer(rtabmap::LoopClosureViewer * loopClosureViewer)
-{ 
+{
-	UASSERT(loopClosureViewer); 
+	UASSERT(loopClosureViewer);
-	delete _loopClosureViewer; 
+	delete _loopClosureViewer;
 	_loopClosureViewer = loopClosureViewer;
 	_loopClosureViewer->setParent(_ui->layout_loopClosureViewer);
 	_loopClosureViewer->setObjectName("widget_loopClosureViewer");