RtabmapThread: changed state stack for a queue to process events in FIFO order (instead of FILO). Tango: Added detector type parameter (FREAK is now the default with 400 features).

app/android/jni/RTABMapApp.cpp

@@ -72,11 +72,9 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 
 	parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
 
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6"))); // GFTT/BRIEF
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
@@ -157,6 +155,7 @@ RTABMapApp::RTABMapApp() :
 		exportedMesh_(new pcl::TextureMesh)
 
 {
+	mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
 }
 
 RTABMapApp::~RTABMapApp() {
@@ -995,7 +994,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if(!camera_->isRunning())
+	if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -1090,7 +1089,7 @@ void RTABMapApp::setMaxCloudDepth(float value)
 
 void RTABMapApp::setMeshDecimation(int value)
 {
-	LOGE("Set decimation to level %d", value);
+	LOGW("Set mesh decimation to level %d", value);
 	meshDecimation_ = 1;
 	if(camera_)
 	{
@@ -1180,7 +1179,13 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
 	if(rtabmap::Parameters::getDefaultParameters().find(compatibleKey) != rtabmap::Parameters::getDefaultParameters().end())
 	{
 		LOGI(uFormat("Setting param \"%s\"  to \"%s\"", compatibleKey.c_str(), value.c_str()).c_str());
-		uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
+		if(compatibleKey.compare(rtabmap::Parameters::kKpDetectorStrategy()) == 0 &&
+		   mappingParameters_.at(rtabmap::Parameters::kKpDetectorStrategy()).compare(value) != 0)
+		{
+			// Changing feature type should reset mapping!
+			resetMapping();
+		}
+		uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value));
 		UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
 		return 0;
 	}
@@ -2040,6 +2045,7 @@ bool RTABMapApp::postExportation(bool visualize)
 		exportedMesh_.reset(new pcl::TextureMesh);
 		exportedTexture_ = cv::Mat();
 		exportedMeshUpdated_ = false;
+		visualizingMesh_ = false;
 	}
 
 	return visualizingMesh_;

app/android/res/layout/activity_rtabmap.xml

@@ -298,4 +298,14 @@
         android:textOff="@string/pause"
         android:textOn="@string/resume" />
     
+    <Button
+        android:id="@+id/close_visualization_button"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignParentBottom="true"
+        android:layout_marginLeft="5dp"
+        android:paddingLeft="5dp"
+        android:text="@string/close_visualization"/>
+        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -108,6 +108,13 @@
    			android:entries="@array/pref_features_keys"
    			android:entryValues="@array/pref_features_values"
    			android:defaultValue="@string/pref_default_features"/>
+        <ListPreference 
+            android:key="@string/pref_key_features_type"
+    		android:title="@string/pref_title_features_type"
+    		android:summary="@string/pref_summary_features_type"
+   			android:entries="@array/pref_features_type_keys"
+   			android:entryValues="@array/pref_features_type_values"
+   			android:defaultValue="@string/pref_default_features_type"/>
     </PreferenceScreen>
     </PreferenceCategory>
     

app/android/res/values/strings.xml

@@ -14,6 +14,7 @@
     <string name="resume">Paused</string>
     <string name="light_on">Lighting</string>
     <string name="light_off">Lighting</string>
+    <string name="close_visualization">Close Visualization</string>
     <string name="start">Start</string>
     <string name="nodes">"Nodes (WM): "</string>
     <string name="points">"Number of points: "</string>
@@ -47,7 +48,8 @@
     <string name="pref_key_time_thr">pref_key_time_thr</string>					<string name="pref_default_time_thr">800</string>
     <string name="pref_key_loop_thr">pref_key_loop_thr</string>					<string name="pref_default_loop_thr">0.11</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>				<string name="pref_default_opt_error">0.1</string>
