CameraThread: Added stereo to depth option. Added parameter "Mem/SaveDepth16Format".

corelib/src/CameraThread.cpp

@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/CameraRGBD.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/core/util3d.h"
 
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -40,7 +42,8 @@ namespace rtabmap
 CameraThread::CameraThread(Camera * camera) :
 		_camera(camera),
 		_mirroring(false),
-		_colorOnly(false)
+		_colorOnly(false),
+		_stereoToDepth(false)
 {
 	UASSERT(_camera != 0);
 }
@@ -96,6 +99,16 @@ void CameraThread::mainLoop()
 				data.setDepthOrRightRaw(tmpDepth);
 			}
 		}
+		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
+		{
+			cv::Mat depth = util2d::depthFromDisparity(
+					util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().baseline());
+			data.setCameraModel(data.stereoCameraModel().left());
+			data.setDepthOrRightRaw(depth);
+			data.setStereoCameraModel(StereoCameraModel());
+		}
 
 		this->post(new CameraEvent(data, _camera->getSerial()));
 	}

corelib/src/Compression.cpp

@@ -88,7 +88,16 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 	std::vector<unsigned char> bytes;
 	if(!image.empty())
 	{
-		cv::imencode(format, image, bytes);
+		if(image.type() == CV_32FC1)
+		{
+			//save in 8bits-4channel
+			cv::Mat bgra(image.size(), CV_8UC4, image.data);
+			cv::imencode(format, bgra, bytes);
+		}
+		else
+		{
+			cv::imencode(format, image, bytes);
+		}
 	}
 	return bytes;
 }
@@ -114,6 +123,10 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 #else
 		image = cv::imdecode(bytes, -1);
 #endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
 	}
 	return image;
 }
@@ -128,6 +141,10 @@ cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 #else
 		image = cv::imdecode(bytes, -1);
 #endif
+		if(image.type() == CV_8UC4)
+		{
+			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+		}
 	}
 	return image;
 }

corelib/src/Memory.cpp

@@ -68,6 +68,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_similarityThreshold(Parameters::defaultMemRehearsalSimilarity()),
 	_rawDataKept(Parameters::defaultMemImageKept()),
 	_binDataKept(Parameters::defaultMemBinDataKept()),
+	_saveDepth16Format(Parameters::defaultMemSaveDepth16Format()),
 	_notLinkedNodesKeptInDb(Parameters::defaultMemNotLinkedNodesKept()),
 	_incrementalMemory(Parameters::defaultMemIncrementalMemory()),
 	_maxStMemSize(Parameters::defaultMemSTMSize()),
@@ -399,6 +400,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 
 	Parameters::parse(parameters, Parameters::kMemImageKept(), _rawDataKept);
 	Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
+	Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
 	Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
 	Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
@@ -4257,10 +4259,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
-		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
+		if(_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
-			UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
-			depthOrRightImage = util3d::cvtDepthFromFloat(depthOrRightImage);
+			UWARN("Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion.");
+			depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 		}
 
 		rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
@@ -4347,7 +4349,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		s->sensorData().setUserDataRaw(data.userDataRaw());
 	}
 
-
 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
 	UDEBUG("time compressing data (id=%d) %fs", id, t);

corelib/src/util2d.cpp

