Tango: Added Data Recorder mode (See Mapping menu)

app/android/jni/RTABMapApp.cpp

@@ -103,6 +103,16 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.3")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
 
+	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
+	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemRehearsalSimilarity()));
+	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded()));
+	if(dataRecorderMode_)
+	{
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1")));
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0"))); // deactivate rehearsal
+		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
+	}
+
 	return parameters;
 }
 
@@ -123,6 +133,7 @@ RTABMapApp::RTABMapApp() :
 		meshTrianglePix_(1),
 		meshAngleToleranceDeg_(15.0),
 		paused_(false),
+		dataRecorderMode_(false),
 		clearSceneOnNextRender_(false),
 		filterPolygonsOnNextRender_(false),
 		gainCompensationOnNextRender_(0),
@@ -206,6 +217,10 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 
 	rtabmap_->init(parameters, databasePath);
 	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
+	if(parameters.find(rtabmap::Parameters::kRtabmapDetectionRate()) != parameters.end())
+	{
+		rtabmapThread_->setDetectorRate(uStr2Float(parameters.at(rtabmap::Parameters::kRtabmapDetectionRate())));
+	}
 
 	// Generate all meshes
 	std::map<int, rtabmap::Signature> signatures;
@@ -402,7 +417,7 @@ int RTABMapApp::Render()
 
 		// update buffered signatures
 		std::map<int, rtabmap::SensorData> bufferedSensorData;
-		if(!trajectoryMode_)
+		if(!trajectoryMode_ && !dataRecorderMode_)
 		{
 			for(std::list<rtabmap::Statistics>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
 			{
@@ -834,6 +849,14 @@ void RTABMapApp::setFullResolution(bool enabled)
 	}
 }
 
+void RTABMapApp::setDataRecorderMode(bool enabled)
+{
+	if(dataRecorderMode_ != enabled)
+	{
+		dataRecorderMode_ = enabled; // parameters will be set when resuming (we assume we are paused)
+	}
+}
+
 void RTABMapApp::setMaxCloudDepth(float value)
 {
 	maxCloudDepth_ = value;

app/android/jni/RTABMapApp.h

@@ -122,6 +122,7 @@ class RTABMapApp : public UEventsHandler {
   void setGridVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
+  void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
   void setMeshAngleTolerance(float value);
   void setMeshTriangleSize(int value);
@@ -159,6 +160,7 @@ class RTABMapApp : public UEventsHandler {
   rtabmap::ParametersMap mappingParameters_;
 
   bool paused_;
+  bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;

app/android/jni/jni_interface.cpp

@@ -192,6 +192,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
 	return app.setFullResolution(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setDataRecorderMode(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 		JNIEnv*, jobject, float value)
 {

app/android/res/menu/optionmenu.xml

@@ -67,6 +67,7 @@
 	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
 	           <item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" />
 	           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
+	           <item android:id="@+id/data_recorder" android:checked="false" android:title="Data Recorder Mode" />
 	           <item android:id="@+id/menu_param_settings" android:checkable="false" android:title="Parameters...">
 		           <menu >
 			           <item android:id="@+id/update_rate" android:title="Map Update Rate..." />

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

@@ -79,6 +79,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private MenuItem mItemRenderingPointCloud;
   private MenuItem mItemRenderingMesh;
   private MenuItem mItemRenderingTextureMesh;
+  private MenuItem mItemDataRecorderMode;
   
   
   private String mOpenedDatabasePath = "";
@@ -337,10 +338,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	  mItemRenderingPointCloud = menu.findItem(R.id.point_cloud);
 	  mItemRenderingMesh = menu.findItem(R.id.mesh);
 	  mItemRenderingTextureMesh = menu.findItem(R.id.texture_mesh);
+	  mItemDataRecorderMode = menu.findItem(R.id.data_recorder);
 	  mItemSave.setEnabled(false);
 	  mItemExport.setEnabled(false);
 	  mItemOpen.setEnabled(false);
 	  mItemPostProcessing.setEnabled(false);
+	  mItemDataRecorderMode.setEnabled(false);
 	  
 	  return true;
   }
@@ -525,6 +528,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					mItemPostProcessing.setEnabled(false);
 					mItemSave.setEnabled(false);
 					mItemExport.setEnabled(false);
+					mItemDataRecorderMode.setEnabled(false);
 					RTABMapLib.setPausedMapping(false); // resume mapping
 				}
 			}
@@ -658,9 +662,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
       {
     	  item.setChecked(!item.isChecked());
     	  mItemSave.setEnabled(item.isChecked());
-		  mItemExport.setEnabled(item.isChecked());
+		  mItemExport.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
-    	  mItemOpen.setEnabled(item.isChecked());
+    	  mItemOpen.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
-    	  mItemPostProcessing.setEnabled(item.isChecked());
+    	  mItemPostProcessing.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
+    	  mItemDataRecorderMode.setEnabled(item.isChecked());
     	 // mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
     	  if(item.isChecked())
     	  {
@@ -675,8 +680,15 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  else
     	  {
     		  RTABMapLib.setPausedMapping(false);
-    		  ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?"Localization":"Mapping");
+    		  if(mItemDataRecorderMode.isChecked())
-    		  mToast.makeText(getActivity(), String.format("On resume, a new map is created. Tip: Try relocalizing in the previous area."), mToast.LENGTH_LONG).show();
+    		  {
+    			  mToast.makeText(getActivity(), String.format("Data Recorder Mode: no map is created, only raw data is recorded."), mToast.LENGTH_LONG).show();
+    		  }
+    		  else
+    		  {
+	    		  ((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)
@@ -1112,6 +1124,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 										mItemOpen.setEnabled(false);
 										mItemPostProcessing.setEnabled(false);
 										mItemExport.setEnabled(false);
+										mItemDataRecorderMode.setEnabled(false);
 				                    }
 				                })
 				                .setNegativeButton("No", new DialogInterface.OnClickListener() {
@@ -1135,6 +1148,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 							  mItemOpen.setEnabled(false);
 							  mItemPostProcessing.setEnabled(false);
 							  mItemExport.setEnabled(false);
+							  mItemDataRecorderMode.setEnabled(false);
 						  }
 					  }
 				  }
@@ -1153,6 +1167,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			  mItemOpen.setEnabled(false);
 			  mItemPostProcessing.setEnabled(false);
 			  mItemExport.setEnabled(false);
+			  mItemDataRecorderMode.setEnabled(false);
     	  }
       }
       else if(itemId == R.id.reset)
@@ -1181,6 +1196,56 @@ public class RTABMapActivity extends Activity implements OnClickListener {
         	  RTABMapLib.openDatabase(mTempDatabasePath);
 		  }
       }
