0.11.2: First Google Tango release

corelib/src/CMakeLists.txt

@@ -158,9 +158,6 @@ IF(FlyCapture2_FOUND)
 ENDIF(FlyCapture2_FOUND)
 
 IF(G2O_FOUND)
-	IF(G2O_CHOMOLD_FOUND)
-		ADD_DEFINITIONS("-DWITH_G2O_CHOMOLD")
-	ENDIF(G2O_CHOMOLD_FOUND)
 	ADD_DEFINITIONS("-DWITH_G2O")
 	SET(INCLUDE_DIRS 
 		${INCLUDE_DIRS} 

corelib/src/Camera.cpp

@@ -26,7 +26,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/DBDriver.h"
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>

corelib/src/DBDriverSqlite3.cpp

@@ -436,14 +436,14 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
 
 long DBDriverSqlite3::getMemoryUsedQuery() const
 {
-	if(_dbInMemory)
-	{
+	//if(_dbInMemory)
+	//{
 		return sqlite3_memory_used();
-	}
-	else
-	{
-		return UFile::length(this->getUrl());
-	}
+	//}
+	//else // Commented because it can lag
+	//{
+	//	return UFile::length(this->getUrl());
+	//}
 }
 
 long DBDriverSqlite3::getImagesMemoryUsedQuery() const
@@ -841,7 +841,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 				{
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+					// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
 					// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
 					if(dataSize > 0 && data)
 					{
@@ -874,6 +874,26 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 									dataFloat[4], // baseline
 									localTransform);
 						}
+						else if((unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+						{
+							int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+							int max = cameraCount*(6+localTransform.size());
+							for(int i=0; i<max; i+=6+localTransform.size())
+							{
+								memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+								models.push_back(CameraModel(
+										(double)dataFloat[i],
+										(double)dataFloat[i+1],
+										(double)dataFloat[i+2],
+										(double)dataFloat[i+3],
+										localTransform));
+								models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+								UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+										dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+										localTransform.prettyPrint().c_str());
+							}
+						}
 						else
 						{
 							UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
@@ -1545,9 +1565,18 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 		// Prepare the query... Get the map from signature and visual words
 		std::stringstream query2;
-		query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
-				 "FROM Map_Node_Word "
-				 "WHERE node_id = ? ";
+		if(uStrNumCmp(_version, "0.11.2") >= 0)
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor "
+					 "FROM Map_Node_Word "
+					 "WHERE node_id = ? ";
+		}
+		else
+		{
+			query2 << "SELECT word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z "
+					 "FROM Map_Node_Word "
+					 "WHERE node_id = ? ";
+		}
 
 		query2 << " ORDER BY word_id"; // Needed for fast insertion below
 		query2 << ";";
@@ -1563,9 +1592,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
 			int visualWordId = 0;
+			int descriptorSize = 0;
+			const void * descriptor = 0;
+			int dRealSize = 0;
 			cv::KeyPoint kpt;
 			std::multimap<int, cv::KeyPoint> visualWords;
 			std::multimap<int, cv::Point3f> visualWords3;
+			std::multimap<int, cv::Mat> descriptors;
 			cv::Point3f depth(0,0,0);
 
 			// Process the result if one
@@ -1582,8 +1615,40 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 				depth.x = sqlite3_column_double(ppStmt, index++);
 				depth.y = sqlite3_column_double(ppStmt, index++);
 				depth.z = sqlite3_column_double(ppStmt, index++);
+
 				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
 				visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
+
+				if(uStrNumCmp(_version, "0.11.2") >= 0)
+				{
+					descriptorSize = sqlite3_column_int(ppStmt, index++); // VisualWord descriptor size
+					descriptor = sqlite3_column_blob(ppStmt, index); 	// VisualWord descriptor array
+					dRealSize = sqlite3_column_bytes(ppStmt, index++);
+
+					if(descriptor && descriptorSize>0 && dRealSize>0)
+					{
+						cv::Mat d;
+						if(dRealSize == descriptorSize)
+						{
+							// CV_8U binary descriptors
+							d = cv::Mat(1, descriptorSize, CV_8U);
+						}
+						else if(dRealSize/int(sizeof(float)) == descriptorSize)
+						{
+							// CV_32F
+							d = cv::Mat(1, descriptorSize, CV_32F);
+						}
+						else
+						{
+							UFATAL("Saved buffer size (%d bytes) is not the same as descriptor size (%d)", dRealSize, descriptorSize);
+						}
+
+						memcpy(d.data, descriptor, dRealSize);
+
+						descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
+					}
+				}
+
 				rc = sqlite3_step(ppStmt);
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1596,7 +1661,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			{
 				(*iter)->setWords(visualWords);
 				(*iter)->setWords3(visualWords3);
-				ULOGGER_DEBUG("Add %d keypoints and %d 3d points to node %d", visualWords.size(), visualWords3.size(), (*iter)->id());
+				(*iter)->setWordsDescriptors(descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), (int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
 			}
 
 			//reset
@@ -2338,22 +2404,29 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
 			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			if((*i)->getWords3().size())
+			UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
+
+			std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
+			std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
+			for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 			{
-				std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin();
-				std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
-				for(; w!=(*i)->getWords().end(); ++w, ++p)
+				cv::Point3f pt(0,0,0);
+				if(p!=(*i)->getWords3().end())
 				{
 					UASSERT(w->first == p->first); // must be same id!
-					stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, p->second);
+					pt = p->second;
+					++p;
 				}
-			}
-			else
-			{
-				for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
+
+				cv::Mat descriptor;
+				if(d!=(*i)->getWordsDescriptors().end())
 				{
-					stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, cv::Point3f(0,0,0));
+					UASSERT(w->first == d->first); // must be same id!
+					descriptor = d->second;
+					++d;
 				}
+
+				stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
 			}
 		}
 		// Finalize (delete) the statement
@@ -2840,19 +2913,21 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 
 	// calibration
 	std::vector<float> calibration;
-	// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
+	// multi-cameras [fx,fy,cx,cy,width,height,local_transform, ... ,fx,fy,cx,cy,width,height,local_transform] (6+12)*float * numCameras
 	// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
 	if(sensorData.cameraModels().size())
 	{
-		calibration.resize(sensorData.cameraModels().size() * (4+Transform().size()));
+		calibration.resize(sensorData.cameraModels().size() * (6+Transform().size()));
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
 			const Transform & localTransform = sensorData.cameraModels()[i].localTransform();
-			calibration[i*(4+localTransform.size())] = sensorData.cameraModels()[i].fx();
-			calibration[i*(4+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
-			calibration[i*(4+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
-			calibration[i*(4+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
-			memcpy(calibration.data()+i*(4+localTransform.size())+4, localTransform.data(), localTransform.size()*sizeof(float));
+			calibration[i*(6+localTransform.size())] = sensorData.cameraModels()[i].fx();
+			calibration[i*(6+localTransform.size())+1] = sensorData.cameraModels()[i].fy();
+			calibration[i*(6+localTransform.size())+2] = sensorData.cameraModels()[i].cx();
+			calibration[i*(6+localTransform.size())+3] = sensorData.cameraModels()[i].cy();
+			calibration[i*(6+localTransform.size())+4] = sensorData.cameraModels()[i].imageWidth();
+			calibration[i*(6+localTransform.size())+5] = sensorData.cameraModels()[i].imageHeight();
+			memcpy(calibration.data()+i*(6+localTransform.size())+6, localTransform.data(), localTransform.size()*sizeof(float));
 		}
 	}
 	else if(sensorData.stereoCameraModel().isValidForProjection())
@@ -3052,9 +3127,18 @@ void DBDriverSqlite3::stepWordsChanged(sqlite3_stmt * ppStmt, int nodeId, int ol
 
 std::string DBDriverSqlite3::queryStepKeypoint() const
 {
+	if(uStrNumCmp(_version, "0.11.2") >= 0)
+	{
+		return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z, descriptor_size, descriptor) VALUES(?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
 	return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
 }
-void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const
+void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
+		int nodeId,
+		int wordId,
+		const cv::KeyPoint & kp,
+		const cv::Point3f & pt,
+		const cv::Mat & descriptor) const
 {
 	if(!ppStmt)
 	{
@@ -3083,6 +3167,32 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId
 	rc = sqlite3_bind_double(ppStmt, index++, pt.z);
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
+	//descriptor
+	if(uStrNumCmp(_version, "0.11.2") >= 0)
+	{
+		rc = sqlite3_bind_int(ppStmt, index++, descriptor.cols);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		UASSERT(descriptor.empty() || descriptor.type() == CV_32F || descriptor.type() == CV_8U);
+		if(descriptor.empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+		}
+		else
+		{
+			if(descriptor.type() == CV_32F)
+			{
+				// CV_32F
+				rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(float), SQLITE_STATIC);
+			}
+			else
+			{
+				// CV_8U
+				rc = sqlite3_bind_blob(ppStmt, index++, descriptor.data, descriptor.cols*sizeof(char), SQLITE_STATIC);
+			}
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 

corelib/src/DBDriverSqlite3.h

@@ -109,7 +109,7 @@ private:
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;

corelib/src/Features2d.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_features.h"
 #include "rtabmap/core/Stereo.h"
+#include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/ULogger.h"
@@ -626,20 +627,18 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 		if(_maxDepth > 0.0f || _minDepth > 0.0f)
 		{
 			UASSERT(keypoints3D.size() == keypoints.size());
-			bool isInMM = data.depthRaw().type() == CV_16UC1;
 			float bad_point = std::numeric_limits<float>::quiet_NaN ();
 			for(unsigned int i=0; i<keypoints.size(); ++i)
 			{
-				int u = int(keypoints[i].pt.x+0.5f);
-				int v = int(keypoints[i].pt.y+0.5f);
+				float d = util2d::getDepth(
+						data.depthRaw(),
+						keypoints[i].pt.x/float((data.imageRaw().cols/data.depthRaw().cols)),
+						keypoints[i].pt.y/float((data.imageRaw().rows/data.depthRaw().rows)),
+						false);
 				bool reject = true;
-				if(u >=0 && u<data.depthRaw().cols && v >=0 && v<data.depthRaw().rows)
+				if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
 				{
-					float d = isInMM?(float)data.depthRaw().at<uint16_t>(v,u)*0.001f:data.depthRaw().at<float>(v,u);
-					if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
-					{
-						reject = false;
-					}
+					reject = false;
 				}
 				if(reject)
 				{

corelib/src/Graph.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/core/Memory.h>
+#include <rtabmap/core/util3d_filtering.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/common/eigen.h>
 #include <pcl/common/common.h>
@@ -550,6 +551,55 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }
 
+std::map<int, Transform> frustumPosesFiltering(
+		const std::map<int, Transform> & poses,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative)
+{
+	std::map<int, Transform> output;
+
+	if(poses.size())
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		std::vector<int> ids(poses.size());
+
+		cloud->resize(poses.size());
+		ids.resize(poses.size());
+		int oi=0;
+		for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(!iter->second.isNull())
+			{
+				(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+				ids[oi++] = iter->first;
+			}
+		}
+		cloud->resize(oi);
+		ids.resize(oi);
+
+		pcl::IndicesPtr indices = util3d::frustumFiltering(
+				cloud,
+				pcl::IndicesPtr(new std::vector<int>),
+				cameraPose,
+				horizontalFOV,
+				verticalFOV,
+				nearClipPlaneDistance,
+				farClipPlaneDistance,
+				negative);
+
+
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			output.insert(*poses.find(ids[indices->at(i)]));
+		}
+	}
+	return output;
+}
+
 std::map<int, Transform> radiusPosesFiltering(
 		const std::map<int, Transform> & poses,
 		float radius,
@@ -1441,15 +1491,14 @@ std::map<int, float> getNodesInRadius(
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
-		std::vector<float> dist;
+		std::vector<float> sqrdDist;
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
-		kdTree->radiusSearch(pt, radius, ind, dist, 0);
+		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
 			{
-				UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
-				foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
+				foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
 			}
 		}
 	}
@@ -1461,7 +1510,8 @@ std::map<int, float> getNodesInRadius(
 std::map<int, Transform> getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
-		float radius)
+		float radius,
+		float angle)
 {
 	UASSERT(uContains(nodes, nodeId));
 	std::map<int, Transform> foundNodes;
@@ -1494,15 +1544,31 @@ std::map<int, Transform> getPosesInRadius(
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
-		std::vector<float> dist;
+		std::vector<float> sqrdDist;
 		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
-		kdTree->radiusSearch(pt, radius, ind, dist, 0);
+		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
+
+		Eigen::Vector3f vA = fromT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
 			if(ind[i] >=0)
 			{
-				UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i]));
-				foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+				if(angle > 0.0f)
+				{
+					const Transform & checkT = nodes.at(ids[ind[i]]);
+					// same orientation?
+					Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+					double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+					if(a <= angle)
+					{
+						foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+					}
+				}
+				else
+				{
+					foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
+				}
 			}
 		}
 	}

corelib/src/Memory.cpp

@@ -70,8 +70,8 @@ const int Memory::kIdInvalid = 0;
 Memory::Memory(const ParametersMap & parameters) :
 	_dbDriver(0),
 	_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
-	_rawDataKept(Parameters::defaultMemImageKept()),
 	_binDataKept(Parameters::defaultMemBinDataKept()),
+	_rawDescriptorsKept(Parameters::defaultMemRawDescriptorsKept()),
 	_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
@@ -302,14 +302,14 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 {
-	UDEBUG("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
+	UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
 	if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
 
 	if(!databaseSaved || (!_memoryChanged && !_linksChanged))
 	{
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("No changes added to database.")));
 
-		UDEBUG("");
+		UINFO("No changes added to database.");
 		if(_dbDriver)
 		{
 			if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
@@ -324,7 +324,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
 	}
 	else
 	{
-		UDEBUG("");
+		UINFO("Saving memory...");
 		if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory..."));
 		if(!_memoryChanged && _linksChanged && _dbDriver)
 		{
@@ -384,8 +384,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	UDEBUG("");
 	ParametersMap::const_iterator iter;
 
-	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
+	Parameters::parse(parameters, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
 	Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
 	Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
 	Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
@@ -2031,6 +2031,18 @@ void Memory::removeLink(int oldId, int newId)
 	}
 }
 
+void Memory::removeRawData(int id)
+{
+	Signature * s = this->_getSignature(id);
+	if(s)
+	{
+		s->sensorData().setImageRaw(cv::Mat());
+		s->sensorData().setDepthOrRightRaw(cv::Mat());
+		s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
+		s->sensorData().setUserDataRaw(cv::Mat());
+	}
+}
+
 // compute transform fromId -> toId
 Transform Memory::computeTransform(
 		int fromId,
@@ -2064,8 +2076,12 @@ Transform Memory::computeTransform(
 			{
 				tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
 				tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
+				tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+				tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 				tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
 				tmpTo.setWords3(std::multimap<int, cv::Point3f>());
+				tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+				tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 			}
 
 			Transform guess = Transform::getIdentity();
@@ -2244,7 +2260,7 @@ bool Memory::addLink(const Link & link)
 {
 	UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
 
-	ULOGGER_INFO("to=%d, from=%d transform: %s", link.to(), link.from(), link.transform().prettyPrint().c_str());
+	ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
 	Signature * toS = _getSignature(link.to());
 	Signature * fromS = _getSignature(link.from());
 	if(toS && fromS)
@@ -3006,8 +3022,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.imageRaw().type() == CV_8UC1 ||
 			data.imageRaw().type() == CV_8UC3);
 	UASSERT_MSG(data.depthOrRightRaw().empty() ||
-			(  (data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_8UC1) &&
-				((data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) || (data.depthOrRightRaw().rows == data.imageRaw().rows && data.depthOrRightRaw().cols == data.imageRaw().cols))),
+			(  ( data.depthOrRightRaw().type() == CV_16UC1 ||
+				 data.depthOrRightRaw().type() == CV_32FC1 ||
+				 data.depthOrRightRaw().type() == CV_8UC1)
+			   &&
+				( (data.imageRaw().empty() && data.depthOrRightRaw().type() != CV_8UC1) ||
+				  (data.imageRaw().rows % data.depthOrRightRaw().rows == 0 && data.imageRaw().cols % data.depthOrRightRaw().cols == 0))),
 				uFormat("image=(%d/%d) depth=(%d/%d, type=%d [accepted=%d,%d,%d])",
 						data.imageRaw().cols,
 						data.imageRaw().rows,
@@ -3093,9 +3113,18 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				imageMono = data.imageRaw();
 			}
 
+			cv::Mat depthMask;
+			if(_useDepthAsMask && !data.depthRaw().empty())
+			{
+				if(imageMono.rows/data.depthRaw().rows == imageMono.cols/data.depthRaw().cols)
+				{
+					depthMask = util2d::interpolate(data.depthRaw(), imageMono.rows/data.depthRaw().rows, 0.1f);
+				}
+			}
+
 			keypoints = _feature2D->generateKeypoints(
 					imageMono,
-					_useDepthAsMask&&!data.depthRaw().empty()?data.depthRaw():cv::Mat());
+					depthMask);
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3173,6 +3202,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 	std::multimap<int, cv::KeyPoint> words;
 	std::multimap<int, cv::Point3f> words3D;
+	std::multimap<int, cv::Mat> wordsDescriptors;
 	if(wordIds.size() > 0)
 	{
 		UASSERT(wordIds.size() == keypoints.size());
@@ -3196,46 +3226,62 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			{
 				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
 			}
+			if(_rawDescriptorsKept)
+			{
+				wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
+			}
 		}
 	}
 
-	if(words.size() > 8 &&
-		words3D.size() == 0 &&
-		!pose.isNull() &&
+	if(!pose.isNull() &&
 		data.cameraModels().size() == 1 &&
-		_signatures.size())
+		words.size() &&
+		words3D.size() == 0)
 	{
-		UDEBUG("Generate 3D words using odometry");
-		Signature * previousS = _signatures.rbegin()->second;
-		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
+		bool fillWithNaN = true;
+		if(_signatures.size())
 		{
-			Transform cameraTransform = pose.inverse() * previousS->getPose();
-			// compute 3D words by epipolar geometry with the previous signature
-			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-					uMultimapToMapUnique(words),
-					uMultimapToMapUnique(previousS->getWords()),
-					data.cameraModels()[0],
-					cameraTransform);
+			UDEBUG("Generate 3D words using odometry");
+			Signature * previousS = _signatures.rbegin()->second;
+			if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
+			{
+				Transform cameraTransform = pose.inverse() * previousS->getPose();
+				// compute 3D words by epipolar geometry with the previous signature
+				std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
+						uMultimapToMapUnique(words),
+						uMultimapToMapUnique(previousS->getWords()),
+						data.cameraModels()[0],
+						cameraTransform);
 
-			// words3D should have the same size than words
+				// words3D should have the same size than words
+				float bad_point = std::numeric_limits<float>::quiet_NaN ();
+				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+				{
+					std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
+					if(jter != inliers.end())
+					{
+						words3D.insert(std::make_pair(iter->first, jter->second));
+					}
+					else
+					{
+						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+					}
+				}
+
+				t = timer.ticks();
+				UASSERT(words3D.size() == words.size());
+				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+				UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
+				fillWithNaN = false;
+			}
+		}
+		if(fillWithNaN)
+		{
 			float bad_point = std::numeric_limits<float>::quiet_NaN ();
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
-				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-				if(jter != inliers.end())
-				{
-					words3D.insert(std::make_pair(iter->first, jter->second));
-				}
-				else
-				{
-					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
-				}
+				words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
 			}
-
-			t = timer.ticks();
-			UASSERT(words3D.size() == words.size());
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
 		}
 	}
 
@@ -3245,7 +3291,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
 
 	// apply decimation?
-	if((this->isBinDataKept() || this->isRawDataKept()) && _imageDecimation > 1)
+	if(_imageDecimation > 1)
 	{
 		image = util2d::decimate(image, _imageDecimation);
 		depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
@@ -3360,13 +3406,14 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 	s->setWords(words);
 	s->setWords3(words3D);
-	if(this->isRawDataKept())
-	{
-		s->sensorData().setImageRaw(image);
-		s->sensorData().setDepthOrRightRaw(depthOrRightImage);
-		s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
-		s->sensorData().setUserDataRaw(data.userDataRaw());
-	}
+	s->setWordsDescriptors(wordsDescriptors);
+
+	// set raw data
+	s->sensorData().setImageRaw(image);
+	s->sensorData().setDepthOrRightRaw(depthOrRightImage);
+	s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
+	s->sensorData().setUserDataRaw(data.userDataRaw());
+
 	s->sensorData().setGroundTruth(data.groundTruth());
 
 	t = timer.ticks();
@@ -3516,13 +3563,23 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 	for(std::list<Signature *>::iterator j=surfSigns.begin(); j!=surfSigns.end(); ++j)
 	{
 		const std::vector<int> & keys = uKeys((*j)->getWords());
-		// Add all references
-		for(std::vector<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
-		{
-			_vwd->addWordRef(*i, (*j)->id());
-		}
 		if(keys.size())
 		{
+			const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
+			UASSERT(wordFirst!=0);
+			//Descriptors used for Memory::computeTransform()
+			cv::Mat descriptors(keys.size(), wordFirst->getDescriptor().cols, wordFirst->getDescriptor().type());
+			// Add all references
+			for(unsigned int i=0; i<keys.size(); ++i)
+			{
+				_vwd->addWordRef(keys.at(i), (*j)->id());
+				const VisualWord * word = _vwd->getWord(keys.at(i));
+				UASSERT(word != 0);
+
+				word->getDescriptor().copyTo(descriptors.row(i));
+
+			}
+			(*j)->sensorData().setFeatures(std::vector<cv::KeyPoint>(), descriptors);
 			(*j)->setEnabled(true);
 		}
 	}

corelib/src/OdometryMono.cpp

@@ -100,7 +100,6 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
 	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
-	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
 	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
@@ -872,7 +871,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
 							else if(inliersRef.size())
 							{
 								// find centroid of the cloud and set it to 1 meter
-								Eigen::Vector4f centroid;
+								Eigen::Vector4f centroid(0,0,0,0);
 								pcl::PointCloud<pcl::PointXYZ> inliersRefCloud;
 								inliersRefCloud.resize(inliersRef.size());
 								for(unsigned int i=0; i<inliersRef.size(); ++i)

corelib/src/OptimizerG2O.cpp

@@ -35,15 +35,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OptimizerG2O.h>
 
 #ifdef WITH_G2O
+#include "g2o/config.h"
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/block_solver.h"
 #include "g2o/core/factory.h"
 #include "g2o/core/optimization_algorithm_factory.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
+#ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
+#endif
 #include "g2o/solvers/pcg/linear_solver_pcg.h"
-#ifdef WITH_G2O_CHOMOLD
+#ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
 #include "g2o/types/slam3d/vertex_se3.h"
@@ -52,9 +55,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "g2o/types/slam2d/edge_se2.h"
 
 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
-typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
-#ifdef WITH_G2O_CHOMOLD
+#ifdef G2O_HAVE_CSPARSE
+typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
+#endif
+#ifdef G2O_HAVE_CHOLMOD
 typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearCholmodSolver;
 #endif
 
@@ -76,6 +81,48 @@ bool OptimizerG2O::available()
 #endif
 }
 
+bool OptimizerG2O::isCSparseAvailable()
+{
+#ifdef G2O_HAVE_CSPARSE
+	return true;
+#else
+	return false;
+#endif
+}
+
+bool OptimizerG2O::isCholmodAvailable()
+{
+#ifdef G2O_HAVE_CHOLMOD
+	return true;
+#else
+	return false;
+#endif
+}
+
+void OptimizerG2O::parseParameters(const ParametersMap & parameters)
+{
+	Optimizer::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
+	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
+
+#ifndef G2O_HAVE_CHOLMOD
+	if(solver_ == 2)
+	{
+		UWARN("g2o is not built with chmold, so it cannot be used as solver. Using CSparse instead.");
+		solver_ = 0;
+	}
+#endif
+
+#ifndef G2O_HAVE_CSPARSE
+	if(solver_ == 0)
+	{
+		UWARN("g2o is not built with csparse, so it cannot be used as solver. Using PCG instead.");
+		solver_ = 1;
+	}
+#endif
+}
+
 std::map<int, Transform> OptimizerG2O::optimize(
 		int rootId,
 		const std::map<int, Transform> & poses,
@@ -97,31 +144,32 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int solverApproach = 0;
 		int optimizationApproach = 1;
 
-		SlamBlockSolver * blockSolver;
-#ifdef WITH_G2O_CHOMOLD
-		if(solverApproach == 0)
+		SlamBlockSolver * blockSolver = 0;
+
+		if(solverApproach == 2)
 		{
+#ifdef G2O_HAVE_CHOLMOD
 			//chmold
 			SlamLinearCholmodSolver * linearSolver = new SlamLinearCholmodSolver();
 			linearSolver->setBlockOrdering(false);
 			blockSolver = new SlamBlockSolver(linearSolver);
-		}
-		else if
-#else
-		if
 #endif
-			(solverApproach == 1)
-		{
-			//pcg
-			SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
-			blockSolver = new SlamBlockSolver(linearSolver);
 		}
-		else
+		else if(solverApproach == 0)
 		{
+#ifdef G2O_HAVE_CSPARSE
 			//csparse
 			SlamLinearCSparseSolver* linearSolver = new SlamLinearCSparseSolver();
 			linearSolver->setBlockOrdering(false);
 			blockSolver = new SlamBlockSolver(linearSolver);
+#endif
+		}
+
+		if(blockSolver == 0)
+		{
+			//pcg
+			SlamLinearPCGSolver * linearSolver = new SlamLinearPCGSolver();
+			blockSolver = new SlamBlockSolver(linearSolver);
 		}
 
 		if(optimizationApproach == 1)

corelib/src/Parameters.cpp

@@ -53,7 +53,7 @@ Parameters::~Parameters()
 {
 }
 
-std::string Parameters::getDefaultWorkingDirectory()
+std::string Parameters::createDefaultWorkingDirectory()
 {
 	std::string path = UDirectory::homeDir();
 	if(!path.empty())

corelib/src/RegistrationVis.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_features.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/util2d.h>
 #include <rtabmap/core/Features2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -71,6 +72,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
 
 	this->parseParameters(parameters);
 }
@@ -175,17 +177,19 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 
-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d kpts, %d descriptors",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d descriptors",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
+			(int)fromSignature.getWordsDescriptors().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
 			fromSignature.sensorData().descriptors().rows);
 
-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d kpts, %d descriptors",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
+			(int)toSignature.getWordsDescriptors().size(),
 			(int)toSignature.sensorData().keypoints().size(),
 			toSignature.sensorData().descriptors().rows);
 
@@ -194,7 +198,9 @@ Transform RegistrationVis::computeTransformationImpl(
 	////////////////////
 	// Find correspondences
 	////////////////////
-	if((_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
+	//recompute correspondences if descriptors are provided
+	if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
+	   (_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
 	   (_estimationType==0 || toSignature.getWords().size()) && // required only for 3D->2D or 2D->2D
 	   fromSignature.getWords3().size() && // required in all estimation approaches
 	   (_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
@@ -209,11 +215,15 @@ Transform RegistrationVis::computeTransformationImpl(
 		UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
 		UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
-				fromSignature.sensorData().descriptors().rows == 0);
+				fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
+				fromSignature.sensorData().descriptors().rows == 0 ||
+				fromSignature.getWordsDescriptors().size() == 0);
 		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
-				toSignature.sensorData().descriptors().rows == 0);
+				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
+				toSignature.sensorData().descriptors().rows == 0 ||
+				toSignature.getWordsDescriptors().size() == 0);
 		UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC3);
 		UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
@@ -232,9 +242,20 @@ Transform RegistrationVis::computeTransformationImpl(
 					fromSignature.sensorData().setImageRaw(tmp);
 				}
 
+				cv::Mat depthMask;
+				if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
+				{
+					if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
+						fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+						fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+					{
+						depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+					}
+				}
+
 				kptsFrom = detector->generateKeypoints(
 						fromSignature.sensorData().imageRaw(),
-						_useDepthAsMask&&!fromSignature.sensorData().depthRaw().empty()?fromSignature.sensorData().depthRaw():cv::Mat());
+						depthMask);
 			}
 			else
 			{
@@ -325,8 +346,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				for(unsigned int i=0; i<status.size(); ++i)
 				{
 					if(status[i] &&
-					   uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().depthOrRightRaw().cols)) &&
-					   uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().depthOrRightRaw().rows)))
+					   uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().imageRaw().cols)) &&
+					   uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().imageRaw().rows)))
 					{
 						kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
 						kptsFrom3DKept[ki] = kptsFrom3D[i];
@@ -389,9 +410,21 @@ Transform RegistrationVis::computeTransformationImpl(
 						cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
 						toSignature.sensorData().setImageRaw(tmp);
 					}
+
+					cv::Mat depthMask;
+					if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
+					{
+						if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
+							fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+							fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+						{
+							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+						}
+					}
+
 					kptsTo = detector->generateKeypoints(
 							toSignature.sensorData().imageRaw(),
-							_useDepthAsMask&&!toSignature.sensorData().depthRaw().empty()?toSignature.sensorData().depthRaw():cv::Mat());
+							depthMask);
 				}
 				else
 				{
@@ -407,22 +440,55 @@ Transform RegistrationVis::computeTransformationImpl(
 			UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
 			UDEBUG("kptsTo=%d", (int)kptsTo.size());
 			cv::Mat descriptorsFrom;
+			if(kptsFrom.size())
+			{
+				if(fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
+				{
+					descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
+							fromSignature.getWordsDescriptors().begin()->second.cols,
+							fromSignature.getWordsDescriptors().begin()->second.type());
+					int i=0;
+					for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
+						iter!=fromSignature.getWordsDescriptors().end();
+						++iter, ++i)
+					{
+						iter->second.copyTo(descriptorsFrom.row(i));
+					}
+				}
+				else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+				{
+					descriptorsFrom = fromSignature.sensorData().descriptors();
+				}
+				else if(!fromSignature.sensorData().imageRaw().empty())
+				{
+					descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
+				}
+			}
+
 			cv::Mat descriptorsTo;
-			if(fromSignature.getWords().empty() && fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+			if(kptsTo.size())
 			{
-				descriptorsFrom = fromSignature.sensorData().descriptors();
-			}
-			else
-			{
-				descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
-			}
-			if(toSignature.getWords().empty() && toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
-			{
-				descriptorsTo = toSignature.sensorData().descriptors();
-			}
-			else if(!toSignature.sensorData().imageRaw().empty())
-			{
-				descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
+				if(toSignature.getWordsDescriptors().size() == (int)kptsTo.size())
+				{
+					descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
+							toSignature.getWordsDescriptors().begin()->second.cols,
+							toSignature.getWordsDescriptors().begin()->second.type());
+					int i=0;
+					for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
+						iter!=toSignature.getWordsDescriptors().end();
+						++iter, ++i)
+					{
+						iter->second.copyTo(descriptorsTo.row(i));
+					}
+				}
+				else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
+				{
+					descriptorsTo = toSignature.sensorData().descriptors();
+				}
+				else if(!toSignature.sensorData().imageRaw().empty())
+				{
+					descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
+				}
 			}
 
 			// create 3D keypoints

corelib/src/Rtabmap.cpp

@@ -83,6 +83,7 @@ Rtabmap::Rtabmap() :
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
 	_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
+	_rawDataKept(Parameters::defaultMemImageKept()),
 	_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
 	_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
 	_statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
@@ -120,7 +121,7 @@ Rtabmap::Rtabmap() :
 	_memory(0),
 	_foutFloat(0),
 	_foutInt(0),
-	_wDir("."),
+	_wDir(""),
 	_mapCorrection(Transform::getIdentity()),
 	_lastLocalizationNodeId(0),
 	_pathStatus(0),
@@ -151,7 +152,7 @@ void Rtabmap::setupLogFiles(bool overwrite)
 		_foutInt = 0;
 	}
 
