Added GPS class for convenience, database viewer can view GPS values and export to KML format

app/android/jni/CameraTango.cpp

@@ -116,8 +116,7 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
 		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0),
-		colorCameraToDisplayRotation_(ROTATION_0),
-		lastKnownGPS_(std::vector<double>(6,0))
+		colorCameraToDisplayRotation_(ROTATION_0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -190,7 +189,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
-	lastKnownGPS_ = std::vector<double>(6,0);
+	lastKnownGPS_ = GPS();
 
 	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
@@ -516,19 +515,9 @@ std::string CameraTango::getSerial() const
 	return "Tango";
 }
 
-void CameraTango::setGPS(double stamp,
-	  		double longitude,
-	  		double latitude,
-	  		double altitude,
-	  		double accuracy,
-	  		double bearing)
+void CameraTango::setGPS(const GPS & gps)
 {
-	lastKnownGPS_[0] = stamp;
-	lastKnownGPS_[1] = longitude;
-	lastKnownGPS_[2] = latitude;
-	lastKnownGPS_[3] = altitude;
-	lastKnownGPS_[4] = accuracy;
-	lastKnownGPS_[5] = bearing;
+	lastKnownGPS_ = gps;
 }
 
 rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
@@ -856,13 +845,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			}
 			data.setGroundTruth(odom);
 
-			if(lastKnownGPS_[0] > 0.0 && rgbStamp-lastKnownGPS_[0]<2.0)
+			if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<2.0)
 			{
-				data.setGPS(lastKnownGPS_[0], lastKnownGPS_[1], lastKnownGPS_[2], lastKnownGPS_[3], lastKnownGPS_[4], lastKnownGPS_[5]);
+				data.setGPS(lastKnownGPS_);
 			}
-			else if(lastKnownGPS_[0]>0.0)
+			else if(lastKnownGPS_.stamp()>0.0)
 			{
-				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_[0]);
+				LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp());
 			}
 		}
 		else

app/android/jni/CameraTango.h

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define CAMERATANGO_H_
 
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
@@ -89,12 +90,7 @@ public:
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
 	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
-	void setGPS(double stamp,
-	  		double longitude,
-	  		double latitude,
-	  		double altitude,
-	  		double accuracy,
-	  		double bearing);
+	void setGPS(const GPS & gps);
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
@@ -132,7 +128,7 @@ private:
 	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
-	std::vector<double> lastKnownGPS_;
+	GPS lastKnownGPS_;
 };
 
 } /* namespace rtabmap */

app/android/jni/RTABMapApp.cpp

@@ -1975,16 +1975,11 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
 	}
 }
 
-void RTABMapApp::setGPS(double stamp,
-  		double longitude,
-  		double latitude,
-  		double altitude,
-  		double accuracy,
-  		double bearing)
+void RTABMapApp::setGPS(const rtabmap::GPS & gps)
 {
 	if(camera_)
 	{
-		camera_->setGPS(stamp, longitude, latitude, altitude, accuracy, bearing);
+		camera_->setGPS(gps);
 	}
 }
 

app/android/jni/RTABMapApp.h

@@ -147,12 +147,7 @@ class RTABMapApp : public UEventsHandler {
   void setRenderingTextureDecimation(int value);
   void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
-  void setGPS(double stamp,
-  		double longitude,
-  		double latitude,
-  		double altitude,
-  		double accuracy,
-  		double bearing);
+  void setGPS(const rtabmap::GPS & gps);
 
   void resetMapping();
   void save(const std::string & databasePath);

app/android/jni/jni_interface.cpp

@@ -349,12 +349,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setGPS(
 		double accuracy,
 		double bearing)
 {
-	return app.setGPS(stamp,
+	return app.setGPS(GPS(stamp,
 			longitude,
 			latitude,
 			altitude,
 			accuracy,
-			bearing);
+			bearing));
 }
 
 JNIEXPORT void JNICALL

corelib/include/rtabmap/core/DBDriver.h

@@ -159,7 +159,7 @@ public:
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -253,7 +253,7 @@ private:
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+
 
 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_
@@ -52,12 +66,58 @@ public:
 	cv::Point3d toGeocentric_WGS84() const;
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
 
+	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
+	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
 private:
 	double latitude_;  // deg
 	double longitude_; // deg
 	double altitude_;  // m
 };
 
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
 }
 
 #endif /* GEODETICCOORDS_H_ */

corelib/include/rtabmap/core/Graph.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <map>
 #include <list>
 #include <rtabmap/core/Link.h>
+#include <rtabmap/core/GeodeticCoords.h>
 
 namespace rtabmap {
 class Memory;
@@ -58,6 +59,11 @@ bool RTABMAP_EXP importPoses(
 		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
 		std::map<int, double> * stamps = 0); // optional for format 1
 
+bool RTABMAP_EXP exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba = 0xFFFFFFFF);
+
 /**
  * Compute translation and rotation errors for KITTI datasets.
  * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.

corelib/include/rtabmap/core/Memory.h

@@ -177,7 +177,7 @@ public:
 			double & stamp,
 			Transform & groundTruth,
 			std::vector<float> & velocity,
-			std::vector<double> & gps,
+			GPS & gps,
 			bool lookInDatabase = false) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int nodeId, bool uncompressedData = false) const;
@@ -301,6 +301,7 @@ private:
 	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
+	GPS _gpsOrigin;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -53,7 +53,6 @@ private:
 private:
 #ifdef RTABMAP_ORB_SLAM2
 	ORBSLAM2System * orbslam2_;
-	ORB_SLAM2::System * system_;
 	bool firstFrame_;
 #endif
 	Transform originLocalTransform_;

corelib/include/rtabmap/core/Optimizer.h

@@ -75,6 +75,7 @@ public:
 	bool isCovarianceIgnored() const {return covarianceIgnored_;}
 	double epsilon() const {return epsilon_;}
 	bool isRobust() const {return robust_;}
+	bool priorsIgnored() const {return priorsIgnored_;}
 
 	// setters
 	void setIterations(int iterations) {iterations_ = iterations;}
@@ -82,6 +83,7 @@ public:
 	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}
+	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
 
 	virtual void parseParameters(const ParametersMap & parameters);
 
@@ -128,7 +130,8 @@ protected:
 			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
-			bool robust            = Parameters::defaultOptimizerRobust());
+			bool robust            = Parameters::defaultOptimizerRobust(),
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored());
 	Optimizer(const ParametersMap & parameters);
 
 private:
@@ -137,6 +140,7 @@ private:
 	bool covarianceIgnored_;
 	double epsilon_;
 	bool robust_;
+	bool priorsIgnored_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -362,6 +362,7 @@ class RTABMAP_EXP Parameters
 #endif
     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, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
 
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
     RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");

corelib/include/rtabmap/core/SensorData.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/LaserScanInfo.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
@@ -225,17 +226,11 @@ public:
 	const Transform & globalPose() const {return globalPose_;}
 	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
 
-	void setGPS(double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing)
+	void setGPS(const GPS & gps)
 	{
-		gps_ = std::vector<double>(6,0.0);
-		gps_[0]=stamp;
-		gps_[1]=longitude;
-		gps_[2]=latitude;
-		gps_[3]=altitude;
-		gps_[4]=accuracy;
-		gps_[5]=bearing;
+		gps_ = gps;
 	}
-	const std::vector<double> & gps() const {return gps_;}
+	const GPS & gps() const {return gps_;}
 
 	long getMemoryUsed() const; // Return memory usage in Bytes
 
@@ -278,7 +273,7 @@ private:
 	Transform globalPose_;
 	cv::Mat globalPoseCovariance_; // 6x6 double
 
-	std::vector<double> gps_;
+	GPS gps_;
 };
 
 }

corelib/src/DBDriver.cpp

@@ -713,7 +713,7 @@ bool DBDriver::getNodeInfo(
 		double & stamp,
 		Transform & groundTruthPose,
 		std::vector<float> & velocity,
-		std::vector<double> & gps) const
+		GPS & gps) const
 {
 	bool found = false;
 	// look in the trash

corelib/src/DBDriverSqlite3.cpp

@@ -1756,7 +1756,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		double & stamp,
 		Transform & groundTruthPose,
 		std::vector<float> & velocity,
-		std::vector<double> & gps) const
+		GPS & gps) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -1854,12 +1854,13 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 
 					if(uStrNumCmp(_version, "0.14.0") >= 0)
 					{
-						gps.resize(6,0);
+						std::vector<double> gpsV(6,0);
 						data = sqlite3_column_blob(ppStmt, index); // velocity
 						dataSize = sqlite3_column_bytes(ppStmt, index++);
-						if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
+						if((unsigned int)dataSize == gpsV.size()*sizeof(double) && data)
 						{
-							memcpy(gps.data(), data, dataSize);
+							memcpy(gpsV.data(), data, dataSize);
+							gps = GPS(gpsV[0], gpsV[1], gpsV[2], gpsV[3], gpsV[4], gpsV[5]);
 						}
 					}
 				}
@@ -2395,7 +2396,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				}
 				if(gps.size() == 6)
 				{
-					s->sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
+					s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]));
 				}
 				s->setSaved(true);
 				nodes.push_back(s);
@@ -4320,6 +4321,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 		}
 	}
 
+	std::vector<double> gps;
 	if(uStrNumCmp(_version, "0.10.1") >= 0)
 	{
 		// ignore user_data
@@ -4345,14 +4347,21 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 
 			if(uStrNumCmp(_version, "0.14.0") >= 0)
 			{
-				if(s->sensorData().gps().empty())
+				if(s->sensorData().gps().stamp() <= 0.0)
 				{
 					rc = sqlite3_bind_null(ppStmt, index++);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 				}
 				else
 				{
-					rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().gps().data(), s->sensorData().gps().size()*sizeof(double), SQLITE_STATIC);
+					gps.resize(6,0.0);
+					gps[0] = s->sensorData().gps().stamp();
+					gps[1] = s->sensorData().gps().longitude();
+					gps[2] = s->sensorData().gps().latitude();
+					gps[3] = s->sensorData().gps().altitude();
+					gps[4] = s->sensorData().gps().error();
+					gps[5] = s->sensorData().gps().bearing();
+					rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC);
 					UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 				}
 			}

corelib/src/DBDriverSqlite3.h

@@ -127,7 +127,7 @@ private:
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;

corelib/src/DBReader.cpp

@@ -268,7 +268,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
 						int mapId;
 						Transform localTransform, pose, groundTruth;
 						std::vector<float> velocity;
-						std::vector<double> gps;
+						GPS gps;
 						_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
 						if(previousStamp && stamp && stamp > previousStamp)
 						{
@@ -323,7 +323,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			double stamp;
 			Transform groundTruth;
 			std::vector<float> velocity;
-			std::vector<double> gps;
+			GPS gps;
 			_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
 
 			cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
@@ -437,6 +437,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			data.setId(seq);
 			data.setStamp(stamp);
 			data.setGroundTruth(groundTruth);
+			data.setGPS(gps);
 			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
 					data.laserScanRaw().empty()?0:1,
 					data.imageRaw().empty()?0:1,

corelib/src/GeodeticCoords.cpp

@@ -51,8 +51,24 @@ namespace rtabmap {
 
 
 inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
+inline double RAD2DEG(const double x) { return x*180.0/M_PI;}
 inline double square(const double & value) {return value*value;}
 
+GeodeticCoords::GeodeticCoords() :
+		latitude_(0.0),
+		longitude_(0.0),
+		altitude_(0.0)
+{
+
+}
+GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
+		latitude_(latitude),
+		longitude_(longitude),
+		altitude_(altitude)
+{
+
+}
+
 //*---------------------------------------------------------------
 //			geodeticToGeocentric_WGS84
 // ---------------------------------------------------------------*/
