Tango #57: Increased version to 0.11.3, updated Post-Processing actions, added mesh rendering actions, fixed point cloud rendering

app/android/jni/RTABMapApp.cpp

@@ -50,8 +50,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/obj_io.h>
 
 const int kVersionStringLength = 128;
-const float meshAngleTolerance = 0.1745; // 10 degrees
-const int meshTrianglePixels = 1;
 
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
@@ -86,7 +84,6 @@ RTABMapApp::RTABMapApp() :
 		rtabmapThread_(0),
 		rtabmap_(0),
 		logHandler_(0),
-		mapCloudShown_(true),
 		odomCloudShown_(true),
 		graphOptimization_(true),
 		localizationMode_(false),
@@ -94,6 +91,8 @@ RTABMapApp::RTABMapApp() :
 		autoExposure_(false),
 		fullResolution_(false),
 		maxCloudDepth_(0.0),
+		meshTrianglePix_(1),
+		meshAngleToleranceDeg_(10.0),
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
@@ -193,21 +192,6 @@ void RTABMapApp::openDatabase(const std::string & databasePath)
 			true,
 			true);
 
-	if(poses.size() > 1 &&
-		rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
-	{
-		rtabmap::ParametersMap param;
-		param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
-		rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
-		poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
-		delete sba;
-
-		if(poses.size())
-		{
-			rtabmap_->setOptimizedPoses(poses);
-		}
-	}
-
 	clearSceneOnNextRender_ = true;
 	rtabmap::Statistics stats;
 	stats.setSignatures(signatures);
@@ -421,7 +405,7 @@ int RTABMapApp::Render()
 								filter.setInputCloud(cloud);
 								filter.filter(*output);
 
-								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
+								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 								std::vector<pcl::Vertices> outputPolygons;
 
@@ -464,7 +448,7 @@ int RTABMapApp::Render()
 			iter!=addedClouds.end();
 			++iter)
 		{
-			if(*iter > 0 && (!mapCloudShown_ || poses.find(*iter) == poses.end()))
+			if(*iter > 0 && poses.find(*iter) == poses.end())
 			{
 				main_scene_.setCloudVisible(*iter, false);
 			}
@@ -485,22 +469,24 @@ int RTABMapApp::Render()
 			}
 		}
 
+		main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
+
 		//just process the last one
 		if(set && !event.pose().isNull())
 		{
-			main_scene_.setCloudVisible(-1, false);
 			if(odomCloudShown_ && !trajectoryMode_)
 			{
 				if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
 				{
-					LOGI("Creating Odom cloud (rgb=%dx%d depth=%dx%d)",
-							event.data().imageRaw().cols, event.data().imageRaw().rows,
-							event.data().depthRaw().cols, event.data().depthRaw().rows);
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 					cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
 					if(cloud->size())
 					{
-						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleTolerance, false, meshTrianglePixels);
+						LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
+													event.data().imageRaw().cols, event.data().imageRaw().rows,
+													event.data().depthRaw().cols, event.data().depthRaw().rows,
+													(int)cloud->width, (int)cloud->height);
+						std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
 						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
 						main_scene_.setCloudVisible(-1, true);
 					}
@@ -551,7 +537,7 @@ void RTABMapApp::setPausedMapping(bool paused)
 }
 void RTABMapApp::setMapCloudShown(bool shown)
 {
-	mapCloudShown_ = shown;
+	main_scene_.setMapRendering(shown);
 }
 void RTABMapApp::setOdomCloudShown(bool shown)
 {
@@ -618,6 +604,16 @@ void RTABMapApp::setMaxCloudDepth(float value)
 	maxCloudDepth_ = value;
 }
 
+void RTABMapApp::setMeshAngleTolerance(float value)
+{
+	meshAngleToleranceDeg_ = value;
+}
+
+void RTABMapApp::setMeshTriangleSize(int value)
+{
+	 meshTrianglePix_ = value;
+}
+
 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
 {
 	if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
@@ -763,6 +759,14 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 
 				UINFO("Saving obj to %s.", filePath.c_str());
 				success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
+				if(success)
+				{
+					UINFO("Saved obj to %s!", filePath.c_str());
+				}
+				else
+				{
+					UERROR("Failed saving obj to %s!", filePath.c_str());
+				}
 			}
 		}
 	}
@@ -806,39 +810,64 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 			pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
 			mesh.polygons = mergedPolygons;
 
-			UINFO("Saving to %s.", filePath.c_str());
+			UINFO("Saving ply to %s.", filePath.c_str());
 			success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
+			if(success)
+			{
+				UINFO("Saved ply to %s!", filePath.c_str());
+			}
+			else
+			{
+				UERROR("Failed saving ply to %s!", filePath.c_str());
+			}
 		}
 	}
 	return success;
 }
 