-	if(_statisticLogged)
+	if(_statisticLogged && !_wDir.empty())
 	{
 		std::string attributes = "a+"; // append to log files
 		if(overwrite)
@@ -234,6 +235,10 @@ void Rtabmap::setupLogFiles(bool overwrite)
 	}
 	else
 	{
+		if(_statisticLogged)
+		{
+			UWARN("Working directory is not set, log disabled!");
+		}
 		UDEBUG("Log disabled!");
 	}
 }
@@ -314,7 +319,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
 
 void Rtabmap::close(bool databaseSaved)
 {
-	UINFO("");
+	UINFO("databaseSaved=%d", databaseSaved?1:0);
 	_highestHypothesis = std::make_pair(0,0.0f);
 	_loopClosureHypothesis = std::make_pair(0,0.0f);
 	_lastProcessTime = 0.0;
@@ -386,6 +391,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
 	Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
+	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
 	Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
@@ -1073,6 +1079,7 @@ bool Rtabmap::process(
 			newPose = _mapCorrection * signature->getPose();
 		}
 
+		UDEBUG("Added pose %s", newPose.prettyPrint().c_str());
 		// Update Poses and Constraints
 		_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
 		_lastLocalizationPose = newPose; // keep in cache the latest corrected pose
@@ -1707,10 +1714,10 @@ bool Rtabmap::process(
 	{
 		//Compute transform if metric data are present
 		Transform transform;
-		float variance = 1.0f;
+		RegistrationInfo info;
+		info.variance = 1.0f;
 		if(_rgbdSlamMode)
 		{
-			RegistrationInfo info;
 			transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
 			loopClosureVisualInliers = info.inliers;
 			rejectedHypothesis = transform.isNull();
@@ -1723,8 +1730,8 @@ bool Rtabmap::process(
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			UASSERT(variance > 0.0);
-			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, variance, variance));
+			UASSERT(info.variance > 0.0);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.variance, info.variance));
 			if(!rejectedHypothesis)
 			{
 				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1798,42 +1805,51 @@ bool Rtabmap::process(
 					iter!=nearestPaths.end() && (_memory->isIncremental() || lastLocalSpaceClosureId == 0);
 					++iter)
 				{
-					const std::map<int, Transform> & path = *iter;
+					std::map<int, Transform> path = *iter;
 					UASSERT(path.size());
-					//find the nearest pose on the path
+
+					//find the nearest pose on the path looking in the same direction
+					path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
+					path = graph::getPosesInRadius(signature->id(), path, _localRadius, M_PI/4);
 					int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
-					UASSERT(nearestId > 0);
-
-					// nearest pose must not be linked to current location and enough
-					if(!signature->hasLink(nearestId) &&
-						(_proximityFilteringRadius <= 0.0f ||
-						 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
+					if(nearestId > 0)
 					{
-						RegistrationInfo info;
-						Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
-						if(!transform.isNull())
+						// nearest pose must not be linked to current location and enough
+						if(!signature->hasLink(nearestId) &&
+							(_proximityFilteringRadius <= 0.0f ||
+							 _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
 						{
-							if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
+							RegistrationInfo info;
+							Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
+							if(!transform.isNull())
 							{
-								UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
-										signature->id(),
-										nearestId,
-										transform.prettyPrint().c_str());
-								UASSERT(info.variance > 0.0);
-								_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
-								loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
-
-								if(_loopClosureHypothesis.first == 0)
+								if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
 								{
-									++localSpaceClosuresAddedVisually;
-									lastLocalSpaceClosureId = nearestId;
+									UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
+											signature->id(),
+											nearestId,
+											transform.prettyPrint().c_str());
+									UASSERT(info.variance > 0.0);
+									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
+									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
+
+									if(loopClosureVisualInliers == 0)
+									{
+										loopClosureVisualInliers = info.inliers;
+									}
+
+									if(_loopClosureHypothesis.first == 0)
+									{
+										++localSpaceClosuresAddedVisually;
+										lastLocalSpaceClosureId = nearestId;
+									}
+								}
+								else
+								{
+									UWARN("Ignoring local loop closure with %d because resulting "
+										  "transform is to large!? (%fm > %fm)",
+											nearestId, transform.getNorm(), _proximityFilteringRadius);
 								}
-							}
-							else
-							{
-								UWARN("Ignoring local loop closure with %d because resulting "
-									  "transform is to large!? (%fm > %fm)",
-										nearestId, transform.getNorm(), _proximityFilteringRadius);
 							}
 						}
 					}
@@ -2283,6 +2299,10 @@ bool Rtabmap::process(
 	{
 		lastSignatureData = *signature;
 	}
+	if(!_rawDataKept)
+	{
+		_memory->removeRawData(signature->id());
+	}
 
 	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
 	int signatureRemoved = _memory->cleanup();
@@ -2453,10 +2473,12 @@ bool Rtabmap::process(
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
 		statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
+		statistics_.addStatistic(Statistics::kMemoryDatabase_memory_used(), _memory->getDatabaseMemoryUsed());
 
 		std::map<int, Signature> signatures;
 		if(_publishLastSignatureData)
 		{
+			UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
 			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
 		}
 		// Set local graph
@@ -2618,6 +2640,11 @@ void Rtabmap::setWorkingDirectory(std::string path)
 			}
 		}
 	}
+	else if(path.empty())
+	{
+		_wDir.clear();
+		setupLogFiles();
+	}
 	else
 	{
 		ULOGGER_ERROR("Directory \"%s\" doesn't exist!", path.c_str());
@@ -2647,7 +2674,14 @@ void Rtabmap::dumpData() const
 	UDEBUG("");
 	if(_memory)
 	{
-		_memory->dumpMemory(this->getWorkingDir());
+		if(this->getWorkingDir().empty())
+		{
+			UERROR("Working directory not set.");
+		}
+		else
+		{
+			_memory->dumpMemory(this->getWorkingDir());
+		}
 	}
 }
 
@@ -3003,6 +3037,11 @@ void Rtabmap::dumpPrediction() const
 {
 	if(_memory && _bayesFilter)
 	{
+		if(this->getWorkingDir().empty())
+		{
+			UERROR("Working directory not set.");
+			return;
+		}
 		std::list<int> signaturesToCompare;
 		for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
 			iter!=_memory->getWorkingMem().end();

corelib/src/RtabmapThread.cpp

@@ -63,11 +63,9 @@ RtabmapThread::~RtabmapThread()
 {
 	UEventsManager::removeHandler(this);
 
-	// Stop the thread first
-	join(true);
+	close(true);
 
 	delete _frameRateTimer;
-	delete _rtabmap;
 }
 
 void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
@@ -116,41 +114,68 @@ void RtabmapThread::setDataBufferSize(unsigned int size)
 
 void RtabmapThread::createIntermediateNodes(bool enabled)
 {
-	enabled = _createIntermediateNodes;
+	_createIntermediateNodes = enabled;
+}
+
+void RtabmapThread::close(bool databaseSaved)
+{
+	this->join(true);
+	if(_rtabmap)
+	{
+		_rtabmap->close(databaseSaved);
+		delete _rtabmap;
+		_rtabmap = 0;
+	}
 }
 
 void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
 {
-	std::map<int, Signature> signatures;
-	std::map<int, Transform> poses;
-	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-
-	if(graphOnly)
+	if(_rtabmap)
 	{
-		_rtabmap->getGraph(poses,
-				constraints,
-				optimized,
-				full,
-				&signatures);
+		std::map<int, Signature> signatures;
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+		std::map<int, int> mapIds;
+		std::map<int, double> stamps;
+		std::map<int, std::string> labels;
+		std::map<int, std::vector<unsigned char> > userDatas;
+
+		if(graphOnly)
+		{
+			_rtabmap->getGraph(poses,
+					constraints,
+					optimized,
+					full,
+					&signatures);
+		}
+		else
+		{
+			_rtabmap->get3DMap(
+					signatures,
+					poses,
+					constraints,
+					optimized,
+					full);
+		}
+
+		this->post(new RtabmapEvent3DMap(
+				signatures,
+				poses,
+				constraints));
 	}
 	else
 	{
-		_rtabmap->get3DMap(
-				signatures,
-				poses,
-				constraints,
-				optimized,
-				full);
+		UERROR("Rtabmap is null!");
 	}
+}
 
-	this->post(new RtabmapEvent3DMap(
-			signatures,
-			poses,
-			constraints));
+void RtabmapThread::mainLoopBegin()
+{
+	if(_rtabmap == 0)
+	{
+		UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");
+		this->kill();
+	}
 }
 
 void RtabmapThread::mainLoopKill()
@@ -297,177 +322,180 @@ void RtabmapThread::mainLoop()
 
 void RtabmapThread::handleEvent(UEvent* event)
 {
-	if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
+	if(this->isRunning())
 	{
-		UDEBUG("CameraEvent");
-		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeData)
+		if(event->getClassName().compare("CameraEvent") == 0)
 		{
-			this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
-		}
-	}
-	else if(event->getClassName().compare("OdometryEvent") == 0)
-	{
-		UDEBUG("OdometryEvent");
-		OdometryEvent * e = (OdometryEvent*)event;
-		if(!e->pose().isNull())
-		{
-			this->addData(*e);
-		}
-		else
-		{
-			lastPose_.setNull();
-		}
-	}
-	else if(event->getClassName().compare("UserDataEvent") == 0)
-	{
-		if(!_paused)
-		{
-			UDEBUG("UserDataEvent");
-			bool updated = false;
-			UserDataEvent * e = (UserDataEvent*)event;
-			_userDataMutex.lock();
-			if(!e->data().empty())
+			UDEBUG("CameraEvent");
+			CameraEvent * e = (CameraEvent*)event;
+			if(e->getCode() == CameraEvent::kCodeData)
 			{
-				updated = !_userData.empty();
-				_userData = e->data();
+				this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
 			}
-			_userDataMutex.unlock();
-			if(updated)
+		}
+		else if(event->getClassName().compare("OdometryEvent") == 0)
+		{
+			UDEBUG("OdometryEvent");
+			OdometryEvent * e = (OdometryEvent*)event;
+			if(!e->pose().isNull())
 			{
-				UWARN("New user data received before the last one was processed... replacing "
-					"user data with this new one. Note that UserDataEvent should be used only "
-					"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
+				this->addData(*e);
 			}
 			else
 			{
-				pushNewState(kStateAddingUserData);
+				lastPose_.setNull();
 			}
 		}
-	}
-	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
-	{
-		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
-		RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
-		if(cmd == RtabmapEventCmd::kCmdInit)
+		else if(event->getClassName().compare("UserDataEvent") == 0)
 		{
-			ULOGGER_DEBUG("CMD_INIT");
-			ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
-			UASSERT(rtabmapEvent->value1().isStr());
-			UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
-			pushNewState(kStateInit, parameters);
+			if(!_paused)
+			{
+				UDEBUG("UserDataEvent");
+				bool updated = false;
+				UserDataEvent * e = (UserDataEvent*)event;
+				_userDataMutex.lock();
+				if(!e->data().empty())
+				{
+					updated = !_userData.empty();
+					_userData = e->data();
+				}
+				_userDataMutex.unlock();
+				if(updated)
+				{
+					UWARN("New user data received before the last one was processed... replacing "
+						"user data with this new one. Note that UserDataEvent should be used only "
+						"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
+				}
+				else
+				{
+					pushNewState(kStateAddingUserData);
+				}
+			}
 		}
-		else if(cmd == RtabmapEventCmd::kCmdClose)
+		else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 		{
-			ULOGGER_DEBUG("CMD_CLOSE");
-			UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
-			ParametersMap param;
-			param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
-			pushNewState(kStateClose, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdResetMemory)
-		{
-			ULOGGER_DEBUG("CMD_RESET_MEMORY");
-			pushNewState(kStateReseting);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
-		{
-			ULOGGER_DEBUG("CMD_DUMP_MEMORY");
-			pushNewState(kStateDumpingMemory);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
-		{
-			ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
-			pushNewState(kStateDumpingPrediction);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
-		{
-			ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
-			UASSERT(rtabmapEvent->value1().isBool());
-			UASSERT(rtabmapEvent->value2().isStr());
-			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
-			UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-			ParametersMap param;
-			param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
-			param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
-			param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
-			pushNewState(kStateExportingDOTGraph, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdExportPoses)
-		{
-			ULOGGER_DEBUG("CMD_EXPORT_POSES");
-			UASSERT(rtabmapEvent->value1().isBool());
-			UASSERT(rtabmapEvent->value2().isBool());
-			UASSERT(rtabmapEvent->value3().isStr());
-			UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
-			ParametersMap param;
-			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-			param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
-			param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
-			param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
-			pushNewState(kStateExportingPoses, param);
+			RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
+			RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
+			if(cmd == RtabmapEventCmd::kCmdInit)
+			{
+				ULOGGER_DEBUG("CMD_INIT");
+				ParametersMap parameters = ((RtabmapEventCmd*)event)->getParameters();
+				UASSERT(rtabmapEvent->value1().isStr());
+				UASSERT(parameters.insert(ParametersPair("RtabmapThread/DatabasePath", rtabmapEvent->value1().toStr())).second);
+				pushNewState(kStateInit, parameters);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdClose)
+			{
+				ULOGGER_DEBUG("CMD_CLOSE");
+				UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
+				ParametersMap param;
+				param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
+				pushNewState(kStateClose, param);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdResetMemory)
+			{
+				ULOGGER_DEBUG("CMD_RESET_MEMORY");
+				pushNewState(kStateReseting);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
+			{
+				ULOGGER_DEBUG("CMD_DUMP_MEMORY");
+				pushNewState(kStateDumpingMemory);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
+			{
+				ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
+				pushNewState(kStateDumpingPrediction);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdGenerateDOTGraph)
+			{
+				ULOGGER_DEBUG("CMD_GENERATE_DOT_GRAPH");
+				UASSERT(rtabmapEvent->value1().isBool());
+				UASSERT(rtabmapEvent->value2().isStr());
+				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value3().isInt() || rtabmapEvent->value3().isUInt());
+				UASSERT(rtabmapEvent->value1().toBool() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
+				ParametersMap param;
+				param.insert(ParametersPair("path", rtabmapEvent->value2().toStr()));
+				param.insert(ParametersPair("id", !rtabmapEvent->value1().toBool()?rtabmapEvent->value3().toStr():"0"));
+				param.insert(ParametersPair("margin", !rtabmapEvent->value1().toBool()?rtabmapEvent->value4().toStr():"0"));
+				pushNewState(kStateExportingDOTGraph, param);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdExportPoses)
+			{
+				ULOGGER_DEBUG("CMD_EXPORT_POSES");
+				UASSERT(rtabmapEvent->value1().isBool());
+				UASSERT(rtabmapEvent->value2().isBool());
+				UASSERT(rtabmapEvent->value3().isStr());
+				UASSERT(rtabmapEvent->value4().isUndef() || rtabmapEvent->value4().isInt() || rtabmapEvent->value4().isUInt());
+				ParametersMap param;
+				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+				param.insert(ParametersPair("optimized", rtabmapEvent->value1().toStr()));
+				param.insert(ParametersPair("path", rtabmapEvent->value3().toStr()));
+				param.insert(ParametersPair("type", rtabmapEvent->value4().isInt()?rtabmapEvent->value4().toStr():"0"));
+				pushNewState(kStateExportingPoses, param);
 
+			}
+			else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
+			{
+				ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
+				pushNewState(kStateCleanDataBuffer);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
+			{
+				ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+				UASSERT(rtabmapEvent->value1().isBool());
+				UASSERT(rtabmapEvent->value2().isBool());
+				UASSERT(rtabmapEvent->value3().isBool());
+				ParametersMap param;
+				param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
+				param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
+				param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
+				pushNewState(kStatePublishingMap, param);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
+			{
+				ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
+				pushNewState(kStateTriggeringMap);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdPause)
+			{
+				ULOGGER_DEBUG("CMD_PAUSE");
+				_paused = !_paused;
+			}
+			else if(cmd == RtabmapEventCmd::kCmdGoal)
+			{
+				ULOGGER_DEBUG("CMD_GOAL");
+				UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
+				ParametersMap param;
+				param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
+				param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
+				pushNewState(kStateSettingGoal, param);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
+			{
+				ULOGGER_DEBUG("CMD_CANCEL_GOAL");
+				pushNewState(kStateCancellingGoal);
+			}
+			else if(cmd == RtabmapEventCmd::kCmdLabel)
+			{
+				ULOGGER_DEBUG("CMD_LABEL");
+				UASSERT(rtabmapEvent->value1().isStr());
+				UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
+				ParametersMap param;
+				param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
+				param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
+				pushNewState(kStateLabelling, param);
+			}
+			else
+			{
+				UWARN("Cmd %d unknown!", cmd);
+			}
 		}
-		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
+		else if(event->getClassName().compare("ParamEvent") == 0)
 		{
-			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
-			pushNewState(kStateCleanDataBuffer);
+			ULOGGER_DEBUG("changing parameters");
+			pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
 		}
-		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
-		{
-			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
-			UASSERT(rtabmapEvent->value1().isBool());
-			UASSERT(rtabmapEvent->value2().isBool());
-			UASSERT(rtabmapEvent->value3().isBool());
-			ParametersMap param;
-			param.insert(ParametersPair("global", rtabmapEvent->value1().toStr()));
-			param.insert(ParametersPair("optimized", rtabmapEvent->value2().toStr()));
-			param.insert(ParametersPair("graph_only", rtabmapEvent->value3().toStr()));
-			pushNewState(kStatePublishingMap, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
-		{
-			ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
-			pushNewState(kStateTriggeringMap);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdPause)
-		{
-			ULOGGER_DEBUG("CMD_PAUSE");
-			_paused = !_paused;
-		}
-		else if(cmd == RtabmapEventCmd::kCmdGoal)
-		{
-			ULOGGER_DEBUG("CMD_GOAL");
-			UASSERT(rtabmapEvent->value1().isStr() || rtabmapEvent->value1().isInt() || rtabmapEvent->value1().isUInt());
-			ParametersMap param;
-			param.insert(ParametersPair("label", rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():""));
-			param.insert(ParametersPair("id", !rtabmapEvent->value1().isStr()?rtabmapEvent->value1().toStr():"0"));
-			pushNewState(kStateSettingGoal, param);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdCancelGoal)
-		{
-			ULOGGER_DEBUG("CMD_CANCEL_GOAL");
-			pushNewState(kStateCancellingGoal);
-		}
-		else if(cmd == RtabmapEventCmd::kCmdLabel)
-		{
-			ULOGGER_DEBUG("CMD_LABEL");
-			UASSERT(rtabmapEvent->value1().isStr());
-			UASSERT(rtabmapEvent->value2().isUndef() || rtabmapEvent->value2().isInt() || rtabmapEvent->value2().isUInt());
-			ParametersMap param;
-			param.insert(ParametersPair("label", rtabmapEvent->value1().toStr()));
-			param.insert(ParametersPair("id", rtabmapEvent->value2().isUndef()?"0":rtabmapEvent->value2().toStr()));
-			pushNewState(kStateLabelling, param);
-		}
-		else
-		{
-			UWARN("Cmd %d unknown!", cmd);
-		}
-	}
-	else if(event->getClassName().compare("ParamEvent") == 0)
-	{
-		ULOGGER_DEBUG("changing parameters");
-		pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
 	}
 }
 

corelib/src/SensorData.cpp

@@ -481,10 +481,11 @@ void SensorData::setUserData(const cv::Mat & userData)
 
 void SensorData::uncompressData()
 {
-	uncompressData(_imageCompressed.empty()?0:&_imageRaw,
-				_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
-				_laserScanCompressed.empty()?0:&_laserScanRaw,
-				_userDataCompressed.empty()?0:&_userDataRaw);
+	cv::Mat tmpA, tmpB, tmpC, tmpD;
+	uncompressData(_imageCompressed.empty()?0:&tmpA,
+				_depthOrRightCompressed.empty()?0:&tmpB,
+				_laserScanCompressed.empty()?0:&tmpC,
+				_userDataCompressed.empty()?0:&tmpD);
 }
 
 void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
@@ -493,6 +494,18 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
 		_imageRaw = *imageRaw;
+		//backward compatibility, set image size in camera model if not set
+		if(!_imageRaw.empty() && _cameraModels.size())
+		{
+			cv::Size size(_imageRaw.cols/_cameraModels.size(), _imageRaw.rows/_cameraModels.size());
+			for(unsigned int i=0; i<_cameraModels.size(); ++i)
+			{
+				if(_cameraModels[i].isValidForProjection() && _cameraModels[i].imageWidth() == 0)
+				{
+					_cameraModels[i].setImageSize(size);
+				}
+			}
+		}
 	}
 	if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
 	{

corelib/src/Signature.cpp

@@ -195,8 +195,10 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	if(kps.size())
 	{
 		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
+		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
 		_words.erase(oldWordId);
 		_words3.erase(oldWordId);
+		_wordsDescriptors.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
 		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
 		{
@@ -206,6 +208,10 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 		{
 			_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
 		}
+		for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
+		{
+			_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
+		}
 	}
 }
 
@@ -218,12 +224,14 @@ void Signature::removeAllWords()
 {
 	_words.clear();
 	_words3.clear();
+	_wordsDescriptors.clear();
 }
 
 void Signature::removeWord(int wordId)
 {
 	_words.erase(wordId);
 	_words3.erase(wordId);
+	_wordsDescriptors.clear();
 }
 
 cv::Mat Signature::getPoseCovariance() const

corelib/src/VWDictionary.cpp

@@ -79,7 +79,14 @@ public:
 			}
 			else
 			{
-				delete (rtflann::Index<rtflann::L2<float> >*)index_;
+				if(useDistanceL1_)
+				{
+					delete (rtflann::Index<rtflann::L1<float> >*)index_;
+				}
+				else
+				{
+					delete (rtflann::Index<rtflann::L2<float> >*)index_;
+				}
 			}
 			index_ = 0;
 		}
@@ -101,7 +108,14 @@ public:
 		}
 		else
 		{
-			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
+			if(useDistanceL1_)
+			{
+				return ((const rtflann::Index<rtflann::L1<float> >*)index_)->size();
+			}
+			else
+			{
+				return ((const rtflann::Index<rtflann::L2<float> >*)index_)->size();
+			}
 		}
 	}
 
@@ -118,19 +132,29 @@ public:
 		}
 		else
 		{
-			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+			if(useDistanceL1_)
+			{
+				return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
+			}
+			else
+			{
+				return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
+			}
 		}
 	}
 
+	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void build(
 			const cv::Mat & features,
-			const rtflann::IndexParams& params)
+			const rtflann::IndexParams& params,
+			bool useDistanceL1)
 	{
 		this->release();
 		UASSERT(index_ == 0);
 		UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
 		featuresType_ = features.type();
 		featuresDim_ = features.cols;
+		useDistanceL1_ = useDistanceL1;
 
 		if(featuresType_ == CV_8UC1)
 		{
@@ -141,8 +165,16 @@ public:
 		else
 		{
 			rtflann::Matrix<float> dataset((float*)features.data, features.rows, features.cols);
-			index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
-			((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+			if(useDistanceL1_)
+			{
+				index_ = new rtflann::Index<rtflann::L1<float> >(dataset, params);
+				((rtflann::Index<rtflann::L1<float> >*)index_)->buildIndex();
+			}
+			else
+			{
+				index_ = new rtflann::Index<rtflann::L2<float> >(dataset, params);
+				((rtflann::Index<rtflann::L2<float> >*)index_)->buildIndex();
+			}
 		}
 
 		if(features.rows == 1)
@@ -193,18 +225,37 @@ public:
 		else
 		{
 			rtflann::Matrix<float> point((float*)feature.data, feature.rows, feature.cols);
-			rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
-			index->addPoints(point, 0);
-			// Rebuild index if it doubles in size
-			if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+			if(useDistanceL1_)
 			{
-				// clean not used features
-				for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+				rtflann::Index<rtflann::L1<float> > * index = (rtflann::Index<rtflann::L1<float> >*)index_;
+				index->addPoints(point, 0);
+				// Rebuild index if it doubles in size
+				if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
 				{
-					addedDescriptors_.erase(*iter);
+					// clean not used features
+					for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+					{
+						addedDescriptors_.erase(*iter);
+					}
+					removedIndexes_.clear();
+					index->buildIndex();
+				}
+			}
+			else
+			{
+				rtflann::Index<rtflann::L2<float> > * index = (rtflann::Index<rtflann::L2<float> >*)index_;
+				index->addPoints(point, 0);
+				// Rebuild index if it doubles in size
+				if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
+				{
+					// clean not used features
+					for(std::list<int>::iterator iter=removedIndexes_.begin(); iter!=removedIndexes_.end(); ++iter)
+					{
+						addedDescriptors_.erase(*iter);
+					}
+					removedIndexes_.clear();
+					index->buildIndex();
 				}
-				removedIndexes_.clear();
-				index->buildIndex();
 			}
 		}
 
@@ -230,10 +281,15 @@ public:
 		{
 			((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->removePoint(index);
 		}
+		else if(useDistanceL1_)
+		{
+			((rtflann::Index<rtflann::L1<float> >*)index_)->removePoint(index);
+		}
 		else
 		{
 			((rtflann::Index<rtflann::L2<float> >*)index_)->removePoint(index);
 		}
+
 		removedIndexes_.push_back(index);
 	}
 
@@ -264,7 +320,14 @@ public:
 		{
 			rtflann::Matrix<float> distsF((float*)dists.data, dists.rows, dists.cols);
 			rtflann::Matrix<float> queryF((float*)query.data, query.rows, query.cols);
-			((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+			if(useDistanceL1_)
+			{
+				((rtflann::Index<rtflann::L1<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+			}
+			else
+			{
+				((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
+			}
 		}
 	}
 
@@ -274,6 +337,7 @@ private:
 	int featuresType_;
 	int featuresDim_;
 	bool isLSH_;
+	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
 
 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)
@@ -292,6 +356,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_dictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
 	_lastWordId(0),
+	useDistanceL1_(false),
 	_flannIndex(new FlannIndex()),
 	_strategy(kNNBruteForce)
 {
@@ -491,23 +556,14 @@ void VWDictionary::setNNStrategy(NNStrategy strategy)
 #endif
 #endif
 
-		if(RTABMAP_NONFREE == 0 && strategy == kNNFlannKdTree)
+		bool update = _strategy != strategy;
+		_strategy = strategy;
+		if(update)
 		{
-			UWARN("KdTree (%d) nearest neighbor is not available because RTAB-Map isn't built "
-				  "with OpenCV nonfree module (KdTree only used for SURF/SIFT features). "
-				  "NN strategy is not modified (current=%d).", (int)kNNFlannKdTree, (int)_strategy);
-		}
-		else
-		{
-			bool update = _strategy != strategy;
-			_strategy = strategy;
-			if(update)
-			{
-				_dataTree = cv::Mat();
-				_notIndexedWords = uKeysSet(_visualWords);
-				_removedIndexedWords.clear();
-				this->update();
-			}
+			_dataTree = cv::Mat();
+			_notIndexedWords = uKeysSet(_visualWords);
+			_removedIndexedWords.clear();
+			this->update();
 		}
 	}
 }
@@ -576,15 +632,15 @@ void VWDictionary::update()
 						switch(_strategy)
 						{
 						case kNNFlannNaive:
-							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams());
+							_flannIndex->build(w->getDescriptor(), rtflann::LinearIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannKdTree:
 							UASSERT_MSG(w->getDescriptor().type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams());
+							_flannIndex->build(w->getDescriptor(), rtflann::KDTreeIndexParams(), useDistanceL1_);
 							break;
 						case kNNFlannLSH:
 							UASSERT_MSG(w->getDescriptor().type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2));
+							_flannIndex->build(w->getDescriptor(), rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
 							break;
 						default:
 							UFATAL("Not supposed to be here!");
@@ -666,15 +722,15 @@ void VWDictionary::update()
 				switch(_strategy)
 				{
 				case kNNFlannNaive:
-					_flannIndex->build(_dataTree, rtflann::LinearIndexParams());
+					_flannIndex->build(_dataTree, rtflann::LinearIndexParams(), useDistanceL1_);
 					break;
 				case kNNFlannKdTree:
 					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-					_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams());
+					_flannIndex->build(_dataTree, rtflann::KDTreeIndexParams(), useDistanceL1_);
 					break;
 				case kNNFlannLSH:
 					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-					_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2));
+					_flannIndex->build(_dataTree, rtflann::LshIndexParams(12, 20, 2), useDistanceL1_);
 					break;
 				default:
 					break;
@@ -723,6 +779,7 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
 	_mapIdIndex.clear();
 	_unusedWords.clear();
 	_flannIndex->release();
+	useDistanceL1_ = false;
 }
 
 int VWDictionary::getNextId()
@@ -764,11 +821,29 @@ void VWDictionary::removeAllWordRef(int wordId, int signatureId)
 	}
 }
 
-std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
+std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptorsIn,
 							   int signatureId)
 {
 	UASSERT(signatureId > 0);
 
+	cv::Mat descriptors;
+	if(descriptorsIn.type() == CV_8U)
+	{
+		useDistanceL1_ = true;
+		if(_strategy == kNNFlannKdTree || _strategy == kNNFlannNaive)
+		{
+			descriptorsIn.convertTo(descriptors, CV_32F);
+		}
+		else
+		{
+			descriptors = descriptorsIn;
+		}
+	}
+	else
+	{
+		descriptors = descriptorsIn;
+	}
+
 	UDEBUG("id=%d descriptors=%d", signatureId, descriptors.rows);
 	UTimer timer;
 	std::list<int> wordIds;
@@ -1313,6 +1388,18 @@ void VWDictionary::deleteUnusedWords()
 
 void VWDictionary::exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const
 {
+	if(_visualWords.empty())
+	{
+		UWARN("Dictionary is empty, cannot export it!");
+		return;
+	}
+
+	if(_visualWords.at(0)->getDescriptor().type() != CV_32FC1)
+	{
+		UERROR("Exporting binary descriptors is not implemented!");
+		return;
+	}
+
     FILE* foutRef = 0;
     FILE* foutDesc = 0;
 #ifdef _MSC_VER

corelib/src/pcl18/readme.txt

@@ -0,0 +1,2 @@
+Copied pcl 1.8 functions to be used in older pcl versions. They are 
+not included if 1.8 is detected (the originals are used directly).

corelib/src/pcl18/surface/impl/organized_fast_mesh.hpp

@@ -0,0 +1,284 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2011, Dirk Holz (University of Bonn)
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
+#define PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_
+
+#include <pcl18/surface/organized_fast_mesh.h>
+#include <rtabmap/utilite/ULogger.h>
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
+{
+  reconstructPolygons (output.polygons);
+
+  // Get the field names
+  int x_idx = pcl::getFieldIndex (output.cloud, "x");
+  int y_idx = pcl::getFieldIndex (output.cloud, "y");
+  int z_idx = pcl::getFieldIndex (output.cloud, "z");
+  if (x_idx == -1 || y_idx == -1 || z_idx == -1)
+    return;
+  // correct all measurements,
+  // (running over complete image since some rows and columns are left out
+  // depending on triangle_pixel_size)
+  // avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
+  for (unsigned int i = 0; i < input_->points.size (); ++i)
+    if (!isFinite (input_->points[i]))
+      resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
+{
+  reconstructPolygons (polygons);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
+{
+  if (triangulation_type_ == TRIANGLE_RIGHT_CUT)
+    makeRightCutMesh (polygons);
+  else if (triangulation_type_ == TRIANGLE_LEFT_CUT)
+    makeLeftCutMesh (polygons);
+  else if (triangulation_type_ == TRIANGLE_ADAPTIVE_CUT)
+    makeAdaptiveCutMesh (polygons);
+  else if (triangulation_type_ == QUAD_MESH)
+    makeQuadMesh (polygons);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::makeQuadMesh (std::vector<pcl::Vertices>& polygons)
+{
+  int last_column = input_->width - triangle_pixel_size_columns_;
+  int last_row = input_->height - triangle_pixel_size_rows_;
+
+  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
+  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
+      x_big_incr = y_big_incr + triangle_pixel_size_columns_;
+  // Reserve enough space
+  polygons.resize (input_->width * input_->height);
+
+  // Go over the rows first
+  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
+  {
+    // Initialize a new row
+    i = y * input_->width;
+    index_right = i + triangle_pixel_size_columns_;
+    index_down = i + y_big_incr;
+    index_down_right = i + x_big_incr;
+
+    // Go over the columns
+    for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
+                                     i += triangle_pixel_size_columns_,
+                                     index_right += triangle_pixel_size_columns_,
+                                     index_down += triangle_pixel_size_columns_,
+                                     index_down_right += triangle_pixel_size_columns_)
+    {
+      if (isValidQuad (i, index_right, index_down_right, index_down))
+        if (store_shadowed_faces_ || !isShadowedQuad (i, index_right, index_down_right, index_down))
+          addQuad (i, index_right, index_down_right, index_down, idx++, polygons);
+    }
+  }
+  polygons.resize (idx);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::makeRightCutMesh (std::vector<pcl::Vertices>& polygons)
+{
+  int last_column = input_->width - triangle_pixel_size_columns_;
+  int last_row = input_->height - triangle_pixel_size_rows_;
+
+  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
+  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
+      x_big_incr = y_big_incr + triangle_pixel_size_columns_;
+  // Reserve enough space
+  polygons.resize (input_->width * input_->height * 2);
+
+  // Go over the rows first
+  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
+  {
+    // Initialize a new row
+    i = y * input_->width;
+    index_right = i + triangle_pixel_size_columns_;
+    index_down = i + y_big_incr;
+    index_down_right = i + x_big_incr;
+
+    // Go over the columns
+    for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
+                                     i += triangle_pixel_size_columns_,
+                                     index_right += triangle_pixel_size_columns_,
+                                     index_down += triangle_pixel_size_columns_,
+                                     index_down_right += triangle_pixel_size_columns_)
+    {
+      if (isValidTriangle (i, index_down_right, index_right))
+        if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
+          addTriangle (i, index_down_right, index_right, idx++, polygons);
+
+      if (isValidTriangle (i, index_down, index_down_right))
+        if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
+          addTriangle (i, index_down, index_down_right, idx++, polygons);
+    }
+  }
+  polygons.resize (idx);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::makeLeftCutMesh (std::vector<pcl::Vertices>& polygons)
+{
+  int last_column = input_->width - triangle_pixel_size_columns_;
+  int last_row = input_->height - triangle_pixel_size_rows_;
+
+  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
+  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
+      x_big_incr = y_big_incr + triangle_pixel_size_columns_;
+  // Reserve enough space
+  polygons.resize (input_->width * input_->height * 2);
+
+  // Go over the rows first
+  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
+  {
+    // Initialize a new row
+    i = y * input_->width;
+    index_right = i + triangle_pixel_size_columns_;
+    index_down = i + y_big_incr;
+    index_down_right = i + x_big_incr;
+
+    // Go over the columns
+    for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
+                                     i += triangle_pixel_size_columns_,
+                                     index_right += triangle_pixel_size_columns_,
+                                     index_down += triangle_pixel_size_columns_,
+                                     index_down_right += triangle_pixel_size_columns_)
+    {
+      if (isValidTriangle (i, index_down, index_right))
+        if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
+          addTriangle (i, index_down, index_right, idx++, polygons);
+
+      if (isValidTriangle (index_right, index_down, index_down_right))
+        if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
+          addTriangle (index_right, index_down, index_down_right, idx++, polygons);
+    }
+  }
+  polygons.resize (idx);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointInT> void
+pcl::OrganizedFastMesh<PointInT>::makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons)
+{
+  int last_column = input_->width - triangle_pixel_size_columns_;
+  int last_row = input_->height - triangle_pixel_size_rows_;
+
+  int i = 0, index_down = 0, index_right = 0, index_down_right = 0, idx = 0;
+  int y_big_incr = triangle_pixel_size_rows_ * input_->width,
+      x_big_incr = y_big_incr + triangle_pixel_size_columns_;
+  // Reserve enough space
+  polygons.resize (input_->width * input_->height * 2);
+
+  // Go over the rows first
+  for (int y = 0; y < last_row; y += triangle_pixel_size_rows_)
+  {
+    // Initialize a new row
+    i = y * input_->width;
+    index_right = i + triangle_pixel_size_columns_;
+    index_down = i + y_big_incr;
+    index_down_right = i + x_big_incr;
+
+    // Go over the columns
+    for (int x = 0; x < last_column; x += triangle_pixel_size_columns_,
+                                     i += triangle_pixel_size_columns_,
+                                     index_right += triangle_pixel_size_columns_,
+                                     index_down += triangle_pixel_size_columns_,
+                                     index_down_right += triangle_pixel_size_columns_)
+    {
+      const bool right_cut_upper = isValidTriangle (i, index_down_right, index_right);
+      const bool right_cut_lower = isValidTriangle (i, index_down, index_down_right);
+      const bool left_cut_upper = isValidTriangle (i, index_down, index_right);
+      const bool left_cut_lower = isValidTriangle (index_right, index_down, index_down_right);
+
+      if (right_cut_upper && right_cut_lower && left_cut_upper && left_cut_lower)
+      {
+        float dist_right_cut = fabsf (input_->points[index_down].z - input_->points[index_right].z);
+        float dist_left_cut = fabsf (input_->points[i].z - input_->points[index_down_right].z);
+        if (dist_right_cut >= dist_left_cut)
+        {
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
+            addTriangle (i, index_down_right, index_right, idx++, polygons);
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
+            addTriangle (i, index_down, index_down_right, idx++, polygons);
+        }
+        else
+        {
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
+            addTriangle (i, index_down, index_right, idx++, polygons);
+          if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
+            addTriangle (index_right, index_down, index_down_right, idx++, polygons);
+        }
+      }
+      else
+      {
+        if (right_cut_upper)
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down_right, index_right))
+            addTriangle (i, index_down_right, index_right, idx++, polygons);
+        if (right_cut_lower)
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_down_right))
+            addTriangle (i, index_down, index_down_right, idx++, polygons);
+        if (left_cut_upper)
+          if (store_shadowed_faces_ || !isShadowedTriangle (i, index_down, index_right))
+            addTriangle (i, index_down, index_right, idx++, polygons);
+        if (left_cut_lower)
+          if (store_shadowed_faces_ || !isShadowedTriangle (index_right, index_down, index_down_right))
+            addTriangle (index_right, index_down, index_down_right, idx++, polygons);
+      }
+    }
+  }
+  polygons.resize (idx);
+}
+
+#define PCL_INSTANTIATE_OrganizedFastMesh(T)                \
+  template class PCL_EXPORTS pcl::OrganizedFastMesh<T>;
+
+#endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_HPP_

corelib/src/pcl18/surface/organized_fast_mesh.h

@@ -0,0 +1,490 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2011, Dirk Holz, University of Bonn.
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_SURFACE_ORGANIZED_FAST_MESH_H_
+#define PCL_SURFACE_ORGANIZED_FAST_MESH_H_
+
+#include <pcl/common/angles.h>
+#include <pcl/surface/reconstruction.h>
+
+namespace pcl
+{
+
+  /** \brief Simple triangulation/surface reconstruction for organized point
+    * clouds. Neighboring points (pixels in image space) are connected to
+    * construct a triangular (or quad) mesh.
+    *
+    * \note If you use this code in any academic work, please cite:
+    *   D. Holz and S. Behnke.
+    *   Fast Range Image Segmentation and Smoothing using Approximate Surface Reconstruction and Region Growing.
+    *   In Proceedings of the 12th International Conference on Intelligent Autonomous Systems (IAS),
+    *   Jeju Island, Korea, June 26-29 2012.
+    *   <a href="http://purl.org/holz/papers/holz_2012_ias.pdf">http://purl.org/holz/papers/holz_2012_ias.pdf</a>
+    *
+    * \author Dirk Holz, Radu B. Rusu
+    * \ingroup surface
+    */
+  template <typename PointInT>
+  class OrganizedFastMesh : public pcl::MeshConstruction<PointInT>
+  {
+    public:
+      typedef boost::shared_ptr<OrganizedFastMesh<PointInT> > Ptr;
+      typedef boost::shared_ptr<const OrganizedFastMesh<PointInT> > ConstPtr;
+
+      using pcl::MeshConstruction<PointInT>::input_;
+      using pcl::MeshConstruction<PointInT>::check_tree_;
+
+      typedef typename pcl::PointCloud<PointInT>::Ptr PointCloudPtr;
+
+      typedef std::vector<pcl::Vertices> Polygons;
+
+      enum TriangulationType
+      {
+        TRIANGLE_RIGHT_CUT,     // _always_ "cuts" a quad from top left to bottom right
+        TRIANGLE_LEFT_CUT,      // _always_ "cuts" a quad from top right to bottom left
+        TRIANGLE_ADAPTIVE_CUT,  // "cuts" where possible and prefers larger differences in 'z' direction
+        QUAD_MESH               // create a simple quad mesh
+      };
+
+      /** \brief Constructor. Triangulation type defaults to \a QUAD_MESH. */
+      OrganizedFastMesh ()
+      : max_edge_length_a_ (0.0f)
+      , max_edge_length_b_ (0.0f)
+      , max_edge_length_c_ (0.0f)
+      , max_edge_length_set_ (false)
+      , max_edge_length_dist_dependent_ (false)
+      , triangle_pixel_size_rows_ (1)
+      , triangle_pixel_size_columns_ (1)
+      , triangulation_type_ (QUAD_MESH)
+      , viewpoint_ (Eigen::Vector3f::Zero ())
+      , store_shadowed_faces_ (false)
+      , cos_angle_tolerance_ (fabsf (cosf (pcl::deg2rad (12.5f))))
+      , distance_tolerance_ (-1.0f)
+      , distance_dependent_ (false)
+      , use_depth_as_distance_(false)
+      {
+        check_tree_ = false;
+      };
+
+      /** \brief Destructor. */
+      virtual ~OrganizedFastMesh () {};
+
+      /** \brief Set a maximum edge length. 
+        * Using not only the scalar \a a, but also \a b and \a c, allows for using a distance threshold in the form of:
+        * threshold(x) = c*x*x + b*x + a 
+        * \param[in] a scalar coefficient of the (distance-dependent polynom) threshold
+        * \param[in] b linear coefficient of the (distance-dependent polynom) threshold
+        * \param[in] c quadratic coefficient of the (distance-dependent polynom) threshold
+        */
+      inline void
+      setMaxEdgeLength (float a, float b = 0.0f, float c = 0.0f)
+      {
+        max_edge_length_a_ = a;
+        max_edge_length_b_ = b;
+        max_edge_length_c_ = c;
+        if ((max_edge_length_a_ + max_edge_length_b_ + max_edge_length_c_) > std::numeric_limits<float>::min())
+          max_edge_length_set_ = true;
+        else
+          max_edge_length_set_ = false;
+      };
+
+      inline void
+      unsetMaxEdgeLength ()
+      {
+        max_edge_length_set_  = false;
+      }
+
+      /** \brief Set the edge length (in pixels) used for constructing the fixed mesh.
+        * \param[in] triangle_size edge length in pixels
+        * (Default: 1 = neighboring pixels are connected)
+        */
+      inline void
+      setTrianglePixelSize (int triangle_size)
+      {
+        setTrianglePixelSizeRows (triangle_size);
+        setTrianglePixelSizeColumns (triangle_size);
+      }
+
+      /** \brief Set the edge length (in pixels) used for iterating over rows when constructing the fixed mesh.
+        * \param[in] triangle_size edge length in pixels
+        * (Default: 1 = neighboring pixels are connected)
+        */
+      inline void
+      setTrianglePixelSizeRows (int triangle_size)
+      {
+        triangle_pixel_size_rows_ = std::max (1, (triangle_size - 1));
+      }
+
+      /** \brief Set the edge length (in pixels) used for iterating over columns when constructing the fixed mesh.
+        * \param[in] triangle_size edge length in pixels
+        * (Default: 1 = neighboring pixels are connected)
+        */
+      inline void
+      setTrianglePixelSizeColumns (int triangle_size)
+      {
+        triangle_pixel_size_columns_ = std::max (1, (triangle_size - 1));
+      }
+
+      /** \brief Set the triangulation type (see \a TriangulationType)
+        * \param[in] type quad mesh, triangle mesh with fixed left, right cut,
+        * or adaptive cut (splits a quad w.r.t. the depth (z) of the points)
+        */
+      inline void
+      setTriangulationType (TriangulationType type)
+      {
+        triangulation_type_ = type;
+      }
+
+      /** \brief Set the viewpoint from where the input point cloud has been acquired.
+       * \param[in] viewpoint Vector containing the viewpoint coordinates (in the coordinate system of the data)
+       */
+      inline void setViewpoint (const Eigen::Vector3f& viewpoint)
+      {
+        viewpoint_ = viewpoint;
+      }
+
+      /** \brief Get the viewpoint from where the input point cloud has been acquired. */
+      const inline Eigen::Vector3f& getViewpoint () const
+      {
+        return viewpoint_;
+      }
+
+      /** \brief Store shadowed faces or not.
+       * \param[in] enable set to true to store shadowed faces
+       */
+      inline void
+      storeShadowedFaces (bool enable)
+      {
+        store_shadowed_faces_ = enable;
+      }
+
+      /** \brief Set the angle tolerance used for checking whether or not an edge is occluded.
+       * Standard values are 5deg to 15deg (input in rad!). Set a value smaller than zero to
+       * disable the check for shadowed edges.
+       * \param[in] angle_tolerance Angle tolerance (in rad). Set a value <0 to disable.
+       */
+      inline void
+      setAngleTolerance(float angle_tolerance)
+      {
+        if (angle_tolerance > 0)
+          cos_angle_tolerance_ = fabsf (cosf (angle_tolerance));
+        else
+          cos_angle_tolerance_ = -1.0f;
+      }
+
+
+      inline void setDistanceTolerance(float distance_tolerance, bool depth_dependent = false)
+      {
+        distance_tolerance_ = distance_tolerance;
+        if (distance_tolerance_ < 0)
+          return;
+
+        distance_dependent_ = depth_dependent;
+        if (!distance_dependent_)
+          distance_tolerance_ *= distance_tolerance_;
+      }
+
+      /** \brief Use the points' depths (z-coordinates) instead of measured distances (points' distances to the viewpoint).
+        * \param[in] enable Set to true skips comptations and further speeds up computation by using depth instead of computing distance. false to disable. */
+      inline void useDepthAsDistance(bool enable)
+      {
+        use_depth_as_distance_ = enable;
+      }
+
+    protected:
+      /** \brief max length of edge, scalar component */
+      float max_edge_length_a_;
+      /** \brief max length of edge, scalar component */
+      float max_edge_length_b_;
+      /** \brief max length of edge, scalar component */
+      float max_edge_length_c_;
+      /** \brief flag whether or not edges are limited in length */
+      bool max_edge_length_set_;
+
+      /** \brief flag whether or not max edge length is distance dependent. */
+      bool max_edge_length_dist_dependent_;
+
+      /** \brief size of triangle edges (in pixels) for iterating over rows. */
+      int triangle_pixel_size_rows_;
+
+      /** \brief size of triangle edges (in pixels) for iterating over columns*/
+      int triangle_pixel_size_columns_;
+
+      /** \brief Type of meshing scheme (quads vs. triangles, left cut vs. right cut ... */
+      TriangulationType triangulation_type_;
+
+      /** \brief Viewpoint from which the point cloud has been acquired (in the same coordinate frame as the data). */
+      Eigen::Vector3f viewpoint_;
+
+      /** \brief Whether or not shadowed faces are stored, e.g., for exploration */
+      bool store_shadowed_faces_;
+
+      /** \brief (Cosine of the) angle tolerance used when checking whether or not an edge between two points is shadowed. */
+      float cos_angle_tolerance_;
+
+      /** \brief distance tolerance for filtering out shadowed/occluded edges */
+      float distance_tolerance_;
+
+      /** \brief flag whether or not \a distance_tolerance_ is distance dependent (multiplied by the squared distance to the point) or not. */
+      bool distance_dependent_;
+
+      /** \brief flag whether or not the points' depths are used instead of measured distances (points' distances to the viewpoint).
+          This flag may be set using useDepthAsDistance(true) for (RGB-)Depth cameras to skip computations and gain additional speed up. */
+      bool use_depth_as_distance_;
+
+
+      /** \brief Perform the actual polygonal reconstruction.
+        * \param[out] polygons the resultant polygons
+        */
+      void
+      reconstructPolygons (std::vector<pcl::Vertices>& polygons);
+
+      /** \brief Create the surface.
+        * \param[out] polygons the resultant polygons, as a set of vertices. The Vertices structure contains an array of point indices.
+        */
+      virtual void
+      performReconstruction (std::vector<pcl::Vertices> &polygons);
+
+      /** \brief Create the surface.
+        *
+        * Simply uses image indices to create an initial polygonal mesh for organized point clouds.
+        * \a indices_ are ignored!
+        *
+        * \param[out] output the resultant polygonal mesh
+        */
+      void
+      performReconstruction (pcl::PolygonMesh &output);
+
+      /** \brief Add a new triangle to the current polygon mesh
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
+        * \param[out] polygons the polygon mesh to be updated
+        */
+      inline void
+      addTriangle (int a, int b, int c, int idx, std::vector<pcl::Vertices>& polygons)
+      {
+        assert (idx < static_cast<int> (polygons.size ()));
+        polygons[idx].vertices.resize (3);
+        polygons[idx].vertices[0] = a;
+        polygons[idx].vertices[1] = b;
+        polygons[idx].vertices[2] = c;
+      }
+
+      /** \brief Add a new quad to the current polygon mesh
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        * \param[in] d index of the fourth vertex
+        * \param[in] idx the index in the set of polygon vertices (assumes \a idx is valid in \a polygons)
+        * \param[out] polygons the polygon mesh to be updated
+        */
+      inline void
+      addQuad (int a, int b, int c, int d, int idx, std::vector<pcl::Vertices>& polygons)
+      {
+        assert (idx < static_cast<int> (polygons.size ()));
+        polygons[idx].vertices.resize (4);
+        polygons[idx].vertices[0] = a;
+        polygons[idx].vertices[1] = b;
+        polygons[idx].vertices[2] = c;
+        polygons[idx].vertices[3] = d;
+      }
+
+      /** \brief Set (all) coordinates of a particular point to the specified value
+        * \param[in] point_index index of point
+        * \param[out] mesh to modify
+        * \param[in] value value to use when re-setting
+        * \param[in] field_x_idx the X coordinate of the point
+        * \param[in] field_y_idx the Y coordinate of the point
+        * \param[in] field_z_idx the Z coordinate of the point
+        */
+      inline void
+      resetPointData (const int &point_index, pcl::PolygonMesh &mesh, const float &value = 0.0f,
+                      int field_x_idx = 0, int field_y_idx = 1, int field_z_idx = 2)
+      {
+        float new_value = value;
+        memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_x_idx].offset], &new_value, sizeof (float));
+        memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_y_idx].offset], &new_value, sizeof (float));
+        memcpy (&mesh.cloud.data[point_index * mesh.cloud.point_step + mesh.cloud.fields[field_z_idx].offset], &new_value, sizeof (float));
+      }
+
+      /** \brief Check if a point is shadowed by another point
+        * \param[in] point_a the first point
+        * \param[in] point_b the second point
+        */
+      inline bool
+      isShadowed (const PointInT& point_a, const PointInT& point_b)
+      {
+        bool valid = true;
+
+        Eigen::Vector3f dir_a = viewpoint_ - point_a.getVector3fMap ();
+        Eigen::Vector3f dir_b = point_b.getVector3fMap () - point_a.getVector3fMap ();
+        float distance_to_points = dir_a.norm ();
+        float distance_between_points = dir_b.norm ();
+
+        if (cos_angle_tolerance_ > 0)
+        {
+          float cos_angle = dir_a.dot (dir_b) / (distance_to_points*distance_between_points);
+          if (cos_angle != cos_angle)
+            cos_angle = 1.0f;
+          bool check_angle = fabs (cos_angle) >= cos_angle_tolerance_;
+
+          bool check_distance = true;
+          if (check_angle && (distance_tolerance_ > 0))
+          {
+            float dist_thresh = distance_tolerance_;
+            if (distance_dependent_)
+            {
+              float d = distance_to_points;
+              if (use_depth_as_distance_)
+                d = std::max(point_a.z, point_b.z);
+              dist_thresh *= d*d;
+              dist_thresh *= dist_thresh;  // distance_tolerance_ is already squared if distance_dependent_ is false.
+            }
+            check_distance = (distance_between_points > dist_thresh);
+          }
+          valid = !(check_angle && check_distance);
+        }
+
+        // check if max. edge length is not exceeded
+        if (max_edge_length_set_)
+        {
+          float dist = (use_depth_as_distance_ ? std::max(point_a.z, point_b.z) : distance_to_points);
+          float dist_thresh = max_edge_length_a_;
+          if (fabs(max_edge_length_b_) > std::numeric_limits<float>::min())
+            dist_thresh += max_edge_length_b_ * dist;
+          if (fabs(max_edge_length_c_) > std::numeric_limits<float>::min())
+            dist_thresh += max_edge_length_c_ * dist * dist;
+          valid = (distance_between_points <= dist_thresh);
+        }
+
+        return !valid;
+      }
+
+      /** \brief Check if a triangle is valid.
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        */
+      inline bool
+      isValidTriangle (const int& a, const int& b, const int& c)
+      {
+        if (!pcl::isFinite (input_->points[a])) return (false);
+        if (!pcl::isFinite (input_->points[b])) return (false);
+        if (!pcl::isFinite (input_->points[c])) return (false);
+        return (true);
+      }
+
+      /** \brief Check if a triangle is shadowed.
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        */
+      inline bool
+      isShadowedTriangle (const int& a, const int& b, const int& c)
+      {
+        if (isShadowed (input_->points[a], input_->points[b])) return (true);
+        if (isShadowed (input_->points[b], input_->points[c])) return (true);
+        if (isShadowed (input_->points[c], input_->points[a])) return (true);
+        return (false);
+      }
+
+      /** \brief Check if a quad is valid.
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        * \param[in] d index of the fourth vertex
+        */
+      inline bool
+      isValidQuad (const int& a, const int& b, const int& c, const int& d)
+      {
+        if (!pcl::isFinite (input_->points[a])) return (false);
+        if (!pcl::isFinite (input_->points[b])) return (false);
+        if (!pcl::isFinite (input_->points[c])) return (false);
+        if (!pcl::isFinite (input_->points[d])) return (false);
+        return (true);
+      }
+
+      /** \brief Check if a triangle is shadowed.
+        * \param[in] a index of the first vertex
+        * \param[in] b index of the second vertex
+        * \param[in] c index of the third vertex
+        * \param[in] d index of the fourth vertex
+        */
+      inline bool
+      isShadowedQuad (const int& a, const int& b, const int& c, const int& d)
+      {
+        if (isShadowed (input_->points[a], input_->points[b])) return (true);
+        if (isShadowed (input_->points[b], input_->points[c])) return (true);
+        if (isShadowed (input_->points[c], input_->points[d])) return (true);
+        if (isShadowed (input_->points[d], input_->points[a])) return (true);
+        return (false);
+      }
+
+      /** \brief Create a quad mesh.
+        * \param[out] polygons the resultant mesh
+        */
+      void
+      makeQuadMesh (std::vector<pcl::Vertices>& polygons);
+
+      /** \brief Create a right cut mesh.
+        * \param[out] polygons the resultant mesh
+        */
+      void
+      makeRightCutMesh (std::vector<pcl::Vertices>& polygons);
+
+      /** \brief Create a left cut mesh.
+        * \param[out] polygons the resultant mesh
+        */
+      void
+      makeLeftCutMesh (std::vector<pcl::Vertices>& polygons);
+
+      /** \brief Create an adaptive cut mesh.
+        * \param[out] polygons the resultant mesh
+        */
+      void
+      makeAdaptiveCutMesh (std::vector<pcl::Vertices>& polygons);
+  };
+}
+
+#include <pcl18/surface/impl/organized_fast_mesh.hpp>
+
+#endif  // PCL_SURFACE_ORGANIZED_FAST_MESH_H_

corelib/src/resources/DatabaseSchema.sql.in

@@ -29,7 +29,7 @@ CREATE TABLE Data (
 	id INTEGER NOT NULL,
 	image BLOB,               -- compressed image (Grayscale or RGB)
 	depth BLOB,               -- compressed image (Depth or Right image)
-	calibration BLOB,         -- fx, fy, cx, cy [,baseline] local_transform
+	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
 	scan BLOB,                -- compressed data (Laser scan)
 	scan_max_pts INTEGER,     -- Laser scan max points
 	scan_max_range FLOAT,     -- Laser max range
@@ -70,6 +70,8 @@ CREATE TABLE Map_Node_Word (
 	depth_x FLOAT,
 	depth_y FLOAT,
 	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
 	FOREIGN KEY (node_id) REFERENCES Node(id),
 	FOREIGN KEY (word_id) REFERENCES Word(id)
 );

corelib/src/toro3d/posegraph2.cpp

@@ -54,8 +54,8 @@ typedef unsigned int uint;
 #define LINESIZE 81920
 
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cerr
+//#define DEBUG(i) \
+//        if (verboseLevel>i) cerr
 
 
 bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
@@ -75,8 +75,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
       int id;
       Pose p;
       ls >> id >> p.x() >> p.y() >> p.theta(); 
-      if (addVertex(id,p))
- 	DEBUG(2) << "V " << id << endl;
+      addVertex(id,p);
+ 	//DEBUG(2) << "V " << id << endl;
       
     }
 
@@ -98,8 +98,8 @@ bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
       TreePoseGraph2::Vertex* v1=vertex(id1);
       TreePoseGraph2::Vertex* v2=vertex(id2);
       Transformation t(p);
-      if (addEdge(v1, v2,t ,m))
-	 DEBUG(2) << "E " << id1 << " " << id2 <<  endl;
+      addEdge(v1, v2,t ,m);
+	 //DEBUG(2) << "E " << id1 << " " << id2 <<  endl;
     }
   }
   return true;
@@ -411,11 +411,11 @@ void TreePoseGraph2::collapseEdge(Edge* e){
       double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
       double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
       
-      DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
-      DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
+      //DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
+      //DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
       
       Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
-      DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
+      //DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
       
       e1x->transformation=Transformation(pFinal);
       e1x->informationMatrix=IM;

corelib/src/toro3d/posegraph3.cpp

@@ -51,8 +51,8 @@ namespace AISNavigation {
 #define LINESIZE 81920
 
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cerr
+//#define DEBUG(i) \
+//       if (verboseLevel>i) cerr
 
 
 bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){

corelib/src/toro3d/treeoptimizer2.cpp

@@ -51,8 +51,8 @@ using namespace std;
 
 namespace AISNavigation {
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cerr
+//#define DEBUG(i) \
+//        if (verboseLevel>i) cerr
 
 /** \brief A class (struct) to compute the parameterization of the vertex v **/
 struct ParameterPropagator{
@@ -83,9 +83,9 @@ void TreeOptimizer2::initializeTreeParameters(){
 void TreeOptimizer2::initializeOptimization(){
   // compute the size of the preconditioning matrix
   int sz=maxIndex()+1;
-  DEBUG(1) << "Size= " << sz << endl;
+  //DEBUG(1) << "Size= " << sz << endl;
   M.resize(sz);
-  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
   iteration=1;
 
   // sorting edges
@@ -99,9 +99,9 @@ void TreeOptimizer2::initializeOptimization(){
 void TreeOptimizer2::initializeOnlineOptimization(){
   // compute the size of the preconditioning matrix
   int sz=maxIndex()+1;
-  DEBUG(1) << "Size= " << sz << endl;
+  //DEBUG(1) << "Size= " << sz << endl;
   M.resize(sz);
-  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
   iteration=1;
 }
 
@@ -114,8 +114,8 @@ void TreeOptimizer2::computePreconditioner(){
   int edgeCount=0;
   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
     edgeCount++;
-    if (! (edgeCount%10000))
-      DEBUG(1) << "m";
+    //if (! (edgeCount%10000))
+    //  DEBUG(1) << "m";
 
     Edge* e=*it;
     Transformation t=e->transformation;
@@ -166,7 +166,7 @@ void TreeOptimizer2::propagateErrors(){
   
   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
       edgeCount++;
-      if (! (edgeCount%10000)) 	DEBUG(1) << "c";
+      //if (! (edgeCount%10000)) 	DEBUG(1) << "c";
       
       Edge* e=*it;
       Vertex* top=e->top;
@@ -176,13 +176,13 @@ void TreeOptimizer2::propagateErrors(){
       Vertex* v2=e->v2;
 
       double l=e->length;
-      DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+      //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
       
       Pose p1=getPose(v1, top);
       Pose p2=getPose(v2, top);
 
-      DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
-      DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+      //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+      //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
       
       Transformation et=e->transformation;
       Transformation t1(p1);
@@ -191,13 +191,13 @@ void TreeOptimizer2::propagateErrors(){
       Transformation t12=t1*et;
     
       Pose p12=t12.toPoseType();
-      DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+      //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
 
       Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
       double angle=r.theta();
       angle=atan2(sin(angle),cos(angle));
       r.theta()=angle;
-      DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+      //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
     
       InformationMatrix S=e->informationMatrix;
       InformationMatrix R;
@@ -216,7 +216,7 @@ void TreeOptimizer2::propagateErrors(){
       InformationMatrix W=R*S*R.transpose();
       Pose d=W*r*2.;
 
-      DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
+      //DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
     
       assert(l>0);
 
@@ -239,8 +239,8 @@ void TreeOptimizer2::propagateErrors(){
       beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
       beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
 
-      DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
-      DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
+      //DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
+      //DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
       
       for (int dir=0; dir<2; dir++) {
 	Vertex* n = (dir==0)? v1 : v2;
@@ -253,13 +253,13 @@ void TreeOptimizer2::propagateErrors(){
 
 	  Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
 	  delta=delta*sign;
-	  DEBUG(2) << "   " << dir << ":"  << i <<"," << n->parent->id << ":"
-	       << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
+	  //DEBUG(2) << "   " << dir << ":"  << i <<"," << n->parent->id << ":"
+	  //     << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
 
 	  n->parameters.x()+=delta.x();
 	  n->parameters.y()+=delta.y();
 	  n->parameters.theta()+=delta.theta();
-	  DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
+	  //DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
 	  n=n->parent;
 	}
       }
@@ -269,9 +269,9 @@ void TreeOptimizer2::propagateErrors(){
       Pose pf1=v1->pose;
       Pose pf2=v2->pose;
       
-      DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
-      DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
-      DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
+      //DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
+      //DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
+      //DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
     }
   
 }
@@ -320,8 +320,8 @@ double TreeOptimizer2::error(const Edge* e) const{
   Pose p1=v1->pose;
   Pose p2=v2->pose;
   
-  DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
-  DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+  //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+  //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
   
   Transformation et=e->transformation;
   Transformation t1(p1);
@@ -330,13 +330,13 @@ double TreeOptimizer2::error(const Edge* e) const{
   Transformation t12=t1*et;
     
   Pose p12=t12.toPoseType();
-  DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+  //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
 
   Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
   double angle=r.theta();
   angle=atan2(sin(angle),cos(angle));
   r.theta()=angle;
-  DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+  //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
     
   InformationMatrix S=e->informationMatrix;
   InformationMatrix R;

corelib/src/toro3d/treeoptimizer3.cpp

@@ -50,8 +50,8 @@ using namespace std;
 
 namespace AISNavigation {
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cerr
+//#define DEBUG(i) \
+//        if (verboseLevel>i) cerr
 
 
 TreeOptimizer3::TreeOptimizer3(){
@@ -180,7 +180,7 @@ double TreeOptimizer3::error(const Edge* e) const{
 
   Pose ps=e->informationMatrix*p12;
   double err=p12*ps;
-  DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
+  //DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
   return err;
  
 }
@@ -294,10 +294,10 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
   edgeCompareMode=mode;
   // compute the size of the preconditioning matrix
   int sz=maxIndex()+1;
-  DEBUG(1) << "Size= " << sz << endl;
+  //DEBUG(1) << "Size= " << sz << endl;
   M.resize(sz);
 
-  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
   iteration=1;
 
   // sorting edges
@@ -313,9 +313,9 @@ void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
 
 void TreeOptimizer3::initializeOnlineIterations(){
   int sz=maxIndex()+1;
-  DEBUG(1) << "Size= " << sz << endl;
+  //DEBUG(1) << "Size= " << sz << endl;
   M.resize(sz);
-  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  //DEBUG(1) << "allocating M(" << sz << ")" << endl;
   iteration=1;
   maxRotationalErrors.clear();
   maxTranslationalErrors.clear();
@@ -337,18 +337,18 @@ void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
 }
 
 void TreeOptimizer3::onStepStart(Edge* e){
-  DEBUG(5) << "entering edge" << e << endl;
+  //DEBUG(5) << "entering edge" << e << endl;
 }  
 void TreeOptimizer3::onStepFinished(Edge* e){
-  DEBUG(5) << "exiting edge" << e << endl;
+  //DEBUG(5) << "exiting edge" << e << endl;
 }
 
 void TreeOptimizer3::onIterationStart(int iteration){
-  DEBUG(5) << "entering iteration " << iteration << endl;
+  //DEBUG(5) << "entering iteration " << iteration << endl;
 }
 
 void TreeOptimizer3::onIterationFinished(int iteration){
-  DEBUG(5) << "exiting iteration " << iteration << endl;
+  //DEBUG(5) << "exiting iteration " << iteration << endl;
 }
 
 void TreeOptimizer3::onRestartBegin(){}

corelib/src/toro3d/treeoptimizer3_iteration.cpp

@@ -42,8 +42,8 @@ using namespace std;
 
 namespace AISNavigation {
 
-#define DEBUG(i) \
-        if (verboseLevel>i) cerr
+//#define DEBUG(i) \
+//        if (verboseLevel>i) cerr
 
 //helper functions. Should I explain :-)?
 inline double max3( const double& a, const double& b, const double& c){
@@ -94,8 +94,8 @@ void TreeOptimizer3::computePreconditioner(){
   int edgeCount=0;
   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
     edgeCount++;
-    if (! (edgeCount%1000))
-      DEBUG(1) << "m";
+    //if (! (edgeCount%1000))
+    //  DEBUG(1) << "m";
 
     Edge* e=*it;
     //Transformation t=e->transformation;
@@ -138,8 +138,8 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
   onIterationStart(iteration);
   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
     edgeCount++;
-    if (! (edgeCount%1000))
-      DEBUG(1) << "c";
+    //if (! (edgeCount%1000))
+    //  DEBUG(1) << "c";
 
     if (isDone())
       return;
@@ -153,7 +153,7 @@ void TreeOptimizer3::propagateErrors(bool usePreconditioner){
     
     recomputeTransformations(v1,top);
     recomputeTransformations(v2,top);
-    DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+    //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
 
     //BEGIN: Path and weight computation 
     int pc=0;

corelib/src/util2d.cpp

@@ -957,13 +957,22 @@ float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,
 		bool smoothing,
-		float maxZError)
+		float maxZError,
+		bool estWithNeighborsIfNull)
 {
 	UASSERT(!depthImage.empty());
 	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
 
 	int u = int(x+0.5f);
 	int v = int(y+0.5f);
+	if(u == depthImage.cols && x<float(depthImage.cols))
+	{
+		u = depthImage.cols - 1;
+	}
+	if(v == depthImage.rows && y<float(depthImage.rows))
+	{
+		v = depthImage.rows - 1;
+	}
 
 	if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
 	{
@@ -986,6 +995,41 @@ float getDepth(
 	int v_end = std::min(v+1, depthImage.rows-1);
 
 	float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u);
+
+	if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
+	{
+		// all cells no2 must be under the zError to be accepted
+		float tmp = 0.0f;
+		int count = 0;
+		for(int uu = u_start; uu <= u_end; ++uu)
+		{
+			for(int vv = v_start; vv <= v_end; ++vv)
+			{
+				if((uu == u && vv!=v) || (uu != u && vv==v))
+				{
+					float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
+					if(d!=0.0f && uIsFinite(d))
+					{
+						if(tmp == 0.0f)
+						{
+							tmp = d;
+							++count;
+						}
+						else if(fabs(d - tmp) < maxZError)
+						{
+							tmp+=d;
+							++count;
+						}
+					}
+				}
+			}
+		}
+		if(count > 1)
+		{
+			depth = tmp/float(count);
+		}
+	}
+
 	if(depth!=0.0f && uIsFinite(depth))
 	{
 		if(smoothing)
@@ -1078,6 +1122,117 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
 	return out;
 }
 
+cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
+{
+	UASSERT(factor >= 1);
+	cv::Mat out;
+	if(!image.empty())
+	{
+		if(factor > 1)
+		{
+			if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
+			{
+				UASSERT(depthErrorRatio>0.0f);
+				out = cv::Mat::zeros(image.rows*factor, image.cols*factor, image.type());
+				for(int j=0; j<out.rows; j+=factor)
+				{
+					for(int i=0; i<out.cols; i+=factor)
+					{
+						if(i>0 && j>0)
+						{
+							float dTopLeft;
+							float dTopRight;
+							float dBottomLeft;
+							float dBottomRight;
+							if(image.type() == CV_32FC1)
+							{
+								dTopLeft = image.at<float>(j/factor-1, i/factor-1);
+								dTopRight = image.at<float>(j/factor-1, i/factor);
+								dBottomLeft = image.at<float>(j/factor, i/factor-1);
+								dBottomRight = image.at<float>(j/factor, i/factor);
+							}
+							else
+							{
+								dTopLeft = image.at<unsigned short>(j/factor-1, i/factor-1);
+								dTopRight = image.at<unsigned short>(j/factor-1, i/factor);
+								dBottomLeft = image.at<unsigned short>(j/factor, i/factor-1);
+								dBottomRight = image.at<unsigned short>(j/factor, i/factor);
+							}
+
+							if(dTopLeft>0 && dTopRight>0 && dBottomLeft>0 && dBottomRight > 0)
+							{
+								float depthError = depthErrorRatio*(dTopLeft+dTopRight+dBottomLeft+dBottomRight)/4.0f;
+								if(fabs(dTopLeft-dTopRight) <= depthError &&
+								   fabs(dTopLeft-dBottomLeft) <= depthError &&
+								   fabs(dTopLeft-dBottomRight) <= depthError)
+								{
+									// bilinear interpolation
+									// do first and last rows then columns
+									float slopeTop = (dTopRight-dTopLeft)/float(factor);
+									float slopeBottom = (dBottomRight-dBottomLeft)/float(factor);
+									if(image.type() == CV_32FC1)
+									{
+										for(int z=i-factor; z<=i; ++z)
+										{
+											out.at<float>(j-factor, z) = dTopLeft+(slopeTop*float(z-(i-factor)));
+											out.at<float>(j, z) = dBottomLeft+(slopeBottom*float(z-(i-factor)));
+										}
+									}
+									else
+									{
+										for(int z=i-factor; z<=i; ++z)
+										{
+											out.at<unsigned short>(j-factor, z) = (unsigned short)(dTopLeft+(slopeTop*float(z-(i-factor))));
+											out.at<unsigned short>(j, z) = (unsigned short)(dBottomLeft+(slopeBottom*float(z-(i-factor))));
+										}
+									}
+
+									// fill the columns
+									if(image.type() == CV_32FC1)
+									{
+										for(int z=i-factor; z<=i; ++z)
+										{
+											float top = out.at<float>(j-factor, z);
+											float bottom = out.at<float>(j, z);
+											float slope = (bottom-top)/float(factor);
+											for(int d=j-factor+1; d<j; ++d)
+											{
+												out.at<float>(d, z) = top+(slope*float(d-(j-factor)));
+											}
+										}
+									}
+									else
+									{
+										for(int z=i-factor; z<=i; ++z)
+										{
+											float top = out.at<unsigned short>(j-factor, z);
+											float bottom = out.at<unsigned short>(j, z);
+											float slope = (bottom-top)/float(factor);
+											for(int d=j-factor+1; d<j; ++d)
+											{
+												out.at<unsigned short>(d, z) = (unsigned short)(top+(slope*float(d-(j-factor))));
+											}
+										}
+									}
+								}
+							}
+						}
+					}
+				}
+			}
+			else
+			{
+				cv::resize(image, out, cv::Size(), float(factor), float(factor));
+			}
+		}
+		else
+		{
+			out = image;
+		}
+	}
+	return out;
+}
+
 // Registration Depth to RGB (return registered depth image)
 cv::Mat registerDepth(
 		const cv::Mat & depth,
@@ -1152,6 +1307,114 @@ cv::Mat registerDepth(
 	return registered;
 }
 
+cv::Mat fillDepthHoles(const cv::Mat & registeredDepth, int maximumHoleSize, float errorRatio)
+{
+	UASSERT(registeredDepth.type() == CV_16UC1);
+	UASSERT(maximumHoleSize > 0);
+	cv::Mat output = registeredDepth.clone();
+	for(int y=0; y<registeredDepth.rows-2; ++y)
+	{
+		for(int x=0; x<registeredDepth.cols-2; ++x)
+		{
+			float a = registeredDepth.at<unsigned short>(y, x);
+			float bRight = registeredDepth.at<unsigned short>(y, x+1);
+			float bDown = registeredDepth.at<unsigned short>(y+1, x);
+
+			if(a > 0.0f && (bRight == 0.0f || bDown == 0.0f))
+			{
+				bool horizontalSet = bRight != 0.0f;
+				bool verticalSet = bDown != 0.0f;
+				int stepX = 0;
+				for(int h=1; h<=maximumHoleSize && (!horizontalSet || !verticalSet); ++h)
+				{
+					// horizontal
+					if(!horizontalSet)
+					{
+						if(x+1+h >= registeredDepth.cols)
+						{
+							horizontalSet = true;
+						}
+						else
+						{
+							float c = registeredDepth.at<unsigned short>(y, x+1+h);
+							if(c == 0)
+							{
+								// ignore this size
+							}
+							else
+							{
+								// fill hole
+								float depthError = errorRatio*float(a+c)/2.0f;
+								if(fabs(a-c) <= depthError)
+								{
+									//linear interpolation
+									float slope = (c-a)/float(h+1);
+									for(int z=x+1; z<x+1+h; ++z)
+									{
+										if(output.at<unsigned short>(y, z) == 0)
+										{
+											output.at<unsigned short>(y, z) = (unsigned short)(a+(slope*float(z-x)));
+										}
+										else
+										{
+											// average with the previously set value
+											output.at<unsigned short>(y, z) = (output.at<unsigned short>(y, z)+(unsigned short)(a+(slope*float(z-x))))/2;
+										}
+									}
+								}
+								horizontalSet = true;
+								stepX = h;
+							}
+						}
+					}
+
+					// vertical
+					if(!verticalSet)
+					{
+						if(y+1+h >= registeredDepth.rows)
+						{
+							verticalSet = true;
+						}
+						else
+						{
+							float c = registeredDepth.at<unsigned short>(y+1+h, x);
+							if(c == 0)
+							{
+								// ignore this size
+							}
+							else
+							{
+								// fill hole
+								float depthError = errorRatio*float(a+c)/2.0f;
+								if(fabs(a-c) <= depthError)
+								{
+									//linear interpolation
+									float slope = (c-a)/float(h+1);
+									for(int z=y+1; z<y+1+h; ++z)
+									{
+										if(output.at<unsigned short>(z, x) == 0)
+										{
+											output.at<unsigned short>(z, x) = (unsigned short)(a+(slope*float(z-y)));
+										}
+										else
+										{
+											// average with the previously set value
+											output.at<unsigned short>(z, x) = (output.at<unsigned short>(z, x)+(unsigned short)(a+(slope*float(z-y))))/2;
+										}
+									}
+								}
+								verticalSet = true;
+							}
+						}
+					}
+				}
+				x+=stepX;
+			}
+		}
+	}
+	return output;
+}
+
 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
 {
 	UASSERT(registeredDepth.type() == CV_16UC1);

corelib/src/util3d.cpp

@@ -221,7 +221,7 @@ pcl::PointXYZ projectDepthTo3D(
 	pcl::PointXYZ pt;
 
 	float depth = util2d::getDepth(depthImage, x, y, smoothing, maxZError);
-	if(depth)
+	if(depth > 0.0f)
 	{
 		// Use correct principal point from calibration
 		cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
@@ -243,7 +243,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
-		int decimation)
+		int decimation,
+		float maxDepth,
+		std::vector<int> * validIndices)
 {
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
 	UASSERT(imageDepth.rows % decimation == 0);
@@ -259,11 +261,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	cloud->height = imageDepth.rows/decimation;
 	cloud->width  = imageDepth.cols/decimation;
 	cloud->is_dense = false;
-
 	cloud->resize(cloud->height * cloud->width);
+	if(validIndices)
+	{
+		validIndices->resize(cloud->size());
+	}
 
-	int count = 0 ;
-
+	int oi = 0;
 	for(int h = 0; h < imageDepth.rows; h+=decimation)
 	{
 		for(int w = 0; w < imageDepth.cols; w+=decimation)
@@ -271,12 +275,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
 			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
-			pt.x = ptXYZ.x;
-			pt.y = ptXYZ.y;
-			pt.z = ptXYZ.z;
-			++count;
+			if(maxDepth<=0.0f || ptXYZ.z <= maxDepth)
+			{
+				pt.x = ptXYZ.x;
+				pt.y = ptXYZ.y;
+				pt.z = ptXYZ.z;
+				if(validIndices)
+				{
+					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+				}
+			}
+			else
+			{
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			}
 		}
 	}
+	if(validIndices)
+	{
+		validIndices->reserve(oi);
+	}
 
 	return cloud;
 }
@@ -286,13 +304,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
 		float fx, float fy,
-		int decimation)
+		int decimation,
+		float maxDepth,
+		std::vector<int> * validIndices)
 {
 	UDEBUG("");
-	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
+	UASSERT(imageRgb.rows % imageDepth.rows == 0 && imageRgb.cols % imageDepth.cols == 0);
 	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
-	UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
-	UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageDepth.rows, decimation).c_str());
+	UASSERT_MSG(imageRgb.rows % decimation == 0, uFormat("imageDepth.rows=%d decimation=%d", imageRgb.rows, decimation).c_str());
+	UASSERT_MSG(imageRgb.cols % decimation == 0, uFormat("imageDepth.cols=%d decimation=%d", imageRgb.cols, decimation).c_str());
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
@@ -315,23 +335,41 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	}
 
 	//cloud.header = cameraInfo.header;
-	cloud->height = imageDepth.rows/decimation;
-	cloud->width  = imageDepth.cols/decimation;
+	cloud->height = imageRgb.rows/decimation;
+	cloud->width  = imageRgb.cols/decimation;
 	cloud->is_dense = false;
 	cloud->resize(cloud->height * cloud->width);
-
-	for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
+	if(validIndices)
 	{
-		for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
+		validIndices->resize(cloud->size());
+	}
+
+	float rgbToDepthFactorX = 1.0f/float((imageRgb.cols / imageDepth.cols));
+	float rgbToDepthFactorY = 1.0f/float((imageRgb.rows / imageDepth.rows));
+	float depthFx = fx * rgbToDepthFactorX;
+	float depthFy = fy * rgbToDepthFactorY;
+	float depthCx = cx * rgbToDepthFactorX;
+	float depthCy = cy * rgbToDepthFactorY;
+
+	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+			imageRgb.cols, imageRgb.rows,
+			imageDepth.cols, imageDepth.rows,
+			fx, fy, cx, cy,
+			rgbToDepthFactorX,
+			rgbToDepthFactorY,
+			decimation);
+
+	int oi = 0;
+	for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
+	{
+		for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
 		{
 			pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
-			bool invalidColor = false;
 			if(!mono)
 			{
 				pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
 				pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
 				pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
-				invalidColor = pt.b <= 5 && pt.g <= 5 && pt.r <= 5;
 			}
 			else
 			{
@@ -339,29 +377,44 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.b = v;
 				pt.g = v;
 				pt.r = v;
-				invalidColor = v <= 0;
 			}
 
-			if(invalidColor)
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w*rgbToDepthFactorX, h*rgbToDepthFactorY, depthCx, depthCy, depthFx, depthFy, false);
+			if(pcl::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 			{
-				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
-			}
-			else
-			{
-				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
 				pt.x = ptXYZ.x;
 				pt.y = ptXYZ.y;
 				pt.z = ptXYZ.z;
+				if(validIndices)
+				{
+					validIndices->at(oi) = (h/decimation)*cloud->width + (w/decimation);
+				}
+				++oi;
+			}
+			else
+			{
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
 			}
 		}
 	}
+	if(validIndices)
+	{
+		validIndices->resize(oi);
+	}
+	if(oi == 0)
+	{
+		UWARN("Cloud with only NaN values created!");
+	}
+	UDEBUG("");
 	return cloud;
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 		const cv::Mat & imageDisparity,
 		const StereoCameraModel & model,
-		int decimation)
+		int decimation,
+		float maxDepth,
+		std::vector<int> * validIndices)
 {
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
 	UASSERT(imageDisparity.rows % decimation == 0);
@@ -375,7 +428,12 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 	cloud->width  = imageDisparity.cols/decimation;
 	cloud->is_dense = false;
 	cloud->resize(cloud->height * cloud->width);
+	if(validIndices)
+	{
+		validIndices->resize(cloud->size());
+	}
 
+	int oi = 0;
 	if(imageDisparity.type()==CV_16SC1)
 	{
 		for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
@@ -384,7 +442,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			{
 				float disp = float(imageDisparity.at<short>(h,w))/16.0f;
 				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-				cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+				if(maxDepth <= 0.0f || pt.z <= maxDepth)
+				{
+					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+					if(validIndices)
+					{
+						validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+					}
+				}
+				else
+				{
+					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
+							std::numeric_limits<float>::quiet_NaN(),
+							std::numeric_limits<float>::quiet_NaN(),
+							std::numeric_limits<float>::quiet_NaN());
+				}
 			}
 		}
 	}
@@ -396,10 +468,28 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 			{
 				float disp = imageDisparity.at<float>(h,w);
 				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-				cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+				if(maxDepth <= 0.0f || pt.z <= maxDepth)
+				{
+					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+					if(validIndices)
+					{
+						validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+					}
+				}
+				else
+				{
+					cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(
+							std::numeric_limits<float>::quiet_NaN(),
+							std::numeric_limits<float>::quiet_NaN(),
+							std::numeric_limits<float>::quiet_NaN());
+				}
 			}
 		}
 	}
+	if(validIndices)
+	{
+		validIndices->resize(oi);
+	}
 	return cloud;
 }
 
@@ -407,7 +497,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,
 		const StereoCameraModel & model,
-		int decimation)
+		int decimation,
+		float maxDepth,
+		std::vector<int> * validIndices)
 {
 	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
 	UASSERT(imageRgb.rows == imageDisparity.rows &&
@@ -434,7 +526,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 	cloud->width  = imageRgb.cols/decimation;
 	cloud->is_dense = false;
 	cloud->resize(cloud->height * cloud->width);
+	if(validIndices)
+	{
+		validIndices->resize(cloud->size());
+	}
 
+	int oi=0;
 	for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
 	{
 		for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
@@ -456,11 +553,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 
 			float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
 			cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
-			pt.x = ptXYZ.x;
-			pt.y = ptXYZ.y;
-			pt.z = ptXYZ.z;
+			if(util3d::isFinite(ptXYZ) && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+			{
+				pt.x = ptXYZ.x;
+				pt.y = ptXYZ.y;
+				pt.z = ptXYZ.z;
+				if(validIndices)
+				{
+					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+				}
+			}
+			else
+			{
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			}
 		}
 	}
+	if(validIndices)
+	{
+		validIndices->resize(oi);
+	}
 	return cloud;
 }
 
@@ -468,7 +580,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		const cv::Mat & imageLeft,
 		const cv::Mat & imageRight,
 		const StereoCameraModel & model,
-		int decimation)
+		int decimation,
+		float maxDepth,
+		std::vector<int> * validIndices)
 {
 	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
@@ -505,7 +619,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			leftColor,
 			util2d::disparityFromStereoImages(leftMono, rightMono),
 			modelDecimation,
-			decimation);
+			decimation,
+			maxDepth,
+			validIndices);
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
@@ -513,7 +629,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		int decimation,
 		float maxDepth,
 		float voxelSize,
-		int samples)
+		int samples,
+		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -532,16 +649,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 						sensorData.cameraModels()[i].cy(),
 						sensorData.cameraModels()[i].fx(),
 						sensorData.cameraModels()[i].fy(),
-						decimation);
+						decimation,
+						maxDepth,
+						sensorData.cameraModels().size()==1?validIndices:0);
 
 				if(tmp->size())
 				{
 					bool filtered = false;
-					if(tmp->size() && maxDepth)
-					{
-						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
-						filtered = true;
-					}
 
 					if(tmp->size() && voxelSize)
 					{
@@ -555,7 +669,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 						filtered = true;
 					}
 
-					if(tmp->size() && !filtered)
+					if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
 					{
 						tmp = util3d::removeNaNFromPointCloud(tmp);
 					}
@@ -565,7 +679,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
 					}
 
-					*cloud += *tmp;
+					if(sensorData.cameraModels().size() > 1)
+					{
+						*cloud += *tmp;
+					}
+					else
+					{
+						cloud = tmp;
+					}
 				}
 			}
 			else
@@ -574,7 +695,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 			}
 		}
 
-		if(cloud->size() && voxelSize)
+		if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
 		{
 			cloud = util3d::voxelize(cloud, voxelSize);
 		}
@@ -596,26 +717,15 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		cloud = cloudFromDisparity(
 				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw()),
 				sensorData.stereoCameraModel(),
-				decimation);
+				decimation,
+				maxDepth,
+				validIndices);
 
 		if(cloud->size())
 		{
-			bool filtered = false;
-			if(cloud->size() && maxDepth)
-			{
-				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
-				filtered = true;
-			}
-
 			if(cloud->size() && voxelSize)
 			{
 				cloud = util3d::voxelize(cloud, voxelSize);
-				filtered = true;
-			}
-
-			if(cloud->size() && !filtered)
-			{
-				cloud = util3d::removeNaNFromPointCloud(cloud);
 			}
 
 			if(cloud->size())
@@ -632,7 +742,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		int decimation,
 		float maxDepth,
 		float voxelSize,
-		int samples)
+		int samples,
+		std::vector<int> * validIndices)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -644,37 +755,40 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		//depth
 		UDEBUG("");
 		UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
-		UASSERT(sensorData.depthRaw().size() == sensorData.imageRaw().size());
-		int subImageWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		UASSERT(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0);
+		UASSERT(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0);
+		int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
+		int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+
+		if(subRGBWidth % decimation != 0 || subDepthWidth % decimation != 0)
+		{
+			UWARN("Image size (rgb=%d,%d depth=%d,%d) modulus decimation (%d) is not null "
+				  "for the cloud creation! Setting decimation to 1...",
+				  subRGBWidth, sensorData.imageRaw().rows,
+				  subDepthWidth, sensorData.depthRaw().rows,
+				  decimation);
+			decimation = 1;
+		}
+
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
-				if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
-				{
-					UWARN("Image size (%d,%d) modulus decimation (%d) is not null "
-						  "for the cloud creation! Setting decimation to 1...",
-						  subImageWidth, sensorData.depthRaw().rows, decimation);
-					decimation = 1;
-				}
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
-						cv::Mat(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows)),
-						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
+						cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
 						sensorData.cameraModels()[i].cx(),
 						sensorData.cameraModels()[i].cy(),
 						sensorData.cameraModels()[i].fx(),
 						sensorData.cameraModels()[i].fy(),
-						decimation);
+						decimation,
+						maxDepth,
+						sensorData.cameraModels().size() == 1?validIndices:0);
 
 				if(tmp->size())
 				{
 					bool filtered = false;
-					if(tmp->size() && maxDepth)
-					{
-						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
-						filtered = true;
-					}
-
 					if(tmp->size() && voxelSize)
 					{
 						tmp = util3d::voxelize(tmp, voxelSize);
@@ -687,7 +801,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 						filtered = true;
 					}
 
-					if(tmp->size() && !filtered)
+					if(tmp->size() && !filtered && sensorData.cameraModels().size() > 1)
 					{
 						tmp = util3d::removeNaNFromPointCloud(tmp);
 					}
@@ -697,7 +811,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
 					}
 
