Tango: add Maximum Motion Speed option (default low) to avoid blurry images

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 14)
-SET(RTABMAP_PATCH_VERSION 0)
+SET(RTABMAP_PATCH_VERSION 1)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

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

app/android/jni/CameraTango.cpp

@@ -46,7 +46,7 @@ const int scanDownsampling = 1;
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(point_cloud->num_points>0)
+	if(app->isRunning() && point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,31 +55,34 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	cv::Mat tangoImage;
+	if(app->isRunning())
-	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 	{
-		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+		cv::Mat tangoImage;
-	}
+		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+		{
-	{
+			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-	else if(color->format == 35)
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == 35)
-	else
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		LOGE("Not supported color format : %d.", color->format);
+		}
-	}
+		else
+		{
+			LOGE("Not supported color format : %d.", color->format);
+		}
 
-	if(!tangoImage.empty())
+		if(!tangoImage.empty())
-	{
+		{
-		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		}
 	}
 }
 

app/android/jni/RTABMapApp.cpp

@@ -1245,8 +1245,9 @@ int RTABMapApp::Render()
 
 						// Don't create mesh for the last node added if rehearsal happened or if discarded (small movement)
 						int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
+						int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 						int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
-						if(smallMovement == 0 && rehearsalMerged == 0)
+						if(smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0)
 						{
 							for(std::map<int, rtabmap::Signature>::const_iterator jter=stats.getSignatures().begin(); jter!=stats.getSignatures().end(); ++jter)
 							{
@@ -1290,6 +1291,10 @@ int RTABMapApp::Render()
 						{
 							main_scene_.setBackgroundColor(0, 0, 0.2f); // blue
 						}
+						else if(!paused_ && fastMovement)
+						{
+							main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
+						}
 						else
 						{
 							main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
@@ -2020,12 +2025,19 @@ void RTABMapApp::save(const std::string & databasePath)
 		dataRecorderMode_ = false;
 	}
 
-	if(appendModeBackup || dataRecorderModeBackup)
+	bool localizationModeBackup = localizationMode_;
+	if(localizationMode_)
+	{
+		localizationMode_ = false;
+	}
+
+	if(appendModeBackup || dataRecorderModeBackup || localizationModeBackup)
 	{
 		rtabmap::ParametersMap parameters = getRtabmapParameters();
 		rtabmap_->parseParameters(parameters);
 		appendMode_ = appendModeBackup;
 		dataRecorderMode_ = dataRecorderModeBackup;
+		localizationMode_ = localizationModeBackup;
 	}
 
 	std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
@@ -3180,6 +3192,8 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f)));
 			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
+			uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
 		}
 		// else use last data
 
@@ -3195,6 +3209,8 @@ bool RTABMapApp::handleEvent(UEvent * event)
 		float optimizationMaxError = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f);
 		float rehearsalValue = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f);
 		float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
+		float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
+		int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 
 		// Call JAVA callback with some stats
 		UINFO("Send statistics to GUI");
@@ -3208,7 +3224,7 @@ bool RTABMapApp::handleEvent(UEvent * event)
 				jclass clazz = env->GetObjectClass(RTABMapActivity);
 				if(clazz)
 				{
-					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFF)V" );
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFI)V" );
 					if(methodID)
 					{
 						env->CallVoidMethod(RTABMapActivity, methodID,
@@ -3229,7 +3245,9 @@ bool RTABMapApp::handleEvent(UEvent * event)
 								renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
 								rejected,
 								rehearsalValue,
-								optimizationMaxError);
+								optimizationMaxError,
+								distanceTravelled,
+								fastMovement);
 						success = true;
 					}
 				}

app/android/res/layout/activity_settings.xml

@@ -109,6 +109,13 @@
 		            android:entries="@array/pref_update_rate_keys"
 		   			android:entryValues="@array/pref_update_rate_values"
 		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference
+		            android:key="@string/pref_key_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
 		        <ListPreference 
 		            android:key="@string/pref_key_time_thr"
 		    		android:title="@string/pref_title_time_thr"

app/android/res/values/strings.xml

