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,7 +44,7 @@ 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()) :

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;

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)
 		{
@@ -1391,11 +1408,13 @@ bool OptimizerG2O::saveGraph(
 		}
 
 		// 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