-					*cloud += *tmp;
+					if(sensorData.cameraModels().size() > 1)
+					{
+						*cloud += *tmp;
+					}
+					else
+					{
+						cloud = tmp;
+					}
 				}
 			}
 			else
@@ -706,10 +827,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 			}
 		}
 
-		if(cloud->size() && voxelSize)
+		if(cloud->size() && voxelSize && sensorData.cameraModels().size() > 1)
 		{
 			cloud = util3d::voxelize(cloud, voxelSize);
 		}
+
+		if(cloud->is_dense && validIndices)
+		{
+			//generate indices for all points (they are all valid)
+			validIndices->resize(cloud->size());
+			for(unsigned int i=0; i<cloud->size(); ++i)
+			{
+				validIndices->at(i) = i;
+			}
+		}
 	}
 	else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
 	{
@@ -718,26 +849,15 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		cloud = cloudFromStereoImages(sensorData.imageRaw(),
 				sensorData.rightRaw(),
 				sensorData.stereoCameraModel(),
-				decimation);
+				decimation,
+				maxDepth,
+				validIndices);
 
 		if(cloud->size())
 		{
-			bool filtered = false;
-			if(cloud->size() && maxDepth)
-			{
-				cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
-				filtered = true;
-			}
-
 			if(cloud->size() && voxelSize)
 			{
 				cloud = util3d::voxelize(cloud, voxelSize);
-				filtered = true;
-			}
-
-			if(cloud->size() && !filtered)
-			{
-				cloud = util3d::removeNaNFromPointCloud(cloud);
 			}
 
 			if(cloud->size())

corelib/src/util3d_features.cpp

@@ -72,20 +72,24 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
 		UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
 		float subImageWidth = depth.cols/cameraModels.size();
 		keypoints3d.resize(keypoints.size());
+		float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/depth.cols:1);
+		float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1);
 		for(unsigned int i=0; i!=keypoints.size(); ++i)
 		{
-			int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
+			float x = keypoints[i].pt.x*rgbToDepthFactorX;
+			float y = keypoints[i].pt.y*rgbToDepthFactorY;
+			int cameraIndex = int(x / subImageWidth);
 			UASSERT_MSG(cameraIndex < (int)cameraModels.size(),
-					uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f",
-							cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
+					uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+							cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth, cameraModels[0].imageWidth()).c_str());
 			pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D(
 					depth,
-					keypoints[i].pt.x-subImageWidth*cameraIndex,
-					keypoints[i].pt.y,
-					cameraModels.at(cameraIndex).cx(),
-					cameraModels.at(cameraIndex).cy(),
-					cameraModels.at(cameraIndex).fx(),
-					cameraModels.at(cameraIndex).fy(),
+					x-subImageWidth*cameraIndex,
+					y,
+					cameraModels.at(cameraIndex).cx()*rgbToDepthFactorX,
+					cameraModels.at(cameraIndex).cy()*rgbToDepthFactorY,
+					cameraModels.at(cameraIndex).fx()*rgbToDepthFactorX,
+					cameraModels.at(cameraIndex).fy()*rgbToDepthFactorY,
 					true);
 
 			cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
