Tango: fixed export with optimization, added "Nodes Filtering" option

app/android/AndroidManifest.xml.in

@@ -2,7 +2,7 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="5"
+        android:versionCode="7"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />

app/android/jni/RTABMapApp.cpp

@@ -45,6 +45,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/ParamEvent.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/Memory.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/io/obj_io.h>
@@ -87,6 +89,7 @@ RTABMapApp::RTABMapApp() :
 		logHandler_(0),
 		odomCloudShown_(true),
 		graphOptimization_(true),
+		nodesFiltering_(false),
 		localizationMode_(false),
 		trajectoryMode_(false),
 		autoExposure_(false),
@@ -196,6 +199,9 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 	clearSceneOnNextRender_ = true;
 	rtabmap::Statistics stats;
 	stats.setSignatures(signatures);
+	stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
+	stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
+	stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
 	stats.setPoses(poses);
 	stats.setConstraints(links);
 	rtabmapEvents_.push_back(stats);
@@ -258,6 +264,20 @@ void RTABMapApp::SetViewPort(int width, int height)
 	main_scene_.SetupViewPort(width, height);
 }
 
+class PostRenderEvent : public UEvent
+{
+public:
+	PostRenderEvent(const rtabmap::Statistics & stats) :
+		stats_(stats)
+	{
+
+	}
+	virtual std::string getClassName() const {return "PostRenderEvent";}
+	const rtabmap::Statistics & getStats() const {return stats_;}
+private:
+	rtabmap::Statistics stats_;
+};
+
 // OpenGL thread
 int RTABMapApp::Render()
 {
@@ -357,20 +377,14 @@ int RTABMapApp::Render()
 			}
 		}
 
+		const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
 		if(poses.size())
 		{
-			const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
-
 			//update graph
 			main_scene_.updateGraph(poses, links);
 
 			// update clouds
-
+			boost::mutex::scoped_lock  lock(meshesMutex_);
-			//filter poses?
-
-			// make sure the last pose is here though
-			//poses.insert(*rtabmapEvents.back().poses().rbegin());
-
 			std::set<std::string> strIds;
 			for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -382,6 +396,15 @@ int RTABMapApp::Render()
 						//just update pose
 						main_scene_.setCloudPose(id, iter->second);
 						main_scene_.setCloudVisible(id, true);
+						std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
+						if(meshIter!=createdMeshes_.end())
+						{
+							meshIter->second.pose = iter->second;
+						}
+						else
+						{
+							UERROR("Not found mesh %d !?!?", id);
+						}
 					}
 					else if(uContains(bufferedSensorData, id))
 					{
@@ -423,7 +446,6 @@ int RTABMapApp::Render()
 
 
 									// protect createdMeshes_ used also by exportMesh() method
-									boost::mutex::scoped_lock  lock(meshesMutex_);
 									std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
 									UASSERT(inserted.second);
 									inserted.first->second.cloud = outputCloud;
@@ -443,6 +465,22 @@ int RTABMapApp::Render()
 			}
 		}
 
+		//filter poses?
+		if(poses.size() > 2)
+		{
+			if(nodesFiltering_)
+			{
+				for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+				{
+					if(iter->second.type() != rtabmap::Link::kNeighbor)
+					{
+						int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
+						poses.erase(oldId);
+					}
+				}
+			}
+		}
+
 		//update cloud visibility
 		std::set<int> addedClouds = main_scene_.getAddedClouds();
 		for(std::set<int>::const_iterator iter=addedClouds.begin();
@@ -506,6 +544,13 @@ int RTABMapApp::Render()
 
     lastDrawnCloudsCount_ = main_scene_.Render();
 
+    if(rtabmapEvents.size())
+    {
+    	// send statistics to GUI
+		LOGI("Posting PostRenderEvent!");
+		UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
+    }
+
     return notifyDataLoaded?1:0;
 }
 
