New feature: Depth confidence (#1520)

* New feature: Depth confidence * iOS app updated to save depth confidence, added util2d::depthBleedingFiltering function * Updated tools to show/extract depth confidence * Android: moved smoothing in post-processing, fixed confidence registration, added depth bleeding error option. * Fixed warning * removed debug log * Added new feature types, fixed rendering when exporting texture >4096 (#1469), added depth bleeding filter option to iOS * fixed some warnings, android: added bleeding error option * CI: try updating ros2 key * added sudo * antoher test * bump ios app version
2026-09-02 09:30:25 +08:00 · 2025-06-01 14:14:29 -07:00
parent 90d195237f
commit 6d4e8a4173
51 changed files with 2368 additions and 1006 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -816,6 +816,7 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql.in ${CMA

 SET(RESOURCES
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
+	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_3.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_18_0.sql
 	${CMAKE_CURRENT_SOURCE_DIR}/resources/backward_compatibility/DatabaseSchema_0_17_0.sql
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Compression.h"
 #include "DatabaseSchema_sql.h"
+#include "DatabaseSchema_0_20_0_sql.h"
 #include "DatabaseSchema_0_18_3_sql.h"
 #include "DatabaseSchema_0_18_0_sql.h"
 #include "DatabaseSchema_0_17_0_sql.h"
@@ -402,6 +403,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 			schemas.push_back(std::make_pair("0.17.0", DATABASESCHEMA_0_17_0_SQL));
 			schemas.push_back(std::make_pair("0.18.0", DATABASESCHEMA_0_18_0_SQL));
 			schemas.push_back(std::make_pair("0.18.3", DATABASESCHEMA_0_18_3_SQL));
+			schemas.push_back(std::make_pair("0.20.0", DATABASESCHEMA_0_20_0_SQL));
 			schemas.push_back(std::make_pair(uNumber2Str(RTABMAP_VERSION_MAJOR)+"."+uNumber2Str(RTABMAP_VERSION_MINOR), DATABASESCHEMA_SQL));
 			for(size_t i=0; i<schemas.size(); ++i)
 			{
@@ -1317,7 +1319,15 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo

 			if(images)
 			{
-				fields << "image, depth, calibration";
+				if(uStrNumCmp(_version, "0.22.0") >= 0)
+				{
+					fields << "image, depth, depth_confidence, calibration";
+				}
+				else
+				{
+					fields << "image, depth, calibration";
+				}
+				
 				if(scan || userData || occupancyGrid)
 				{
 					fields << ", ";
@@ -1448,6 +1458,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo

 				cv::Mat imageCompressed;
 				cv::Mat depthOrRightCompressed;
+				cv::Mat depthConfidenceCompressed;
 				std::vector<CameraModel> models;
 				std::vector<StereoCameraModel> stereoModels;
 				Transform localTransform = Transform::getIdentity();
@@ -1472,6 +1483,17 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
 					}

+					if(uStrNumCmp(_version, "0.22.0") >= 0)
+					{
+						//Create the depth image
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if(dataSize>4 && data)
+						{
+							depthConfidenceCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+						}
+					}
+
 					if(uStrNumCmp(_version, "0.10.0") < 0)
 					{
 						data = sqlite3_column_blob(ppStmt, index); // local transform
@@ -1823,7 +1845,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				{
 					if(models.size())
 					{
-						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, models);
+						(*iter)->sensorData().setRGBDImage(imageCompressed, depthOrRightCompressed, depthConfidenceCompressed, models);
 					}
 					else
 					{
@@ -4478,6 +4500,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 			{
 				if(!(*i)->sensorData().imageCompressed().empty() ||
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
+				   !(*i)->sensorData().depthConfidenceCompressed().empty() ||
 				   !(*i)->sensorData().laserScanCompressed().isEmpty() ||
 				   !(*i)->sensorData().userDataCompressed().empty() ||
 				   !(*i)->sensorData().cameraModels().empty() ||
@@ -6186,7 +6209,11 @@ void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const La
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.16.0") >= 0)
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, depth_confidence, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.16.0") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, empty_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?,?);";
 	}
@@ -6250,6 +6277,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

+	//depth confidence
+	if(uStrNumCmp(_version, "0.22.0") >= 0)
+	{
+		if(!sensorData.depthConfidenceCompressed().empty())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, sensorData.depthConfidenceCompressed().data, (int)sensorData.depthConfidenceCompressed().cols, SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+		}
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
 	// calibration
 	std::vector<unsigned char> calibrationData;
 	std::vector<float> calibration;
--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -607,6 +607,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				// update images and local transforms
 				cv::Mat combinedImages;
 				cv::Mat combinedDepthImages;
+				cv::Mat combinedDepthConfidenceImages;
 				std::vector<CameraModel> combinedModels;
 				std::vector<StereoCameraModel> combinedStereoModels;
 				for(size_t i=0; i<_cameraIndices.size(); ++i)
@@ -645,6 +646,18 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 						fromROI = cv::Mat(data.depthOrRightRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthOrRightRaw().rows));
 						toROI = cv::Mat(combinedDepthImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthImages.rows));
 						fromROI.copyTo(toROI);
+
+						if(!data.depthConfidenceRaw().empty())
+						{
+							UASSERT(data.depthConfidenceRaw().size() == data.depthOrRightRaw().size());
+							if(combinedDepthConfidenceImages.empty())
+							{
+								combinedDepthConfidenceImages = cv::Mat(data.depthConfidenceRaw().rows, subImageWidth*(_cameraIndices.size()-i), data.depthConfidenceRaw().type());
+							}
+							fromROI = cv::Mat(data.depthConfidenceRaw(), cv::Rect(_cameraIndices[i]*subImageWidth, 0, subImageWidth, data.depthConfidenceRaw().rows));
+							toROI = cv::Mat(combinedDepthConfidenceImages, cv::Rect(addedCameras*subImageWidth, 0, subImageWidth, combinedDepthConfidenceImages.rows));
+							fromROI.copyTo(toROI);
+						}
 					}

 					if(!data.cameraModels().empty())
@@ -671,7 +684,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 				}
 				if(!combinedModels.empty())
 				{
-					data.setRGBDImage(combinedImages, combinedDepthImages, combinedModels);
+					data.setRGBDImage(combinedImages, combinedDepthImages, combinedDepthConfidenceImages, combinedModels);
 				}
 				else
 				{
@@ -728,10 +741,11 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
 			}
 			data.setLandmarks(landmarks);

-			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Conf=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
 					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,
 					data.depthOrRightRaw().empty()?0:1,
+					data.depthConfidenceRaw().empty()?0:1,
 					data.gridCellSize()==0.0f?0:1,
 					data.userDataRaw().empty()?0:1,
 					globalPose.isNull()?0:1,
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -4893,9 +4893,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					else
 					{
 						decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
+						UASSERT(data.depthConfidenceRaw().empty() || data.depthConfidenceRaw().size() == data.depthRaw().size());
 					}
 				}
-				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth);
+				UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d (conf? %d)", data.imageRaw().rows, _imagePreDecimation, data.depthOrRightRaw().rows, decimationDepth, data.depthConfidenceRaw().empty()?0:1);

 				std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
 				for(unsigned int i=0; i<cameraModels.size(); ++i)
@@ -4907,6 +4908,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					decimatedData.setRGBDImage(
 							util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
 							util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
+							util2d::decimate(decimatedData.depthConfidenceRaw(), decimationDepth),
 							cameraModels);
 				}

@@ -5656,6 +5658,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
+	cv::Mat depthConfidence = data.depthConfidenceRaw();
+
 	std::vector<CameraModel> cameraModels = data.cameraModels();
 	std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();

@@ -5666,6 +5670,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
+			depthConfidence = decimatedData.depthConfidenceRaw();
 			cameraModels = decimatedData.cameraModels();
 			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
@@ -5879,9 +5884,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	Signature * s;
 	if(this->isBinDataKept() && (!isIntermediateNode || _saveIntermediateNodeData))
 	{
-		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
+		UDEBUG("Bin data kept: rgb=%d, depth=%d, conf=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
 				depthOrRightImage.empty()?0:1,
+				depthConfidence.empty()?0:1,
 				laserScan.isEmpty()?0:1,
 				data.userDataRaw().empty()?0:1);

@@ -5928,12 +5934,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 		cv::Mat compressedImage;
 		cv::Mat compressedDepth;
+		cv::Mat compressedDepthConfidence;
 		cv::Mat compressedScan;
 		cv::Mat compressedUserData;
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
 			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			if(!image.empty())
@@ -5944,6 +5952,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepth.start();
 			}
+			if(!depthConfidence.empty())
+			{
+				ctDepthConfidence.start();
+			}
 			if(!laserScan.isEmpty())
 			{
 				ctLaserScan.start();
@@ -5954,11 +5966,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			ctImage.join();
 			ctDepth.join();
+			ctDepthConfidence.join();
 			ctLaserScan.join();
 			ctUserData.join();

 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
+			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
 			compressedScan = ctLaserScan.getCompressedData();
 			compressedUserData = ctUserData.getCompressedData();
 		}
@@ -5966,6 +5980,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			compressedImage = compressImage2(image, _rgbCompressionFormat);
 			compressedDepth = compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			compressedDepthConfidence = compressData2(depthConfidence);
 			compressedScan = compressData2(laserScan.data());
 			compressedUserData = compressData2(data.userDataRaw());
 		}
@@ -6016,6 +6031,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.localTransform()),
 						compressedImage.empty()?data.imageCompressed():compressedImage,
 						compressedDepth.empty()?data.depthOrRightCompressed():compressedDepth,