@@ -192,7 +196,9 @@ std::map<int, cv::Point3f> generateWords3DMono(
 	UASSERT(cameraModel.isValidForProjection());
 	std::map<int, cv::Point3f> words3D;
 	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-	if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8)
+	int pairsFound = EpipolarGeometry::findPairs(refWords, nextWords, pairs);
+	UDEBUG("pairsFound=%d", pairsFound);
+	if(pairsFound > 8)
 	{
 		std::vector<unsigned char> status;
 		cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
@@ -277,6 +283,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 					cv::Mat pts4D;
 					cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
 
+					UASSERT((int)indexes.size() == pts4D.cols && pts4D.rows == 4);
 					for(unsigned int i=0; i<indexes.size(); ++i)
 					{
 						//if(cloud->at(i).z > 0)
@@ -290,6 +297,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
 						}
 					}
 
+					UDEBUG("ref guess=%d", (int)refGuess3D.size());
 					if(refGuess3D.size())
 					{
 						// scale estimation
@@ -359,24 +367,25 @@ std::map<int, cv::Point3f> generateWords3DMono(
 							{
 								std::vector<cv::Point3f> objectPoints(indexes.size());
 								std::vector<cv::Point2f> imagePoints(indexes.size());
-								int oi=0;
+								int oi2=0;
+								UASSERT(indexes.size() == newCorners.size());
 								for(unsigned int i=0; i<indexes.size(); ++i)
 								{
 									std::map<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
-									if(util3d::isFinite(iter->second))
+									if(iter!=words3D.end() && util3d::isFinite(iter->second))
 									{
 										iter->second.x *= scale;
 										iter->second.y *= scale;
 										iter->second.z *= scale;
-										objectPoints[oi].x = iter->second.x;
-										objectPoints[oi].y = iter->second.y;
-										objectPoints[oi].z = iter->second.z;
-										imagePoints[oi] = newCorners[i];
-										++oi;
+										objectPoints[oi2].x = iter->second.x;
+										objectPoints[oi2].y = iter->second.y;
+										objectPoints[oi2].z = iter->second.z;
+										imagePoints[oi2] = newCorners[i];
+										++oi2;
 									}
 								}
-								objectPoints.resize(oi);
-								imagePoints.resize(oi);
+								objectPoints.resize(oi2);
+								imagePoints.resize(oi2);
 
 								//PnPRansac
 								Transform guess = cameraModel.localTransform().inverse();

corelib/src/util3d_filtering.cpp

@@ -42,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/segmentation/extract_clusters.h>
 
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
 
 namespace rtabmap
 {
@@ -144,6 +146,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
@@ -151,11 +154,16 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 	pcl::VoxelGrid<pcl::PointXYZ> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
+	if(indices->size())
+	{
+		filter.setIndices(indices);
+	}
 	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
@@ -163,11 +171,16 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
 	pcl::VoxelGrid<pcl::PointNormal> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
+	if(indices->size())
+	{
+		filter.setIndices(indices);
+	}
 	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
@@ -175,11 +188,16 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
+	if(indices->size())
+	{
+		filter.setIndices(indices);
+	}
 	filter.filter(*output);
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
 		float voxelSize)
 {
 	UASSERT(voxelSize > 0.0f);
@@ -187,10 +205,43 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
 	pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
+	if(indices->size())
+	{
+		filter.setIndices(indices);
+	}
 	filter.filter(*output);
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float voxelSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return voxelize(cloud, indices, voxelSize);
+}
+pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float voxelSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return voxelize(cloud, indices, voxelSize);
+}
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float voxelSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return voxelize(cloud, indices, voxelSize);
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return voxelize(cloud, indices, voxelSize);
+}
+
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
@@ -256,6 +307,39 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 	return output;
 }
 