@@ -123,19 +139,72 @@ cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
 	return out;
 }
 
-GeodeticCoords::GeodeticCoords() :
-		latitude_(0.0),
-		longitude_(0.0),
-		altitude_(0.0)
+void GeodeticCoords::fromGeocentric_WGS84(const cv::Point3d& geocentric)
 {
+	static const double a = 6378137;		// Semi-major axis of the Earth (meters)
+	static const double b = 6356752.3142;	// Semi-minor axis:
 
+	const double sa2 = a*a;
+	const double sb2 = b*b;
+
+	const double e2 = (sa2 - sb2) / sa2;
+	const double ep2 = (sa2 - sb2) / sb2;
+	const double p = std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y);
+	const double theta = atan2(geocentric.z * a, p * b);
+
+	longitude_ = atan2(geocentric.y, geocentric.x);
+	latitude_ = atan2(
+			geocentric.z + ep2 * b * sin(theta) * sin(theta) * sin(theta),
+		p - e2 * a * cos(theta) * cos(theta) * cos(theta));
+
+	const double clat = cos(latitude_);
+	const double slat = sin(latitude_);
+	const double N = sa2 / std::sqrt(sa2 * clat * clat + sb2 * slat * slat);
+
+	altitude_ = p / clat - N;
+	longitude_ = RAD2DEG(longitude_);
+	latitude_ = RAD2DEG(latitude_);
 }
-GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
-		latitude_(latitude),
-		longitude_(longitude),
-		altitude_(altitude)
+
+void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
 {
+	fromGeocentric_WGS84(ENU_WGS84ToGeocentric_WGS84(enu, origin));
+}
 
+cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
+{
+	// Generate reference 3D point:
+	cv::Point3f originGeocentric;
+	originGeocentric = origin.toGeocentric_WGS84();
+
+	cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
+
+	// Z axis -> In direction out-ward the center of the Earth:
+	cv::Vec3d REF_X, REF_Y, REF_Z;
+	REF_Z = cv::normalize(P_ref);
+
+	// 1st column: Starting at the reference point, move in the tangent
+	// direction
+	//   east-ward: I compute this as the derivative of P_ref wrt "longitude":
+	//      A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon);  --> -Z[1]
+	//      A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon);  -->  Z[0]
+	//      A_east[2] = 0;                                       -->  0
+	// ---------------------------------------------------------------------------
+	cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
+	REF_X = cv::normalize(AUX_X);
+
+	// 2nd column: The cross product:
+	REF_Y = REF_Z.cross(REF_X);
+
+	cv::Point3d out_coords;
+	out_coords.x =
+		REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
+	out_coords.y =
+		REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
+	out_coords.z =
+		REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;
+
+	return out_coords;
 }
 
 }

corelib/src/Graph.cpp

@@ -425,6 +425,105 @@ bool importPoses(
 	return false;
 }
 
