Tango: Handling app pause/resume without having to restart mapping (OpenGL context flushed issue).

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="11"
+        android:versionCode="12"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/CameraTango.cpp

@@ -77,8 +77,11 @@ void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuf
 
 void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 {
-	CameraTango* app = static_cast<CameraTango*>(context);
+	if(pose->status_code == TANGO_POSE_VALID)
-	app->poseReceived(app->tangoPoseToTransform(pose, true));
+	{
+		CameraTango* app = static_cast<CameraTango*>(context);
+		app->poseReceived(app->tangoPoseToTransform(pose, true));
+	}
 }
 
 void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
@@ -318,6 +321,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			-1.0f, 0.0f, 0.0f, 0.0f,
 			0.0f, -1.0f, 0.0f, 0.0f));
 
+	cameraStartedTime_.restart();
+
 	return true;
 }
 
@@ -634,7 +639,11 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 void CameraTango::mainLoopBegin()
 {
-	uSleep(2000); // just to make sure that the camera is started
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
 }
 
 void CameraTango::mainLoop()

app/android/jni/CameraTango.h

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
 
 class TangoPoseData;
@@ -97,6 +98,7 @@ private:
 private:
 	void * tango_config_;
 	bool firstFrame_;
+	UTimer cameraStartedTime_;
 	int decimation_;
 	bool autoExposure_;
 	cv::Mat cloud_;

app/android/jni/RTABMapApp.cpp

@@ -114,18 +114,20 @@ RTABMapApp::RTABMapApp() :
 		driftCorrection_(false),
 		localizationMode_(false),
 		trajectoryMode_(false),
-		autoExposure_(false),
+		autoExposure_(true),
 		fullResolution_(false),
 		maxCloudDepth_(0.0),
 		meshTrianglePix_(1),
 		meshAngleToleranceDeg_(15.0),
+		paused_(false),
 		clearSceneOnNextRender_(false),
 		filterPolygonsOnNextRender_(false),
 		gainCompensationOnNextRender_(false),
+		cameraJustInitialized_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
 		lastDrawnCloudsCount_(0),
-		renderingFPS_(0.0f)
+		renderingTime_(0.0f)
 
 {
 }
@@ -158,7 +160,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
 	totalPoints_ = 0;
 	totalPolygons_ = 0;
 	lastDrawnCloudsCount_ = 0;
