diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 6ec18349..d35d7b09 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -125,7 +125,7 @@ RTABMapApp::RTABMapApp() : paused_(false), clearSceneOnNextRender_(false), filterPolygonsOnNextRender_(false), - gainCompensationOnNextRender_(false), + gainCompensationOnNextRender_(0), cameraJustInitialized_(false), totalPoints_(0), totalPolygons_(0), @@ -600,10 +600,9 @@ int RTABMapApp::Render() } } - if(notifyDataLoaded || gainCompensationOnNextRender_) + if(notifyDataLoaded || gainCompensationOnNextRender_>0) { LOGI("Gain compensation..."); - gainCompensationOnNextRender_ = false; boost::mutex::scoped_lock lock(meshesMutex_); if(createdMeshes_.size() > 1) { @@ -616,6 +615,23 @@ int RTABMapApp::Render() std::map poses; std::multimap links; rtabmap_->getGraph(poses, links, false, true); + if(gainCompensationOnNextRender_ == 2) + { + // full compensation + links.clear(); + for(std::map::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) + { + int from = iter->first; + std::map::Ptr>::const_iterator jter = iter; + ++jter; + for(;jter!=clouds.end(); ++jter) + { + int to = jter->first; + links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to)))); + } + } + } + compensator.feed(clouds, links); for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { @@ -633,6 +649,7 @@ int RTABMapApp::Render() iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg"); } } + gainCompensationOnNextRender_ = 0; notifyDataLoaded = true; } @@ -1082,7 +1099,7 @@ int RTABMapApp::postProcessing(int approach) LOGE("g2o not available!"); } } - else if(approach!=4 || approach!=5) + else if(approach!=4 && approach!=5) { // simple graph optmimization rtabmap_->getGraph(poses, links, true, true); @@ -1099,7 +1116,7 @@ int RTABMapApp::postProcessing(int approach) rtabmap_->setOptimizedPoses(poses); } - else if(approach!=4 || approach!=5) + else if(approach!=4 && approach!=5) { returnedValue = -1; } @@ -1114,10 +1131,10 @@ int RTABMapApp::postProcessing(int approach) } // gain compensation - if(approach == -1 || approach == 5) + if(approach == -1 || approach == 5 || approach == 6) { boost::mutex::scoped_lock lock(renderingMutex_); - gainCompensationOnNextRender_ = true; + gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast } } } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 84a80696..54e91478 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -161,7 +161,7 @@ class RTABMapApp : public UEventsHandler { bool paused_; bool clearSceneOnNextRender_; bool filterPolygonsOnNextRender_; - bool gainCompensationOnNextRender_; + int gainCompensationOnNextRender_; bool cameraJustInitialized_; int totalPoints_; int totalPolygons_; diff --git a/app/android/res/menu/optionmenu.xml b/app/android/res/menu/optionmenu.xml index 93f70d16..f7aaae41 100644 --- a/app/android/res/menu/optionmenu.xml +++ b/app/android/res/menu/optionmenu.xml @@ -10,11 +10,12 @@ - + - + + diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 9066d324..ae82bf30 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -130,7 +130,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setTitle(R.string.menu_name); - + // Query screen size, the screen size is used for computing the normalized // touch point. Display display = getWindowManager().getDefaultDisplay(); @@ -791,13 +791,20 @@ public class RTABMapActivity extends Activity implements OnClickListener { mProgressDialog.show(); RTABMapLib.postProcessing(4); } - else if (itemId == R.id.gain_compensation) + else if (itemId == R.id.gain_compensation_fast) { mProgressDialog.setTitle("Post-Processing"); - mProgressDialog.setMessage(String.format("Gain compensation...")); + mProgressDialog.setMessage(String.format("Fast gain compensation...")); mProgressDialog.show(); RTABMapLib.postProcessing(5); } + else if (itemId == R.id.gain_compensation_full) + { + mProgressDialog.setTitle("Post-Processing"); + mProgressDialog.setMessage(String.format("Full gain compensation...")); + mProgressDialog.show(); + RTABMapLib.postProcessing(6); + } else if (itemId == R.id.sba) { mProgressDialog.setTitle("Post-Processing");