Tango #57: Added 720p option, Export PLY or OBJ, Added Rendering and Mapping menus, RtabmapThread: Fixed large covariance (9999) detection

app/android/jni/CameraTango.cpp

@@ -99,8 +99,12 @@ static rtabmap::Transform opticalRotation(
 CameraTango::CameraTango(int decimation, bool autoExposure) :
 		Camera(0, opticalRotation),
 		tango_config_(0),
+		firstFrame_(true),
 		decimation_(decimation),
-		autoExposure_(autoExposure)
+		autoExposure_(autoExposure),
+		cloudStamp_(0),
+		tangoColorType_(0),
+		tangoColorStamp_(0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -318,6 +322,7 @@ void CameraTango::close()
 		tango_config_ = nullptr;
 		TangoService_disconnect();
 	}
+	firstFrame_ = true;
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -624,7 +629,9 @@ void CameraTango::mainLoop()
 		{
 			rtabmap::Transform pose = data.groundTruth();
 			data.setGroundTruth(Transform());
-			this->post(new OdometryEvent(data, pose, 0.0001, 0.0001));
+			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.0001);
+			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.0001, firstFrame_?9999:0.0001));
+			firstFrame_ = false;
 		}
 		else if(!this->isKilled())
 		{

app/android/jni/CameraTango.h

@@ -77,6 +77,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose, bool inOpenGLFrame) const;
+	void setDecimation(int value) {decimation_ = value;}
 	void setAutoExposure(bool enabled) {autoExposure_ = enabled;}
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
@@ -95,6 +96,7 @@ private:
 
 private:
 	void * tango_config_;
+	bool firstFrame_;
 	int decimation_;
 	bool autoExposure_;
 	cv::Mat cloud_;

app/android/jni/RTABMapApp.cpp

@@ -49,18 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/obj_io.h>
 
 const int kVersionStringLength = 128;
-const int cameraTangoDecimation = 2;
-const int renderingCloudDecimation = 8;
-const float renderingCloudMaxDepth = 4.0f;
-const int maxFeatures = 400;
 const float meshAngleTolerance = 0.1745; // 10 degrees
 const int meshTrianglePixels = 1;
-const bool substractFiltering = false;
-const float subtractRadius = 0.02;
-const float subtractMaxAngle = M_PI/4.0f;
-const bool textureMeshing = true;
-const int minNeighborsInRadius = 5;
-const float closeVerticesDistance = 0.02f;
 
 static JavaVM *jvm;
 static jobject RTABMapActivity = 0;
@@ -69,34 +59,22 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 {
 	rtabmap::ParametersMap parameters;
 
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapLoopThr(), "0.11"));
+	parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false")));
+
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6"))); // GFTT/BRIEF
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTMinDistance(), std::string("10")));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTMinDistance(), std::string(fullResolution_?"15":"5")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImageKept(), "false"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), "true")); // for visual registration
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), std::string("1"))); // Kd-tree
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNndrRatio(), std::string("0.8"))); // set the one for kd-tree
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), !loopClosureDetection_?std::string("-1"):uNumber2Str(maxFeatures))); // Kd-tree
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), uNumber2Str(graphOptimization_?rtabmap::Parameters::defaultOptimizerIterations():0)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
 	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::kMemImageDecimation(), std::string("2")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerRobust(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerVarianceIgnored(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("700")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisRefineIterations(), std::string("5")));
 
 	return parameters;
 }
@@ -107,16 +85,18 @@ RTABMapApp::RTABMapApp() :
 		logHandler_(0),
 		mapCloudShown_(true),
 		odomCloudShown_(true),
-		loopClosureDetection_(true),
 		graphOptimization_(true),
 		localizationMode_(false),
 		trajectoryMode_(false),
 		autoExposure_(false),
+		fullResolution_(false),
+		maxCloudDepth_(0.0),
 		clearSceneOnNextRender_(false),
 		totalPoints_(0),
 		totalPolygons_(0),
 		lastDrawnCloudsCount_(0)
 {
+
 }
 
 RTABMapApp::~RTABMapApp() {
@@ -143,9 +123,6 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 
 	LOGI("RTABMapApp::TangoInitialize()");
 	createdMeshes_.clear();
-	previousCloud_.first = 0;
-	previousCloud_.second.first.reset();
-	previousCloud_.second.second.reset();
 	rawPoses_.clear();
 	clearSceneOnNextRender_ = true;
 	totalPoints_ = 0;
@@ -172,7 +149,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity)
 
 	this->registerToEventsManager();
 
