Tango: added Noise Filtering post-processing option, set RGBD/MaxLocalRetrieved to 0 (avoiding some high processing time peaks), fill depth holes up to 5 pixels

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())
 		{

app/android/jni/RTABMapApp.cpp

@@ -52,6 +52,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/obj_io.h>
 
 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<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
+		{
+			if(iter->second.polygons.size())
+			{
+				// filter polygons
+				std::vector<std::set<int> > neighbors;
+				std::vector<std::set<int> > vertexToPolygons;
+				rtabmap::util3d::createPolygonIndexes(
+						iter->second.polygons,
+						iter->second.cloud->size(),
+						neighbors,
+						vertexToPolygons);
+				std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(
+						neighbors,
+						minPolygonClusterSize);
+				std::vector<pcl::Vertices> filteredPolygons(iter->second.polygons.size());
+				int oi=0;
+				for(std::list<std::list<int> >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter)
+				{
+					for(std::list<int>::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;
 }
 

app/android/jni/RTABMapApp.h

@@ -160,6 +160,7 @@ class RTABMapApp : public UEventsHandler {
 
 
   bool clearSceneOnNextRender_;
+  bool filterPolygonsOnNextRender_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;

app/android/jni/point_cloud_drawable.cpp

@@ -138,21 +138,7 @@ PointCloudDrawable::PointCloudDrawable(
 
 	nPoints_ = cloud->size();
 
-	if(polygons.size())
+	updatePolygons(polygons);
-	{
-		int polygonSize = polygons[0].vertices.size();
-		UASSERT(polygonSize == 3);
-		polygons_.resize(polygons.size() * polygonSize);
-		int oi = 0;
-		for(unsigned int i=0; i<polygons.size(); ++i)
-		{
-			UASSERT((int)polygons[i].vertices.size() == polygonSize);
-			for(int j=0; j<polygonSize; ++j)
-			{
-				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
-			}
-		}
-	}
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -173,6 +159,26 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }
 
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & 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.size(); ++i)
+		{
+			UASSERT((int)polygons[i].vertices.size() == polygonSize);
+			for(int j=0; j<polygonSize; ++j)
+			{
+				polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
+			}
+		}
+	}
+}
+
 void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
 {
 	UASSERT(!pose.isNull());

app/android/jni/point_cloud_drawable.h

@@ -49,6 +49,7 @@ class PointCloudDrawable {
 		  const cv::Mat & image = cv::Mat());
   virtual ~PointCloudDrawable();
 
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   rtabmap::Transform getPose() const {return glmToTransform(pose_);}

app/android/jni/scene.cpp

@@ -471,3 +471,12 @@ std::set<int> Scene::getAddedClouds() const
 {
 	return uKeysSet(pointClouds_);
 }
+
+void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons)
+{
+	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
+	if(iter != pointClouds_.end())
+	{
+		iter->second->updatePolygons(polygons);
+	}
+}

app/android/jni/scene.h

@@ -108,6 +108,7 @@ class Scene {
   void setCloudVisible(int id, bool visible);
   bool hasCloud(int id) const;
   std::set<int> getAddedClouds() const;
+  void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
 
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}

app/android/res/menu/optionmenu.xml

@@ -15,6 +15,7 @@
 	              <item android:id="@+id/detect_more_loop_closures" android:title="Detect More Loop Closures" />
 	              <item android:id="@+id/icp_refining" android:title="ICP Refining" />
 	              <item android:id="@+id/sba" android:title="Bundle Adjustement" />
+	              <item android:id="@+id/polygons_filtering" android:title="Noise Filtering" />
 	            </menu>  
 	         </item>
           </menu>

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");