-int RTABMapApp::postProcessing(bool graphOptimizationOnly)
+int RTABMapApp::postProcessing(int approach)
 {
-	int detectedLoopClosures = 0;
+	int returnedValue = 0;
 	if(rtabmap_)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
-		if(graphOptimizationOnly)
+		if(approach == 2 || approach == 0)
 		{
-			rtabmap_->getGraph(poses, links, true, true);
+			if(approach == 2)
+			{
+				// detect more loop closures
+				returnedValue = rtabmap_->detectMoreLoopClosures();
+			}
+
+			if(returnedValue >= 0)
+			{
+				// simple graph optmimization
+				rtabmap_->getGraph(poses, links, true, true);
+			}
 		}
-		else
+		else if (approach == 1)
 		{
-			detectedLoopClosures = rtabmap_->detectMoreLoopClosures();
-
-			std::map<int, rtabmap::Signature> signatures;
-			rtabmap_->getGraph(poses, links, false, true, &signatures);
-
 			if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
 			{
+				std::map<int, rtabmap::Signature> signatures;
+				rtabmap_->getGraph(poses, links, false, true, &signatures);
+
 				rtabmap::ParametersMap param;
-				param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
+				param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
 				rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
 				poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
 				delete sba;
 			}
+			else
+			{
+				LOGE("g2o not available!");
+			}
+		}
+		else
+		{
+			LOGE("Invalid approach %d (should be 0 (graph optimization), 1 (sba) or 2 (detect more loop closures))", approach);
+			returnedValue = -1;
 		}
 
 		if(poses.size())
@@ -851,8 +880,12 @@ int RTABMapApp::postProcessing(bool graphOptimizationOnly)
 
 			rtabmap_->setOptimizedPoses(poses);
 		}
+		else
+		{
+			returnedValue = -1;
+		}
 	}
-	return detectedLoopClosures;
+	return returnedValue;
 }
 
 void RTABMapApp::handleEvent(UEvent * event)

app/android/jni/RTABMapApp.h