+pcl::IndicesPtr frustumFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & cameraPose,
+		float horizontalFOV, // in degrees
+		float verticalFOV,   // in degrees
+		float nearClipPlaneDistance,
+		float farClipPlaneDistance,
+		bool negative)
+{
+	UASSERT(horizontalFOV > 0.0f && verticalFOV > 0.0f);
+	UASSERT(farClipPlaneDistance > nearClipPlaneDistance);
+	UASSERT(!cameraPose.isNull());
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::FrustumCulling<pcl::PointXYZ> fc;
+	fc.setNegative(negative);
+	fc.setInputCloud (cloud);
+	if(indices.get() && indices->size())
+	{
+		fc.setIndices(indices);
+	}
+	fc.setVerticalFOV (verticalFOV);
+	fc.setHorizontalFOV (horizontalFOV);
+	fc.setNearPlaneDistance (nearClipPlaneDistance);
+	fc.setFarPlaneDistance (farClipPlaneDistance);
+
+	fc.setCameraPose (cameraPose.toEigen4f());
+	fc.filter (*output);
+
+	return output;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr frustumFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & cameraPose,
@@ -366,6 +450,14 @@ pcl::IndicesPtr radiusFiltering(
 	pcl::IndicesPtr indices(new std::vector<int>);
 	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
 }
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
+}
 
 
 pcl::IndicesPtr radiusFiltering(
@@ -458,6 +550,51 @@ pcl::IndicesPtr radiusFiltering(
 		return output;
 	}
 }