+bool exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba)
+{
+	UDEBUG("%s", filePath.c_str());
+	std::string tmpPath = filePath;
+
+	std::string ext = UFile::getExtension(filePath);
+
+	if(ext.compare("kml")!=0 && ext.compare("txt")!=0)
+	{
+		UERROR("Only txt and kml formats are supported!");
+		return false;
+	}
+
+	FILE* fout = 0;
+#ifdef _MSC_VER
+	fopen_s(&fout, tmpPath.c_str(), "w");
+#else
+	fout = fopen(tmpPath.c_str(), "w");
+#endif
+	if(fout)
+	{
+		if(ext.compare("kml")==0)
+		{
+			std::string values;
+			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
+			{
+				values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
+			}
+
+			// switch argb (Qt format) -> abgr
+			unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
+
+			std::string colorHexa = uFormat("%08x", abgr);
+
+			fprintf(fout, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
+			fprintf(fout, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
+			fprintf(fout, "<Document>\n"
+						  "	<name>%s</name>\n", tmpPath.c_str());
+			fprintf(fout, "	<StyleMap id=\"msn_ylw-pushpin\">\n"
+						  "		<Pair>\n"
+						  "			<key>normal</key>\n"
+						  "			<styleUrl>#sn_ylw-pushpin</styleUrl>\n"
+						  "		</Pair>\n"
+						  "		<Pair>\n"
+						  "			<key>highlight</key>\n"
+						  "			<styleUrl>#sh_ylw-pushpin</styleUrl>\n"
+						  "		</Pair>\n"
+						  "	</StyleMap>\n"
+						  "	<Style id=\"sh_ylw-pushpin\">\n"
+						  "		<IconStyle>\n"
+						  "			<scale>1.2</scale>\n"
+						  "		</IconStyle>\n"
+						  "		<LineStyle>\n"
+						  "			<color>%s</color>\n"
+						  "		</LineStyle>\n"
+						  "	</Style>\n"
+						  "	<Style id=\"sn_ylw-pushpin\">\n"
+						  "		<LineStyle>\n"
+						  "			<color>%s</color>\n"
+						  "		</LineStyle>\n"
+						  "	</Style>\n", colorHexa.c_str(), colorHexa.c_str());
+			fprintf(fout, "	<Placemark>\n"
+						  "		<name>%s</name>\n"
+						  "		<styleUrl>#msn_ylw-pushpin</styleUrl>"
+						  "		<LineString>\n"
+						  "			<coordinates>\n"
+						  "				%s\n"
+						  "			</coordinates>\n"
+						  "		</LineString>\n"
+						  "	</Placemark>\n"
+						  "</Document>\n"
+						  "</kml>\n",
+						  uSplit(tmpPath, '.').front().c_str(),
+						  values.c_str());
+		}
+		else
+		{
+			fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
+			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
+			{
+				fprintf(fout, "%f %f %f %f %f %f\n",
+						iter->second.stamp(),
+						iter->second.longitude(),
+						iter->second.latitude(),
+						iter->second.altitude(),
+						iter->second.error(),
+						iter->second.bearing());
+			}
+		}
+
+		fclose(fout);
+		return true;
+	}
+	return false;
+}
+
 // KITTI evaluation
 float lengths[] = {100,200,300,400,500,600,700,800};
 int32_t num_lengths = 8;

corelib/src/Memory.cpp

@@ -1401,6 +1401,7 @@ void Memory::clear()
 	_idMapCount = kIdStart;
 	_memoryChanged = false;
 	_linksChanged = false;
+	_gpsOrigin = GPS();
 
 	if(_dbDriver)
 	{
@@ -3051,7 +3052,7 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
 	std::string label;
 	double stamp;
 	std::vector<float> velocity;
-	std::vector<double> gps;
+	GPS gps;
 	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
 	return pose;
 }
@@ -3063,7 +3064,7 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
 	std::string label;
 	double stamp;
 	std::vector<float> velocity;
-	std::vector<double> gps;
+	GPS gps;
 	getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
 	return groundTruth;
 }
@@ -3076,7 +3077,7 @@ bool Memory::getNodeInfo(int signatureId,
 		double & stamp,
 		Transform & groundTruth,
 		std::vector<float> & velocity,
-		std::vector<double> & gps,
+		GPS & gps,
 		bool lookInDatabase) const
 {
 	const Signature * s = this->getSignature(signatureId);
@@ -3967,10 +3968,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	s->sensorData().setUserDataRaw(data.userDataRaw());
 
 	s->sensorData().setGroundTruth(data.groundTruth());
-	if(!data.gps().empty())
-	{
-		s->sensorData().setGPS(data.gps()[0], data.gps()[1], data.gps()[2], data.gps()[3], data.gps()[4], data.gps()[5]);
-	}
+	s->sensorData().setGPS(data.gps());
 
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
@@ -3996,9 +3994,39 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
 
 	// prior
-	if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
+	if(!isIntermediateNode)
 	{
-		s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
+		if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
+		{
+			s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
+
+			/*if(data.gps().stamp() > 0.0)
+			{
+				UWARN("GPS constraint ignored as global pose is also set.");
+			}*/
+		}
+		else if(data.gps().stamp() > 0.0)
+		{
+			// TODO: What kind of covariance should we set to have decent gtsam and g2o results!?
+			/*if(_gpsOrigin.stamp() <= 0.0)
+			{
+				_gpsOrigin =  data.gps();
+			}
+			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
+			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
+			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)*0.00000001;
+			if(data.gps().error() > 0.0)
+			{
+				// only set x, y as we don't know variance for other degrees of freedom.
+				gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1;
+				gpsInfMatrix.at<double>(2,2) = 100000;
+				s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
+			}
+			else
+			{
+				UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
+			}*/
+		}
 	}
 
 	return s;

corelib/src/OdometryORBSLAM2.cpp

@@ -749,7 +749,6 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
 #ifdef RTABMAP_ORB_SLAM2
     ,
 	orbslam2_(0),
-	system_(0),
 	firstFrame_(true)
 #endif
 {

corelib/src/Optimizer.cpp

@@ -181,7 +181,10 @@ void Optimizer::getConnectedGraph(
 				uFormat("Input links should be unique between two poses (%d->%d).",
 						iter->second.from(), iter->second.to()).c_str());
 		biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
-		biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
+		if(iter->second.from() != iter->second.to())
+		{
+			biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
+		}
 	}
 
 	while((depth == 0 || d < depth) && nextDepth.size())
@@ -199,18 +202,26 @@ void Optimizer::getConnectedGraph(
 				for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
 				{
 					int nextId = iter->second;
-					if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
+					if(uContains(posesIn, nextId))
 					{
-						nextDepth.insert(nextId);
+						if(ids.find(nextId) == ids.end())
+						{
+							nextDepth.insert(nextId);
 
-						std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
-						if(depth == 0 || d < depth-1)
-						{
-							linksOut.insert(*kter);
+							std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
+							if(depth == 0 || d < depth-1)
+							{
+								linksOut.insert(*kter);
+							}
+							else if(curentDepth.find(nextId) != curentDepth.end() ||
+									ids.find(nextId) != ids.end())
+							{
+								linksOut.insert(*kter);
+							}
 						}
-						else if(curentDepth.find(nextId) != curentDepth.end() ||
-								ids.find(nextId) != ids.end())
+						else if(*jter == nextId)
 						{
+							std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
 							linksOut.insert(*kter);
 						}
 					}
@@ -221,12 +232,13 @@ void Optimizer::getConnectedGraph(
 	}
 }
 
-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) :
 		iterations_(iterations),
 		slam2d_(slam2d),
 		covarianceIgnored_(covarianceIgnored),
 		epsilon_(epsilon),
-		robust_(robust)
+		robust_(robust),
+		priorsIgnored_(priorsIgnored)
 {
 }
 
@@ -235,7 +247,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
 		slam2d_(Parameters::defaultRegForce3DoF()),
 		covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
 		epsilon_(Parameters::defaultOptimizerEpsilon()),
-		robust_(Parameters::defaultOptimizerRobust())
+		robust_(Parameters::defaultOptimizerRobust()),
+		priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored())
 {
 	parseParameters(parameters);
 }
@@ -247,6 +260,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
 	Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
 	Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
+	Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
 }
 
 std::map<int, Transform> Optimizer::optimize(

corelib/src/OptimizerG2O.cpp

@@ -235,17 +235,19 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		}
 
 		// detect if there is a global pose prior set, if so remove rootId
-		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		if(!priorsIgnored())
 		{
-			if(iter->second.from() == iter->second.to())
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 			{
-				rootId = 0;
-				break;
+				if(iter->second.from() == iter->second.to())
+				{
+					rootId = 0;
+					break;
+				}
 			}
 		}
 
 		UDEBUG("fill poses to g2o...");
-		std::map<int, std::pair<Transform, cv::Mat> > geoPoses; // pose / information matrix
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			UASSERT(!iter->second.isNull());
@@ -292,47 +294,50 @@ std::map<int, Transform> OptimizerG2O::optimize(
 
 			if(id1 == id2)
 			{
-				if(isSlam2d())
+				if(!priorsIgnored())
 				{
-					g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
-					g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
-					priorEdge->setVertex(0, v1);
-					priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
-					priorEdge->setParameterId(0, PARAM_OFFSET);
-					Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
-					if(!isCovarianceIgnored())
+					if(isSlam2d())
 					{
-						information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
-						information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
-						information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
-						information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
-						information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
-						information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
-						information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
-						information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
-						information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
+						g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior();
+						g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
+						priorEdge->setVertex(0, v1);
+						priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
+						priorEdge->setParameterId(0, PARAM_OFFSET);
+						Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+							information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+							information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+							information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+							information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+							information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+							information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+							information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+							information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
+						}
+						priorEdge->setInformation(information);
+						edge = priorEdge;
 					}
-					priorEdge->setInformation(information);
-					edge = priorEdge;
-				}
-				else
-				{
-					g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
-					g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
-					priorEdge->setVertex(0, v1);
-					Eigen::Affine3d a = iter->second.transform().toEigen3d();
-					Eigen::Isometry3d pose;
-					pose = a.rotation();
-					pose.translation() = a.translation();
-					priorEdge->setMeasurement(pose);
-					priorEdge->setParameterId(0, PARAM_OFFSET);
-					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
-					if(!isCovarianceIgnored())
+					else
 					{
-						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+						g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior();
+						g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
+						priorEdge->setVertex(0, v1);
+						Eigen::Affine3d a = iter->second.transform().toEigen3d();
+						Eigen::Isometry3d pose;
+						pose = a.rotation();
+						pose.translation() = a.translation();
+						priorEdge->setMeasurement(pose);
+						priorEdge->setParameterId(0, PARAM_OFFSET);
+						Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+						}
+						priorEdge->setInformation(information);
+						edge = priorEdge;
 					}
-					priorEdge->setInformation(information);
-					edge = priorEdge;
 				}
 			}
 			else
@@ -462,7 +467,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 				}
 			}
 
-			if (!optimizer.addEdge(edge))
+			if (edge && !optimizer.addEdge(edge))
 			{
 				delete edge;
 				UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
@@ -767,34 +772,37 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				int id1 = iter->second.from();
 				int id2 = iter->second.to();
 
-				UASSERT(!iter->second.transform().isNull());
-
-				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
-				memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-
-				// between cameras, not base_link
-				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-				UDEBUG("added edge %d->%d (in cam frame=%s)",
-						id1,
-						id2,
-						camLink.prettyPrint().c_str());
-				Eigen::Affine3d a = camLink.toEigen3d();
-
-				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
-				g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
-				g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
-				UASSERT(v1 != 0);
-				UASSERT(v2 != 0);
-				e->setVertex(0, v1);
-				e->setVertex(1, v2);
-				e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
-				e->setInformation(information);
-
-				if (!optimizer.addEdge(e))
+				if(id1 != id2) // not supporting prior
 				{
-					delete e;
-					UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
-					return optimizedPoses;
+					UASSERT(!iter->second.transform().isNull());
+
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+
+					// between cameras, not base_link
+					Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
+					UDEBUG("added edge %d->%d (in cam frame=%s)",
+							id1,
+							id2,
+							camLink.prettyPrint().c_str());
+					Eigen::Affine3d a = camLink.toEigen3d();
+
+					g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
+					g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
+					g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					e->setVertex(0, v1);
+					e->setVertex(1, v2);
+					e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation()));
+					e->setInformation(information);
+
+					if (!optimizer.addEdge(e))
+					{
+						delete e;
+						UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2);
+						return optimizedPoses;
+					}
 				}
 			}
 		}

corelib/src/OptimizerGTSAM.cpp

