Added util3d::projectCloudToCamera() method (creating a registered depth image from the laser scan), used for "Generate depth from scan" option of CameraImages

corelib/src/CameraRGB.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_surface.h>
 
 #include <iostream>
 #include <cmath>
@@ -64,6 +65,7 @@ CameraImages::CameraImages() :
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_depthFromScan(false),
 		_filenamesAreTimestamps(false),
 		_groundTruthFormat(0)
 	{}
@@ -85,6 +87,7 @@ CameraImages::CameraImages(const std::string & path,
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
 	_scanNormalsK(0),
+	_depthFromScan(false),
 	_filenamesAreTimestamps(false),
 	_groundTruthFormat(0)
 {
@@ -381,6 +384,7 @@ SensorData CameraImages::captureImage()
 	cv::Mat scan;
 	double stamp = UTimer::now();
 	Transform groundTruthPose;
+	cv::Mat depthFromScan;
 	UDEBUG("");
 	if(_dir->isValid())
 	{
@@ -392,6 +396,8 @@ SensorData CameraImages::captureImage()
 				_scanDir->update();
 			}
 		}
+		std::string imageFilePath;
+		std::string scanFilePath;
 		if(_startAt < 0)
 		{
 			const std::list<std::string> & fileNames = _dir->getFileNames();
@@ -400,8 +406,7 @@ SensorData CameraImages::captureImage()
 				if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
 				{
 					_lastFileName = *fileNames.rbegin();
-					std::string fullPath = _path + _lastFileName;
-					img = cv::imread(fullPath.c_str());
+					imageFilePath = _path + _lastFileName;
 				}
 			}
 			if(_scanDir)
@@ -412,109 +417,148 @@ SensorData CameraImages::captureImage()
 					if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
 					{
 						_lastScanFileName = *scanFileNames.rbegin();
-						std::string fullPath = _scanPath + _lastScanFileName;
-
-						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
+						scanFilePath = _scanPath + _lastScanFileName;
 					}
 				}
 			}
 		}
 		else
 		{
-			if(stamps_.size())
-			{
-				stamp = stamps_.front();
-				stamps_.pop_front();
-				if(groundTruth_.size())
-				{
-					groundTruthPose = groundTruth_.front();
-					groundTruth_.pop_front();
-				}
-			}
-
 			std::string fileName;
-			std::string fullPath;
 			fileName = _dir->getNextFileName();
-			if(fileName.size())
+			if(!fileName.empty())
 			{
-				fullPath = _path + fileName;
+				imageFilePath = _path + fileName;
 				while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
 				{
-					fullPath = _path + fileName;
-				}
-				if(fileName.size())
-				{
-					ULOGGER_DEBUG("Loading image : %s", fullPath.c_str());
-
-#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-					img = cv::imread(fullPath.c_str(), cv::IMREAD_UNCHANGED);
-#else
-					img = cv::imread(fullPath.c_str(), -1);
-#endif
-					UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
-							img.cols, img.rows, img.channels(), img.elemSize(), img.total());
-
-					if(_isDepth)
-					{
-						if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
-						{
-							UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
-									"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
-									fileName.c_str());
-							img = cv::Mat();
-						}
-
-						if(_depthScaleFactor > 1.0f)
-						{
-							img /= _depthScaleFactor;
-						}
-					}
-					else
-					{
-#if CV_MAJOR_VERSION < 3
-						// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
-						if(img.depth() != CV_8U)
-						{
-							// The depth should be 8U
-							UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
-							IplImage * i = cvLoadImage(fullPath.c_str());
-							img = cv::Mat(i, true);
-							cvReleaseImage(&i);
-						}
-#endif
-
-						if(img.channels()>3)
-						{
-							UWARN("Conversion from 4 channels to 3 channels (file=%s)", fullPath.c_str());
-							cv::Mat out;
-							cv::cvtColor(img, out, CV_BGRA2BGR);
-							img = out;
-						}
-					}
+					imageFilePath = _path + fileName;
 				}
 			}
-
 			if(_scanDir)
 			{
 				fileName = _scanDir->getNextFileName();
-				if(fileName.size())
+				if(!fileName.empty())
 				{
-					fullPath = _scanPath + fileName;
+					scanFilePath = _scanPath + fileName;
 					while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
 					{
-						fullPath = _scanPath + fileName;
-					}
-					if(fileName.size())
-					{
-						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
+						scanFilePath = _scanPath + fileName;
 					}
 				}
 			}
 		}
 