-    <string name="pref_key_features">pref_key_features</string>					<string name="pref_default_features">200</string>
+    <string name="pref_key_features">pref_key_features</string>					<string name="pref_default_features">400</string>
+    <string name="pref_key_features_type">pref_key_features_type</string>		<string name="pref_default_features_type">5</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>					<string name="pref_default_cloud_voxel">0</string>
     <string name="pref_key_texture_size">pref_key_texture_size</string>					<string name="pref_default_texture_size">4096</string>
@@ -168,6 +170,8 @@
     <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this threshold.</string>
     <string name="pref_title_features">Max Features Extracted</string>
     <string name="pref_summary_features">Extracting more features per image would result in better loop closure detection but more processing time required.</string>
+  	<string name="pref_title_features_type">Feature Type</string>
+    <string name="pref_summary_features_type">BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
     
     <string-array name="pref_update_rate_keys">
     	<item>"Max"</item>
@@ -294,6 +298,15 @@
     	<item>"-1"</item>
 	</string-array>    
 	
+	<string-array name="pref_features_type_keys">
+    	<item>"BRIEF"</item>
+    	<item>"FREAK"</item>
+    </string-array>
+	<string-array name="pref_features_type_values">
+    	<item>"6"</item>
+    	<item>"5"</item>
+	</string-array>   
+	
 	<string name="pref_title_export">Exporting</string>
 	<string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
     

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

@@ -126,6 +126,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	private ToggleButton mButtonTop;
 	private ToggleButton mButtonPause;
 	private ToggleButton mButtonLighting;
+	private Button mButtonCloseVisualization;
 
 	private String mOpenedDatabasePath = "";
 	private String mWorkingDirectory = "";
@@ -184,14 +185,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mButtonTop = (ToggleButton)findViewById(R.id.top_down_button);
 		mButtonPause = (ToggleButton)findViewById(R.id.pause_button);
 		mButtonLighting = (ToggleButton)findViewById(R.id.light_button);
+		mButtonCloseVisualization = (Button)findViewById(R.id.close_visualization_button);
 		mButtonFirst.setOnClickListener(this);
 		mButtonThird.setOnClickListener(this);
 		mButtonTop.setOnClickListener(this);
 		mButtonPause.setOnClickListener(this);
 		mButtonLighting.setOnClickListener(this);
+		mButtonCloseVisualization.setOnClickListener(this);
 		mButtonFirst.setChecked(true);
 		mButtonLighting.setChecked(true);
 		mButtonLighting.setVisibility(View.INVISIBLE);
+		mButtonCloseVisualization.setVisibility(View.INVISIBLE);
 
 		mToast = Toast.makeText(getApplicationContext(), "", Toast.LENGTH_SHORT);
 
@@ -266,6 +270,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		mLoopThr = sharedPref.getString(getString(R.string.pref_key_loop_thr), getString(R.string.pref_default_loop_thr));
 		String optError = sharedPref.getString(getString(R.string.pref_key_opt_error), getString(R.string.pref_default_opt_error));
 		mMaxFeatures = sharedPref.getString(getString(R.string.pref_key_features), getString(R.string.pref_default_features));
+		String featureType = sharedPref.getString(getString(R.string.pref_key_features_type), getString(R.string.pref_default_features_type));
 
 		RTABMapLib.setNodesFiltering(sharedPref.getBoolean(getString(R.string.pref_key_nodes_filtering), Boolean.parseBoolean(getString(R.string.pref_default_nodes_filtering))));
 		RTABMapLib.setDriftCorrection(sharedPref.getBoolean(getString(R.string.pref_key_drift_correction), Boolean.parseBoolean(getString(R.string.pref_default_drift_correction))));
@@ -277,6 +282,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		RTABMapLib.setMappingParameter("Kp/MaxFeatures", mMaxFeatures.compareTo("Disabled")==0?"-1":mMaxFeatures.compareTo("No Limit")==0?"0":mMaxFeatures);
 		RTABMapLib.setMappingParameter("Rtabmap/LoopThr", mLoopThr.compareTo("Disabled")==0?"1":mLoopThr);
 		RTABMapLib.setMappingParameter("RGBD/OptimizeMaxError", optError.compareTo("Disabled")==0?"0":optError);