@@ -42,12 +42,13 @@ namespace util2d
 
 cv::Mat disparityFromStereoImages(
 		const cv::Mat & leftImage,
-		const cv::Mat & rightImage)
+		const cv::Mat & rightImage,
+		int type)
 {
-	UASSERT(!leftImage.empty() && !rightImage.empty() &&
-			(leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
-			leftImage.cols == rightImage.cols &&
-			leftImage.rows == rightImage.rows);
+	UASSERT(!leftImage.empty() && !rightImage.empty());
+	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
+	UASSERT((leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1);
+	UASSERT(type == CV_32FC1 || type == CV_16SC1);
 
 	cv::Mat leftMono;
 	if(leftImage.channels() == 3)
@@ -70,7 +71,7 @@ cv::Mat disparityFromStereoImages(
 	stereo.state->textureThreshold = 10;
 	stereo.state->speckleWindowSize = 100;
 	stereo.state->speckleRange = 4;
-	stereo(leftMono, rightImage, disparity, CV_16SC1);
+	stereo(leftMono, rightImage, disparity, type);
 #else
 	cv::Ptr<cv::StereoBM> stereo = cv::StereoBM::create();
 	stereo->setBlockSize(15);
@@ -97,10 +98,9 @@ cv::Mat disparityFromStereoImages(
 		double flowEps,
 		float maxCorrespondencesSlope)
 {
-	UASSERT(!leftImage.empty() && !rightImage.empty() &&
-			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
-			leftImage.cols == rightImage.cols &&
-			leftImage.rows == rightImage.rows);
+	UASSERT(!leftImage.empty() && !rightImage.empty());
+	UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
+	UASSERT(leftImage.cols == rightImage.cols && leftImage.rows == rightImage.rows);
 
 	// Find features in the new left image
 	std::vector<unsigned char> status;
@@ -122,6 +122,51 @@ cv::Mat disparityFromStereoImages(
 	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
 }
 
+cv::Mat depthFromDisparity(const cv::Mat & disparity,
+		float fx, float baseline,
+		int type)
+{
+	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
+	UASSERT(type == CV_32FC1 || type == CV_16UC1);
+	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
+	int countOverMax = 0;
+	for (int i = 0; i < disparity.rows; i++)
+	{
+		for (int j = 0; j < disparity.cols; j++)
+		{
+			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
+			if (disparity_value > 0.0f)
+			{
+				// baseline * focal / disparity
+				float d = baseline * fx / disparity_value;
+				if(d>0)
+				{
+					if(depth.type() == CV_32FC1)
+					{
+						depth.at<float>(i,j) = d;
+					}
+					else
+					{
+						if(d*1000.0f <= (float)USHRT_MAX)
+						{
+							depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
+						}
+						else
+						{
+							++countOverMax;
+						}
+					}
+				}
+			}
+		}
+	}
+	if(countOverMax)
+	{
+		UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
+	}
+	return depth;
+}
+
 cv::Mat depthFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
@@ -209,6 +254,58 @@ cv::Mat depthFromStereoCorrespondences(
 	return depth;
 }
 
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		int countOverMax = 0;
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth > 0 && depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				else if(depth > (float)USHRT_MAX)
+				{
+					++countOverMax;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+		if(countOverMax)
+		{
+			UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
 float getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,

corelib/src/util3d.cpp

@@ -286,6 +286,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation)
 {
+	UDEBUG("");
 	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
 	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());
@@ -504,14 +505,13 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float voxelSize,
 		int samples)
 {
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
 	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
 		//depth
 		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
 		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
-		cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
 			if(sensorData.cameraModels()[i].isValid())
@@ -627,117 +627,118 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float voxelSize,
 		int samples)
 {
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+	UASSERT(!sensorData.imageRaw().empty());
+	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
+			(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid()));
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 
-	if(!sensorData.imageRaw().empty())
+	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
-		if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
+		//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();
+		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
-			//depth
-			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();
-			cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-			for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
+			if(sensorData.cameraModels()[i].isValid())
 			{
-				if(sensorData.cameraModels()[i].isValid())
+				if(subImageWidth % decimation != 0 || sensorData.depthRaw().rows % decimation != 0)
 				{
-					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)),
+						sensorData.cameraModels()[i].cx(),
+						sensorData.cameraModels()[i].cy(),
+						sensorData.cameraModels()[i].fx(),
+						sensorData.cameraModels()[i].fy(),
+						decimation);
+
+				if(tmp->size())
+				{
+					bool filtered = false;
+					if(tmp->size() && maxDepth)
 					{
-						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;
+						tmp = util3d::passThrough(tmp, "z", 0, maxDepth);
+						filtered = true;
+					}
+
+					if(tmp->size() && voxelSize)
+					{
+						tmp = util3d::voxelize(tmp, voxelSize);
+						filtered = true;
+					}
+
+					if(tmp->size() && samples)
+					{
+						tmp = util3d::sampling(tmp, samples);
+						filtered = true;
+					}
+
+					if(tmp->size() && !filtered)
+					{
+						tmp = util3d::removeNaNFromPointCloud(tmp);
 					}
-					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)),
-							sensorData.cameraModels()[i].cx(),
-							sensorData.cameraModels()[i].cy(),
-							sensorData.cameraModels()[i].fx(),
-							sensorData.cameraModels()[i].fy(),
-							decimation);
 
 					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);
-							filtered = true;
-						}
-
-						if(tmp->size() && samples)
-						{
-							tmp = util3d::sampling(tmp, samples);
-							filtered = true;
-						}
-
-						if(tmp->size() && !filtered)
-						{
-							tmp = util3d::removeNaNFromPointCloud(tmp);
-						}
-
-						if(tmp->size())
-						{
-							tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
-						}
-
-						*cloud += *tmp;
+						tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
 					}
