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