@@ -567,6 +612,27 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
+	if(!camera_->isRunning())
+	{
+		std::map<int, rtabmap::Transform> poses;
+		std::multimap<int, rtabmap::Link> links;
+		rtabmap_->getGraph(poses, links, true, true);
+		if(poses.size())
+		{
+			boost::mutex::scoped_lock  lock(rtabmapMutex_);
+			rtabmap::Statistics stats = rtabmap_->getStatistics();
+			stats.setPoses(poses);
+			stats.setConstraints(links);
+			rtabmapEvents_.push_back(stats);
+
+			rtabmap_->setOptimizedPoses(poses);
+		}
+	}
+}
+void RTABMapApp::setNodesFiltering(bool enabled)
+{
+	nodesFiltering_ = enabled;
+	setGraphOptimization(graphOptimization_); // this will resend the graph if paused
 }
 void RTABMapApp::setGraphVisible(bool visible)
 {
@@ -659,6 +725,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	{
 		pcl::TextureMesh textureMesh;
 		std::vector<cv::Mat> textures;
+		int totalPolygons = 0;
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		{
 			boost::mutex::scoped_lock  lock(meshesMutex_);
@@ -716,6 +783,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 						textureMesh.tex_polygons[oi][j] = vertices;
 
 					}
+					totalPolygons += densePolygons.size();
 					polygonsStep += denseCloud->size();
 
 					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
@@ -761,7 +829,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 					textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png";
 				}
 
-				UINFO("Saving obj to %s.", filePath.c_str());
+				UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str());
 				success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
 				if(success)
 				{
@@ -814,7 +882,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
 			mesh.polygons = mergedPolygons;
 
-			UINFO("Saving ply to %s.", filePath.c_str());
+			UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str());
 			success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
 			if(success)
 			{
@@ -878,7 +946,7 @@ int RTABMapApp::postProcessing(int approach)
 		if(poses.size())
 		{
 			boost::mutex::scoped_lock  lock(rtabmapMutex_);
-			rtabmap::Statistics stats;
+			rtabmap::Statistics stats = rtabmap_->getStatistics();
 			stats.setPoses(poses);
 			stats.setConstraints(links);
 			rtabmapEvents_.push_back(stats);
@@ -900,7 +968,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 		// called from events manager thread, so protect the data
 		if(event->getClassName().compare("OdometryEvent") == 0)
 		{
-			LOGI("GUI: Received OdometryEvent!");
+			LOGI("Received OdometryEvent!");
 			if(odomMutex_.try_lock())
 			{
 				odomEvents_.clear();
@@ -914,67 +982,11 @@ void RTABMapApp::handleEvent(UEvent * event)
 		if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
 		   event->getClassName().compare("RtabmapEvent") == 0)
 		{
-			LOGI("GUI: Received RtabmapEvent!");
+			LOGI("Received RtabmapEvent!");
-			int nodes =0;
+			if(camera_->isRunning())
-			int words = 0;
-			int loopClosureId = 0;
-			float updateTime = 0.0f;
-			int databaseMemoryUsed = 0;
-			int inliers = 0;
-			int featuresExtracted = 0;
-			float hypothesis = 0.0f;
 			{
-				boost::mutex::scoped_lock  lock(rtabmapMutex_);
+				boost::mutex::scoped_lock lock(rtabmapMutex_);
-				if(camera_->isRunning())
+				rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
-				{
-					rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
-					nodes = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
-							uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
-					words = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
-					updateTime = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
-					loopClosureId = rtabmapEvents_.back().loopClosureId()>0?rtabmapEvents_.back().loopClosureId():rtabmapEvents_.back().proximityDetectionId()>0?rtabmapEvents_.back().proximityDetectionId():0;
-					databaseMemoryUsed = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
-					inliers = (int)uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
-					featuresExtracted = rtabmapEvents_.back().getSignatures().size()?rtabmapEvents_.back().getSignatures().rbegin()->second.getWords().size():0;
-					hypothesis = uValue(rtabmapEvents_.back().data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
-				}
-			}
-
-			// Call JAVA callback with some stats
-			bool success = false;
-			if(jvm && RTABMapActivity)
-			{
-				JNIEnv *env = 0;
-				jint rs = jvm->AttachCurrentThread(&env, NULL);
-				if(rs == JNI_OK && env)
-				{
-					jclass clazz = env->GetObjectClass(RTABMapActivity);
-					if(clazz)
-					{
-						jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
-						if(methodID)
-						{
-							env->CallVoidMethod(RTABMapActivity, methodID,
-									nodes,
-									words,
-									totalPoints_,
-									totalPolygons_,
-									updateTime,
-									loopClosureId,
-									databaseMemoryUsed,
-									inliers,
-									featuresExtracted,
-									hypothesis,
-									lastDrawnCloudsCount_);
-							success = true;
-						}
-					}
-				}
-				jvm->DetachCurrentThread();
-			}
-			if(!success)
-			{
-				UERROR("Failed to call RTABMapActivity::updateStatsCallback");
 			}
 		}
 	}
@@ -1025,7 +1037,7 @@ void RTABMapApp::handleEvent(UEvent * event)
 
 	if(event->getClassName().compare("RtabmapEventInit") == 0)
 	{
-		LOGI("GUI: Received RtabmapEventInit!");
+		LOGI("Received RtabmapEventInit!");
 		status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
 		status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
 
@@ -1062,5 +1074,58 @@ void RTABMapApp::handleEvent(UEvent * event)
 			UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
 		}
 	}
+
+	if(event->getClassName().compare("PostRenderEvent") == 0)
+	{
+		LOGI("Received PostRenderEvent!");
+		const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getStats();
+		int nodes = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
+				uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
+		int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
+		float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
+		int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
+		int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
+		int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
+		int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
+		float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
+
+		// Call JAVA callback with some stats
+		UINFO("Send statistics to GUI");
+		bool success = false;
+		if(jvm && RTABMapActivity)
+		{
+			JNIEnv *env = 0;
+			jint rs = jvm->AttachCurrentThread(&env, NULL);
+			if(rs == JNI_OK && env)
+			{
+				jclass clazz = env->GetObjectClass(RTABMapActivity);
+				if(clazz)
+				{
+					jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIFI)V" );
+					if(methodID)
+					{
+						env->CallVoidMethod(RTABMapActivity, methodID,
+								nodes,
+								words,
+								totalPoints_,
+								totalPolygons_,
+								updateTime,
+								loopClosureId,
+								databaseMemoryUsed,
+								inliers,
+								featuresExtracted,
+								hypothesis,
+								lastDrawnCloudsCount_);
+						success = true;
+					}
+				}
+			}
+			jvm->DetachCurrentThread();
+		}
+		if(!success)
+		{
+			UERROR("Failed to call RTABMapActivity::updateStatsCallback");
+		}
+	}
 }
 