-	renderingFPS_ = 0.0f;
+	renderingTime_ = 0.0f;
 
 	if(camera_)
 	{
@@ -249,7 +251,11 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
 		if(camera_->init())
 		{
 			LOGI("Start camera thread");
-			camera_->start();
+			if(!paused_)
+			{
+				camera_->start();
+			}
+			cameraJustInitialized_ = true;
 			return true;
 		}
 		LOGE("Failed camera initialization!");
@@ -303,6 +309,8 @@ private:
 // OpenGL thread
 int RTABMapApp::Render()
 {
+	UTimer fpsTime;
+	bool notifyDataLoaded = false;
 	boost::mutex::scoped_lock  lock(renderingMutex_);
 
 	// should be before clearSceneOnNextRender_ in case openDatabase is called
@@ -330,7 +338,23 @@ int RTABMapApp::Render()
 		totalPoints_ = 0;
 		totalPolygons_ = 0;
 		lastDrawnCloudsCount_ = 0;
-		renderingFPS_ = 0.0f;
+		renderingTime_ = 0.0f;
+	}
+
+	// Did we lose OpenGL context? If so, recreate the context;
+	if(main_scene_.getAddedClouds().size() != createdMeshes_.size())
+	{
+		for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
+		{
+			if(!main_scene_.hasCloud(iter->first))
+			{
+				cv::Mat compressed = iter->second.texture;
+				iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
+				main_scene_.addMesh(iter->first, iter->second, iter->second.pose);
+				main_scene_.setCloudVisible(iter->first, iter->second.visible);
+				iter->second.texture = compressed;
+			}
+		}
 	}
 
 	// Process events
@@ -347,9 +371,28 @@ int RTABMapApp::Render()
 	{
 		// update camera pose?
 		main_scene_.SetCameraPose(pose);
+		if(!camera_->isRunning() && cameraJustInitialized_)
+		{
+			notifyDataLoaded = true;
+			cameraJustInitialized_ = false;
+		}
+	}
+	rtabmap::OdometryEvent odomEvent;
+	{
+		boost::mutex::scoped_lock  lock(odomMutex_);
+		if(odomEvents_.size())
+		{
+			LOGI("Process odom events");
+			odomEvent = odomEvents_.back();
+			odomEvents_.clear();
+			if(cameraJustInitialized_)
+			{
+				notifyDataLoaded = true;
+				cameraJustInitialized_ = false;
+			}
+		}
 	}
 
-	bool notifyDataLoaded = false;
 	if(rtabmapEvents.size())
 	{
 		LOGI("Process rtabmap events");
@@ -422,14 +465,9 @@ int RTABMapApp::Render()
 						main_scene_.setCloudPose(id, iter->second);
 						main_scene_.setCloudVisible(id, true);
 						std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
-						if(meshIter!=createdMeshes_.end())
+						UASSERT(meshIter!=createdMeshes_.end());
-						{
+						meshIter->second.pose = iter->second;
-							meshIter->second.pose = iter->second;
+						meshIter->second.visible = true;
-						}
-						else
-						{
-							UERROR("Not found mesh %d !?!?", id);
-						}
 					}
 					else if(uContains(bufferedSensorData, id))
 					{
@@ -474,11 +512,12 @@ int RTABMapApp::Render()
 									inserted.first->second.height = cloud->height;
 									inserted.first->second.polygons = outputPolygons;
 									inserted.first->second.pose = iter->second;
+									inserted.first->second.visible = true;
 									inserted.first->second.texture = data.imageRaw();
 
 									main_scene_.addMesh(id, inserted.first->second, iter->second);
 
-									inserted.first->second.texture = data.imageCompressed(); // keep comrpessed
+									inserted.first->second.texture = data.imageCompressed(); // keep compressed
 								}
 								else
 								{
@@ -517,42 +556,32 @@ int RTABMapApp::Render()
 			if(*iter > 0 && poses.find(*iter) == poses.end())
 			{
 				main_scene_.setCloudVisible(*iter, false);
+				std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
+				UASSERT(meshIter!=createdMeshes_.end());
+				meshIter->second.visible = true;
 			}
 		}
 	}
 	else
 	{
-		rtabmap::OdometryEvent event;
-		bool set = false;
-		{
-			boost::mutex::scoped_lock  lock(odomMutex_);
-			if(odomEvents_.size())
-			{
-				LOGI("Process odom events");
-				event = odomEvents_.back();
-				odomEvents_.clear();
-				set = true;
-			}
-		}
-
 		main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
 
 		//just process the last one
-		if(set && !event.pose().isNull())
+		if(!odomEvent.pose().isNull())
 		{
 			if(odomCloudShown_ && !trajectoryMode_)
 			{
-				if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
+				if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), 1, maxCloudDepth_);
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), 1, maxCloudDepth_);
 					if(cloud->size())
 					{
 						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
-													event.data().imageRaw().cols, event.data().imageRaw().rows,
+								odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
-													event.data().depthRaw().cols, event.data().depthRaw().rows,
+								odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
-													(int)cloud->width, (int)cloud->height);
+							   (int)cloud->width, (int)cloud->height);
-						main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*event.pose());
+						main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*odomEvent.pose());
 						main_scene_.setCloudVisible(-1, true);
 					}
 					else
@@ -641,9 +670,11 @@ int RTABMapApp::Render()
 		notifyDataLoaded = true;
 	}
 
-	UTimer fpsTime;
     lastDrawnCloudsCount_ = main_scene_.Render();
-    renderingFPS_ = 1.0/fpsTime.elapsed();
+    if(renderingTime_ < fpsTime.elapsed())
+    {
+    	renderingTime_ = fpsTime.elapsed();
+    }
 
     if(rtabmapEvents.size())
     {
@@ -668,16 +699,19 @@ void RTABMapApp::OnTouchEvent(int touch_count,
 
 void RTABMapApp::setPausedMapping(bool paused)
 {
+	paused_ = paused;
 	if(camera_)
 	{
-		if(paused)
+		if(paused_)
 		{
 			LOGW("Pause!");
 			camera_->kill();
+
 		}
 		else
 		{
 			LOGW("Resume!");
+			UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdTriggerNewMap));
 			camera_->start();
 		}
 	}
@@ -1246,7 +1280,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 								featuresExtracted,
 								hypothesis,
 								lastDrawnCloudsCount_,
-								renderingFPS_,
+								renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
 								rejected);
 						success = true;
 					}
@@ -1258,6 +1292,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 		{
 			UERROR("Failed to call RTABMapActivity::updateStatsCallback");
 		}
+		renderingTime_ = 0.0f;
 	}
 }
 

app/android/jni/RTABMapApp.h

@@ -158,14 +158,15 @@ class RTABMapApp : public UEventsHandler {
 
   rtabmap::ParametersMap mappingParameters_;
 
-
+  bool paused_;
   bool clearSceneOnNextRender_;
   bool filterPolygonsOnNextRender_;
   bool gainCompensationOnNextRender_;
+  bool cameraJustInitialized_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
-  float renderingFPS_;
+  float renderingTime_;
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.

app/android/jni/util.h

@@ -140,9 +140,10 @@ struct Mesh
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // dense or organized cloud
 	std::vector<pcl::Vertices> polygons;
 	std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
-	int width;  // width of the organized cloud
+	unsigned int width;  // width of the organized cloud
-	int height; // height of the organized cloud
+	unsigned int height; // height of the organized cloud
 	rtabmap::Transform pose;
+	bool visible;
 	cv::Mat texture;
 };
 