+						compressedDepthConfidence.empty()?data.depthConfidenceCompressed():compressedDepthConfidence,
 						cameraModels,
 						id,
 						0,
@@ -6113,7 +6129,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	// set raw data
 	if(!cameraModels.empty())
 	{
-		s->sensorData().setRGBDImage(image, depthOrRightImage, cameraModels, false);
+		s->sensorData().setRGBDImage(image, depthOrRightImage, depthConfidence, cameraModels, false);
 	}
 	else
 	{
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -107,6 +107,25 @@ SensorData::SensorData(
 	setUserData(userData);
 }

+// RGB-D constructor + confidence + laser scan
+SensorData::SensorData(
+		const LaserScan & laserScan,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const CameraModel & cameraModel,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModel);
+	setLaserScan(laserScan);
+	setUserData(userData);
+}
+
 // Multi-cameras RGB-D constructor
 SensorData::SensorData(
 		const cv::Mat & rgb,
@@ -123,6 +142,23 @@ SensorData::SensorData(
 	setUserData(userData);
 }

+// Multi-cameras RGB-D constructor + confidence
+SensorData::SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModels);
+	setUserData(userData);
+}
+
 // Multi-cameras RGB-D constructor + laser scan
 SensorData::SensorData(
 		const LaserScan & laserScan,
@@ -141,6 +177,25 @@ SensorData::SensorData(
 	setUserData(userData);
 }

+// Multi-cameras RGB-D constructor + confidence + laser scan
+SensorData::SensorData(
+		const LaserScan & laserScan,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
+		const std::vector<CameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setRGBDImage(rgb, depth, depthConfidence, cameraModels);
+	setLaserScan(laserScan);
+	setUserData(userData);
+}
+
 // Stereo constructor
 SensorData::SensorData(
 		const cv::Mat & left,
@@ -234,9 +289,29 @@ void SensorData::setRGBDImage(
 	models.push_back(model);
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
+void SensorData::setRGBDImage(
+	const cv::Mat & rgb,
+	const cv::Mat & depth,
+	const cv::Mat & depthConfidence,
+	const CameraModel & model,
+	bool clearPreviousData)
+{
+	std::vector<CameraModel> models;
+	models.push_back(model);
+	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
+}
+void SensorData::setRGBDImage(
+	const cv::Mat & rgb,
+	const cv::Mat & depth,
+	const std::vector<CameraModel> & models,
+	bool clearPreviousData)
+{
+	setRGBDImage(rgb, depth, models, clearPreviousData);
+}
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
+		const cv::Mat & depthConfidence,
 		const std::vector<CameraModel> & models,
 		bool clearPreviousData)
 {
@@ -300,6 +375,30 @@ void SensorData::setRGBDImage(
 		_depthOrRightRaw = cv::Mat();
 		_depthOrRightCompressed = cv::Mat();
 	}
+
+	if(depthConfidence.rows == 1)
+	{
+		UASSERT(depthConfidence.type() == CV_8UC1); // Bytes
+		_depthConfidenceCompressed = depthConfidence;
+		if(clearData)
+		{
+			_depthConfidenceRaw = cv::Mat();
+		}
+	}
+	else if(!depthConfidence.empty())
+	{
+		UASSERT(depthConfidence.type() == CV_8UC1);
+		_depthConfidenceRaw = depthConfidence;
+		if(clearData)
+		{
+			_depthConfidenceCompressed = cv::Mat();
+		}
+	}
+	else if(clearData)
+	{
+		_depthConfidenceRaw = cv::Mat();
+		_depthConfidenceCompressed = cv::Mat();
+	}
 }
 void SensorData::setStereoImage(
 		const cv::Mat & left,
@@ -528,7 +627,7 @@ void SensorData::setOccupancyGrid(

 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
+	cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG, tmpH;
 	LaserScan tmpC;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
@@ -536,7 +635,8 @@ void SensorData::uncompressData()
 				_userDataCompressed.empty()?0:&tmpD,
 				_groundCellsCompressed.empty()?0:&tmpE,
 				_obstacleCellsCompressed.empty()?0:&tmpF,
-				_emptyCellsCompressed.empty()?0:&tmpG);
+				_emptyCellsCompressed.empty()?0:&tmpG,
+				_depthConfidenceCompressed.empty()?0:&tmpH);
 }

 void SensorData::uncompressData(
@@ -546,16 +646,27 @@ void SensorData::uncompressData(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw)
+		cv::Mat * emptyCellsRaw,
+		cv::Mat * depthConfidenceRaw)
 {
-	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
+	UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d,%d)", 
+		this->id(),
+		imageRaw?1:0,
+		depthRaw?1:0,
+		laserScanRaw?1:0,
+		userDataRaw?1:0,
+		groundCellsRaw?1:0,
+		obstacleCellsRaw?1:0, 
+		emptyCellsRaw?1:0,
+		depthConfidenceRaw?1:0);
 	if(imageRaw == 0 &&
 		depthRaw == 0 &&
 		laserScanRaw == 0 &&
 		userDataRaw == 0 &&
 		groundCellsRaw == 0 &&
 		obstacleCellsRaw == 0 &&
-		emptyCellsRaw == 0)
+		emptyCellsRaw == 0 &&
+		depthConfidenceRaw == 0)
 	{
 		return;
 	}