-		if(!img.empty() && _model.isValid() && _rectifyImages)
+		if(stamps_.size())
 		{
-			img = _model.rectifyImage(img);
+			stamp = stamps_.front();
+			stamps_.pop_front();
+			if(groundTruth_.size())
+			{
+				groundTruthPose = groundTruth_.front();
+				groundTruth_.pop_front();
+			}
+		}
+
+		if(!imageFilePath.empty())
+		{
+			ULOGGER_DEBUG("Loading image : %s", imageFilePath.c_str());
+
+#if CV_MAJOR_VERSION >2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
+			img = cv::imread(imageFilePath.c_str(), cv::IMREAD_UNCHANGED);
+#else
+			img = cv::imread(imageFilePath.c_str(), -1);
+#endif
+			UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
+					img.cols, img.rows, img.channels(), img.elemSize(), img.total());
+
+			if(_isDepth)
+			{
+				if(img.type() != CV_16UC1 && img.type() != CV_32FC1)
+				{
+					UERROR("Depth is on and the loaded image has not a format supported (file = \"%s\"). "
+							"Formats supported are 16 bits 1 channel and 32 bits 1 channel.",
+							imageFilePath.c_str());
+					img = cv::Mat();
+				}
+
+				if(_depthScaleFactor > 1.0f)
+				{
+					img /= _depthScaleFactor;
+				}
+			}
+			else
+			{
+#if CV_MAJOR_VERSION < 3
+				// FIXME : it seems that some png are incorrectly loaded with opencv c++ interface, where c interface works...
+				if(img.depth() != CV_8U)
+				{
+					// The depth should be 8U
+					UWARN("Cannot read the image correctly, falling back to old OpenCV C interface...");
+					IplImage * i = cvLoadImage(fullPath.c_str());
+					img = cv::Mat(i, true);
+					cvReleaseImage(&i);
+				}
+#endif
+
+				if(img.channels()>3)
+				{
+					UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
+					cv::Mat out;
+					cv::cvtColor(img, out, CV_BGRA2BGR);
+					img = out;
+				}
+			}
+
+			if(!img.empty() && _model.isValid() && _rectifyImages)
+			{
+				img = _model.rectifyImage(img);
+			}
+		}
+
+		if(!scanFilePath.empty())
+		{
+			// load without filtering
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::loadCloud(scanFilePath, _scanLocalTransform);
+			UDEBUG("Loaded scan=%d points", (int)cloud->size());
+			if(_depthFromScan && !img.empty())
+			{
+				UDEBUG("Computing depth from scan...");
+				if(!_model.isValid())
+				{
+					UWARN("Depth from laser scan: Camera model should be valid.");
+				}
+				else if(_isDepth)
+				{
+					UWARN("Depth from laser scan: Loading already a depth image.");
+				}
+				else
+				{
+					depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
+					util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHolesVertical, _depthFromScanFillHolesFromBorder);
+				}
+			}
+			// filter the scan after registration
+			int previousSize = (int)cloud->size();
+			if(_scanDownsampleStep > 1 && cloud->size())
+			{
+				cloud = util3d::downsample(cloud, _scanDownsampleStep);
+				UDEBUG("Downsampling scan (step=%d): %d -> %d", _scanDownsampleStep, previousSize, (int)cloud->size());
+			}
+			previousSize = (int)cloud->size();
+			if(_scanVoxelSize > 0.0f && cloud->size())
+			{
+				cloud = util3d::voxelize(cloud, _scanVoxelSize);
+				UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
+			}
+			if(_scanNormalsK > 0 && cloud->size())
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, _scanNormalsK);
+				scan = util3d::laserScanFromPointCloud(*cloudNormals);
+			}
+			else
+			{
+				scan = util3d::laserScanFromPointCloud(*cloud);
+			}
 		}
 	}
 	else
@@ -522,7 +566,7 @@ SensorData CameraImages::captureImage()
 		UWARN("Directory is not set, camera must be initialized.");
 	}
 
-	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:cv::Mat(), _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
 	return data;
 }

corelib/src/util2d.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
@@ -1077,7 +1078,7 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
 	return out;
 }
 