@@ -99,18 +99,34 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 	{
 		gtsam::NonlinearFactorGraph graph;
 
-		//prior first pose
-		UASSERT(uContains(poses, rootId));
-		const Transform & initialPose = poses.at(rootId);
-		if(isSlam2d())
+		// detect if there is a global pose prior set, if so remove rootId
+		if(!priorsIgnored())
 		{
-			gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
-			graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+			{
+				if(iter->second.from() == iter->second.to())
+				{
+					rootId = 0;
+					break;
+				}
+			}
 		}
-		else
+
+		//prior first pose
+		if(rootId != 0)
 		{
-			gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
-			graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
+			UASSERT(uContains(poses, rootId));
+			const Transform & initialPose = poses.at(rootId);
+			if(isSlam2d())
+			{
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01));
+				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
+			}
+			else
+			{
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
+			}
 		}
 
 		UDEBUG("fill poses to gtsam...");
@@ -134,93 +150,130 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			int id1 = iter->second.from();
 			int id2 = iter->second.to();
+			UASSERT(!iter->second.transform().isNull());
 			if(id1 == id2)
 			{
-				// not supporting pose prior
-				continue;
-			}
-
-			UASSERT(!iter->second.transform().isNull());
-
-#ifdef RTABMAP_VERTIGO
-			if(this->isRobust() &&
-			   iter->second.type()!=Link::kNeighbor &&
-			   iter->second.type() != Link::kNeighborMerged)
-			{
-				// create new switch variable
-				// Sunderhauf IROS 2012:
-				// "Since it is reasonable to initially accept all loop closure constraints,
-				//  a proper and convenient initial value for all switch variables would be
-				//  sij = 1 when using the linear switch function"
-				double prior = 1.0;
-				initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
-
-				// create switch prior factor
-				// "If the front-end is not able to assign sound individual values
-				//  for Ξij , it is save to set all Ξij = 1, since this value is close
-				//  to the individual optimal choice of Ξij for a large range of
-				//  outliers."
-				gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
-				graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
-			}
-#endif
-
-			if(isSlam2d())
-			{
-				Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
-				if(!isCovarianceIgnored())
+				if(!priorsIgnored())
 				{
-					// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
-					information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
-					information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
-					information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
-					information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
-					information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
-					information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
-					information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
-					information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
-					information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
-				}
-				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+					if(isSlam2d())
+					{
+						Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+							information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
+							information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
+							information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
+							information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
+							information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
+							information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
+							information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
+							information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
+							information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
+						}
 
-#ifdef RTABMAP_VERTIGO
-				if(this->isRobust() &&
-				   iter->second.type()!=Link::kNeighbor &&
-				   iter->second.type() != Link::kNeighborMerged)
-				{
-					// create switchable edge factor
-					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
-				}
-				else
-#endif
-				{
-					graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+						gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+						graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
+					else
+					{
+						Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+							// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+							information = information / 1000.0;
+						}
+
+						gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+						graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
 				}
 			}
 			else
 			{
-				Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
-				if(!isCovarianceIgnored())
-				{
-					memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
-					// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
-					information = information / 1000.0;
-				}
-
-				gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
-
 #ifdef RTABMAP_VERTIGO
 				if(this->isRobust() &&
-				   iter->second.type()!=Link::kNeighbor &&
-				   iter->second.type() != Link::kNeighborMerged)
+				   iter->second.type() != Link::kNeighbor &&
+				   iter->second.type() != Link::kNeighborMerged &&
+				   iter->second.type() != Link::kPosePrior)
 				{
-					// create switchable edge factor
-					graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					// create new switch variable
+					// Sunderhauf IROS 2012:
+					// "Since it is reasonable to initially accept all loop closure constraints,
+					//  a proper and convenient initial value for all switch variables would be
+					//  sij = 1 when using the linear switch function"
+					double prior = 1.0;
+					initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
+
+					// create switch prior factor
+					// "If the front-end is not able to assign sound individual values
+					//  for Ξij , it is save to set all Ξij = 1, since this value is close
+					//  to the individual optimal choice of Ξij for a large range of
+					//  outliers."
+					gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
+					graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
+				}
+#endif
+
+				if(isSlam2d())
+				{
+					Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+						information(0,0) = iter->second.infMatrix().at<double>(0,0)/1000.0; // x-x
+						information(0,1) = iter->second.infMatrix().at<double>(0,1)/1000.0; // x-y
+						information(0,2) = iter->second.infMatrix().at<double>(0,5)/1000.0; // x-theta
+						information(1,0) = iter->second.infMatrix().at<double>(1,0)/1000.0; // y-x
+						information(1,1) = iter->second.infMatrix().at<double>(1,1)/1000.0; // y-y
+						information(1,2) = iter->second.infMatrix().at<double>(1,5)/1000.0; // y-theta
+						information(2,0) = iter->second.infMatrix().at<double>(5,0)/1000.0; // theta-x
+						information(2,1) = iter->second.infMatrix().at<double>(5,1)/1000.0; // theta-y
+						information(2,2) = iter->second.infMatrix().at<double>(5,5)/1000.0; // theta-theta
+					}
+					gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+
+#ifdef RTABMAP_VERTIGO
+					if(this->isRobust() &&
+					   iter->second.type()!=Link::kNeighbor &&
+					   iter->second.type() != Link::kNeighborMerged)
+					{
+						// create switchable edge factor
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
+					else
+#endif
+					{
+						graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
 				}
 				else
-#endif
 				{
-					graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+						// For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization)
+						information = information / 1000.0;
+					}
+
+					gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+
+#ifdef RTABMAP_VERTIGO
+					if(this->isRobust() &&
+					   iter->second.type() != Link::kNeighbor &&
+					   iter->second.type() != Link::kNeighborMerged &&
+					   iter->second.type() != Link::kPosePrior)
+					{
+						// create switchable edge factor
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
+					else
+#endif
+					{
+						graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
 				}
 			}
 		}

corelib/src/Rtabmap.cpp

@@ -761,7 +761,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
 				std::string l;
 				double stamp = 0.0;
 				std::vector<float> v;
-				std::vector<double> gps;
+				GPS gps;
 				_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true);
 				stamps.insert(std::make_pair(iter->first, stamp));
 			}
@@ -2651,7 +2651,7 @@ bool Rtabmap::process(
 			double stamp = 0;
 			Transform groundTruth;
 			std::vector<float> velocity;
-			std::vector<double> gps;
+			GPS gps;
 			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false);
 			signatures.insert(std::make_pair(iter->first,
 					Signature(iter->first,
@@ -2665,10 +2665,7 @@ bool Rtabmap::process(
 			{
 				signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 			}
-			if(!gps.empty())
-			{
-				signatures.at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
-			}
+			signatures.at(iter->first).sensorData().setGPS(gps);
 		}
 		localGraphSize = (int)poses.size();
 		if(!lastSignatureLocalizedPose.isNull())
@@ -3346,7 +3343,7 @@ void Rtabmap::get3DMap(
 			double stamp = 0;
 			Transform groundTruth;
 			std::vector<float> velocity;
-			std::vector<double> gps;
+			GPS gps;
 			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true);
 			SensorData data = _memory->getNodeData(*iter);
 			data.setId(*iter);
@@ -3370,10 +3367,7 @@ void Rtabmap::get3DMap(
 			{
 				signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 			}
-			if(!gps.empty())
-			{
-				signatures.at(*iter).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
-			}
+			signatures.at(*iter).sensorData().setGPS(gps);
 		}
 	}
 	else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3426,7 +3420,7 @@ void Rtabmap::getGraph(
 				double stamp = 0;
 				Transform groundTruth;
 				std::vector<float> velocity;
-				std::vector<double> gps;
+				GPS gps;
 				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
@@ -3455,10 +3449,7 @@ void Rtabmap::getGraph(
 				{
 					signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
 				}
-				if(!gps.empty())
-				{
-					signatures->at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
-				}
+				signatures->at(iter->first).sensorData().setGPS(gps);
 			}
 		}
 	}

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <set>
+#include <vector>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
@@ -94,6 +95,9 @@ private slots:
 	void exportPosesKITTI();
 	void exportPosesTORO();
 	void exportPosesG2O();
+	void exportPosesKML();
+	void exportGPS_TXT();
+	void exportGPS_KML();
 	void generateLocalGraph();
 	void regenerateLocalMaps();
 	void regenerateCurrentLocalMaps();
@@ -162,6 +166,7 @@ private:
 	void refineConstraint(int from, int to,  bool silent);
 	bool addConstraint(int from, int to, bool silent);
 	void exportPoses(int format);
+	void exportGPS(int format);
 
 private:
 	Ui_DatabaseViewer * ui_;
@@ -181,6 +186,8 @@ private:
 	std::multimap<int, rtabmap::Link> graphLinks_;
 	std::map<int, rtabmap::Transform> poses_;
 	std::map<int, rtabmap::Transform> groundTruthPoses_;
+	std::map<int, rtabmap::Transform> gpsPoses_;
+	std::map<int, GPS> gpsValues_;
 	std::multimap<int, rtabmap::Link> links_;
 	std::multimap<int, rtabmap::Link> linksRefined_;
 	std::multimap<int, rtabmap::Link> linksAdded_;

guilib/include/rtabmap/gui/GraphViewer.h

@@ -34,8 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QMap>
 #include <QtCore/QSettings>
 #include <rtabmap/core/Link.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <opencv2/opencv.hpp>
 #include <map>
+#include <vector>
 
 class QGraphicsItem;
 class QGraphicsPixmapItem;
@@ -58,6 +60,9 @@ public:
 					 const std::multimap<int, Link> & constraints,
 					 const std::map<int, int> & mapIds);
 	void updateGTGraph(const std::map<int, Transform> & poses);