-	camera_ = new rtabmap::CameraTango(cameraTangoDecimation, autoExposure_);
+	camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
 
 
   // The first thing we need to do for any Tango enabled application is to
@@ -300,9 +277,6 @@ int RTABMapApp::Render()
 		main_scene_.clear();
 		clearSceneOnNextRender_ = false;
 		createdMeshes_.clear();
-		previousCloud_.first = 0;
-		previousCloud_.second.first.reset();
-		previousCloud_.second.second.reset();
 		rawPoses_.clear();
 		totalPoints_ = 0;
 		totalPolygons_ = 0;
@@ -410,85 +384,37 @@ int RTABMapApp::Render()
 						if(!data.imageRaw().empty() && !data.depthRaw().empty())
 						{
 							// Voxelize and filter depending on the previous cloud?
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
 							LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
-							cloudWithoutNormals = rtabmap::util3d::cloudRGBFromSensorData(data, renderingCloudDecimation, renderingCloudMaxDepth, 0, 0, indices.get());
+							cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, 0, indices.get());
-
-							//compute normals
-							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::computeNormals(cloudWithoutNormals, 6);
 
 							if(cloud->size() && indices->size())
 							{
 								UTimer time;
 
-								// substract? set points to NaN which are over previous cloud
-								pcl::IndicesPtr indicesKept = indices;
-								if(substractFiltering &&
-									subtractRadius > 0.0 &&
-									indices->size() &&
-									previousCloud_.first > 0 &&
-									previousCloud_.second.first.get() != 0 &&
-									previousCloud_.second.second.get() != 0 &&
-									previousCloud_.second.second->size() &&
-									poses.find(previousCloud_.first) != poses.end())
-								{
-									rtabmap::Transform t = iter->second.inverse() * poses.at(previousCloud_.first);
-									pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(previousCloud_.second.first, t);
-									indicesKept = rtabmap::util3d::subtractFiltering(
-											cloud,
-											indices,
-											previousCloud,
-											previousCloud_.second.second,
-											subtractRadius,
-											subtractMaxAngle,
-											minNeighborsInRadius);
-									UINFO("Subtraction %fs", time.ticks());
-								}
-
-								previousCloud_.first = id;
-								previousCloud_.second.first = cloud;
-								previousCloud_.second.second = indices;
-
 								// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-								pcl::ExtractIndices<pcl::PointXYZRGBNormal> filter;
+								pcl::ExtractIndices<pcl::PointXYZRGB> filter;
-								filter.setIndices(indicesKept);
+								filter.setIndices(indices);
 								filter.setKeepOrganized(true);
 								filter.setInputCloud(cloud);
 								filter.filter(*output);
-								LOGE("Filtering %d from %d -> %d (%fs)", (int)indices->size(), (int)indices->size(), (int)indicesKept->size(), time.ticks());
 
 								std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleTolerance, false, meshTrianglePixels);
-								pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+								pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 								std::vector<pcl::Vertices> outputPolygons;
 
-								if(!textureMeshing)
-								{
-									rtabmap::util3d::filterNotUsedVerticesFromMesh(
-											*output,
-											polygons,
-											*outputCloud,
-											outputPolygons);
-								}
-								else
-								{
 								outputCloud = output;
 								outputPolygons = polygons;
-								}
+
 								LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
 
-								if(outputCloud->size() && (!textureMeshing || outputPolygons.size()))
+								if(outputCloud->size() && outputPolygons.size())
 								{
 									totalPolygons_ += outputPolygons.size();
-									if(textureMeshing)
+
-									{
 									main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
-									}
-									else
-									{
-										main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second);
-									}
 
 
 									// protect createdMeshes_ used also by exportMesh() method
@@ -498,11 +424,8 @@ int RTABMapApp::Render()
 									inserted.first->second.cloud = outputCloud;
 									inserted.first->second.polygons = outputPolygons;
 									inserted.first->second.pose = iter->second;