@@ -34,6 +34,7 @@
     <string name="memory">"Used Memory (MB): "</string>
     <string name="hypothesis">"Hypothesis (%): "</string>
     <string name="fps">"FPS (rendering): "</string>
+    <string name="distance">"Distance travelled: "</string>
     <string name="gps">"GPS (long,lat,alt,bearing,acc): "</string>
     <string name="time">"Time: "</string>
     
@@ -77,6 +78,8 @@
     
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0.2</string>
     <string name="pref_key_time_thr">pref_key_time_thr</string>
     <string name="pref_default_time_thr">1000</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
@@ -317,6 +320,8 @@
     <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
     <string name="pref_title_time_thr">Time Limit</string>
     <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
     <string name="pref_title_mem_thr">Memory Limit</string>
@@ -366,6 +371,20 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
     <string-array name="pref_time_thr_keys">
         <item>"No Limit"</item>
         <item>"1500 ms"</item>

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

@@ -181,6 +181,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 	private long mOnPauseStamp = 0;
 	private boolean mOnPause = false;
 	private Date mDateOnPause = new Date();
+	private long mLastFastMovementNotificationStamp = 0;
 	private boolean mBlockBack = true;
 
 	private MenuItem mItemSave;
@@ -435,6 +436,10 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		    @Override
 		    public void onClick(DialogInterface dialog, int which) {
 		    	RTABMapLib.cancelProcessing();
+		    	
+		    	mProgressDialog.setTitle("");
+			mProgressDialog.setMessage(String.format("Cancelling..."));
+			mProgressDialog.show();
 		    }
 		});
 
@@ -449,6 +454,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		mWorkingDirectory = "";
 		mWorkingDirectoryHuman = "";
 		mTotalLoopClosures = 0;
+		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
 
 		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0)
 		{
@@ -639,23 +645,22 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		if(!DISABLE_LOG) Log.i(TAG, "onPause()");
 		mOnPause = true;
 		
-		mLocationManager.removeUpdates(mLocationListener);
-		mSensorManager.unregisterListener(this);
-
-		
-		// This deletes OpenGL context!
-		mGLView.onPause();
-
-		RTABMapLib.onPause();
-
-		unbindService(mTangoServiceConnection);
-
 		if(!mButtonPause.isChecked())
 		{
 			mButtonPause.setChecked(true);
 			pauseMapping();
 		}
 		
+		mLocationManager.removeUpdates(mLocationListener);
+		mSensorManager.unregisterListener(this);
+
+		RTABMapLib.onPause();
+		
+		unbindService(mTangoServiceConnection);
+		
+		// This deletes OpenGL context!
+		mGLView.onPause();
+		
 		mOnPauseStamp = System.currentTimeMillis()/1000;
 	}
 
@@ -690,6 +695,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			if(!DISABLE_LOG) Log.d(TAG, "update preferences...");
 			SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
 			mUpdateRate = sharedPref.getString(getString(R.string.pref_key_update_rate), getString(R.string.pref_default_update_rate));
+			String maxSpeed = sharedPref.getString(getString(R.string.pref_key_max_speed), getString(R.string.pref_default_max_speed));
 			mTimeThr = sharedPref.getString(getString(R.string.pref_key_time_thr), getString(R.string.pref_default_time_thr));
 			String memThr = sharedPref.getString(getString(R.string.pref_key_mem_thr), getString(R.string.pref_default_mem_thr));
 			mLoopThr = sharedPref.getString(getString(R.string.pref_key_loop_thr), getString(R.string.pref_default_loop_thr));
@@ -720,6 +726,8 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRate);
 			RTABMapLib.setMappingParameter("Rtabmap/TimeThr", mTimeThr);
 			RTABMapLib.setMappingParameter("Rtabmap/MemoryThr", memThr);