app/android/res/menu/optionmenu.xml

@@ -22,7 +22,6 @@
           </menu>
        </item>
        <item android:id="@+id/open" android:title="Open"/>
-       <item android:id="@+id/save" android:title="Save"/>
        <item android:id="@+id/export" android:title="Export...">
           <menu>
 	         <group android:id="@+id/group_export">
@@ -31,6 +30,7 @@
 	         </group>   
           </menu>
        </item>
+       <item android:id="@+id/save" android:title="Save"/>
        <item android:id="@+id/reset" android:title="Reset"/>
        
        <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options...">
@@ -53,7 +53,7 @@
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
             <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
             <item android:id="@+id/grid_visible" android:checked="true" android:title="Grid Visible" />
-            <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
+            <item android:id="@+id/auto_exposure" android:checked="true" android:title="Auto Exposure" />
         </group>   
         </menu>
 	    </item>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -67,6 +67,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   // Screen size for normalizing the touch input for orbiting the render camera.
   private Point mScreenSize = new Point();
   private boolean mPauseFirstTime = true;
+  private boolean mOnPause = false;
   
   private MenuItem mItemPause;
   private MenuItem mItemSave;
@@ -161,6 +162,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     mLayoutDebug.setVisibility(LinearLayout.GONE);
     
     mProgressDialog = new ProgressDialog(this);
+    mProgressDialog.setCanceledOnTouchOutside(false);
     mRenderer.setProgressDialog(mProgressDialog);
 
     // Check if the Tango Core is out dated.
@@ -218,6 +220,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   @Override
   protected void onResume() {
     super.onResume();
+
+    mProgressDialog.setTitle("");
+	mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service..."));
+	mProgressDialog.show();
+    
+	if(mOnPause)
+	{
+		mToast.makeText(this, "Mapping is paused!", mToast.LENGTH_LONG).show();
+	}
+	else
+	{
+		mToast.makeText(this, "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\".", mToast.LENGTH_LONG).show();
+	}
+	
+	mOnPause = false;
     
     TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection);
     
@@ -226,16 +243,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     if (Tango.hasPermission(this, Tango.PERMISSIONTYPE_MOTION_TRACKING)) {
 
     	mGLView.onResume();
-        
-        mTotalLoopClosures = 0;
-        if(mItemOpen != null)
-        {
-    	    mItemOpen.setEnabled(false);
-    	    mItemPause.setChecked(false);
-    	    mItemSave.setEnabled(false);
-    		mItemExport.setEnabled(false);
-    		mItemPostProcessing.setEnabled(false);
-        }
 
     } else {
     	Log.i(TAG, String.format("Asking for motion tracking permission"));
@@ -248,14 +255,20 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   @Override
   protected void onPause() {
     super.onPause();
+    
+    // This deletes OpenGL context!
     mGLView.onPause();
     
-    // Delete all the non-OpenGl resources.
+    mOnPause = true;
+    
     RTABMapLib.onPause();
-    mOpenedDatabasePath = "";
-    RTABMapLib.openDatabase(mTempDatabasePath);
     
     unbindService(mTangoServiceConnection);
+    
+    if(!mItemPause.isChecked())
+    {
+    	onOptionsItemSelected(mItemPause);
+    }
   }
 
   @Override
@@ -440,7 +453,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					File tempFile = new File(mTempDatabasePath);
 					if(tempFile.renameTo(outputFile))
 					{
-						msg = String.format("Database saved to \"%s\".", mNewDatabasePath);
+						msg = String.format("Database saved to \"%s\". Tip: You can open a saved database with \"Open\".", mNewDatabasePath);
 						
 						Intent intent = new Intent(this, RTABMapActivity.class);
 						// use System.currentTimeMillis() to have a unique ID for the pending intent
@@ -653,16 +666,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  {
     		  RTABMapLib.setPausedMapping(true);
     		  ((TextView)findViewById(R.id.status)).setText("Paused");
-    		  if(mPauseFirstTime)
+    		  if(mPauseFirstTime && !mOnPause)
     		  {
     			  mPauseFirstTime = false;
-    			  mToast.makeText(getActivity(), String.format("Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
+    			  mToast.makeText(getActivity(), String.format("Tip: Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
     		  }
     	  }
     	  else
     	  {
     		  RTABMapLib.setPausedMapping(false);
     		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
+    		  mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
     	  }
       } 
       else if (itemId == R.id.post_processing_standard)
@@ -863,6 +877,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setDriftCorrection(item.isChecked());
+    	  if(item.isChecked())
+    	  {
+    		  mToast.makeText(getActivity(), String.format("Tip: With drift correction is enabled, move slowly to get better results."), mToast.LENGTH_LONG).show();
+    	  }
       }
       else if(itemId == R.id.graph_visible)
       {