From 81c8e1b19223faff96ab815724c05efe51460446 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 28 Sep 2017 17:15:53 -0400 Subject: [PATCH] Tango: add Maximum Motion Speed option (default low) to avoid blurry images --- CMakeLists.txt | 2 +- app/android/AndroidManifest.xml.in | 2 +- app/android/jni/CameraTango.cpp | 51 ++++++++------- app/android/jni/RTABMapApp.cpp | 26 ++++++-- app/android/res/layout/activity_settings.xml | 7 +++ app/android/res/values/strings.xml | 19 ++++++ .../com/introlab/rtabmap/RTABMapActivity.java | 62 ++++++++++++++----- .../introlab/rtabmap/SettingsActivity.java | 2 + 8 files changed, 127 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2eaacf9d..a3c5650d 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index b2ccffc4..de74c887 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 4e2e888c..ff976f8f 100644 --- a/app/android/jni/CameraTango.cpp +++ b/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(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(context); - cv::Mat tangoImage; - if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888) + if(app->isRunning()) { - tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data); - } - else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12) - { - 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) - { - tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data); - } - else if(color->format == 35) - { - tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data); - } - else - { - LOGE("Not supported color format : %d.", color->format); - } + cv::Mat tangoImage; + if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888) + { + tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data); + } + else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12) + { + 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) + { + tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data); + } + else if(color->format == 35) + { + tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data); + } + else + { + LOGE("Not supported color format : %d.", color->format); + } - if(!tangoImage.empty()) - { - app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp); + if(!tangoImage.empty()) + { + app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp); + } } } diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 5893483d..c7c7d1f1 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/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::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 poses = rtabmap_->getLocalOptimizedPoses(); @@ -3180,6 +3192,8 @@ bool RTABMapApp::handleEvent(UEvent * event) uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f))); + uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f))); + uInsert(bufferedStatsData_, std::make_pair(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; } } diff --git a/app/android/res/layout/activity_settings.xml b/app/android/res/layout/activity_settings.xml index 97e2993a..9818f496 100644 --- a/app/android/res/layout/activity_settings.xml +++ b/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"/> + "Used Memory (MB): " "Hypothesis (%): " "FPS (rendering): " + "Distance travelled: " "GPS (long,lat,alt,bearing,acc): " "Time: " @@ -77,6 +78,8 @@ pref_key_update_rate 1 + pref_key_max_speed + 0.2 pref_key_time_thr 1000 pref_key_mem_thr @@ -317,6 +320,8 @@ Use fish eye camera instead of the color camera. May not work on some devices. Update Rate Rate at which a new node is added to map. + Maximum Motion Speed + Images taken when the camera is moving too fast are ignored to avoid blurry textures. Time Limit 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. Memory Limit @@ -366,6 +371,20 @@ "1" "0.5" + + "No Limit" + "High" + "Medium" + "Low" + "Very Low" + + + "0" + "0.4" + "0.3" + "0.2" + "0.1" + "No Limit" "1500 ms" diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index c519ff85..7260eedc 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/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); } diff --git a/app/android/src/com/introlab/rtabmap/SettingsActivity.java b/app/android/src/com/introlab/rtabmap/SettingsActivity.java index c15a72c0..f5015f9a 100644 --- a/app/android/src/com/introlab/rtabmap/SettingsActivity.java +++ b/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));