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).

2026-09-02 01:20:25 +08:00 · 2017-01-27 11:47:57 -05:00
parent b4a0fe54cd
commit 79a211ad02
9 changed files with 65 additions and 14 deletions
--- a/app/android/jni/RTABMapApp.cpp
+++ b/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_;
--- a/app/android/res/layout/activity_rtabmap.xml
+++ b/app/android/res/layout/activity_rtabmap.xml
@@ -297,5 +297,15 @@
        android:paddingRight="5dp"
        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>
--- a/app/android/res/layout/activity_settings.xml
+++ b/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>
    
--- a/app/android/res/values/strings.xml
+++ b/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>
    
--- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+++ b/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());
@@ -762,7 +774,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			mItemReset.setEnabled(true);
 			mItemModes.setEnabled(true);
 			mButtonPause.setEnabled(true);
-			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());		  
+			mItemDataRecorderMode.setEnabled(mButtonPause.isChecked());
+			RTABMapLib.postExportation(false);
 			break;
 		}
 	}
--- a/app/android/src/com/introlab/rtabmap/SettingsActivity.java
+++ b/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,7 +68,8 @@ 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));
        	if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k));