+	void updateGPSGraph(
+			const std::map<int, Transform> & gpsMapPoses,
+			const std::map<int, GPS> & gpsValues);
 	void updateReferentialPosition(const Transform & t);
 	void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
 	void updatePosterior(const std::map<int, float> & posterior);
@@ -89,6 +94,7 @@ public:
 	const QColor & getLocalPathColor() const {return _localPathColor;}
 	const QColor & getGlobalPathColor() const {return _globalPathColor;}
 	const QColor & getGTColor() const {return _gtPathColor;}
+	const QColor & getGPSColor() const {return _gpsPathColor;}
 	const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;}
 	const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;}
 	bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;}
@@ -102,6 +108,8 @@ public:
 	bool isGlobalPathVisible() const;
 	bool isLocalPathVisible() const;
 	bool isGtGraphVisible() const;
+	bool isGPSGraphVisible() const;
+	bool isOrientationENU() const;
 
 	// setters
 	void setWorkingDirectory(const QString & path);
@@ -119,6 +127,7 @@ public:
 	void setLocalPathColor(const QColor & color);
 	void setGlobalPathColor(const QColor & color);
 	void setGTColor(const QColor & color);
+	void setGPSColor(const QColor & color);
 	void setIntraSessionLoopColor(const QColor & color);
 	void setInterSessionLoopColor(const QColor & color);
 	void setIntraInterSessionColorsEnabled(bool enabled);
@@ -132,6 +141,8 @@ public:
 	void setGlobalPathVisible(bool visible);
 	void setLocalPathVisible(bool visible);
 	void setGtGraphVisible(bool visible);
+	void setGPSGraphVisible(bool visible);
+	void setOrientationENU(bool enabled);
 
 signals:
 	void configChanged();
@@ -158,6 +169,7 @@ private:
 	QColor _localPathColor;
 	QColor _globalPathColor;
 	QColor _gtPathColor;
+	QColor _gpsPathColor;
 	QColor _loopIntraSessionColor;
 	QColor _loopInterSessionColor;
 	bool _intraInterSessionColors;
@@ -166,10 +178,13 @@ private:
 	QGraphicsItem * _globalPathRoot;
 	QGraphicsItem * _localPathRoot;
 	QGraphicsItem * _gtGraphRoot;
+	QGraphicsItem * _gpsGraphRoot;
 	QMap<int, NodeItem*> _nodeItems;
 	QMultiMap<int, LinkItem*> _linkItems;
 	QMap<int, NodeItem*> _gtNodeItems;
+	QMap<int, NodeItem*> _gpsNodeItems;
 	QMultiMap<int, LinkItem*> _gtLinkItems;
+	QMultiMap<int, LinkItem*> _gpsLinkItems;
 	QMultiMap<int, LinkItem*> _localPathLinkItems;
 	QMultiMap<int, LinkItem*> _globalPathLinkItems;
 	float _nodeRadius;
@@ -181,6 +196,7 @@ private:
 	QGraphicsEllipseItem * _localRadius;
 	float _loopClosureOutlierThr;
 	float _maxLinkLength;
+	bool _orientationENU;
 };
 
 } /* namespace rtabmap */

guilib/src/DatabaseViewer.cpp

@@ -68,6 +68,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/RegistrationIcp.h"
 #include "rtabmap/core/OccupancyGrid.h"
+#include "rtabmap/core/GeodeticCoords.h"
 #include "rtabmap/gui/DataRecorder.h"
 #include "ExportCloudsDialog.h"
 #include "EditDepthArea.h"
@@ -229,6 +230,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
 	connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
 	connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
+	connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
+	connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
+	connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
 	connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
 	connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
 	connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -250,6 +254,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	ui_->pushButton_reject->setEnabled(false);
 
 	ui_->menuExport_poses->setEnabled(false);
+	ui_->menuExport_GPS->setEnabled(false);
+	ui_->actionPoses_KML->setEnabled(false);
 
 	ui_->horizontalSlider_A->setTracking(false);
 	ui_->horizontalSlider_B->setTracking(false);
@@ -698,6 +704,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			graphLinks_.clear();
 			poses_.clear();
 			groundTruthPoses_.clear();
+			gpsPoses_.clear();
+			gpsValues_.clear();
 			mapIds_.clear();
 			links_.clear();
 			linksAdded_.clear();
@@ -710,6 +718,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			ui_->graphViewer->clearAll();
 			occupancyGridViewer_->clear();
 			ui_->menuExport_poses->setEnabled(false);
+			ui_->menuExport_GPS->setEnabled(false);
+			ui_->actionPoses_KML->setEnabled(false);
 			ui_->checkBox_showOptimized->setEnabled(false);
 			ui_->toolBox_statistics->clear();
 			databaseFileName_.clear();
@@ -1011,6 +1021,7 @@ void DatabaseViewer::exportDatabase()
 			std::map<int, Transform> poses;
 			std::map<int, double> stamps;
 			std::map<int, Transform> groundTruths;
+			std::map<int, GPS> gpsValues;
 			for(int i=0; i<ids_.size(); i+=1+framesIgnored)
 			{
 				Transform odomPose, groundTruth;
@@ -1019,7 +1030,7 @@ void DatabaseViewer::exportDatabase()
 				std::string label;
 				double stamp = 0;
 				std::vector<float> velocity;
-				std::vector<double> gps;
+				GPS gps;
 				if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
 				{
 					if(frameRate == 0 ||
@@ -1040,6 +1051,10 @@ void DatabaseViewer::exportDatabase()
 							poses.insert(std::make_pair(ids_[i], odomPose));
 							stamps.insert(std::make_pair(ids_[i], stamp));
 							groundTruths.insert(std::make_pair(ids_[i], groundTruth));
+							if(gps.stamp() > 0.0)
+							{
+								gpsValues.insert(std::make_pair(ids_[i], gps));
+							}
 						}
 					}
 					if(sessionExported >= 0 && mapId > sessionExported)
@@ -1115,7 +1130,14 @@ void DatabaseViewer::exportDatabase()
 							stamps.at(id),
 							userData);
 					}
-					sensorData.setGroundTruth(groundTruths.at(id));
+					if(groundTruths.find(id)!=groundTruths.end())
+					{
+						sensorData.setGroundTruth(groundTruths.at(id));
+					}
+					if(gpsValues.find(id)!=gpsValues.end())
+					{
+						sensorData.setGPS(gpsValues.at(id));
+					}
 
 					recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
 
@@ -1294,8 +1316,12 @@ void DatabaseViewer::updateIds()
 	mapIds_.clear();
 	poses_.clear();
 	groundTruthPoses_.clear();
+	gpsPoses_.clear();
+	gpsValues_.clear();
 	ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
 	ui_->label_alignPosesWithGroundTruth->setVisible(false);
+	ui_->menuExport_GPS->setEnabled(false);
+	ui_->actionPoses_KML->setEnabled(false);
 	links_.clear();
 	linksAdded_.clear();
 	linksRefined_.clear();
@@ -1325,10 +1351,9 @@ void DatabaseViewer::updateIds()
 		double s;
 		int mapId;
 		std::vector<float> v;
-		std::vector<double> gps;
+		GPS gps;
 		dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps);
 		mapIds_.insert(std::make_pair(ids_[i], mapId));
-
 		if(i>0)
 		{
 			if(mapIds_.at(ids_[i-1]) == mapId)
@@ -1392,6 +1417,20 @@ void DatabaseViewer::updateIds()
 			{
 				groundTruthPoses_.insert(std::make_pair(ids_[i], g));
 			}
+			if(gps.stamp() > 0.0)
+			{
+				gpsValues_.insert(std::make_pair(ids_[i], gps));
+
+				cv::Point3f p(0.0f,0.0f,0.0f);
+				if(!gpsPoses_.empty())
+				{
+					GeodeticCoords coords = gps.toGeodeticCoords();
+					GPS originGPS = gpsValues_.begin()->second;
+					p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
+				}
+				Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*180.0/M_PI));
+				gpsPoses_.insert(std::make_pair(ids_[i], pose));
+			}
 		}
 
 		if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
@@ -1403,10 +1442,23 @@ void DatabaseViewer::updateIds()
 			}
 		}
 	}
-	if(!groundTruthPoses_.empty())
+	if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
 	{
 		ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
 		ui_->label_alignPosesWithGroundTruth->setVisible(true);
+		if(!groundTruthPoses_.empty())
+		{
+			ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth"));
+		}
+		else
+		{
+			ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS"));
+		}
+	}
+	if(!gpsValues_.empty())
+	{
+		ui_->menuExport_GPS->setEnabled(true);
+		ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
 	}
 
 	UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size());
@@ -1434,6 +1486,7 @@ void DatabaseViewer::updateIds()
 	ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
 	ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size()));
 	ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
