diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index 9164165d..948c04eb 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -37,8 +37,9 @@ namespace rtabmap { #define nullptr 0 const int kVersionStringLength = 128; -const int holeSize = 1; +const int holeSize = 5; const float maxDepthError = 0.10; +const int scanDownsampling = 10; // Callbacks void onPointCloudAvailableRouter(void* context, const TangoXYZij* xyz_ij) @@ -558,7 +559,6 @@ SensorData CameraTango::captureImage(CameraInfo * info) poseDepth.setNull(); } - int scanDownsampling = 10; cv::Mat scan; if(!poseDepth.isNull() && !poseColor.isNull()) { diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 645399e1..a36732aa 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include const int kVersionStringLength = 128; +const int minPolygonClusterSize = 100; static JavaVM *jvm; static jobject RTABMapActivity = 0; @@ -77,6 +78,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1")); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words) parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true"))); @@ -114,6 +116,7 @@ RTABMapApp::RTABMapApp() : meshTrianglePix_(1), meshAngleToleranceDeg_(15.0), clearSceneOnNextRender_(false), + filterPolygonsOnNextRender_(false), totalPoints_(0), totalPolygons_(0), lastDrawnCloudsCount_(0), @@ -559,6 +562,42 @@ int RTABMapApp::Render() } } + if(filterPolygonsOnNextRender_) + { + filterPolygonsOnNextRender_ = false; + boost::mutex::scoped_lock lock(meshesMutex_); + for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) + { + if(iter->second.polygons.size()) + { + // filter polygons + std::vector > neighbors; + std::vector > vertexToPolygons; + rtabmap::util3d::createPolygonIndexes( + iter->second.polygons, + iter->second.cloud->size(), + neighbors, + vertexToPolygons); + std::list > clusters = rtabmap::util3d::clusterPolygons( + neighbors, + minPolygonClusterSize); + std::vector filteredPolygons(iter->second.polygons.size()); + int oi=0; + for(std::list >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter) + { + for(std::list::iterator kter=jter->begin(); kter!=jter->end(); ++kter) + { + filteredPolygons[oi++] = iter->second.polygons.at(*kter); + } + } + filteredPolygons.resize(oi); + iter->second.polygons = filteredPolygons; + main_scene_.updateCloudPolygons(iter->first, iter->second.polygons); + } + } + notifyDataLoaded = true; + } + UTimer fpsTime; lastDrawnCloudsCount_ = main_scene_.Render(); renderingFPS_ = 1.0/fpsTime.elapsed(); @@ -1004,6 +1043,13 @@ int RTABMapApp::postProcessing(int approach) returnedValue = -1; } } + + // filter polygons + if(approach == 4) + { + filterPolygonsOnNextRender_ = true; + } + return returnedValue; } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index e660627e..6f784f29 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -160,6 +160,7 @@ class RTABMapApp : public UEventsHandler { bool clearSceneOnNextRender_; + bool filterPolygonsOnNextRender_; int totalPoints_; int totalPolygons_; int lastDrawnCloudsCount_; diff --git a/app/android/jni/point_cloud_drawable.cpp b/app/android/jni/point_cloud_drawable.cpp index 7bffebc9..1c261c46 100644 --- a/app/android/jni/point_cloud_drawable.cpp +++ b/app/android/jni/point_cloud_drawable.cpp @@ -138,21 +138,7 @@ PointCloudDrawable::PointCloudDrawable( nPoints_ = cloud->size(); - if(polygons.size()) - { - int polygonSize = polygons[0].vertices.size(); - UASSERT(polygonSize == 3); - polygons_.resize(polygons.size() * polygonSize); - int oi = 0; - for(unsigned int i=0; i & polygons) +{ + polygons_.clear(); + if(polygons.size()) + { + int polygonSize = polygons[0].vertices.size(); + UASSERT(polygonSize == 3); + polygons_.resize(polygons.size() * polygonSize); + int oi = 0; + for(unsigned int i=0; i & polygons); void setPose(const rtabmap::Transform & pose); void setVisible(bool visible) {visible_=visible;} rtabmap::Transform getPose() const {return glmToTransform(pose_);} diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index 501124a0..deecdd47 100644 --- a/app/android/jni/scene.cpp +++ b/app/android/jni/scene.cpp @@ -471,3 +471,12 @@ std::set Scene::getAddedClouds() const { return uKeysSet(pointClouds_); } + +void Scene::updateCloudPolygons(int id, const std::vector & polygons) +{ + std::map::iterator iter=pointClouds_.find(id); + if(iter != pointClouds_.end()) + { + iter->second->updatePolygons(polygons); + } +} diff --git a/app/android/jni/scene.h b/app/android/jni/scene.h index 556c019e..eccee05b 100644 --- a/app/android/jni/scene.h +++ b/app/android/jni/scene.h @@ -108,6 +108,7 @@ class Scene { void setCloudVisible(int id, bool visible); bool hasCloud(int id) const; std::set getAddedClouds() const; + void updateCloudPolygons(int id, const std::vector & polygons); void setMapRendering(bool enabled) {mapRendering_ = enabled;} void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;} diff --git a/app/android/res/menu/optionmenu.xml b/app/android/res/menu/optionmenu.xml index c3cc8b52..438e4ddc 100644 --- a/app/android/res/menu/optionmenu.xml +++ b/app/android/res/menu/optionmenu.xml @@ -15,6 +15,7 @@ + diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index c36d690e..fd8984cf 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -765,6 +765,13 @@ public class RTABMapActivity extends Activity implements OnClickListener { }); workingThread.start(); } + else if (itemId == R.id.polygons_filtering) + { + mProgressDialog.setTitle("Post-Processing"); + mProgressDialog.setMessage(String.format("Noise filtering...")); + mProgressDialog.show(); + RTABMapLib.postProcessing(4); + } else if (itemId == R.id.sba) { mProgressDialog.setTitle("Post-Processing");