+
+					*cloud += *tmp;
 				}
-				else
-				{
-					UERROR("Camera model %d is invalid", i);
-				}
+			}
+			else
+			{
+				UERROR("Camera model %d is invalid", i);
+			}
+		}
+
+		if(cloud->size() && voxelSize)
+		{
+			cloud = util3d::voxelize(cloud, voxelSize);
+		}
+	}
+	else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
+	{
+		//stereo
+		UDEBUG("");
+		cloud = cloudFromStereoImages(sensorData.imageRaw(),
+				sensorData.rightRaw(),
+				sensorData.stereoCameraModel().left().cx(),
+				sensorData.stereoCameraModel().left().cy(),
+				sensorData.stereoCameraModel().left().fx(),
+				sensorData.stereoCameraModel().baseline(),
+				decimation);
+
+		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);
 			}
-		}
-		else if(!sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValid())
-		{
-			//stereo
-			cloud = cloudFromStereoImages(sensorData.imageRaw(),
-					sensorData.rightRaw(),
-					sensorData.stereoCameraModel().left().cx(),
-					sensorData.stereoCameraModel().left().cy(),
-					sensorData.stereoCameraModel().left().fx(),
-					sensorData.stereoCameraModel().baseline(),
-					decimation);
 
 			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())
-				{
-					cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
-				}
+				cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
 			}
 		}
 	}
@@ -779,50 +780,6 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 	return scan;
 }
 
-
-cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
-{
-	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
-	cv::Mat depth16U;
-	if(!depth32F.empty())
-	{
-		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
-		for(int i=0; i<depth32F.rows; ++i)
-		{
-			for(int j=0; j<depth32F.cols; ++j)
-			{
-				float depth = (depth32F.at<float>(i,j)*1000.0f);
-				unsigned short depthMM = 0;
-				if(depth <= (float)USHRT_MAX)
-				{
-					depthMM = (unsigned short)depth;
-				}
-				depth16U.at<unsigned short>(i, j) = depthMM;
-			}
-		}
-	}
-	return depth16U;
-}
-
-cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
-{
-	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
-	cv::Mat depth32F;
-	if(!depth16U.empty())
-	{
-		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
-		for(int i=0; i<depth16U.rows; ++i)
-		{
-			for(int j=0; j<depth16U.cols; ++j)
-			{
-				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
-				depth32F.at<float>(i, j) = depth;
-			}
-		}
-	}
-	return depth32F;
-}
-
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -914,36 +871,6 @@ pcl::PointXYZ projectDisparityTo3D(
 	return pcl::PointXYZ(bad_point, bad_point, bad_point);
 }
 
-cv::Mat depthFromDisparity(const cv::Mat & disparity,
-		float fx, float baseline,
-		int type)
-{
-	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
-	UASSERT(type == CV_32FC1 || type == CV_16U);
-	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
-	for (int i = 0; i < disparity.rows; i++)
-	{
-		for (int j = 0; j < disparity.cols; j++)
-		{
-			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
-			if (disparity_value > 0.0f)
-			{
-				// baseline * focal / disparity
-				float d = baseline * fx / disparity_value;
-				if(depth.type() == CV_32FC1)
-				{
-					depth.at<float>(i,j) = d;
-				}
-				else
-				{
-					depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
-				}
-			}
-		}
-	}
-	return depth;
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);