@@ -123,12 +123,14 @@ class RTABMapApp : public UEventsHandler {
   void setAutoExposure(bool enabled);
   void setFullResolution(bool enabled);
   void setMaxCloudDepth(float value);
+  void setMeshAngleTolerance(float value);
+  void setMeshTriangleSize(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
   void save();
   bool exportMesh(const std::string & filePath);
-  int postProcessing(bool graphOptimizationOnly);
+  int postProcessing(int approach);
 
  protected:
   virtual void handleEvent(UEvent * event);
@@ -142,7 +144,6 @@ class RTABMapApp : public UEventsHandler {
   rtabmap::Rtabmap * rtabmap_;
   LogHandler * logHandler_;
 
-  bool mapCloudShown_;
   bool odomCloudShown_;
   bool graphOptimization_;
   bool localizationMode_;
@@ -150,6 +151,8 @@ class RTABMapApp : public UEventsHandler {
   bool autoExposure_;
   bool fullResolution_;
   float maxCloudDepth_;
+  int meshTrianglePix_;
+  float meshAngleToleranceDeg_;
 
   rtabmap::ParametersMap mappingParameters_;
 

app/android/jni/jni_interface.cpp

@@ -179,6 +179,18 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
 {
 	return app.setMaxCloudDepth(value);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMeshAngleTolerance(value);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMeshTriangleSize(
+		JNIEnv*, jobject, int value)
+{
+	return app.setMeshTriangleSize(value);
+}
 JNIEXPORT jint JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
     JNIEnv* env, jobject, jstring key, jstring value)
@@ -214,9 +226,9 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
 
 JNIEXPORT int JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
-		JNIEnv* env, jobject, bool graphOptimizationOnly)
+		JNIEnv* env, jobject, int approach)
 {
-	return app.postProcessing(graphOptimizationOnly);
+	return app.postProcessing(approach);
 }
 
 

app/android/jni/point_cloud_drawable.cpp

@@ -46,7 +46,8 @@ PointCloudDrawable::PointCloudDrawable(
 		nPoints_(0),
 		pose_(1.0f),
 		visible_(true),
-		shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
+		cloud_shader_program_(cloudShaderProgram),
+		texture_shader_program_(textureShaderProgram)
 {
 	UASSERT(!cloud->empty());
 
@@ -57,7 +58,7 @@ PointCloudDrawable::PointCloudDrawable(
 		return;
 	}
 
-	if(textureShaderProgram)
+	if(!cloud->is_dense && !image.empty())
 	{
 		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
 		UASSERT(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3);
@@ -74,16 +75,19 @@ PointCloudDrawable::PointCloudDrawable(
 	std::vector<float> vertices;
 	if(textures_)
 	{
-		vertices = std::vector<float>(cloud->size()*5);
+		vertices = std::vector<float>(cloud->size()*6);
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
-			vertices[i*5] = cloud->at(i).x;
-			vertices[i*5+1] = cloud->at(i).y;
-			vertices[i*5+2] = cloud->at(i).z;
+			vertices[i*6] = cloud->at(i).x;
+			vertices[i*6+1] = cloud->at(i).y;
+			vertices[i*6+2] = cloud->at(i).z;
+
+			// rgb
+			vertices[i*6+3] = cloud->at(i).rgb;
 
 			// texture uv
-			vertices[i*5+3] = float(i % cloud->width)/float(cloud->width); //u
-			vertices[i*5+4] = float(i/cloud->width)/float(cloud->height);  //v
+			vertices[i*6+4] = float(i % cloud->width)/float(cloud->width); //u
+			vertices[i*6+5] = float(i/cloud->width)/float(cloud->height);  //v
 		}
 	}
 	else
@@ -180,47 +184,60 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
 
 	if(vertex_buffers_ && nPoints_ && visible_)
 	{
-		glUseProgram(shader_program_);
-
-		GLuint mvp_handle_ = glGetUniformLocation(shader_program_, "mvp");
-		glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
-		glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
-
-		if(textures_)
+		if(meshRendering && textures_)
 		{
+			glUseProgram(texture_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(texture_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
 			// Texture activate unit 0
 			glActiveTexture(GL_TEXTURE0);
 			// Bind the texture to this unit.
 			glBindTexture(GL_TEXTURE_2D, textures_);
 			// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
-			GLuint texture_handle = glGetUniformLocation(shader_program_, "u_Texture");
+			GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
 			glUniform1i(texture_handle, 0);
 
-			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
-			GLint attribute_texture = glGetAttribLocation(shader_program_, "a_TexCoordinate");
+			GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
+			GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
 
 			glEnableVertexAttribArray(attribute_vertex);
 			glEnableVertexAttribArray(attribute_texture);
 			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), 0);
-			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 5*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
+			glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
 
 			glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
 		}
 		else // point cloud or colored mesh
 		{
-			GLuint point_size_handle_ = glGetUniformLocation(shader_program_, "point_size");
+			glUseProgram(cloud_shader_program_);
+
+			GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "mvp");
+			glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
+			glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
+
+			GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
 			glUniform1f(point_size_handle_, pointSize);
 
-			GLint attribute_vertex = glGetAttribLocation(shader_program_, "vertex");
-			GLint attribute_color = glGetAttribLocation(shader_program_, "color");
+			GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
+			GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
 
 			glEnableVertexAttribArray(attribute_vertex);
 			glEnableVertexAttribArray(attribute_color);
 			glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-			glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
-			glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
-
+			if(textures_)
+			{
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 6*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
+			else
+			{
+				glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
+				glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
+			}
 			if(meshRendering && polygons_.size())
 			{
 				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());

app/android/jni/point_cloud_drawable.h

@@ -71,7 +71,8 @@ class PointCloudDrawable {
   glm::mat4 pose_;
   bool visible_;
 
-  GLuint shader_program_;
+  GLuint cloud_shader_program_;
+  GLuint texture_shader_program_;
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

app/android/jni/scene.cpp

@@ -114,6 +114,7 @@ Scene::Scene() :
 		cloud_shader_program_(0),
 		texture_mesh_shader_program_(0),
 		graph_shader_program_(0),
+		mapRendering_(true),
 		meshRendering_(true),
 		pointSize_(3.0f) {}
 
@@ -280,7 +281,7 @@ int Scene::Render() {
 
 	bool frustumCulling = true;
 	int cloudDrawn=0;
-	if(frustumCulling)
+	if(mapRendering_ && frustumCulling)
 	{
 		//Use camera frustum to cull nodes that don't need to be drawn
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -334,8 +335,11 @@ int Scene::Render() {
 	{
 		for(std::map<int, PointCloudDrawable*>::const_iterator iter=pointClouds_.begin(); iter!=pointClouds_.end(); ++iter)
 		{
-			++cloudDrawn;
-			iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			if(mapRendering_ || iter->first < 0)
+			{
+				++cloudDrawn;
+				iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_);
+			}
 		}
 	}
 
@@ -420,8 +424,8 @@ void Scene::addCloud(
 	//create
 	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
 	PointCloudDrawable * drawable = new PointCloudDrawable(
-			cloud->is_dense || image.empty()?cloud_shader_program_:0,
-			cloud->is_dense || image.empty()?0:texture_mesh_shader_program_,
+			cloud_shader_program_,
+			texture_mesh_shader_program_,
 			cloud,
 			polygons,
 			image);

app/android/jni/scene.h

@@ -108,6 +108,7 @@ class Scene {
   bool hasCloud(int id) const;
   std::set<int> getAddedClouds() const;
 
+  void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled) {meshRendering_ = enabled;}
   void setPointSize(float size) {pointSize_ = size;}
 
@@ -139,6 +140,7 @@ class Scene {
   GLuint texture_mesh_shader_program_;
   GLuint graph_shader_program_;
 
+  bool mapRendering_;
   bool meshRendering_;
   float pointSize_;
 };