+      else if(itemId == R.id.data_recorder)
+      {
+		  final boolean dataRecorderOldState = item.isChecked();
+		  new AlertDialog.Builder(getActivity())
+          .setTitle("Data Recorder Mode")
+          .setMessage("Changing from/to data recorder mode will close the current session. Do you want to continue?")
+          .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+              public void onClick(DialogInterface dialog, int which) {           	  
+            	  // reset
+            	  ((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
+            	  ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
+            	  ((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
+        		  mTotalLoopClosures = 0;
+        		  ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
+            	   
+        		  mItemDataRecorderMode.setChecked(!dataRecorderOldState);
+            	  RTABMapLib.setDataRecorderMode(mItemDataRecorderMode.isChecked());
+            	  
+            	  mOpenedDatabasePath = "";
+        		  RTABMapLib.openDatabase(mTempDatabasePath);
+        		  
+        		  mItemOpen.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+    			  mItemPostProcessing.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+    			  mItemExport.setEnabled(!mItemDataRecorderMode.isChecked() && mItemPause.isChecked());
+            	  
+            	  if(mItemDataRecorderMode.isChecked())
+            	  {
+            		  mToast.makeText(getActivity(), String.format("Data recorder mode activated! Tip: You can increase data update rate in Parameters menu under Mapping options."), mToast.LENGTH_LONG).show();
+            	  }
+            	  else
+            	  {
+            		  mToast.makeText(getActivity(), String.format("Data recorder mode deactivated!"), mToast.LENGTH_LONG).show();
+            	  }
+              }
+          })
+          .setNegativeButton("No", new DialogInterface.OnClickListener() {
+              public void onClick(DialogInterface dialog, int which) {
+                  dialog.dismiss();
+              }
+          })
+          .show();
+      }
       else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
       {
     	  final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";

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

@@ -68,6 +68,7 @@ public class RTABMapLib
     public static native void setGridVisible(boolean visible);
     public static native void setAutoExposure(boolean enabled);
     public static native void setFullResolution(boolean enabled);
+    public static native void setDataRecorderMode(boolean enabled);
     public static native void setMaxCloudDepth(float value);
     public static native void setMeshAngleTolerance(float value);
     public static native void setMeshTriangleSize(int value);