+			RTABMapLib.setMappingParameter("RGBD/LinearSpeedUpdate", maxSpeed);
+			RTABMapLib.setMappingParameter("RGBD/AngularSpeedUpdate", String.valueOf(Float.parseFloat(maxSpeed)/2.0f));
 			RTABMapLib.setMappingParameter("Mem/RehearsalSimilarity", simThr);
 			RTABMapLib.setMappingParameter("Kp/MaxFeatures", mMaxFeatures);
 			RTABMapLib.setMappingParameter("Vis/MaxFeatures", maxFeaturesLoop);
@@ -1009,6 +1017,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			int matches,
 			int rejected,
 			float optimizationMaxError,
+			boolean fastMovement,
 			String[] statusTexts)
 	{
 		mStatusTexts = statusTexts;
@@ -1083,6 +1092,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		
 		if(mButtonPause!=null && !mButtonPause.isChecked())
 		{
+			long currentTime = System.currentTimeMillis()/1000;
 			if(loopClosureId > 0)
 			{
 				mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
@@ -1107,6 +1117,18 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				}
 				mToast.show();
 			}
+			else if(fastMovement)
+			{
+				if(currentTime - mLastFastMovementNotificationStamp > 2)
+				{
+					mToast.setText("Move slower... blurry images are not added to map (\"Settings->Mapping...->Maximum Motion Speed\" is enabled).");
+					mToast.show();
+				}
+			}
+			else
+			{
+				mLastFastMovementNotificationStamp = currentTime;
+			}
 		}
 	}
 
@@ -1129,11 +1151,13 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			final float fps,
 			final int rejected,
 			final float rehearsalValue,
-			final float optimizationMaxError)
+			final float optimizationMaxError,
+			final float distanceTravelled,
+			final int fastMovement)
 	{
 		if(!DISABLE_LOG) Log.i(TAG, String.format("updateStatsCallback()"));
 
-		final String[] statusTexts = new String[17];
+		final String[] statusTexts = new String[18];
 		if(mButtonPause!=null && !mButtonPause.isChecked())
 		{
 			String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate;
@@ -1212,10 +1236,11 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 		statusTexts[index++] = getString(R.string.inliers)+inliers;
 		statusTexts[index++] = getString(R.string.hypothesis)+(int)(hypothesis*100.0f) +" / " + (int)(Float.parseFloat(mLoopThr)*100.0f) + " (" + (loopClosureId>0?loopClosureId:highestHypId)+")";
 		statusTexts[index++] = getString(R.string.fps)+(int)fps+" Hz";
+		statusTexts[index++] = getString(R.string.distance)+(int)distanceTravelled+" m";
 			
 		runOnUiThread(new Runnable() {
 				public void run() {
-					updateStatsUI(adjustedMemoryUsed, loopClosureId, inliers, matches, rejected, optimizationMaxError, statusTexts);
+					updateStatsUI(adjustedMemoryUsed, loopClosureId, inliers, matches, rejected, optimizationMaxError, fastMovement!=0, statusTexts);
 				} 
 		});
 	}
@@ -1428,6 +1453,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						}
 						else
 						{
+							mProgressDialog.dismiss();
 							mToast.makeText(getActivity(), String.format("Optimization canceled"), mToast.LENGTH_LONG).show();
 						}
 						updateState(State.STATE_IDLE);
@@ -1607,6 +1633,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 				mMemoryWarningDialog=null;
 			}
 			RTABMapLib.setPausedMapping(false);
+			mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
 			
 			if(mItemDataRecorderMode.isChecked())
 			{
@@ -1616,6 +1643,12 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 			{
 				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(mMapIsEmpty && mItemLocalizationMode!=null && mItemLocalizationMode.isChecked())
+			{
+				mItemLocalizationMode.setChecked(false);
+				RTABMapLib.setLocalizationMode(false);
+				mToast.makeText(getActivity(), String.format("Disabled localization mode as the map is empty, now mapping..."), mToast.LENGTH_LONG).show();
+			}
 		}
 	}
 
@@ -2376,6 +2409,7 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem
 						}
 						else
 						{
+							mProgressDialog.dismiss();
 							mToast.makeText(getActivity(), String.format("Export canceled"), mToast.LENGTH_LONG).show();
 							updateState(previousState);
 						}

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

@@ -195,6 +195,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -253,6 +254,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));