-									if(textureMeshing)
-									{
 									inserted.first->second.texture = data.imageCompressed();
 								}
-								}
 								else
 								{
 									LOGE("Not mesh could be created for node %d", id);
@@ -554,11 +477,11 @@ int RTABMapApp::Render()
 							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(), renderingCloudDecimation, renderingCloudMaxDepth);
+					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);
-						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), textureMeshing?event.data().imageRaw():cv::Mat());
+						main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
 						main_scene_.setCloudVisible(-1, true);
 					}
 					else
@@ -655,7 +578,39 @@ void RTABMapApp::setAutoExposure(bool enabled)
 			camera_->setAutoExposure(autoExposure_);
 			onResume();
 		}
-		resetMapping();
+	}
+}
+
+void RTABMapApp::setFullResolution(bool enabled)
+{
+	if(fullResolution_ != enabled)
+	{
+		fullResolution_ = enabled;
+		if(camera_)
+		{
+			camera_->setDecimation(fullResolution_?1:2);
+		}
+	}
+}
+
+void RTABMapApp::setMaxCloudDepth(float value)
+{
+	maxCloudDepth_ = value;
+}
+
+int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
+{
+	if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
+	{
+		LOGI(uFormat("Setting param \"%s\"  to \"\"", key.c_str(), value.c_str()).c_str());
+		uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
+		UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
+		return 0;
+	}
+	else
+	{
+		LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str());
+		return -1;
 	}
 }
 
@@ -680,7 +635,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	bool success = false;
 
 	//Assemble the meshes
-	if(textureMeshing)
+	if(UFile::getExtension(filePath).compare("obj") == 0)
 	{
 		pcl::TextureMesh textureMesh;
 		std::vector<cv::Mat> textures;
@@ -705,12 +660,16 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 					iter->second.cloud->size() &&
 					iter->second.polygons.size())
 				{
+					// OBJ format requires normals
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
+					cloudWithNormals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
+
 					// create dense cloud
 					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 					std::vector<pcl::Vertices> densePolygons;
 					std::map<int, int> newToOldIndices;
 					newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
-							*iter->second.cloud,
+							*cloudWithNormals,
 							iter->second.polygons,
 							*denseCloud,
 							densePolygons);
@@ -789,7 +748,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 	}
 	else
 	{
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
 		std::vector<pcl::Vertices> mergedPolygons;
 
 		{
@@ -799,23 +758,16 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				iter!= createdMeshes_.end();
 				++iter)
 			{
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 				std::vector<pcl::Vertices> densePolygons;
-				if(iter->second.cloud->is_dense)
+
-				{
-					denseCloud = iter->second.cloud;
-					densePolygons = iter->second.polygons;
-				}
-				else
-				{
 				rtabmap::util3d::filterNotUsedVerticesFromMesh(
 						*iter->second.cloud,
 						iter->second.polygons,
 						*denseCloud,
 						densePolygons);
-				}
 
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
 				if(mergedClouds->size() == 0)
 				{
 					*mergedClouds = *transformedCloud;
@@ -827,30 +779,6 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
 				}
 			}
 		}