app/android/jni/RTABMapApp.h

@@ -119,6 +119,7 @@ class RTABMapApp : public UEventsHandler {
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
+  void setNodesFiltering(bool enabled);
   void setGraphVisible(bool visible);
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
@@ -146,6 +147,7 @@ class RTABMapApp : public UEventsHandler {
 
   bool odomCloudShown_;
   bool graphOptimization_;
+  bool nodesFiltering_;
   bool localizationMode_;
   bool trajectoryMode_;
   bool autoExposure_;

app/android/jni/jni_interface.cpp

@@ -156,6 +156,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setGraphOptimization(
 	return app.setGraphOptimization(enabled);
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setNodesFiltering(enabled);
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible(
 		JNIEnv*, jobject, bool visible)
 {

app/android/jni/scene.cpp

@@ -335,7 +335,7 @@ int Scene::Render() {
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			if(mapRendering_ || iter->first < 0)
+			if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
 			{
 				++cloudDrawn;
 				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);

app/android/res/menu/optionmenu.xml

@@ -48,6 +48,7 @@
 	           <item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
 	           <item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
 	           <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
+	           <item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
 	           <item android:id="@+id/update_rate" android:checkable="false" android:title="Map Update Rate..." />
 	           <item android:id="@+id/time_threshold" android:checkable="false" android:title="Time Threshold..." />
 	           <item android:id="@+id/loop_threshold" android:checkable="false" android:title="Loop Closure Threshold..." />

app/android/res/values/strings.xml

@@ -9,7 +9,7 @@
     <string name="third_person">Third</string>
     <string name="top_down">Top</string>
     <string name="start">Start</string>
-    <string name="nodes">"Nodes: "</string>
+    <string name="nodes">"Nodes (WM): "</string>
     <string name="points">"Number of points: "</string>
     <string name="update_time">"Update time (ms): "</string>
     <string name="loop_closure">"Loop closure ID: "</string>

app/android/src/com/introlab/rtabmap/RTABMapActivity.java

@@ -581,7 +581,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 	            @Override
 	            public boolean accept(File dir, String filename) {
 	                File sel = new File(dir, filename);
-	                return filename.endsWith(".db") || sel.isDirectory();
+	                return filename.endsWith(".db");
 	            }
 
 	        };
@@ -735,6 +735,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setGraphOptimization(item.isChecked());
       }
+      else if(itemId == R.id.nodes_filtering)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setNodesFiltering(item.isChecked());
+      }
       else if(itemId == R.id.graph_visible)
       {
     	  item.setChecked(!item.isChecked());
@@ -1001,6 +1006,8 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
     	  final boolean isOBJ = itemId == R.id.export_obj;
     	  
+    	  final int polygons = Integer.parseInt(((TextView)findViewById(R.id.polygons)).getText().toString());
+    	  
     	  AlertDialog.Builder builder = new AlertDialog.Builder(this);
 		  builder.setTitle(String.format("File Name (*%s):", extension));
 		  final EditText input = new EditText(this);
@@ -1027,7 +1034,21 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                    	mItemExport.setEnabled(false);
 			                    			                    	
 			                    	mProgressDialog.setTitle("Exporting");
-			                  	    mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
+			                    	if(polygons > 1000000)
+			                    	{
+			                    		mProgressDialog.setMessage(String.format(
+			                    				"Please wait while exporting \"%s\"...\n"
+			                    				+ "Tip: With more than 1M polygons, to reduce exporting time and file size, consider:\n"
+			                    				+ " a) increasing triangle size (Rendering options)\n"
+			                    				+ " b) decreasing maximum camera depth (Rendering options)\n"
+			                    				+ " c) activate Nodes Filtering (Mapping options)\n"
+			                    				+ "then save/open to refresh the meshes.", fileName+extension));
+			                    	}
+			                    	else
+			                    	{
+			                    		mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
+			                    	}
+			                  	    
 			                  	    mProgressDialog.show();
 			                  	  
 			                    	Thread exportThread = new Thread(new Runnable() {

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -50,6 +50,7 @@ public class RTABMapLib
     public static native void setLocalizationMode(boolean enabled);
     public static native void setTrajectoryMode(boolean enabled);
     public static native void setGraphOptimization(boolean enabled);
+    public static native void setNodesFiltering(boolean enabled);
     public static native void setGraphVisible(boolean visible);
     public static native void setAutoExposure(boolean enabled);
     public static native void setFullResolution(boolean enabled);