+	ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
 	ui_->textEdit_info->append("");
 	long total = 0;
 	long dbSize = UFile::length(dbDriver_->getUrl());
@@ -1663,6 +1716,10 @@ void DatabaseViewer::exportPosesG2O()
 {
 	exportPoses(4);
 }
+void DatabaseViewer::exportPosesKML()
+{
+	exportPoses(5);
+}
 
 void DatabaseViewer::exportPoses(int format)
 {
@@ -1676,6 +1733,104 @@ void DatabaseViewer::exportPoses(int format)
 		}
 	}
 
+	if(format == 5)
+	{
+		if(gpsValues_.empty() || gpsPoses_.empty())
+		{
+			QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
+		}
+		else
+		{
+			std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
+
+			//align with ground truth for more meaningful results
+			pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
+			cloud1.resize(graph.size());
+			cloud2.resize(graph.size());
+			int oi = 0;
+			int idFirst = 0;
+			for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
+			{
+				std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
+				if(iter2!=graph.end())
+				{
+					if(oi==0)
+					{
+						idFirst = iter->first;
+					}
+					cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+					cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
+				}
+			}
+
+			Transform t = Transform::getIdentity();
+			if(oi>5)
+			{
+				cloud1.resize(oi);
+				cloud2.resize(oi);
+
+				t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
+			}
+			else if(idFirst)
+			{
+				t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
+			}
+
+			std::map<int, GPS> values;
+			GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
+			for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
+			{
+				iter->second = t * iter->second;
+
+				GeodeticCoords coord;
+				coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
+				double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
+				if(bearing < 0)
+				{
+					bearing += 360;
+				}
+
+				Transform p, g;
+				int w;
+				std::string l;
+				double stamp=0.0;
+				int mapId;
+				std::vector<float> v;
+				GPS gps;
+				dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps);
+				values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
+			}
+
+			QString output = pathDatabase_ + QDir::separator() + "poses.kml";
+			QString path = QFileDialog::getSaveFileName(
+					this,
+					tr("Save File"),
+					output,
+					tr("Google Earth file (*.kml)"));
+
+			if(!path.isEmpty())
+			{
+				bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
+
+				if(saved)
+				{
+					QMessageBox::information(this,
+							tr("Export poses..."),
+							tr("GPS coordinates saved to \"%1\".")
+							.arg(path));
+				}
+				else
+				{
+					QMessageBox::information(this,
+							tr("Export poses..."),
+							tr("Failed to save GPS coordinates to \"%1\"!")
+							.arg(path));
+				}
+			}
+		}
+		return;
+	}
+
 	std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
 
 	if(optimizedPoses.size())
@@ -1796,7 +1951,7 @@ void DatabaseViewer::exportPoses(int format)
 				double stamp=0.0;
 				int mapId;
 				std::vector<float> v;
-				std::vector<double> gps;
+				GPS gps;
 				if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps))
 				{
 					stamps.insert(std::make_pair(iter->first, stamp));
@@ -1842,6 +1997,48 @@ void DatabaseViewer::exportPoses(int format)
 	}
 }
 
+void DatabaseViewer::exportGPS_TXT()
+{
+	exportGPS(0);
+}
+void DatabaseViewer::exportGPS_KML()
+{
+	exportGPS(1);
+}
+
+void DatabaseViewer::exportGPS(int format)
+{
+	if(!gpsValues_.empty())
+	{
+		QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				output,
+				format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
+
+		if(!path.isEmpty())
+		{
+			bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
+
+			if(saved)
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("GPS coordinates saved to \"%1\".")
+						.arg(path));
+			}
+			else
+			{
+				QMessageBox::information(this,
+						tr("Export poses..."),
+						tr("Failed to save GPS coordinates to \"%1\"!")
+						.arg(path));
+			}
+		}
+	}
+}
+
 void DatabaseViewer::generateGraph()
 {
 	if(!dbDriver_)
@@ -1989,7 +2186,7 @@ void DatabaseViewer::regenerateLocalMaps()
 		double stamp;
 		QString msg;
 		std::vector<float> velocity;
-		std::vector<double> gps;
+		GPS gps;
 		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
 		{
 			Signature s = data;
@@ -2067,7 +2264,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
 		double stamp;
 		QString msg;
 		std::vector<float> velocity;
-		std::vector<double> gps;
+		GPS gps;
 		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
 		{
 			Signature s = data;
@@ -2469,7 +2666,7 @@ void DatabaseViewer::update(int value,
 				std::string l;
 				double s;
 				std::vector<float> v;
-				std::vector<double> gps;
+				GPS gps;
 				dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps);
 
 				weight->setNum(w);
@@ -2482,10 +2679,10 @@ void DatabaseViewer::update(int value,
 					stamp->setText(QString::number(s, 'f'));
 					stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
-				if(gps.size())
+				if(gps.stamp()>0.0)
 				{
-					labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps[0], 'f')).arg(gps[1]).arg(gps[2]).arg(gps[3]).arg(gps[4]).arg(gps[5]));
-					labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps[0]*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
+					labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
+					labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
 				}
 				if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
 				{
@@ -3511,7 +3708,7 @@ void DatabaseViewer::updateConstraintView(
 			double s;
 			Transform p,g;
 			std::vector<float> v;
-			std::vector<double> gps;
+			GPS gps;
 			dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps);
 			if(!p.isNull())
 			{
@@ -3914,16 +4111,22 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 	{
 		std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
 
+		std::map<int, Transform> refPoses = groundTruthPoses_;
+		if(refPoses.empty())
+		{
+			refPoses = gpsPoses_;
+		}
+
 		// Log ground truth statistics (in TUM's RGBD-SLAM format)
-		if(groundTruthPoses_.size())
+		if(refPoses.size())
 		{
 			// compute KITTI statistics before aligning the poses
 			float length = graph::computePathLength(graph);
-			if(groundTruthPoses_.size() == graph.size() && length >= 100.0f)
+			if(refPoses.size() == graph.size() && length >= 100.0f)
 			{
 				float t_err = 0.0f;
 				float r_err = 0.0f;
-				graph::calcKittiSequenceErrors(uValues(groundTruthPoses_), uValues(graph), t_err, r_err);
+				graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
 				UINFO("KITTI t_err = %f %%", t_err);
 				UINFO("KITTI r_err = %f deg/m", r_err);
 				ui_->toolBox_statistics->updateStat("GT/kitti_t_err/%", t_err, false);
@@ -3938,7 +4141,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				cloud2.resize(graph.size());
 				int oi = 0;
 				int idFirst = 0;
-				for(std::map<int, Transform>::const_iterator iter=groundTruthPoses_.begin(); iter!=groundTruthPoses_.end(); ++iter)
+				for(std::map<int, Transform>::const_iterator iter=refPoses.begin(); iter!=refPoses.end(); ++iter)
 				{
 					std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
 					if(iter2!=graph.end())
@@ -3962,7 +4165,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 				}
 				else if(idFirst)
 				{
-					t = groundTruthPoses_.at(idFirst) * graph.at(idFirst).inverse();
+					t = refPoses.at(idFirst) * graph.at(idFirst).inverse();
 				}
 				if(!t.isIdentity())
 				{
@@ -3987,8 +4190,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			int oi=0;
 			for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
 			{
-				std::map<int, Transform>::const_iterator jter = groundTruthPoses_.find(iter->first);
-				if(jter!=groundTruthPoses_.end())
+				std::map<int, Transform>::const_iterator jter = refPoses.find(iter->first);
+				if(jter!=refPoses.end())
 				{
 					Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
 					Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
@@ -4142,6 +4345,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 		}
 
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
+		ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
 		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
 		ui_->graphViewer->clearMap();
 		occupancyGridViewer_->clear();
@@ -4428,6 +4632,7 @@ void DatabaseViewer::updateGraphView()
 		int totalLocalTime = 0;
 		int totalLocalSpace = 0;
 		int totalUser = 0;
+		int totalPriors = 0;
 		for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
 		{
 			if(iter->second.type() == Link::kNeighbor)
@@ -4474,15 +4679,20 @@ void DatabaseViewer::updateGraphView()
 				}
 				++totalUser;
 			}
+			else if(iter->second.type() == Link::kPosePrior)
+			{
+				++totalPriors;
+			}
 			++iter;
 		}
-		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6)")
+		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
 				.arg(totalNeighbor)
 				.arg(totalNeighborMerged)
 				.arg(totalGlobal)
 				.arg(totalLocalSpace)
 				.arg(totalLocalTime)
-				.arg(totalUser));
+				.arg(totalUser)
+				.arg(totalPriors));
 
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 

guilib/src/ExportCloudsDialog.cpp

@@ -2089,7 +2089,7 @@ bool ExportCloudsDialog::getExportedClouds(
 										int m,w;
 										std::string l;
 										double s;
-										std::vector<double> gps;
+										GPS gps;
 										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
 									}
 								}
@@ -2122,7 +2122,7 @@ bool ExportCloudsDialog::getExportedClouds(
 										int m,w;
 										std::string l;
 										double s;
-										std::vector<double> gps;
+										GPS gps;
 										_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
 									}
 								}

guilib/src/GraphViewer.cpp

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtCore/QUrl>
 
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UCv2Qt.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -81,6 +82,8 @@ public:
 		this->setBrush(b);
 	}
 