+pcl::IndicesPtr radiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		tree->setInputCloud(cloud, indices);
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			if(k > minNeighborsInRadius)
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		tree->setInputCloud(cloud);
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			if(k > minNeighborsInRadius)
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
 
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -481,6 +618,7 @@ pcl::IndicesPtr subtractFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius)
 {
+	UASSERT(minNeighborsInRadius > 0);
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
 
 	if(indices->size())
@@ -500,7 +638,7 @@ pcl::IndicesPtr subtractFiltering(
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
 			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
-			if(k <= minNeighborsInRadius)
+			if(k < minNeighborsInRadius)
 			{
 				output->at(oi++) = indices->at(i);
 			}
@@ -525,7 +663,7 @@ pcl::IndicesPtr subtractFiltering(
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
 			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
-			if(k <= minNeighborsInRadius)
+			if(k < minNeighborsInRadius)
 			{
 				output->at(oi++) = i;
 			}
@@ -539,10 +677,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
 		float radiusSearch,
+		float maxAngle,
 		int minNeighborsInRadius)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::copyPointCloud(*cloud, *indicesOut, *out);
 	return out;
@@ -555,8 +694,10 @@ pcl::IndicesPtr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
 		const pcl::IndicesPtr & substractIndices,
 		float radiusSearch,
+		float maxAngle,
 		int minNeighborsInRadius)
 {
+	UASSERT(minNeighborsInRadius > 0);
 	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
 
 	if(indices->size())
@@ -576,7 +717,39 @@ pcl::IndicesPtr subtractFiltering(
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
 			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
-			if(k <= minNeighborsInRadius)
+			if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+			{
+				Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
+				if (uIsFinite(normal[0]) &&
+					uIsFinite(normal[1]) &&
+					uIsFinite(normal[2]))
+				{
+					int count = k;
+					for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+					{
+						Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+						if(uIsFinite(v[0]) &&
+							uIsFinite(v[1]) &&
+							uIsFinite(v[2]))
+						{
+							float angle = pcl::getAngle3D(normal, v);
+							if(angle > maxAngle)
+							{
+								k-=1;
+							}
+						}
+						else
+						{
+							k-=1;
+						}
+					}
+				}
+				else
+				{
+					k=0;
+				}
+			}
+			if(k < minNeighborsInRadius)
 			{
 				output->at(oi++) = indices->at(i);
 			}
@@ -601,7 +774,39 @@ pcl::IndicesPtr subtractFiltering(
 			std::vector<int> kIndices;
 			std::vector<float> kDistances;
 			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
-			if(k <= minNeighborsInRadius)
+			if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+			{
+				Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
+				if (uIsFinite(normal[0]) &&
+					uIsFinite(normal[1]) &&
+					uIsFinite(normal[2]))
+				{
+					int count = k;
+					for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+					{
+						Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+						if(uIsFinite(v[0]) &&
+							uIsFinite(v[1]) &&
+							uIsFinite(v[2]))
+						{
+							float angle = pcl::getAngle3D(normal, v);
+							if(angle > maxAngle)
+							{
+								k-=1;
+							}
+						}
+						else
+						{
+							k-=1;
+						}
+					}
+				}
+				else
+				{
+					k=0;
+				}
+			}
+			if(k < minNeighborsInRadius)
 			{
 				output->at(oi++) = i;
 			}
@@ -611,6 +816,230 @@ pcl::IndicesPtr subtractFiltering(
 	}
 }
 
+pcl::IndicesPtr subtractAdaptiveFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearchRatio,
+		int minNeighborsInRadius,
+		const Eigen::Vector3f & viewpoint)
+{
+	UWARN("Add angle to avoid subtraction of points with opposite normals");
+	UASSERT(minNeighborsInRadius > 0);
+	UASSERT(radiusSearchRatio > 0.0f);
+	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			if(pcl::isFinite(cloud->at(indices->at(i))))
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kSqrdDistances;
+				float radius = radiusSearchRatio*uNorm(
+							cloud->at(indices->at(i)).x-viewpoint[0],
+							cloud->at(indices->at(i)).y-viewpoint[1],
+							cloud->at(indices->at(i)).z-viewpoint[2]);
+				int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
+				if(k < minNeighborsInRadius)
+				{
+					output->at(oi++) = indices->at(i);
+				}
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			if(pcl::isFinite(cloud->at(i)))
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kSqrdDistances;
+				float radius = radiusSearchRatio*uNorm(
+						cloud->at(i).x-viewpoint[0],
+						cloud->at(i).y-viewpoint[1],
+						cloud->at(i).z-viewpoint[2]);
+				int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
+				if(k < minNeighborsInRadius)
+				{
+					output->at(oi++) = i;
+				}
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+pcl::IndicesPtr subtractAdaptiveFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearchRatio,
+		float maxAngle,
+		int minNeighborsInRadius,
+		const Eigen::Vector3f & viewpoint)
+{
+	UASSERT(minNeighborsInRadius > 0);
+	UASSERT(radiusSearchRatio > 0.0f);
+	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			if(pcl::isFinite(cloud->at(indices->at(i))))
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kSqrdDistances;
+				float radius = radiusSearchRatio*uNorm(
+						cloud->at(indices->at(i)).x-viewpoint[0],
+						cloud->at(indices->at(i)).y-viewpoint[1],
+						cloud->at(indices->at(i)).z-viewpoint[2]);
+				int k = tree->radiusSearch(cloud->at(indices->at(i)), radius, kIndices, kSqrdDistances);
+				if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+				{
+					Eigen::Vector4f normal(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
+					if (uIsFinite(normal[0]) &&
+						uIsFinite(normal[1]) &&
+						uIsFinite(normal[2]))
+					{
+						int count = k;
+						for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+						{
+							Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+							if(uIsFinite(v[0]) &&
+								uIsFinite(v[1]) &&
+								uIsFinite(v[2]))
+							{
+								float angle = pcl::getAngle3D(normal, v);
+								if(angle > maxAngle)
+								{
+									k-=1;
+								}
+							}
+							else
+							{
+								k-=1;
+							}
+						}
+					}
+					else
+					{
+						k=0;
+					}
+				}
+
+				if(k < minNeighborsInRadius)
+				{
+					output->at(oi++) = indices->at(i);
+				}
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			if(pcl::isFinite(cloud->at(i)))
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kSqrdDistances;
+				float radius = radiusSearchRatio*uNorm(
+						cloud->at(i).x-viewpoint[0],
+						cloud->at(i).y-viewpoint[1],
+						cloud->at(i).z-viewpoint[2]);
+				int k = tree->radiusSearch(cloud->at(i), radius, kIndices, kSqrdDistances);
+				if(k>=minNeighborsInRadius && maxAngle > 0.0f)
+				{
+					Eigen::Vector4f normal(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
+					if (uIsFinite(normal[0]) &&
+						uIsFinite(normal[1]) &&
+						uIsFinite(normal[2]))
+					{
+						int count = k;
+						for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
+						{
+							Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
+							if(uIsFinite(v[0]) &&
+								uIsFinite(v[1]) &&
+								uIsFinite(v[2]))
+							{
+								float angle = pcl::getAngle3D(normal, v);
+								if(angle > maxAngle)
+								{
+									k-=1;
+								}
+							}
+							else
+							{
+								k-=1;
+							}
+						}
+					}
+					else
+					{
+						k=0;
+					}
+				}
+				if(k < minNeighborsInRadius)
+				{
+					output->at(oi++) = i;
+				}
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
 
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -681,7 +1110,6 @@ pcl::IndicesPtr normalFiltering(
 
 		output->resize(cloud_normals->size());
 		int oi = 0; // output iterator
-		Eigen::Vector3f n(normal[0], normal[1], normal[2]);
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
 		{
 			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
@@ -742,7 +1170,6 @@ pcl::IndicesPtr normalFiltering(
 
 		output->resize(cloud_normals->size());
 		int oi = 0; // output iterator
-		Eigen::Vector3f n(normal[0], normal[1], normal[2]);
 		for(unsigned int i=0; i<cloud_normals->size(); ++i)
 		{
 			Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
@@ -757,6 +1184,51 @@ pcl::IndicesPtr normalFiltering(
 
 	return output;
 }
+pcl::IndicesPtr normalFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint)
+{
+	pcl::IndicesPtr output(new std::vector<int>());
+
+	if(cloud->size())
+	{
+		int oi = 0; // output iterator
+		if(indices->size())
+		{
+			output->resize(indices->size());
+			for(unsigned int i=0; i<indices->size(); ++i)
+			{
+				Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
+				float angle = pcl::getAngle3D(normal, v);
+				if(angle < angleMax)
+				{
+					output->at(oi++) = indices->size()!=0?indices->at(i):i;
+				}
+			}
+		}
+		else
+		{
+			output->resize(cloud->size());
+			for(unsigned int i=0; i<cloud->size(); ++i)
+			{
+				Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
+				float angle = pcl::getAngle3D(normal, v);
+				if(angle < angleMax)
+				{
+					output->at(oi++) = indices->size()!=0?indices->at(i):i;
+				}
+			}
+		}
+
+		output->resize(oi);
+	}
+
+	return output;
+}
 
 std::vector<pcl::IndicesPtr> extractClusters(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -883,28 +1355,141 @@ std::vector<pcl::IndicesPtr> extractClusters(
 
 	return output;
 }
+std::vector<pcl::IndicesPtr> extractClusters(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize,
+		int * biggestClusterIndex)
+{
+	typedef pcl::search::KdTree<pcl::PointXYZRGBNormal> KdTree;
+	typedef KdTree::Ptr KdTreePtr;
 
-pcl::IndicesPtr extractNegativeIndices(
+	KdTreePtr tree(new KdTree);
+	pcl::EuclideanClusterExtraction<pcl::PointXYZRGBNormal> ec;
+	ec.setClusterTolerance (clusterTolerance);
+	ec.setMinClusterSize (minClusterSize);
+	ec.setMaxClusterSize (maxClusterSize);
+	ec.setInputCloud (cloud);
+
+	if(indices->size())
+	{
+		ec.setIndices(indices);
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud(cloud);
+	}
+	ec.setSearchMethod (tree);
+
+	std::vector<pcl::PointIndices> cluster_indices;
+	ec.extract (cluster_indices);
+
+	int maxIndex=-1;
+	unsigned int maxSize = 0;
+	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
+	for(unsigned int i=0; i<cluster_indices.size(); ++i)
+	{
+		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
+
+		if(maxSize < cluster_indices[i].indices.size())
+		{
+			maxSize = (unsigned int)cluster_indices[i].indices.size();
+			maxIndex = i;
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = maxIndex;
+	}
+
+	return output;
+}
+
+pcl::IndicesPtr extractIndices(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices)
+		const pcl::IndicesPtr & indices,
+		bool negative)
 {
 	pcl::IndicesPtr output(new std::vector<int>);
 	pcl::ExtractIndices<pcl::PointXYZ> extract;
 	extract.setInputCloud (cloud);
 	extract.setIndices(indices);
-	extract.setNegative(true);
+	extract.setNegative(negative);
 	extract.filter(*output);
 	return output;
 }
-pcl::IndicesPtr extractNegativeIndices(
+pcl::IndicesPtr extractIndices(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const pcl::IndicesPtr & indices)
+		const pcl::IndicesPtr & indices,
+		bool negative)
 {
 	pcl::IndicesPtr output(new std::vector<int>);
 	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
 	extract.setInputCloud (cloud);
 	extract.setIndices(indices);
-	extract.setNegative(true);
+	extract.setNegative(negative);
+	extract.filter(*output);
+	return output;
+}
+pcl::IndicesPtr extractIndices(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative)
+{
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(negative);
+	extract.filter(*output);
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr extractIndices(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::ExtractIndices<pcl::PointXYZ> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(negative);
+	extract.setKeepOrganized(keepOrganized);
+	extract.filter(*output);
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr extractIndices(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized)
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(negative);
+	extract.setKeepOrganized(keepOrganized);
+	extract.filter(*output);
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr extractIndices(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		bool negative,
+		bool keepOrganized)
+{
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::ExtractIndices<pcl::PointXYZRGBNormal> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(negative);
+	extract.setKeepOrganized(keepOrganized);
 	extract.filter(*output);
 	return output;
 }

corelib/src/util3d_motion_estimation.cpp

@@ -208,6 +208,7 @@ Transform estimateMotion3DTo3D(
 			0,
 			&matches);
 	UASSERT(inliers1.size() == inliers2.size());
+	UDEBUG("Unique correspondences = %d", (int)inliers1.size());
 
 	if(varianceOut)
 	{

corelib/src/util3d_surface.cpp

@@ -36,17 +36,287 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/features/normal_3d.h>
 #include <pcl/surface/mls.h>
 #include <pcl/surface/texture_mapping.h>
+#include <pcl/features/integral_image_normal.h>
 
 #ifndef DISABLE_VTK
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 #endif
 
+#if PCL_VERSION_COMPARE(<, 1, 8, 0)
+#include "pcl18/surface/organized_fast_mesh.h"
+#else
+#include <pcl/surface/organized_fast_mesh.h>
+#endif
+
 namespace rtabmap
 {
 
 namespace util3d
 {
 
+void createPolygonIndexes(
+		const std::vector<pcl::Vertices> & polygons,
+		int cloudSize,
+		std::vector<std::set<int> > & neighbors,
+		std::vector<std::set<int> > & vertexToPolygons)
+{
+	vertexToPolygons = std::vector<std::set<int> >(cloudSize);
+	neighbors = std::vector<std::set<int> >(polygons.size());
+
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());
+
+		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
+		{
+			int v = polygons[i].vertices.at(j);
+			for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
+			{
+				int numSharedVertices = 0;
+				for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
+				{
+					if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
+					{
+						++numSharedVertices;
+					}
+				}
+				if(numSharedVertices >= 2)
+				{
+					neighbors[*iter].insert(i);
+					neighbors[i].insert(*iter);
+				}
+			}
+			vertexToPolygons[v].insert(i);
+		}
+	}
+}
+
+std::vector<pcl::Vertices> organizedFastMesh(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		double angleTolerance,
+		bool quad,
+		int trianglePixelSize)
+{
+	UASSERT(cloud->is_dense == false);
+	UASSERT(cloud->width > 1 && cloud->height > 1);
+
+	pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
+	ofm.setTrianglePixelSize (trianglePixelSize);
+	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
+	ofm.setInputCloud (cloud);
+	ofm.setAngleTolerance(angleTolerance);
+	std::vector<pcl::Vertices> vertices;
+	ofm.reconstruct (vertices);
+
+	if(quad)
+	{
+		//flip all polygons (right handed)
+		std::vector<pcl::Vertices> output(vertices.size());
+		for(unsigned int i=0; i<vertices.size(); ++i)
+		{
+			output[i].vertices.resize(4);
+			output[i].vertices[0] = vertices[i].vertices[0];
+			output[i].vertices[3] = vertices[i].vertices[1];
+			output[i].vertices[2] = vertices[i].vertices[2];
+			output[i].vertices[1] = vertices[i].vertices[3];
+		}
+		return output;
+	}
+
+	return vertices;
+}
+std::vector<pcl::Vertices> organizedFastMesh(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		double angleTolerance,
+		bool quad,
+		int trianglePixelSize)
+{
+	UDEBUG("size=%d angle=%f quand=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
+	UASSERT(cloud->is_dense == false);
+	UASSERT(cloud->width > 1 && cloud->height > 1);
+
+	pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
+	ofm.setTrianglePixelSize (trianglePixelSize);
+	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
+	ofm.setInputCloud (cloud);
+	ofm.setAngleTolerance(angleTolerance);
+	std::vector<pcl::Vertices> vertices;
+	ofm.reconstruct (vertices);
+
+	if(quad)
+	{
+		//flip all polygons (right handed)
+		std::vector<pcl::Vertices> output(vertices.size());
+		for(unsigned int i=0; i<vertices.size(); ++i)
+		{
+			output[i].vertices.resize(4);
+			output[i].vertices[0] = vertices[i].vertices[0];
+			output[i].vertices[3] = vertices[i].vertices[1];
+			output[i].vertices[2] = vertices[i].vertices[2];
+			output[i].vertices[1] = vertices[i].vertices[3];
+		}
+		return output;
+	}
+
+	return vertices;
+}
+
+void appendMesh(
+		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
+		std::vector<pcl::Vertices> & polygonsA,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & 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;
+	}
+}
+
+void filterNotUsedVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
+		std::vector<pcl::Vertices> & outputPolygons)
+{
+	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
+	std::map<int, int> addedVertices; //<oldIndex, newIndex>
+	outputCloud.resize(cloud.size());
+	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));
+				v.vertices[j] = oi++;
+			}
+			else
+			{
+				v.vertices[j] = iter->second;
+			}
+		}
+	}
+	outputCloud.resize(oi);
+}
+
+std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		float radius,
+		float angle,
+		bool keepLatestInRadius)
+{
+	UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
+			(int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
+	std::vector<pcl::Vertices> outputPolygons;
+	pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
+	kdtree->setInputCloud(cloud);
+
+	std::map<int, int> verticesDone;
+	outputPolygons = polygons;
+	for(unsigned int i=0; i<outputPolygons.size(); ++i)
+	{
+		pcl::Vertices & polygon = outputPolygons[i];
+		for(unsigned int j=0; j<polygon.vertices.size(); ++j)
+		{
+			std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
+			if(iter != verticesDone.end())
+			{
+				polygon.vertices[j] = iter->second;
+			}
+			else
+			{
+				std::vector<int> kIndices;
+				std::vector<float> kDistances;
+				kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
+				if(kIndices.size())
+				{
+					int reference = -1;
+					for(unsigned int z=0; z<kIndices.size(); ++z)
+					{
+						if(reference == -1)
+						{
+							reference = kIndices[z];
+						}
+						else if(keepLatestInRadius)
+						{
+							if(kIndices[z] < reference)
+							{
+								reference = kIndices[z];
+							}
+						}
+						else
+						{
+							if(kIndices[z] > reference)
+							{
+								reference = kIndices[z];
+							}
+						}
+					}
+					if(reference >= 0)
+					{
+						for(unsigned int z=0; z<kIndices.size(); ++z)
+						{
+							verticesDone.insert(std::make_pair(kIndices[j], reference));
+						}
+						polygon.vertices[j] = reference;
+					}
+				}
+				else
+				{
+					verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
+				}
+			}
+		}
+	}
+	return outputPolygons;
+}
+
+std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
+{
+	std::vector<pcl::Vertices> output(polygons.size());
+	int oi=0;
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		bool valid = true;
+		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
+		{
+			if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
+			{
+				valid = false;
+				break;
+			}
+		}
+		if(valid)
+		{
+			output[oi++] = polygons[i];
+		}
+	}
+	output.resize(oi);
+	return output;
+}
+
 pcl::PolygonMesh::Ptr createMesh(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
 		float gp3SearchRadius,
@@ -149,9 +419,6 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	// Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
 	// Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
-
 	// Create the texturemesh object that will contain our UV-mapped mesh
 	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
 	textureMesh->cloud = mesh->cloud;
@@ -223,10 +490,23 @@ pcl::TextureMesh::Ptr createTextureMesh(
 	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
 	tm.textureMeshwithMultipleCameras(*textureMesh, cameras);
 
-	// compute normals for the mesh
-	pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
-
-	pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
+	// compute normals for the mesh if not already here
+	bool hasNormals = false;
+	for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
+	{
+		if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
+		{
+			hasNormals = true;
+			break;
+		}
+	}
+	if(!hasNormals)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals = computeNormals(cloud, 20);
+		pcl::toPCLPointCloud2 (*cloudWithNormals, textureMesh->cloud);
+	}
 
 	return textureMesh;
 }
@@ -234,15 +514,34 @@ pcl::TextureMesh::Ptr createTextureMesh(
 pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int normalKSearch)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return computeNormals(cloud, indices, normalKSearch);
+}
+pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		int normalKSearch)
 {
 	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
 	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
-	tree->setInputCloud (cloud);
+	if(indices->size())
+	{
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud (cloud);
+	}
 
 	// Normal estimation*
 	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
 	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
 	n.setInputCloud (cloud);
+	if(indices->size())
+	{
+		n.setIndices(indices);
+	}
 	n.setSearchMethod (tree);
 	n.setKSearch (normalKSearch);
 	n.compute (*normals);
@@ -258,15 +557,34 @@ pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int normalKSearch)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return computeNormals(cloud, indices, normalKSearch);
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		int normalKSearch)
 {
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
-	tree->setInputCloud (cloud);
+	if(indices->size())
+	{
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud (cloud);
+	}
 
 	// Normal estimation*
 	pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
 	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
 	n.setInputCloud (cloud);
+	if(indices->size())
+	{
+		n.setIndices(indices);
+	}
 	n.setSearchMethod (tree);
 	n.setKSearch (normalKSearch);
 	n.compute (*normals);
@@ -279,6 +597,43 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
 	return cloud_with_normals;
 }
 
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize);
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize)
+{
+	UASSERT(cloud->isOrganized());
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+
+	// Normal estimation
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
+	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
+	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
+	ne.setNormalSmoothingSize(normalSmoothingSize);
+	ne.setInputCloud(cloud);
+	if(indices->size())
+	{
+		ne.setIndices(indices);
+	}
+	ne.compute(*normals);
+
+	// Concatenate the XYZ and normal fields
+	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
+
+	return cloud_with_normals;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius,
@@ -289,10 +644,42 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 		int pointDensity,             // RANDOM_UNIFORM_DENSITY
 		float dilationVoxelSize,     // VOXEL_GRID_DILATION
 		int dilationIterations)       // VOXEL_GRID_DILATION
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return mls(cloud,
+			indices,
+			searchRadius,
+			polygonialOrder,
+			upsamplingMethod,
+			upsamplingRadius,
+			upsamplingStep,
+			pointDensity,
+			dilationVoxelSize,
+			dilationIterations);
+}
+
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float searchRadius,
+		int polygonialOrder,
+		int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
+		float upsamplingRadius,      // SAMPLE_LOCAL_PLANE
+		float upsamplingStep,        // SAMPLE_LOCAL_PLANE
+		int pointDensity,             // RANDOM_UNIFORM_DENSITY
+		float dilationVoxelSize,     // VOXEL_GRID_DILATION
+		int dilationIterations)       // VOXEL_GRID_DILATION
 {
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
-	tree->setInputCloud (cloud);
+	if(indices->size())
+	{
+		tree->setInputCloud (cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud (cloud);
+	}
 
 	// Init object (second point type is for the normals)
 	pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
@@ -320,6 +707,10 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 
 	// Reconstruct
 	mls.setInputCloud (cloud);
+	if(indices->size())
+	{
+		mls.setIndices(indices);
+	}
 	mls.setSearchMethod (tree);
 	mls.process (*cloud_with_normals);