diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in
index c849c43c..e6c454b7 100644
--- a/app/android/AndroidManifest.xml.in
+++ b/app/android/AndroidManifest.xml.in
@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="32"
+        android:versionCode="34"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp
index dde006b9..b40915e9 100644
--- a/app/android/jni/CameraTango.cpp
+++ b/app/android/jni/CameraTango.cpp
@@ -98,6 +98,7 @@ CameraTango::CameraTango(int decimation, bool autoExposure) :
 		Camera(0),
 		tango_config_(0),
 		firstFrame_(true),
+		stampEpochOffset_(0.0),
 		decimation_(decimation),
 		autoExposure_(autoExposure),
 		cloudStamp_(0),
@@ -337,45 +338,53 @@ void CameraTango::close()
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
-	if(this->isRunning())
+	if(this->isRunning() && !cloud.empty())
 	{
+		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
+
 		UASSERT(cloud.type() == CV_32FC4);
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
-		bool notify = cloud_.empty();
-		cloud_ = cloud.clone();
-		cloudStamp_ = timestamp;
-		LOGD("Depth received! (%d points)", cloud.cols);
-		if(!tangoColor_.empty() && notify)
+		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
+		// "In the current version of Project Tango Tablet RGB IR camera
+		//  is used for both depth and color images and it can only do one
+		//  or the other for each frame. So in the stream we get 4 RGB frames
+		//  followed by 1 Depth frame resulting in the pattern you observed. This
+		//  is more of a hardware limitation."
+		//
+		//  So, synchronize with the last RGB frame before the Depth is acquired
+		if(!tangoColor_.empty())
 		{
-			LOGD("Cloud: Release semaphore");
-			dataReady_.release();
+			double dt = fabs(timestamp - tangoColorStamp_);
+
+			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
+
+			if(dt >= 0.0 && dt < 0.5)
+			{
+				bool notify = cloud_.empty();
+				cloud_ = cloud.clone();
+				cloudStamp_ = timestamp;
+				if(notify)
+				{
+					//LOGD("Cloud: Release semaphore");
+					dataReady_.release();
+				}
+			}
 		}
 	}
 }
 
 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(this->isRunning())
+	if(this->isRunning() && !tangoImage.empty())
 	{
+		//LOGD("RGB received! %fs", timestamp);
+
 		boost::mutex::scoped_lock  lock(dataMutex_);
 
-		if(!cloud_.empty())
-		{
-			if(!tangoImage.empty())
-			{
-				bool notify = tangoColor_.empty();
-				tangoColor_ = tangoImage.clone();
-				tangoColorStamp_ = timestamp;
-				tangoColorType_ = type;
-				LOGD("RGB received!");
-				if(!cloud_.empty() && notify)
-				{
-					LOGD("RGB: Release semaphore");
-					dataReady_.release();
-				}
-			}
-		}
+		tangoColor_ = tangoImage.clone();
+		tangoColorStamp_ = timestamp;
+		tangoColorType_ = type;
 	}
 }
 
@@ -458,7 +467,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 
 SensorData CameraTango::captureImage(CameraInfo * info)
 {
-	LOGI("Capturing image...");
+	//LOGI("Capturing image...");
 
 	SensorData data;
 	if(!dataReady_.acquire(1, 2000))
@@ -567,7 +576,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			// The Color Camera frame at timestamp t0 with respect to Depth
 			// Camera frame at timestamp t1.
-			LOGI("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
 
 			int pixelsSet = 0;
 			depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm
@@ -607,7 +616,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			{
 				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
 			}
-			LOGI("pixels depth set= %d", pixelsSet);
+			//LOGD("pixels depth set= %d", pixelsSet);
 		}
 		else
 		{
@@ -620,15 +629,15 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
 
-			LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
-			LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
-			LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
+			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
+			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
+			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
 			//Rotate in RTAB-Map's coordinate
 			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
 
-			LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
-			LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
+			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
+			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
 
 			data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			data.setGroundTruth(odom);
@@ -661,6 +670,12 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
+			// convert stamp to epoch
+			if(firstFrame_)
+			{
+				stampEpochOffset_ = UTimer::now()-data.stamp();
+			}
+			data.setStamp(stampEpochOffset_ + data.stamp());
 			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
 			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
 			firstFrame_ = false;
diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h
index 4efdac24..44253e71 100644
--- a/app/android/jni/CameraTango.h
+++ b/app/android/jni/CameraTango.h
@@ -100,6 +100,7 @@ private:
 	void * tango_config_;
 	bool firstFrame_;
 	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
 	int decimation_;
 	bool autoExposure_;
 	cv::Mat cloud_;
diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp
index 6c49f20a..cd84e6b8 100644
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
@@ -191,6 +191,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	if(camera_)
 	{
 	  delete camera_;
+	  camera_ = 0;
 	}
 	if(rtabmapThread_)
 	{
@@ -407,6 +408,8 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
 // OpenGL thread
 int RTABMapApp::Render()
 {
+	UASSERT(camera_!=0 && rtabmap_!=0);
+
 	UTimer fpsTime;
 	boost::mutex::scoped_lock  lock(renderingMutex_);
 
@@ -983,10 +986,6 @@ void RTABMapApp::setLocalizationMode(bool enabled)
 }
 void RTABMapApp::setTrajectoryMode(bool enabled)
 {
-	if(trajectoryMode_ != enabled)
-	{
-		main_scene_.SetCameraType(enabled?tango_gl::GestureCamera::kTopDown:tango_gl::GestureCamera::kThirdPersonFollow);
-	}
 	trajectoryMode_ = enabled;
 	this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 }
@@ -994,6 +993,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
+	UASSERT(camera_ != 0 && rtabmap_!=0 && rtabmap_->getMemory()!=0);
 	if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
index a96419c8..b2936ba1 100644
--- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
@@ -1013,6 +1013,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		{
 			item.setChecked(!item.isChecked());
 			RTABMapLib.setTrajectoryMode(item.isChecked());
+			setCamera(item.isChecked()?2:1);
 		}
 		else if(itemId == R.id.graph_optimization)
 		{