+	int id() const {return _id;};
+	int mapId() const {return _mapId;}
 	const Transform & pose() const {return _pose;}
 	void setPose(const Transform & pose) {this->setPos(-pose.y()*100.0f,-pose.x()*100.0f); _pose=pose;}
 
@@ -104,6 +107,29 @@ private:
 	Transform _pose;
 };
 
+class NodeGPSItem: public NodeItem
+{
+public:
+	NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps) :
+		NodeItem(id, mapId, pose, radius),
+		_gps(gps)
+	{
+	}
+	virtual ~NodeGPSItem() {}
+protected:
+	virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
+	{
+		this->setToolTip(QString("%1 [%2] %3\n"
+				"longitude=%4 latitude=%5 altitude=%6m error=%7m bearing=%8deg")
+				.arg(id()).arg(mapId()).arg(pose().prettyPrint().c_str())
+				.arg(_gps.longitude()).arg(_gps.latitude()).arg(_gps.altitude()).arg(_gps.error()).arg(_gps.bearing()));
+		this->setScale(2);
+		QGraphicsEllipseItem::hoverEnterEvent(event);
+	}
+private:
+	GPS _gps;
+};
+
 class LinkItem: public QGraphicsLineItem
 {
 public:
@@ -195,6 +221,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_localPathColor(Qt::cyan),
 		_globalPathColor(Qt::darkMagenta),
 		_gtPathColor(Qt::gray),
+		_gpsPathColor(Qt::darkCyan),
 		_loopIntraSessionColor(Qt::red),
 		_loopInterSessionColor(Qt::green),
 		_intraInterSessionColors(false),
@@ -209,7 +236,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
 		_gridCellSize(0.0f),
 		_localRadius(0),
 		_loopClosureOutlierThr(0),
-		_maxLinkLength(0.02f)
+		_maxLinkLength(0.02f),
+		_orientationENU(false)
 {
 	this->setScene(new QGraphicsScene(this));
 	this->setDragMode(QGraphicsView::ScrollHandDrag);
@@ -268,6 +296,10 @@ GraphViewer::GraphViewer(QWidget * parent) :
 	_gtGraphRoot->setZValue(2);
 	_gtGraphRoot->setParentItem(_root);
 
+	_gpsGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
+	_gpsGraphRoot->setZValue(2);
+	_gpsGraphRoot->setParentItem(_root);
+
 	this->restoreDefaults();
 
 	this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
@@ -622,6 +654,135 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 	UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
 }
 
+void GraphViewer::updateGPSGraph(
+		const std::map<int, Transform> & poses,
+		const std::map<int, GPS> & gpsValues)
+{
+	UTimer timer;
+	bool wasVisible = _gpsGraphRoot->isVisible();
+	_gpsGraphRoot->show();
+	bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0;
+	UDEBUG("poses=%d", (int)poses.size());
+	//Hide nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
+	{
+		iter.value()->hide();
+		iter.value()->setColor(_gpsPathColor); // reset color
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
+	{
+		iter.value()->hide();
+	}
+
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		if(!iter->second.isNull())
+		{
+			QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
+			if(itemIter != _gpsNodeItems.end())
+			{
+				itemIter.value()->setPose(iter->second);
+				itemIter.value()->show();
+			}
+			else
+			{
+				// create node item
+				const Transform & pose = iter->second;
+				UASSERT(gpsValues.find(iter->first) != gpsValues.end());
+				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first));
+				this->scene()->addItem(item);
+				item->setZValue(20);
+				item->setColor(_gpsPathColor);
+				item->setParentItem(_gpsGraphRoot);
+				_gpsNodeItems.insert(iter->first, item);
+			}
+
+			if(iter!=poses.begin())
+			{
+				std::map<int, Transform>::const_iterator iterPrevious = iter;
+				--iterPrevious;
+				Transform previousPose = iterPrevious->second;
+				Transform currentPose = iter->second;
+
+				LinkItem * linkItem = 0;
+				QMultiMap<int, LinkItem*>::iterator linkIter = _gpsLinkItems.end();
+				if(_gpsLinkItems.contains(iterPrevious->first))
+				{
+					linkIter = _gpsLinkItems.find(iter->first);
+					while(linkIter.key() == iterPrevious->first && linkIter != _gpsLinkItems.end())
+					{
+						if(linkIter.value()->to() == iter->first)
+						{
+							linkIter.value()->setPoses(previousPose, currentPose);
+							linkIter.value()->show();
+							linkItem = linkIter.value();
+							break;
+						}
+						++linkIter;
+					}
+				}
+				if(linkItem == 0)
+				{
+					//create a link item
+					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1);
+					QPen p = linkItem->pen();
+					p.setWidthF(_linkWidth*100.0f);
+					linkItem->setPen(p);
+					linkItem->setZValue(10);
+					this->scene()->addItem(linkItem);
+					linkItem->setParentItem(_gpsGraphRoot);
+					_gpsLinkItems.insert(iterPrevious->first, linkItem);
+				}
+				if(linkItem)
+				{
+					linkItem->setColor(_gpsPathColor);
+				}
+			}
+		}
+	}
+
+	//remove not used nodes and links
+	for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gpsNodeItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end();)
+	{
+		if(!iter.value()->isVisible())
+		{
+			delete iter.value();
+			iter = _gpsLinkItems.erase(iter);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	if(_gpsNodeItems.size() || _gpsLinkItems.size())
+	{
+		this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+
+		if(wasEmpty)
+		{
+			QRectF rect = this->scene()->itemsBoundingRect();
+			this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
+		}
+	}
+
+	_gpsGraphRoot->setVisible(wasVisible);
+
+	UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks());
+}
+
 void GraphViewer::updateReferentialPosition(const Transform & t)
 {
 	QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24()*100.0f, -t.o14()*100.0f);
@@ -820,6 +981,10 @@ void GraphViewer::clearGraph()
 	_gtNodeItems.clear();
 	qDeleteAll(_gtLinkItems);
 	_gtLinkItems.clear();
+	qDeleteAll(_gpsNodeItems);
+	_gpsNodeItems.clear();
+	qDeleteAll(_gpsLinkItems);
+	_gpsLinkItems.clear();
 
 	_referential->resetTransform();
 	_localRadius->resetTransform();
@@ -867,6 +1032,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("local_path_color", this->getLocalPathColor());
 	settings.setValue("global_path_color", this->getGlobalPathColor());
 	settings.setValue("gt_color", this->getGTColor());
+	settings.setValue("gps_color", this->getGPSColor());
 	settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
 	settings.setValue("inter_session_color", this->getInterSessionLoopColor());
 	settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
@@ -880,6 +1046,8 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("global_path_visible", this->isGlobalPathVisible());
 	settings.setValue("local_path_visible", this->isLocalPathVisible());
 	settings.setValue("gt_graph_visible", this->isGtGraphVisible());
+	settings.setValue("gps_graph_visible", this->isGPSGraphVisible());
+	settings.setValue("orientation_ENU", this->isOrientationENU());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -906,6 +1074,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
 	this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
 	this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
+	this->setGPSColor(settings.value("gps_color", this->getGPSColor()).value<QColor>());
 	this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
 	this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
 	this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
@@ -919,6 +1088,8 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
 	this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
 	this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
+	this->setGPSGraphVisible(settings.value("gps_graph_visible", this->isGPSGraphVisible()).toBool());
+	this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -957,6 +1128,14 @@ bool GraphViewer::isGtGraphVisible() const
 {
 	return _gtGraphRoot->isVisible();
 }
+bool GraphViewer::isGPSGraphVisible() const
+{
+	return _gpsGraphRoot->isVisible();
+}
+bool GraphViewer::isOrientationENU() const
+{
+	return _orientationENU;
+}
 
 void GraphViewer::setWorkingDirectory(const QString & path)
 {
@@ -973,6 +1152,10 @@ void GraphViewer::setNodeRadius(float radius)
 	{
 		iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
 	}
+	for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
+	{
+		iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
+	}
 }
 void GraphViewer::setLinkWidth(float width)
 {
@@ -1100,6 +1283,18 @@ void GraphViewer::setGTColor(const QColor & color)
 		iter.value()->setColor(_gtPathColor);
 	}
 }