@@ -566,7 +677,8 @@ void SensorData::uncompressData(
 			userDataRaw,
 			groundCellsRaw,
 			obstacleCellsRaw,
-			emptyCellsRaw);
+			emptyCellsRaw,
+			depthConfidenceRaw);

 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
@@ -588,6 +700,10 @@ void SensorData::uncompressData(
 	{
 		_depthOrRightRaw = *depthRaw;
 	}
+	if(depthConfidenceRaw && !depthConfidenceRaw->empty() && _depthConfidenceRaw.empty())
+	{
+		_depthConfidenceRaw = *depthConfidenceRaw;
+	}
 	if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
 	{
 		_laserScanRaw = *laserScanRaw;
@@ -628,7 +744,8 @@ void SensorData::uncompressDataConst(
 		cv::Mat * userDataRaw,
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw,
-		cv::Mat * emptyCellsRaw) const
+		cv::Mat * emptyCellsRaw,
+		cv::Mat * depthConfidenceRaw) const
 {
 	if(imageRaw)
 	{
@@ -638,6 +755,10 @@ void SensorData::uncompressDataConst(
 	{
 		*depthRaw = _depthOrRightRaw;
 	}
+	if(depthConfidenceRaw)
+	{
+		*depthConfidenceRaw = _depthConfidenceRaw;
+	}
 	if(laserScanRaw)
 	{
 		*laserScanRaw = _laserScanRaw;
@@ -660,6 +781,7 @@ void SensorData::uncompressDataConst(
 	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
+		(depthConfidenceRaw && depthConfidenceRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->isEmpty()) ||
 		(userDataRaw && userDataRaw->empty()) ||
 		(groundCellsRaw && groundCellsRaw->empty()) ||
@@ -668,6 +790,7 @@ void SensorData::uncompressDataConst(
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
+		rtabmap::CompressionThread ctDepthConfidence(_depthConfidenceCompressed, false);
 		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed.data(), false);
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
 		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
@@ -683,6 +806,11 @@ void SensorData::uncompressDataConst(
 			UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
 			ctDepth.start();
 		}
+		if(depthConfidenceRaw && depthConfidenceRaw->empty() && !_depthConfidenceCompressed.empty())
+		{
+			UASSERT(_depthConfidenceCompressed.type() == CV_8UC1);
+			ctDepthConfidence.start();
+		}
 		if(laserScanRaw && laserScanRaw->isEmpty() && !_laserScanCompressed.isEmpty())
 		{
 			UASSERT(_laserScanCompressed.isCompressed());
@@ -710,6 +838,7 @@ void SensorData::uncompressDataConst(
 		}
 		ctImage.join();
 		ctDepth.join();
+		ctDepthConfidence.join();
 		ctLaserScan.join();
 		ctUserData.join();
 		ctGroundCells.join();
@@ -746,6 +875,21 @@ void SensorData::uncompressDataConst(
 				}
 			}
 		}
+		if(depthConfidenceRaw && depthConfidenceRaw->empty())
+		{
+			*depthConfidenceRaw = ctDepthConfidence.getUncompressedData();
+			if(depthConfidenceRaw->empty())
+			{
+				if(_depthConfidenceCompressed.empty())
+				{
+					UWARN("Requested depth confidence data, but the sensor data (%d) doesn't have depth confidence.", this->id());
+				}
+				else
+				{
+					UERROR("Requested depth confidence data, but failed to uncompress (%d).", this->id());
+				}
+			}
+		}
 		if(laserScanRaw && laserScanRaw->isEmpty())
 		{
 			if(_laserScanCompressed.angleIncrement() > 0.0f)
@@ -815,6 +959,8 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			(_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
 			(_depthOrRightCompressed.empty()?0:_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize()) +
 			(_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
+			(_depthConfidenceCompressed.empty()?0:_depthConfidenceCompressed.total()*_depthConfidenceCompressed.elemSize()) +
+			(_depthConfidenceRaw.empty()?0:_depthConfidenceRaw.total()*_depthConfidenceRaw.elemSize()) +
 			(_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
 			(_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
 			(_laserScanCompressed.empty()?0:_laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize()) +
@@ -836,6 +982,7 @@ void SensorData::clearCompressedData(bool images, bool scan, bool userData)
 	{
 		_imageCompressed=cv::Mat();
 		_depthOrRightCompressed=cv::Mat();
+		_depthConfidenceCompressed=cv::Mat();
 	}
 	if(scan)
 	{
@@ -852,6 +999,7 @@ void SensorData::clearRawData(bool images, bool scan, bool userData)
 	{
 		_imageRaw=cv::Mat();
 		_depthOrRightRaw=cv::Mat();
+		_depthConfidenceRaw=cv::Mat();
 	}
 	if(scan)
 	{
--- a/corelib/src/resources/DatabaseSchema.sql.in
+++ b/corelib/src/resources/DatabaseSchema.sql.in
@@ -32,6 +32,7 @@ CREATE TABLE Data (
 	id INTEGER NOT NULL,
 	image BLOB,               -- compressed image (Grayscale or RGB)
 	depth BLOB,               -- compressed image (Depth or Right image)
+	depth_confidence BLOB,    -- compressed data (low=0 high=100)
 	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
 	
 	scan BLOB,                -- compressed data (Laser scan)
--- a/corelib/src/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
+++ b/corelib/src/resources/backward_compatibility/DatabaseSchema_0_20_0.sql
@@ -0,0 +1,182 @@
+-- *******************************************************************
+--  DatabaseSchema: Script for creating the database
+--   Usage:
+--       $ sqlite3 LTM.db < DatabaseSchema.sql
+--
+-- *******************************************************************
+
+-- *******************************************************************
+-- CLEAN
+-- *******************************************************************
+/*DROP TABLE Node;*/
+
+-- *******************************************************************
+-- CREATE
+-- *******************************************************************
+CREATE TABLE Node (
+	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
+	weight INTEGER,
+	stamp FLOAT,
+	pose BLOB,                -- 3x4 float
+	ground_truth_pose BLOB,   -- 3x4 float
+	velocity BLOB,            -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
+	label TEXT,
+	gps BLOB,                 -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
+	env_sensors BLOB,         -- Variable 3xdouble: (sensorId1, value, stamp, sensorId2, value, stamp, ...)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+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,] width, height, local_transform
+	
+	scan BLOB,                -- compressed data (Laser scan)
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, scan_format, local_transform
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	empty_cells BLOB,         -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
+	user_data BLOB,           -- compressed data (User data)
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Link (
+	from_id INTEGER NOT NULL,
+	to_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,    -- kNeighbor=0, kGlobalClosure=1, kLocalSpaceClosure=2, kLocalTimeClosure=3, kUserClosure=4, kVirtualClosure=5, kNeighborMerged=6, kPosePrior=7, kLandmark=8
+	information_matrix BLOB NOT NULL, -- 6x6 double (inverse covariance)
+	transform BLOB,           -- 3x4 float
+	user_data BLOB,           -- compressed data (User data)
+	FOREIGN KEY (from_id) REFERENCES Node(id),
+	FOREIGN KEY (to_id) REFERENCES Node(id)
+);
+
+-- 
+CREATE TABLE Word (
+	id INTEGER NOT NULL,
+	descriptor_size INTEGER NOT NULL,
+	descriptor BLOB NOT NULL,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Feature (
+	node_id INTEGER NOT NULL,
+	word_id INTEGER NOT NULL,
+	pos_x FLOAT NOT NULL,
+	pos_y FLOAT NOT NULL,
+	size INTEGER NOT NULL,
+	dir FLOAT NOT NULL,
+	response FLOAT NOT NULL,
+	octave INTEGER NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
+	descriptor_size INTEGER,
+	descriptor BLOB,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+CREATE TABLE GlobalDescriptor (
+	node_id INTEGER NOT NULL,
+	type INTEGER NOT NULL,
+	info BLOB,
+	data BLOB NOT NULL,
+	FOREIGN KEY (node_id) REFERENCES Node(id)
+);
+
+--
+
+CREATE TABLE Info (
+	STM_size INTEGER,
+	last_sign_added INTEGER,
+	process_mem_used INTEGER,
+	database_mem_used INTEGER,
+	dictionary_size INTEGER,
+	parameters TEXT,
+	time_enter DATE
+);
+
+CREATE TABLE Statistics (
+	id INTEGER NOT NULL,
+	stamp FLOAT,
+	data BLOB,              -- compressed string
+	wm_state BLOB,	        -- compressed data
+	FOREIGN KEY (id) REFERENCES Node(id)
+);
+
+CREATE TABLE Admin (
+	version TEXT,
+	preview_image BLOB,      -- compressed image
+	
+	opt_cloud BLOB,          -- compressed data
+	opt_ids BLOB,            -- Node ids used to generate the optimized cloud/mesh
+	opt_poses BLOB,          -- compressed N*3x4 float 
+	opt_last_localization BLOB, -- 3x4 float
+	opt_polygons_size INTEGER, -- e.g., 3
+	opt_polygons BLOB,       -- compressed data [length_v0, i0,i1,i3, length_v1, i0,i1,i3]
+	opt_tex_coords BLOB,     -- compressed data [length_v0, u0,v0,u1,v1,u2,v2, length_v1, u0,v0,u1,v1,u2,v2]
+	opt_tex_materials BLOB,  -- compressed image
+	opt_map BLOB,            -- compressed CV_8SC1 occupancy grid
+	opt_map_x_min FLOAT,
+	opt_map_y_min FLOAT, 
+	opt_map_resolution FLOAT, 
+
+	time_enter DATE
+);
+
+-- *******************************************************************
+-- TRIGGERS
+-- *******************************************************************
+CREATE TRIGGER insert_Feature BEFORE INSERT ON Feature 
+WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
+BEGIN
+ SELECT RAISE(ABORT, 'Foreign key constraint failed in Feature table');
+END;
+
+ --   Creating a trigger for time_enter
+CREATE TRIGGER insert_Node_timeEnter AFTER INSERT ON Node
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Data_timeEnter AFTER INSERT ON Data
+BEGIN
+ UPDATE Node SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Word_timeEnter AFTER INSERT ON Word
+BEGIN
+ UPDATE Word SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+CREATE TRIGGER insert_Info_timeEnter AFTER INSERT ON Info
+BEGIN
+ UPDATE Info SET time_enter = DATETIME('NOW')  WHERE rowid = new.rowid;
+END;
+
+-- *******************************************************************
+-- INDEXES
+-- *******************************************************************
+CREATE UNIQUE INDEX IDX_Node_id on Node (id);
+CREATE INDEX IDX_Feature_node_id on Feature (node_id);
+CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
+CREATE INDEX IDX_Link_from_id on Link (from_id);
+CREATE UNIQUE INDEX IDX_node_label on Node (label);
+CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);
+
+-- *******************************************************************
+-- VERSION
+-- *******************************************************************
+INSERT INTO Admin(version) VALUES('0.20.0');
+
--- a/corelib/src/util2d.cpp
+++ b/corelib/src/util2d.cpp
@@ -1359,18 +1359,38 @@ cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
 }

 // Registration Depth to RGB (return registered depth image)
+cv::Mat registerDepth(
+	const cv::Mat & depth,
+	const cv::Mat & depthK,
+	const cv::Size & colorSize,
+	const cv::Mat & colorK,
+	const rtabmap::Transform & transform)
+{
+	cv::Mat tmp;
+	return registerDepth(
+		depth,
+		cv::Mat(),
+		depthK,
+		colorSize,
+		colorK,
+		transform,
+		tmp);
+}
 cv::Mat registerDepth(
 		const cv::Mat & depth,
+		const cv::Mat & confidence,
 		const cv::Mat & depthK,
 		const cv::Size & colorSize,
 		const cv::Mat & colorK,
-		const rtabmap::Transform & transform)
+		const rtabmap::Transform & transform,
+		cv::Mat & registeredConfidence)
 {
 	UASSERT(!transform.isNull());
 	UASSERT(!depth.empty());
 	UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1); // mm or m
 	UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
 	UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
+	UASSERT(confidence.empty() || (confidence.size() == depth.size() && confidence.type()==CV_8UC1));

 	float fx = depthK.at<double>(0,0);
 	float fy = depthK.at<double>(1,1);
@@ -1389,10 +1409,19 @@ cv::Mat registerDepth(
 	Eigen::Vector4f P4,P3;
 	P4[3] = 1;
 	cv::Mat registered = cv::Mat::zeros(colorSize, depth.type());
+	registeredConfidence = cv::Mat();
+	if(!confidence.empty())
+	{
+		registeredConfidence = cv::Mat::zeros(colorSize, confidence.type());
+	}

 	bool depthInMM = depth.type() == CV_16UC1;
 	for(int y=0; y<depth.rows; ++y)
 	{
+		const unsigned char * confPtr = 0;
+		if(!confidence.empty()) {
+			confPtr = confidence.ptr<unsigned char>(y);
+		}
 		for(int x=0; x<depth.cols; ++x)
 		{
 			//filtering
@@ -1419,6 +1448,9 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z16 < zReg)
 						{
 							zReg = z16;
+							if(confPtr) {
+								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
+							}
 						}
 					}
 					else
@@ -1427,6 +1459,9 @@ cv::Mat registerDepth(
 						if(zReg == 0 || z < zReg)
 						{
 							zReg = z;
+							if(confPtr) {
+								registeredConfidence.at<unsigned char>(dy, dx) = confPtr[x];
+							}
 						}
 					}
 				}
@@ -1931,6 +1966,61 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
 	return output;
 }

+void depthBleedingFiltering(cv::Mat & depth, float maxDepthError)
+{
+	if(depth.empty())
+	{
+		return;
+	}
+	UASSERT(depth.type() == CV_32FC1 || depth.type() == CV_16UC1);
+
+	// ignore border
+	depth.row(0).setTo(cv::Scalar(0));
+	depth.row(depth.rows-1).setTo(cv::Scalar(0));
+	depth.col(0).setTo(cv::Scalar(0));
+	depth.col(depth.cols-1).setTo(cv::Scalar(0));
+
+	if(depth.type() == CV_32FC1)
+	{
+		float * depthPtr = depth.ptr<float>();
+		for(int v=1; v<depth.rows-1; ++v)
+		{
+			for(int u=1; u<depth.cols-1; ++u)
+			{
+				int row = depth.cols*v;
+				float & ref  = depthPtr[row + u];
+				if((fabs(ref - depthPtr[row + u - 1]) > maxDepthError && 
+				    fabs(ref - depthPtr[row + u + 1]) > maxDepthError) || 
+				   (fabs(ref - depthPtr[depth.cols*(v-1) + u]) > maxDepthError && 
+				    fabs(ref - depthPtr[depth.cols*(v+1) + u]) > maxDepthError)) 
+				{
+					ref = 0.0f;
+				} 
+			}
+		}
+	}
+	else if(depth.type() == CV_16UC1)
+	{
+		unsigned short * depthPtr = depth.ptr<unsigned short>();
+		unsigned short maxDepthErrorMM = (unsigned short)(maxDepthError*1000.0f);
+		for(int v=1; v<depth.rows-1; ++v)
+		{
+			for(int u=1; u<depth.cols-1; ++u)
+			{
+				int row = depth.cols*v;
+				unsigned short & ref  = depthPtr[row + u];
+				if((abs((int)ref - (int)depthPtr[row + u - 1]) > maxDepthErrorMM && 
+				    abs((int)ref - (int)depthPtr[row + u + 1]) > maxDepthErrorMM) || 
+				   (abs((int)ref - (int)depthPtr[depth.cols*(v-1) + u]) > maxDepthErrorMM && 
+				    abs((int)ref - (int)depthPtr[depth.cols*(v+1) + u]) > maxDepthErrorMM)) 
+				{
+					ref = 0;
+				} 
+			}
+		}
+	}
+}
+
 /**
 *  \brief Automatic brightness and contrast optimization with optional histogram clipping
 *  \param [in]src Input image GRAY or BGR or BGRA
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -276,12 +276,33 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	return cloudFromDepth(imageDepth, model, decimation, maxDepth, minDepth, validIndices);
 }

+pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
+	const cv::Mat & imageDepthIn,
+	const CameraModel & model,
+	int decimation,
+	float maxDepth,
+	float minDepth,
+	std::vector<int> * validIndices)
+{
+	return cloudFromDepth(
+		imageDepthIn,
+		cv::Mat(),
+		model,
+		decimation,
+		maxDepth,
+		minDepth,
+		0,
+		validIndices);
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepthIn,
+		const cv::Mat & imageDepthConfidenceIn,
 		const CameraModel & model,
 		int decimation,
 		float maxDepth,
 		float minDepth,
+		unsigned char confidenceThr,
 		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -294,8 +315,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(

 	UASSERT(model.isValidForProjection());
 	UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
+	UASSERT(imageDepthConfidenceIn.empty() || confidenceThr == 0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));

 	cv::Mat imageDepth = imageDepthIn;
+	cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
 	if(model.imageHeight()>0 && model.imageWidth()>0)
 	{
 		UASSERT(model.imageHeight() % imageDepthIn.rows == 0 && model.imageWidth() % imageDepthIn.cols == 0);
@@ -322,6 +345,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 			{
 				UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
 				imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
+				if(!imageDepthConfidence.empty()) {
+					imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
+				}
 				decimation = 1;
 			}
 			else if(targetSize == imageDepthIn.rows)
@@ -373,11 +399,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	float depthCx = model.cx() * rgbToDepthFactorX;
 	float depthCy = model.cy() * rgbToDepthFactorY;

-	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
 			imageDepth.cols, imageDepth.rows,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
+			imageDepthConfidenceIn.empty()?0:1,
+			(int)confidenceThr,
 			decimation);

 	int oi = 0;
@@ -387,21 +415,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));

-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
 			{
-				pt.x = ptXYZ.x;
-				pt.y = ptXYZ.y;
-				pt.z = ptXYZ.z;
-				if(validIndices)
+				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
 				{
-					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+					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();
-			}
 		}
 	}

@@ -427,13 +455,36 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	return cloudFromDepthRGB(imageRgb, imageDepth, model, decimation, maxDepth, minDepth, validIndices);
 }

+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
+	const cv::Mat & imageRgb,
+	const cv::Mat & imageDepthIn,
+	const CameraModel & model,
+	int decimation,
+	float maxDepth,
+	float minDepth,
+	std::vector<int> * validIndices)
+{
+	return cloudFromDepthRGB(
+		imageRgb,
+		imageDepthIn,
+		cv::Mat(),
+		model,
+		decimation,
+		maxDepth,
+		minDepth,
+		0,
+		validIndices);
+}
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDepthIn,
+		const cv::Mat & imageDepthConfidenceIn,
 		const CameraModel & model,
 		int decimation,
 		float maxDepth,
 		float minDepth,
+		unsigned char confidenceThr,
 		std::vector<int> * validIndices)
 {
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -449,6 +500,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	//UASSERT_MSG(imageRgb.rows % imageDepthIn.rows == 0 && imageRgb.cols % imageDepthIn.cols == 0,
 	//		uFormat("rgb=%dx%d depth=%dx%d", imageRgb.cols, imageRgb.rows, imageDepthIn.cols, imageDepthIn.rows).c_str());
 	UASSERT(!imageDepthIn.empty() && (imageDepthIn.type() == CV_16UC1 || imageDepthIn.type() == CV_32FC1));
+	UASSERT(imageDepthConfidenceIn.empty() || confidenceThr==0 || (imageDepthConfidenceIn.type() == CV_8UC1 && imageDepthConfidenceIn.size() == imageDepthIn.size()));
 	if(decimation < 0)
 	{
 		if(imageRgb.rows % decimation != 0 || imageRgb.cols % decimation != 0)
@@ -491,6 +543,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	}

 	cv::Mat imageDepth = imageDepthIn;
+	cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
 	if(decimation < 0)
 	{
 		UDEBUG("Decimation from RGB image (%d)", decimation);
@@ -502,6 +555,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		{
 			UDEBUG("Depth interpolation factor=%d", targetSize/imageDepthIn.rows);
 			imageDepth = util2d::interpolate(imageDepthIn, targetSize/imageDepthIn.rows);
+			if(!imageDepthConfidence.empty()) {
+				imageDepthConfidence = util2d::interpolate(imageDepthConfidenceIn, targetSize/imageDepthConfidenceIn.rows);
+			}
 			decimation = 1;
 		}
 		else if(targetSize == imageDepthIn.rows)
@@ -546,12 +602,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	float depthCx = model.cx() / rgbToDepthFactorX;
 	float depthCy = model.cy() / rgbToDepthFactorY;

-	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	UDEBUG("rgb=%dx%d depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) has confidence=%d (thr=%d) decimation=%d",
 			imageRgb.cols, imageRgb.rows,
 			imageDepth.cols, imageDepth.rows,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
+			imageDepthConfidenceIn.empty()?0:1,
+			(int)confidenceThr,
 			decimation);

 	int oi = 0;
@@ -579,21 +637,21 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.r = v;
 			}

-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
-			if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
+			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
 			{
-				pt.x = ptXYZ.x;
-				pt.y = ptXYZ.y;
-				pt.z = ptXYZ.z;
-				if (validIndices)
+				pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
 				{
-					validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
+					pt.x = ptXYZ.x;
+					pt.y = ptXYZ.y;
+					pt.z = ptXYZ.z;
+					if (validIndices)
+					{
+						validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
+					}
+					++oi;
 				}
-				++oi;
-			}
-			else
-			{
-				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
 			}
 		}
 	}
@@ -868,7 +926,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 		float minDepth,
 		std::vector<pcl::IndicesPtr> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	if(decimation == 0)
 	{
@@ -881,6 +940,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 	{
 		//depth
 		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
+		UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));
 		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
@@ -892,6 +952,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
 				cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
+				cv::Mat depthConfidence;
+				if(!sensorData.depthConfidenceRaw().empty() && confidenceThr > 0) {
+					depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthConfidenceRaw().rows));
+				}
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					(roiRatios[0] > 0.0f ||
@@ -912,6 +976,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 							roiRgb.height%decimation==0)))
 					{
 						depth = cv::Mat(depth, roiDepth);
+						if(!depthConfidence.empty()) {
+							depthConfidence = cv::Mat(depthConfidence, roiDepth);
+						}
 						if(model.imageWidth() != 0 && model.imageHeight() != 0)
 						{
 							model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
@@ -940,10 +1007,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(

 				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
 						depth,
+						depthConfidence,
 						model,
 						decimation,
 						maxDepth,
 						minDepth,
+						confidenceThr,
 						validIndices?validIndices->back().get():0);

 				if(tmp->size())
@@ -1064,7 +1133,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		float minDepth,
 		std::vector<int> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	std::vector<pcl::IndicesPtr> validIndicesV;
 	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
@@ -1074,7 +1144,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios);
+		roiRatios,
+		confidenceThr);

 	if(validIndices)
 	{
@@ -1117,7 +1188,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 		float minDepth,
 		std::vector<pcl::IndicesPtr> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	if(decimation == 0)
 	{
@@ -1136,6 +1208,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 		//UASSERT_MSG(sensorData.imageRaw().rows % sensorData.depthRaw().rows == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().rows, sensorData.depthRaw().rows).c_str());
 		int subRGBWidth = sensorData.imageRaw().cols/sensorData.cameraModels().size();
 		int subDepthWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
+		UASSERT(sensorData.depthConfidenceRaw().empty() || confidenceThr==0 || (sensorData.depthConfidenceRaw().type() == CV_8UC1 && sensorData.depthConfidenceRaw().cols == sensorData.depthRaw().cols && sensorData.depthConfidenceRaw().rows == sensorData.depthRaw().rows));

 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
@@ -1148,6 +1221,10 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 			{
 				cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
 				cv::Mat depth(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
+				cv::Mat depthConfidence;
+				if(!sensorData.depthConfidenceRaw().empty() && confidenceThr>0) {
+					depthConfidence = cv::Mat(sensorData.depthConfidenceRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthConfidenceRaw().rows));
+				}
 				CameraModel model = sensorData.cameraModels()[i];
 				if( roiRatios.size() == 4 &&
 					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
@@ -1163,6 +1240,9 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 						roiRgb.height%decimation==0)
 					{
 						depth = cv::Mat(depth, roiDepth);
+						if(!depthConfidence.empty()) {
+							depthConfidence = cv::Mat(depthConfidence, roiDepth);
+						}
 						rgb = cv::Mat(rgb, roiRgb);
 						model = model.roi(roiRgb);
 					}
@@ -1186,10 +1266,12 @@ std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
 						rgb,
 						depth,
+						depthConfidence,
 						model,
 						decimation,
 						maxDepth,
 						minDepth,
+						confidenceThr,
 						validIndices?validIndices->back().get():0);

 				if(tmp->size())
@@ -1292,7 +1374,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		float minDepth,
 		std::vector<int> * validIndices,
 		const ParametersMap & stereoParameters,
-		const std::vector<float> & roiRatios)
+		const std::vector<float> & roiRatios,
+		unsigned char confidenceThr)
 {
 	std::vector<pcl::IndicesPtr> validIndicesV;
 	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
@@ -1302,7 +1385,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		minDepth,
 		validIndices?&validIndicesV:0,
 		stereoParameters,
-		roiRatios);
+		roiRatios,
+		confidenceThr);

 	if(validIndices)
 	{