-// Registration Depth to RGB
+// Registration Depth to RGB (return registered depth image)
 cv::Mat registerDepth(
 		const cv::Mat & depth,
 		const cv::Mat & depthK,
@@ -1086,7 +1087,7 @@ cv::Mat registerDepth(
 {
 	UASSERT(!transform.isNull());
 	UASSERT(!depth.empty());
-	UASSERT(depth.type() == CV_16UC1); // mm
+	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);
 
@@ -1105,12 +1106,13 @@ cv::Mat registerDepth(
 	P4[3] = 1;
 	cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
 
+	bool depthInMM = depth.type() == CV_16UC1;
 	for(int y=0; y<depth.rows; ++y)
 	{
 		for(int x=0; x<depth.cols; ++x)
 		{
 			//filtering
-			float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
+			float dz = depthInMM?float(depth.at<unsigned short>(y,x))*0.001f:depth.at<float>(y,x); // put in meter for projection
 			if(dz>=0.0f)
 			{
 				// Project to 3D
@@ -1126,11 +1128,22 @@ cv::Mat registerDepth(
 
 				if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
 				{
-					unsigned short z16 = z * 1000; //mm
-					unsigned short &zReg = registered.at<unsigned short>(dy, dx);
-					if(zReg == 0 || z16 < zReg)
+					if(depthInMM)
 					{
-						zReg = z16;
+						unsigned short z16 = z * 1000; //mm
+						unsigned short &zReg = registered.at<unsigned short>(dy, dx);
+						if(zReg == 0 || z16 < zReg)
+						{
+							zReg = z16;
+						}
+					}
+					else
+					{
+						float &zReg = registered.at<float>(dy, dx);
+						if(zReg == 0 || z < zReg)
+						{
+							zReg = z;
+						}
 					}
 				}
 			}

corelib/src/util3d.cpp

@@ -787,7 +787,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
-	bool nullTransform = transform.isNull();
+	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
@@ -812,7 +812,7 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
-	bool nullTransform = transform.isNull();
+	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(unsigned int i=0; i<cloud.size(); ++i)
 	{
@@ -976,6 +976,201 @@ cv::Point3f projectDisparityTo3D(
 	return cv::Point3f(bad_point, bad_point, bad_point);
 }
 
+// Register point cloud to camera (return registered depth image)
+cv::Mat projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
+		const cv::Mat & laserScan,     // assuming laser scan points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform)
+{
+	UASSERT(!cameraTransform.isNull());
+	UASSERT(!laserScan.empty());
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+
+	float fx = cameraMatrixK.at<double>(0,0);
+	float fy = cameraMatrixK.at<double>(1,1);
+	float cx = cameraMatrixK.at<double>(0,2);
+	float cy = cameraMatrixK.at<double>(1,2);
+
+	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
+	Transform t = cameraTransform.inverse();
+
+	int count = 0;
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		// Get 3D from laser scan
+		cv::Point3f ptScan;
+		if(laserScan.type() == CV_32FC2)
+		{
+			ptScan.x = laserScan.at<cv::Vec2f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec2f>(i)[1];
+			ptScan.z = 0;
+		}
+		else if(laserScan.type() == CV_32FC3)
+		{
+			ptScan.x = laserScan.at<cv::Vec3f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec3f>(i)[1];
+			ptScan.z = laserScan.at<cv::Vec3f>(i)[2];
+		}
+		else
+		{
+			ptScan.x = laserScan.at<cv::Vec6f>(i)[0];
+			ptScan.y = laserScan.at<cv::Vec6f>(i)[1];
+			ptScan.z = laserScan.at<cv::Vec6f>(i)[2];
+		}
+		ptScan = util3d::transformPoint(ptScan, t);
+
+		// re-project in camera frame
+		float z = ptScan.z;
+		float invZ = 1.0f/z;
+		int dx = (fx*ptScan.x)*invZ + cx;
+		int dy = (fy*ptScan.y)*invZ + cy;
+
+		if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
+		{
+			++count;
+			float &zReg = registered.at<float>(dy, dx);
+			if(zReg == 0 || z < zReg)
+			{
+				zReg = z;
+			}
+		}
+	}
+	UDEBUG("Points in camera=%d/%d", count, laserScan.cols);
+
+	return registered;
+}
+
+cv::Mat projectCloudToCamera(
+		const cv::Size & imageSize,
+		const cv::Mat & cameraMatrixK, // /base_link -> /camera_link
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,  // assuming points are already in /base_link coordinate
+		const rtabmap::Transform & cameraTransform)
+{
+	UASSERT(!cameraTransform.isNull());
+	UASSERT(!laserScan->empty());
+	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+
+	float fx = cameraMatrixK.at<double>(0,0);
+	float fy = cameraMatrixK.at<double>(1,1);
+	float cx = cameraMatrixK.at<double>(0,2);
+	float cy = cameraMatrixK.at<double>(1,2);
+
+	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
+	Transform t = cameraTransform.inverse();
+
+	int count = 0;
+	for(int i=0; i<(int)laserScan->size(); ++i)
+	{
+		// Get 3D from laser scan
+		pcl::PointXYZ ptScan = laserScan->at(i);
+		ptScan = util3d::transformPoint(ptScan, t);
+
+		// re-project in camera frame
+		float z = ptScan.z;
+		float invZ = 1.0f/z;
+		int dx = (fx*ptScan.x)*invZ + cx;
+		int dy = (fy*ptScan.y)*invZ + cy;
+
+		if(z > 0.0f && uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
+		{
+			++count;
+			float &zReg = registered.at<float>(dy, dx);
+			if(zReg == 0 || z < zReg)
+			{
+				zReg = z;
+			}
+		}
+	}
+	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());
+
+	return registered;
+}
+
+void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection, bool fillToBorder)
+{
+	UASSERT(registeredDepth.type() == CV_32FC1);
+	if(verticalDirection)
+	{
+		// vertical, for each column
+		for(int x=0; x<registeredDepth.cols; ++x)
+		{
+			float valueA = 0.0f;
+			int indexA = -1;
+			for(int y=0; y<registeredDepth.rows; ++y)
+			{
+				float v = registeredDepth.at<float>(y,x);
+				if(fillToBorder && y == registeredDepth.rows-1 && v<=0.0f && indexA>=0)
+				{
+					v = valueA;
+				}
+				if(v > 0.0f)
+				{
+					if(fillToBorder && indexA < 0)
+					{
+						indexA = 0;
+						valueA = v;
+					}
+					if(indexA >=0)
+					{
+						int range = y-indexA;
+						if(range > 1)
+						{
+							float slope = (v-valueA)/(range);
+							for(int k=1; k<range; ++k)
+							{
+								registeredDepth.at<float>(indexA+k,x) = valueA+slope*float(k);
+							}
+						}
+					}
+					valueA = v;
+					indexA = y;
+				}
+			}
+		}
+	}
+	else
+	{
+		// horizontal, for each row
+		for(int y=0; y<registeredDepth.rows; ++y)
+		{
+			float valueA = 0.0f;
+			int indexA = -1;
+			for(int x=0; x<registeredDepth.cols; ++x)
+			{
+				float v = registeredDepth.at<float>(y,x);
+				if(fillToBorder && x == registeredDepth.cols-1 && v<=0.0f && indexA>=0)
+				{
+					v = valueA;
+				}
+				if(v > 0.0f)
+				{
+					if(fillToBorder && indexA < 0)
+					{
+						indexA = 0;
+						valueA = v;
+					}
+					if(indexA >=0)
+					{
+						int range = x-indexA;
+						if(range > 1)
+						{
+							float slope = (v-valueA)/(range);
+							for(int k=1; k<range; ++k)
+							{
+								registeredDepth.at<float>(y,indexA+k) = valueA+slope*float(k);
+							}
+						}
+					}
+					valueA = v;
+					indexA = x;
+				}
+			}
+		}
+	}
+}
+
 bool isFinite(const cv::Point3f & pt)
 {
 	return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
@@ -1114,7 +1309,28 @@ cv::Mat loadScan(
 		int normalsK)
 {
 	cv::Mat scan;
-	UDEBUG("Loading scan (step=%d, voxel=%f m, normalsK=%d) : %s", downsampleStep, voxelSize, normalsK, path.c_str());
+	UDEBUG("Loading scan (normalsK=%d) : %s", normalsK, path.c_str());
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = loadCloud(path, Transform::getIdentity(), downsampleStep, voxelSize);
+	if(normalsK > 0 && cloud->size())
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
+		scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
+	}
+	else
+	{
+		scan = util3d::laserScanFromPointCloud(*cloud, transform);
+	}
+	return scan;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
+		const std::string & path,
+		const Transform & transform,
+		int downsampleStep,
+		float voxelSize)
+{
+	UASSERT(!transform.isNull());
+	UDEBUG("Loading cloud (step=%d, voxel=%f m) : %s", downsampleStep, voxelSize, path.c_str());
 	std::string fileName = UFile::getName(path);
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(UFile::getExtension(fileName).compare("bin") == 0)
@@ -1141,16 +1357,11 @@ cv::Mat loadScan(
 		cloud = util3d::voxelize(cloud, voxelSize);
 		UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
 	}
-	if(normalsK > 0 && cloud->size())
+	if(transform.isIdentity())
 	{
-		pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
-		scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
+		return cloud;
 	}
-	else
-	{
-		scan = util3d::laserScanFromPointCloud(*cloud, transform);
-	}
-	return scan;
+	return util3d::transformPointCloud(cloud, transform);
 }
 
 }