+		RTABMapLib.setMappingParameter("Kp/DetectorStrategy", featureType);
 
 		RTABMapLib.setMeshDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_decimation), getString(R.string.pref_default_decimation))));
 		RTABMapLib.setMaxCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth), getString(R.string.pref_default_depth))));
@@ -387,6 +393,9 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		case R.id.light_button:
 			RTABMapLib.setLighting(mButtonLighting.isChecked());
 			break;
+		case R.id.close_visualization_button:
+			updateState(State.STATE_IDLE);
+			break;
 		default:
 			return;
 		}
@@ -731,6 +740,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 		{
 		case STATE_PROCESSING:
 			mButtonLighting.setVisibility(View.INVISIBLE);
+			mButtonCloseVisualization.setVisibility(View.INVISIBLE);
 			mItemSave.setEnabled(false);
 			mItemExport.setEnabled(false);
 			mItemOpen.setEnabled(false);
@@ -742,6 +752,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			break;
 		case STATE_VISUALIZING:
 			mButtonLighting.setVisibility(View.VISIBLE);
+			mButtonCloseVisualization.setVisibility(View.VISIBLE);
 			mItemSave.setEnabled(mButtonPause.isChecked());
 			mItemExport.setEnabled(mButtonPause.isChecked() && !mItemDataRecorderMode.isChecked());
 			mItemOpen.setEnabled(mButtonPause.isChecked() && !mItemDataRecorderMode.isChecked());
@@ -754,6 +765,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			break;
 		default:
 			mButtonLighting.setVisibility(View.INVISIBLE);
+			mButtonCloseVisualization.setVisibility(View.INVISIBLE);
 			mItemSave.setEnabled(mButtonPause.isChecked());
 			mItemExport.setEnabled(mButtonPause.isChecked() && !mItemDataRecorderMode.isChecked());
 			mItemOpen.setEnabled(mButtonPause.isChecked() && !mItemDataRecorderMode.isChecked());
@@ -763,6 +775,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemModes.setEnabled(true);
 			mButtonPause.setEnabled(true);
 			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
+			RTABMapLib.postExportation(false);
 			break;
 		}
 	}

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

@@ -39,6 +39,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((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));
         ((Preference)findPreference(getString(R.string.pref_key_opt_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_error))).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
         ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
+        ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         
         ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
@@ -67,6 +68,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
         	if(key.compareTo(getString(R.string.pref_key_opt_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_error));
         	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
+        	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         	
         	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));

corelib/include/rtabmap/core/RtabmapThread.h

@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/OdometryEvent.h"
 
-#include <stack>
+#include <queue>
 
 class UTimer;
 
@@ -106,8 +106,8 @@ private:
 
 private:
 	UMutex _stateMutex;
-	std::stack<State> _state;
+	std::queue<State> _state;
-	std::stack<ParametersMap> _stateParam;
+	std::queue<ParametersMap> _stateParam;
 
 	std::list<OdometryEvent> _dataBuffer;
 	UMutex _dataMutex;

corelib/src/Memory.cpp

@@ -466,7 +466,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	}
 	if(detectorStrategy!=Feature2D::kFeatureUndef)
 	{
-		UDEBUG("new detector strategy %d", int(detectorStrategy));
+		UWARN("new detector strategy %d", int(detectorStrategy));
 		if(_feature2D)
 		{
 			delete _feature2D;

corelib/src/RtabmapThread.cpp

@@ -195,9 +195,9 @@ void RtabmapThread::mainLoop()
 	{
 		if(!_state.empty() && !_stateParam.empty())
 		{
-			state = _state.top();
+			state = _state.front();
 			_state.pop();
-			parameters = _stateParam.top();
+			parameters = _stateParam.front();
 			_stateParam.pop();
 		}
 	}