-		if(closeVerticesDistance)
-		{
-			UINFO("Filtering assembled mesh (points=%d, polygons=%d, close vertices=%fm)...",
-					(int)mergedClouds->size(), (int)mergedPolygons.size(), closeVerticesDistance);
-
-			mergedPolygons = rtabmap::util3d::filterCloseVerticesFromMesh(
-					mergedClouds,
-					mergedPolygons,
-					closeVerticesDistance,
-					M_PI/4,
-					true);
-
-			// filter invalid polygons
-			unsigned int count = mergedPolygons.size();
-			mergedPolygons = rtabmap::util3d::filterInvalidPolygons(mergedPolygons);
-			UINFO("Filtered %d invalid polygons.", (int)count-mergedPolygons.size());
-
-			// filter not used vertices
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-			std::vector<pcl::Vertices> filteredPolygons;
-			rtabmap::util3d::filterNotUsedVerticesFromMesh(*mergedClouds, mergedPolygons, *filteredCloud, filteredPolygons);
-			mergedClouds = filteredCloud;
-			mergedPolygons = filteredPolygons;
-		}
 
 		if(mergedClouds->size() && mergedPolygons.size())
 		{

app/android/jni/RTABMapApp.h

@@ -121,6 +121,9 @@ class RTABMapApp : public UEventsHandler {
   void setGraphOptimization(bool enabled);
   void setGraphVisible(bool visible);
   void setAutoExposure(bool enabled);
+  void setFullResolution(bool enabled);
+  void setMaxCloudDepth(float value);
+  int setMappingParameter(const std::string & key, const std::string & value);
 
   void resetMapping();
   void save();
@@ -139,11 +142,15 @@ class RTABMapApp : public UEventsHandler {
 
   bool mapCloudShown_;
   bool odomCloudShown_;
-  bool loopClosureDetection_;
   bool graphOptimization_;
   bool localizationMode_;
   bool trajectoryMode_;
   bool autoExposure_;
+  bool fullResolution_;
+  float maxCloudDepth_;
+
+  rtabmap::ParametersMap mappingParameters_;
+
 
   bool clearSceneOnNextRender_;
   int totalPoints_;
@@ -165,7 +172,7 @@ class RTABMapApp : public UEventsHandler {
 
 	struct Mesh
 	{
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 		std::vector<pcl::Vertices> polygons;
 		rtabmap::Transform pose;
 		cv::Mat texture;
@@ -173,7 +180,6 @@ class RTABMapApp : public UEventsHandler {
 
 	std::map<int, Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
-	std::pair<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > previousCloud_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
 };

app/android/jni/jni_interface.cpp

@@ -167,6 +167,27 @@ Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure(
 {
 	return app.setAutoExposure(enabled);
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
+		JNIEnv*, jobject, bool enabled)
+{
+	return app.setFullResolution(enabled);
+}
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
+		JNIEnv*, jobject, float value)
+{
+	return app.setMaxCloudDepth(value);
+}
+JNIEXPORT jint JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setMappingParameter(
+    JNIEnv* env, jobject, jstring key, jstring value)
+{
+	std::string keyC, valueC;
+	GetJStringContent(env,key,keyC);
+	GetJStringContent(env,value,valueC);
+	return app.setMappingParameter(keyC, valueC);
+}
 
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_resetMapping(

app/android/jni/point_cloud_drawable.cpp

@@ -35,124 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <GLES2/gl2.h>
 
-PointCloudDrawable::PointCloudDrawable(
-		GLuint cloudShaderProgram,
-		GLuint textureShaderProgram,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
-		const cv::Mat & image) :
-		vertex_buffers_(0),
-		textures_(0),
-		nPoints_(0),
-		pose_(1.0f),
-		visible_(true),
-		shader_program_(cloudShaderProgram!=0?cloudShaderProgram:textureShaderProgram)
-{
-	UASSERT(!cloud->empty());
-
-	glGenBuffers(1, &vertex_buffers_);
-	if(!vertex_buffers_)
-	{
-		LOGE("OpenGL: could not generate vertex buffers\n");
-		return;
-	}
-
-	if(textureShaderProgram)
-	{
-		LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows);
-		UASSERT_MSG(polygons.size() && !cloud->is_dense && !image.empty() && image.type() == CV_8UC3,
-				uFormat("polygons=%d dense=%d image=%d", (int)polygons.size(), cloud->is_dense?1:0, image.type()).c_str());
-		glGenTextures(1, &textures_);
-		if(!textures_)
-		{
-			vertex_buffers_ = 0;
-			LOGE("OpenGL: could not generate vertex buffers\n");
-			return;
-		}
-	}
-
-	LOGI("Creating cloud buffer %d", vertex_buffers_);
-	std::vector<float> vertices;
-	if(textures_)
-	{
-		vertices = std::vector<float>(cloud->size()*5);
-		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;
-
-			// 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
-		}
-	}
-	else
-	{
-		vertices = std::vector<float>(cloud->size()*4);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			vertices[i*4] = cloud->at(i).x;
-			vertices[i*4+1] = cloud->at(i).y;
-			vertices[i*4+2] = cloud->at(i).z;
-			vertices[i*4+3] = cloud->at(i).rgb;
-		}
-	}
-
-	glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
-	glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
-	glBindBuffer(GL_ARRAY_BUFFER, 0);
-
-	GLint error = glGetError();
-	if(error != GL_NO_ERROR)
-	{
-		LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
-		vertex_buffers_ = 0;
-		return;
-	}
-
-	if(textures_)
-	{
-		// gen texture from image
-		glBindTexture(GL_TEXTURE_2D, textures_);
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-		cv::Mat rgbImage;
-		cv::cvtColor(image, rgbImage, CV_BGR2RGB);
-		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, (const void *)rgbImage.data);
-		glBindTexture(GL_TEXTURE_2D, 0);
-
-		GLint error = glGetError();
-		if(error != GL_NO_ERROR)
-		{
-			LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
-			textures_ = 0;
-
-			glDeleteBuffers(1, &vertex_buffers_);
-			vertex_buffers_ = 0;
-			return;
-		}
-	}
-
-	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.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(
 		GLuint cloudShaderProgram,
 		GLuint textureShaderProgram,

app/android/jni/point_cloud_drawable.h

@@ -41,12 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // PointCloudDrawable is responsible for the point cloud rendering.
 class PointCloudDrawable {
  public:
-  PointCloudDrawable(
-		  GLuint cloudShaderProgram,
-		  GLuint textureShaderProgram,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(),
-		  const cv::Mat & image = cv::Mat());
   PointCloudDrawable(
 		  GLuint cloudShaderProgram,
 		  GLuint textureShaderProgram,

app/android/jni/scene.cpp

@@ -402,32 +402,6 @@ void Scene::setTraceVisible(bool visible)
 }
 
 //Should only be called in OpenGL thread!
-void Scene::addCloud(
-		int id,
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
-		const rtabmap::Transform & pose,
-		const cv::Mat & image)
-{
-	LOGI("addOrUpdateCloud cloud %d", id);
-	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
-	if(iter != pointClouds_.end())
-	{
-		delete iter->second;
-		pointClouds_.erase(iter);
-	}
-
-	//create
-	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
-	PointCloudDrawable * drawable = new PointCloudDrawable(
-			image.empty()?cloud_shader_program_:0,
-			image.empty()?0:texture_mesh_shader_program_,
-			cloud,
-			polygons,
-			image);
-	drawable->setPose(pose);
-	pointClouds_.insert(std::make_pair(id, drawable));
-}
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -446,8 +420,8 @@ void Scene::addCloud(
 	//create
 	UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
 	PointCloudDrawable * drawable = new PointCloudDrawable(
-			image.empty()?cloud_shader_program_:0,
+			cloud->is_dense || image.empty()?cloud_shader_program_:0,
-			image.empty()?0:texture_mesh_shader_program_,
+			cloud->is_dense || image.empty()?0:texture_mesh_shader_program_,
 			cloud,
 			polygons,
 			image);

app/android/jni/scene.h

@@ -96,12 +96,6 @@ class Scene {
   void setGraphVisible(bool visible);
   void setTraceVisible(bool visible);
 
-  void addCloud(
-		  int id,
-		  const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-		  const std::vector<pcl::Vertices> & polygons,
-		  const rtabmap::Transform & pose,
-		  const cv::Mat & image = cv::Mat());
   void addCloud(
   		  int id,
   		  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,

app/android/res/menu/optionmenu.xml

@@ -8,23 +8,41 @@
     <group android:id="@+id/group_actions">
        <item android:id="@+id/open" android:title="Open"/>
        <item android:id="@+id/save" android:title="Save"/>
-       <item android:id="@+id/export" android:title="Export (*.ply)"/>
+       <item android:id="@+id/export" android:title="Export...">
+          <menu>
+	         <group android:id="@+id/group_export">
+	            <item android:id="@+id/export_ply" android:title="Mesh (.ply)" />
+	            <item android:id="@+id/export_obj" android:title="Mesh with texture (*.obj)" />
+	         </group>   
+          </menu>
+       </item>
        <item android:id="@+id/reset" android:title="Reset"/>
        <item android:id="@+id/about" android:title="About"/>
     </group>
     
-    <item android:id="@+id/menu_settings" android:title="Options..." android:orderInCategory="2">
+    <item android:id="@+id/menu_rendering_settings" android:title="Rendering Options..." android:orderInCategory="2">
         <menu >
-        <group android:id="@+id/group_visibility" android:checkableBehavior="all">
+        <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
             <item android:id="@+id/debug" android:checked="false" android:title="Debug" />
-            <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/mesh_rendering" android:checked="true" android:title="Mesh Rendering" />
-            <item android:id="@+id/auto_exposure" android:checked="false" android:title="Auto Exposure" />
             <item android:id="@+id/map_shown" android:checked="true" android:title="Map Visible" />
             <item android:id="@+id/odom_shown" android:checked="true" android:title="Odom Visible" />
             <item android:id="@+id/graph_visible" android:checked="true" android:title="Graph Visible" />
+            <item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />
+        </group>   
+        </menu>
+    </item>
+    <item android:id="@+id/menu_mapping_settings" android:title="Mapping Options..." android:orderInCategory="3">
+       <menu >
+       <group android:id="@+id/group_mapping_visibility" android:checkableBehavior="all">
+           <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/auto_exposure" android:checked="false" android:title="Auto Exposure" />
            <item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
+           <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/features" android:checkable="false" android:title="Max Features Extracted..." />
+           <item android:id="@+id/resolution" android:checked="false" android:title="720p Mode" />
        </group>   
        </menu>
    </item>

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

@@ -75,6 +75,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
   private String mNewDatabasePath = "";
   private String mWorkingDirectory = "";
   
+  private int mMaxDepthIndex = 5;
+  
+  private int mParamUpdateRateHzIndex = 1;
+  private int mParamTimeThrMsIndex = 1;
+  private int mParamMaxFeaturesIndex = 2;
+  
   private LinearLayout mLayoutDebug;
   
   private int mTotalLoopClosures = 0;
@@ -627,6 +633,96 @@ public class RTABMapActivity extends Activity implements OnClickListener {
     	  item.setChecked(!item.isChecked());
     	  RTABMapLib.setAutoExposure(item.isChecked());
       }
+      else if(itemId == R.id.resolution)
+      {
+    	  item.setChecked(!item.isChecked());
+    	  RTABMapLib.setFullResolution(item.isChecked());
+      }
+      else if(itemId == R.id.max_depth)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Depth (m)");
+		  final String[] values = {"1", "2", "3", "4", "5", "No Limit"};
+		  builder.setSingleChoiceItems(values, mMaxDepthIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 6)
+		        {
+			        mMaxDepthIndex = which;
+			        RTABMapLib.setMaxCloudDepth(which < 5?Integer.parseInt(values[which]):0);
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.update_rate)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Update Rate (Hz)");
+		  final String[] values = {"0.5", "1", "2", "Max"};
+		  builder.setSingleChoiceItems(values, mParamUpdateRateHzIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 4)
+		        {
+		        	mParamUpdateRateHzIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", values[which]) != 0)
+			        {
+			        	Toast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/DetectionRate\"!", Toast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.time_threshold)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Time Threshold (ms, 0 means no limit)");
+		  final String[] values = {"400", "700", "1400", "No Limit"};
+		  builder.setSingleChoiceItems(values, mParamTimeThrMsIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 4)
+		        {
+		        	mParamTimeThrMsIndex = which;
+			        if(RTABMapLib.setMappingParameter("Rtabmap/TimeThr", which==3?"0":values[which]) != 0)
+			        {
+			        	Toast.makeText(getActivity(), "Failed to set parameter \"Rtabmap/TimeThr\"!", Toast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
+      else if(itemId == R.id.features)
+      {
+    	  // get double
+		  AlertDialog.Builder builder = new AlertDialog.Builder(this);
+		  builder.setTitle("Max Features");
+		  final String[] values = {"Disabled", "100", "200", "300", "400", "No Limit"};
+		  builder.setSingleChoiceItems(values, mParamMaxFeaturesIndex, new DialogInterface.OnClickListener() {
+		    @Override
+		    public void onClick(DialogInterface dialog, int which) {
+		        dialog.dismiss();
+		        if(which >=0 && which < 6)
+		        {
+		        	mParamMaxFeaturesIndex = which;
+			        if(RTABMapLib.setMappingParameter("Kp/MaxFeatures", which==0?"-1":which==5?"0":values[which]) != 0)
+			        {
+			        	Toast.makeText(getActivity(),"Failed to set parameter \"Kp/MaxFeatures\"!", Toast.LENGTH_LONG).show();
+			        }
+		        }
+		    }
+		  });
+		  builder.show();
+      }
       else if (itemId == R.id.save)
       {
     	  if(mOpenedDatabasePath.isEmpty())
@@ -729,10 +825,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
         	  RTABMapLib.openDatabase(mTempDatabasePath);
 		  }
       }
-      else if(itemId == R.id.export)
+      else if(itemId == R.id.export_obj || itemId == R.id.export_ply)
       {
+    	  final String extension = itemId == R.id.export_ply ? ".ply" : ".obj";
+    	  
     	  AlertDialog.Builder builder = new AlertDialog.Builder(this);
-		  builder.setTitle("File Name (*.obj):");
+		  builder.setTitle(String.format("File Name (*%s):", extension));
 		  final EditText input = new EditText(this);
 		  input.setInputType(InputType.TYPE_CLASS_TEXT);        
 		  builder.setView(input);
@@ -744,7 +842,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 				  dialog.dismiss();
 				  if(!fileName.isEmpty())
 				  {
-					  File newFile = new File(mWorkingDirectory + fileName + ".obj");
+					  File newFile = new File(mWorkingDirectory + fileName + extension);
 					  if(newFile.exists())
 					  {
 						  new AlertDialog.Builder(getActivity())
@@ -752,12 +850,12 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 			                .setMessage("Do you want to overwrite the existing file?")
 			                .setPositiveButton("Yes", new DialogInterface.OnClickListener() {
 			                    public void onClick(DialogInterface dialog, int which) {
-			                    	final String path = mWorkingDirectory + fileName + ".obj";
+			                    	final String path = mWorkingDirectory + fileName + extension;
 			                    	
 			                    	mItemExport.setEnabled(false);
 			                    			                    	
 			                    	mProgressDialog.setTitle("Exporting");
-			                  	    mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".obj"));
+			                  	    mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
 			                  	    mProgressDialog.show();
 			                  	  
 			                    	Thread exportThread = new Thread(new Runnable() {
@@ -791,10 +889,10 @@ public class RTABMapActivity extends Activity implements OnClickListener {
 					  }
 					  else
 					  {
-						  final String path = mWorkingDirectory + fileName + ".obj";
+						  final String path = mWorkingDirectory + fileName + extension;
 						  mItemExport.setEnabled(false);
 						  mProgressDialog.setTitle("Exporting");
-	                  	  mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+".obj"));
+	                  	  mProgressDialog.setMessage(String.format("Please wait while exporting \"%s\"...", fileName+extension));
 	                  	  mProgressDialog.show();
 						  Thread exportThread = new Thread(new Runnable() {
 	                		    public void run() {

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

@@ -52,6 +52,9 @@ public class RTABMapLib
     public static native void setGraphOptimization(boolean enabled);
     public static native void setGraphVisible(boolean visible);
     public static native void setAutoExposure(boolean enabled);
+    public static native void setFullResolution(boolean enabled);
+    public static native void setMaxCloudDepth(float value);
+    public static native int setMappingParameter(String key, String value);
 
     public static native void resetMapping();
     public static native void save();

corelib/include/rtabmap/core/util3d_surface.h

@@ -78,6 +78,11 @@ void RTABMAP_EXP appendMesh(
 		std::vector<pcl::Vertices> & polygonsA,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
 		const std::vector<pcl::Vertices> & polygonsB);
+void RTABMAP_EXP appendMesh(
+		pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
+		std::vector<pcl::Vertices> & polygonsA,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
+		const std::vector<pcl::Vertices> & polygonsB);
 
 // return map from new to old polygon indices
 std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
@@ -85,6 +90,11 @@ std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
 		const std::vector<pcl::Vertices> & polygons,
 		pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
 		std::vector<pcl::Vertices> & outputPolygons);
+std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
+		std::vector<pcl::Vertices> & outputPolygons);
 
 std::vector<pcl::Vertices> RTABMAP_EXP filterCloseVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,

corelib/src/RtabmapThread.cpp

@@ -544,14 +544,30 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 				ignoreFrame = true;
 			}
 		}
-		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || odomEvent.info().variance>=9999))
+		if(_dataBufferMaxSize > 0 &&
+				((!lastPose_.isIdentity() && odomEvent.pose().isIdentity()) ||
+						odomEvent.info().variance>=9999 ||
+						odomEvent.rotVariance()>=9999 ||
+						odomEvent.transVariance()>=9999))
 		{
-			UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomEvent.info().variance);
+			UWARN("Odometry is reset (identity pose or high variance (>=9999) detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
 			_rotVariance = 0;
 			_transVariance = 0;
 		}
 
+		double maxRotVar = odomEvent.rotVariance();
+		double maxTransVar = odomEvent.transVariance();
+		// FIXME: should merge the transformations/variances like Link::merge();
+		if(maxRotVar > _rotVariance)
+		{
+			_rotVariance = maxRotVar;
+		}
+		if(maxTransVar > _transVariance)
+		{
+			_transVariance = maxTransVar;
+		}
+
 		if(ignoreFrame && !_createIntermediateNodes)
 		{
 			return;
@@ -563,17 +579,6 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 
 		lastPose_ = odomEvent.pose();
-		double maxRotVar = odomEvent.rotVariance();
-		double maxTransVar = odomEvent.transVariance();
-		// FIXME: should merge the transformations/variances like Link::merge();
-		if(maxRotVar > _rotVariance)
-		{
-			_rotVariance = maxRotVar;
-		}
-		if(maxTransVar > _transVariance)
-		{
-			_transVariance = maxTransVar;
-		}
 
 		bool notify = true;
 		_dataMutex.lock();
@@ -598,7 +603,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			{
 				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
 			}
-			UDEBUG("Added data %d", odomEvent.data().id());
+			UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
 
 			_rotVariance = 0;
 			_transVariance = 0;

corelib/src/util3d_surface.cpp

@@ -194,6 +194,32 @@ void appendMesh(
 	}
 }
 
+void appendMesh(
+		pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
+		std::vector<pcl::Vertices> & polygonsA,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
+		const std::vector<pcl::Vertices> & polygonsB)
+{
+	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
+	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());
+
+	int sizeA = cloudA.size();
+	cloudA += cloudB;
+
+	int sizePolygonsA = polygonsA.size();
+	polygonsA.resize(sizePolygonsA+polygonsB.size());
+
+	for(unsigned int i=0; i<polygonsB.size(); ++i)
+	{
+		pcl::Vertices vertices = polygonsB[i];
+		for(unsigned int j=0; j<vertices.vertices.size(); ++j)
+		{
+			vertices.vertices[j] += sizeA;
+		}
+		polygonsA[i+sizePolygonsA] = vertices;
+	}
+}
+
 std::map<int, int> filterNotUsedVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const std::vector<pcl::Vertices> & polygons,
@@ -232,6 +258,44 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
 	return output;
 }
 
+std::map<int, int> filterNotUsedVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
+		std::vector<pcl::Vertices> & outputPolygons)
+{
+	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
+	std::map<int, int> addedVertices; //<oldIndex, newIndex>
+	std::map<int, int> output; //<newIndex, oldIndex>
+	outputCloud.resize(cloud.size());
+	outputCloud.is_dense = true;
+	outputPolygons.resize(polygons.size());
+	int oi = 0;
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices & v = outputPolygons[i];
+		v.vertices.resize(polygons[i].vertices.size());
+		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
+		{
+			std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
+			if(iter == addedVertices.end())
+			{
+				outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
+				addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
+				output.insert(std::make_pair(oi, polygons[i].vertices[j]));
+				v.vertices[j] = oi++;
+			}
+			else
+			{
+				v.vertices[j] = iter->second;
+			}
+		}
+	}
+	outputCloud.resize(oi);
+
+	return output;
+}
+
 std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
 		const std::vector<pcl::Vertices> & polygons,