+void GraphViewer::setGPSColor(const QColor & color)
+{
+	_gpsPathColor = color;
+	for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gpsPathColor);
+	}
+	for(QMultiMap<int, LinkItem*>::iterator iter=_gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
+	{
+		iter.value()->setColor(_gpsPathColor);
+	}
+}
 void GraphViewer::setIntraSessionLoopColor(const QColor & color)
 {
 	_loopIntraSessionColor = color;
@@ -1192,6 +1387,18 @@ void GraphViewer::setGtGraphVisible(bool visible)
 {
 	_gtGraphRoot->setVisible(visible);
 }
+void GraphViewer::setGPSGraphVisible(bool visible)
+{
+	_gpsGraphRoot->setVisible(visible);
+}
+void GraphViewer::setOrientationENU(bool enabled)
+{
+	if(_orientationENU!=enabled)
+	{
+		_orientationENU = enabled;
+		this->rotate(_orientationENU?90:270);
+	}
+}
 
 void GraphViewer::restoreDefaults()
 {
@@ -1258,6 +1465,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
 	QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
 	QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
+	QAction * aChangeGPSColor = menuLink->addAction(tr("GPS"));
 	menuLink->addSeparator();
 	QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
 	QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
@@ -1272,6 +1480,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
 	aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
 	aChangeGTColor->setIcon(createIcon(_gtPathColor));
+	aChangeGPSColor->setIcon(createIcon(_gpsPathColor));
 	aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
 	aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
 	aChangeNeighborColor->setIconVisibleInMenu(true);
@@ -1284,6 +1493,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	aChangeLocalPathColor->setIconVisibleInMenu(true);
 	aChangeGlobalPathColor->setIconVisibleInMenu(true);
 	aChangeGTColor->setIconVisibleInMenu(true);
+	aChangeGPSColor->setIconVisibleInMenu(true);
 	aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
 	aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
 	aSetIntraInterSessionColors->setCheckable(true);
@@ -1302,6 +1512,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aShowHideGlobalPath;
 	QAction * aShowHideLocalPath;
 	QAction * aShowHideGtGraph;
+	QAction * aShowHideGPSGraph;
+	QAction * aOrientationENU;
 	if(_gridMap->isVisible())
 	{
 		aShowHideGridMap = menu.addAction(tr("Hide grid map"));
@@ -1366,10 +1578,22 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
 	}
+	if(_gpsGraphRoot->isVisible())
+	{
+		aShowHideGPSGraph = menu.addAction(tr("Hide GPS graph"));
+	}
+	else
+	{
+		aShowHideGPSGraph = menu.addAction(tr("Show GPS graph"));
+	}
+	aOrientationENU = menu.addAction(tr("ENU Orientation"));
+	aOrientationENU->setCheckable(true);
+	aOrientationENU->setChecked(_orientationENU);
 	aShowHideGraph->setEnabled(_nodeItems.size());
 	aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
 	aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
 	aShowHideGtGraph->setEnabled(_gtNodeItems.size());
+	aShowHideGPSGraph->setEnabled(_gpsNodeItems.size());
 	menu.addSeparator();
 	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
@@ -1487,6 +1711,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			r == aChangeLocalPathColor ||
 			r == aChangeGlobalPathColor ||
 			r == aChangeGTColor ||
+			r == aChangeGPSColor ||
 			r == aChangeIntraSessionLoopColor ||
 			r == aChangeInterSessionLoopColor)
 	{
@@ -1535,6 +1760,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		{
 			color = _gtPathColor;
 		}
+		else if(r == aChangeGPSColor)
+		{
+			color = _gpsPathColor;
+		}
 		else if(r == aChangeIntraSessionLoopColor)
 		{
 			color = _loopIntraSessionColor;
@@ -1595,6 +1824,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 			{
 				this->setGTColor(color);
 			}
+			else if(r == aChangeGPSColor)
+			{
+				this->setGPSColor(color);
+			}
 			else if(r == aChangeIntraSessionLoopColor)
 			{
 				this->setIntraSessionLoopColor(color);
@@ -1671,6 +1904,15 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	{
 		this->setGtGraphVisible(!this->isGtGraphVisible());
 	}
+	else if(r == aShowHideGPSGraph)
+	{
+		this->setGPSGraphVisible(!this->isGPSGraphVisible());
+	}
+	else if(r == aOrientationENU)
+	{
+		this->setOrientationENU(!this->isOrientationENU());
+	}
+
 	if(r)
 	{
 		emit configChanged();

guilib/src/PreferencesDialog.cpp

@@ -792,6 +792,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
 	_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
 	_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
+	_ui->graphOptimization_priorsIgnored->setObjectName(Parameters::kOptimizerPriorsIgnored().c_str());
 
 	_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
 	_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());

guilib/src/ui/DatabaseViewer.ui

@@ -51,8 +51,8 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-           <y>-24</y>
-           <width>393</width>
+           <y>0</y>
+           <width>408</width>
            <height>232</height>
           </rect>
          </property>
@@ -226,8 +226,8 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-           <y>-24</y>
-           <width>393</width>
+           <y>0</y>
+           <width>408</width>
            <height>232</height>
           </rect>
          </property>
@@ -534,6 +534,14 @@
      <addaction name="actionKITTI_format_txt"/>
      <addaction name="actionTORO_graph"/>
      <addaction name="actionG2o_g2o"/>
+     <addaction name="actionPoses_KML"/>
+    </widget>
+    <widget class="QMenu" name="menuExport_GPS">
+     <property name="title">
+      <string>Export GPS...</string>
+     </property>
+     <addaction name="actionGPS_TXT"/>
+     <addaction name="actionGPS_KML"/>
     </widget>
     <addaction name="actionOpen_database"/>
     <addaction name="separator"/>
@@ -543,6 +551,7 @@
     <addaction name="actionExport"/>
     <addaction name="actionExtract_images"/>
     <addaction name="menuExport_poses"/>
+    <addaction name="menuExport_GPS"/>
     <addaction name="separator"/>
     <addaction name="actionQuit"/>
    </widget>
@@ -898,7 +907,7 @@
        <item row="6" column="1">
         <widget class="QLabel" name="label_41">
          <property name="text">
-          <string>Links (N, NM, G, LS, LT, U)</string>
+          <string>Links (N, NM, G, LS, LT, U, P)</string>
          </property>
         </widget>
        </item>
@@ -1187,8 +1196,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>280</width>
-          <height>666</height>
+          <width>309</width>
+          <height>650</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1550,8 +1559,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>201</width>
-          <height>126</height>
+          <width>324</width>
+          <height>168</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1650,8 +1659,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>186</width>
-          <height>496</height>
+          <width>309</width>
+          <height>256</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2263,6 +2272,21 @@
     <string>g2o (*.g2o)</string>
    </property>
   </action>
+  <action name="actionGPS_KML">
+   <property name="text">
+    <string>Google Earth (*.kml)</string>
+   </property>
+  </action>
+  <action name="actionGPS_TXT">
+   <property name="text">
+    <string>Raw format (*.txt)</string>
+   </property>
+  </action>
+  <action name="actionPoses_KML">
+   <property name="text">
+    <string>Google Earth (*.kml)</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,25 +63,16 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-629</y>
-            <width>678</width>
-            <height>2739</height>
+            <y>0</y>
+            <width>673</width>
+            <height>2749</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
+           <property name="margin">
             <number>0</number>
            </property>
            <item>
@@ -95,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>18</number>
+                <number>14</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4570,16 +4561,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="leftMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
+                       <property name="margin">
                         <number>0</number>
                        </property>
                        <item>
@@ -8489,21 +8471,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                      </widget>
                     </item>
-                    <item row="3" column="0" rowspan="2">
+                    <item row="4" column="0" rowspan="2">
                      <widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
                       <property name="text">
                        <string/>
                       </property>
                      </widget>
                     </item>
-                    <item row="3" column="1">
+                    <item row="4" column="1">
                      <widget class="Line" name="line">
                       <property name="orientation">
                        <enum>Qt::Horizontal</enum>
                       </property>
                      </widget>
                     </item>
-                    <item row="4" column="1">
+                    <item row="5" column="1">
                      <widget class="QLabel" name="label_151">
                       <property name="text">
                        <string>Optimize graph from the newest node.</string>
@@ -8516,7 +8498,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                      </widget>
                     </item>
-                    <item row="5" column="1">
+                    <item row="6" column="1">
                      <widget class="QLabel" name="label_211">
                       <property name="text">
                        <string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
@@ -8529,7 +8511,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                      </widget>
                     </item>
-                    <item row="6" column="1">
+                    <item row="7" column="1">
                      <widget class="QLabel" name="label_183">
                       <property name="text">
                        <string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
@@ -8542,6 +8524,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                      </widget>
                     </item>
+                    <item row="3" column="1">
+                     <widget class="QLabel" name="label_431">
+                      <property name="text">
+                       <string>Ignore pose priors.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="0">
+                     <widget class="QCheckBox" name="graphOptimization_priorsIgnored">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>
@@ -12653,16 +12655,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                       <widget class="QWidget" name="page_54">
                        <layout class="QVBoxLayout" name="verticalLayout_85">
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -12802,16 +12795,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </widget>
                       <widget class="QWidget" name="page_55">
                        <layout class="QVBoxLayout" name="verticalLayout_86">
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -12969,16 +12953,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13058,16 +13033,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>
@@ -13179,16 +13145,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                          <number>0</number>
                         </property>
                         <item>

tools/KittiDataset/main.cpp

@@ -480,7 +480,7 @@ int main(int argc, char * argv[])
 				std::string l;
 				double s;
 				std::vector<float> v;
-				std::vector<double> gps;
+				GPS gps;
 				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
 				if(!gtPose.isNull())
 				{

tools/RgbdDataset/main.cpp

@@ -318,7 +318,7 @@ int main(int argc, char * argv[])
 				std::string l;
 				double s;
 				std::vector<float> v;
-				std::vector<double> gps;
+				GPS gps;
 				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
 				if(!gtPose.isNull())
 				{