/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 the Universite de Sherbrooke 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 HOLDER 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.
*/

#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <opencv2/imgproc/imgproc.hpp>

namespace rtabmap
{

namespace util3d
{

cv::Mat rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
{
	cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);

	for(unsigned int h = 0; h < cloud.height; h++)
	{
		for(unsigned int w = 0; w < cloud.width; w++)
		{
			if(bgrOrder)
			{
				frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
				frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
				frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
			}
			else
			{
				frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
				frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
				frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
			}
		}
	}
	return frameBGR;
}

// return float image in meter
cv::Mat depthFromCloud(
		const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
		bool depth16U)
{
	cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
	for(unsigned int h = 0; h < cloud.height; h++)
	{
		for(unsigned int w = 0; w < cloud.width; w++)
		{
			float depth = cloud.at(h*cloud.width + w).z;
			if(depth16U)
			{
				depth *= 1000.0f;
				unsigned short depthMM = 0;
				if(depth <= (float)USHRT_MAX)
				{
					depthMM = (unsigned short)depth;
				}
				frameDepth.at<unsigned short>(h,w) = depthMM;
			}
			else
			{
				frameDepth.at<float>(h,w) = depth;
			}
		}
	}
	return frameDepth;
}

// return (unsigned short 16bits image in mm) (float 32bits image in m)
void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
		cv::Mat & frameBGR,
		cv::Mat & frameDepth,
		bool bgrOrder,
		bool depth16U)
{
	frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
	frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);

	for(unsigned int h = 0; h < cloud.height; h++)
	{
		for(unsigned int w = 0; w < cloud.width; w++)
		{
			//rgb
			if(bgrOrder)
			{
				frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
				frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
				frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
			}
			else
			{
				frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
				frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
				frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
			}

			//depth
			float depth = cloud.at(h*cloud.width + w).z;
			if(depth16U)
			{
				depth *= 1000.0f;
				unsigned short depthMM = 0;
				if(depth <= (float)USHRT_MAX)
				{
					depthMM = (unsigned short)depth;
				}
				frameDepth.at<unsigned short>(h,w) = depthMM;
			}
			else
			{
				frameDepth.at<float>(h,w) = depth;
			}
		}
	}
}

pcl::PointXYZ projectDepthTo3D(
		const cv::Mat & depthImage,
		float x, float y,
		float cx, float cy,
		float fx, float fy,
		bool smoothing,
		float depthErrorRatio)
{
	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);

	pcl::PointXYZ pt;

	float depth = util2d::getDepth(depthImage, x, y, smoothing, depthErrorRatio);
	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)
		cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)

		// Fill in XYZ
		pt.x = (x - cx) * depth / fx;
		pt.y = (y - cy) * depth / fy;
		pt.z = depth;
	}
	else
	{
		pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
	}
	return pt;
}

Eigen::Vector3f projectDepthTo3DRay(
		const cv::Size & imageSize,
		float x, float y,
		float cx, float cy,
		float fx, float fy)
{
	Eigen::Vector3f ray;

	// Use correct principal point from calibration
	cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f; //cameraInfo.K.at(2)
	cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f; //cameraInfo.K.at(5)

	// Fill in XYZ
	ray[0] = (x - cx) / fx;
	ray[1] = (y - cy) / fy;
	ray[2] = 1.0f;

	return ray;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
		const cv::Mat & imageDepth,
		float cx, float cy,
		float fx, float fy,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices)
{
	CameraModel model(fx, fy, cx, cy);
	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>);
	if(decimation == 0)
	{
		decimation = 1;
	}
	float rgbToDepthFactorX = 1.0f;
	float rgbToDepthFactorY = 1.0f;

	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);

		if(decimation < 0)
		{
			UDEBUG("Decimation from model (%d)", decimation);
			if(model.imageHeight() % decimation != 0)
			{
				UERROR("Decimation is not valid for current image size (model.imageHeight()=%d decimation=%d). The cloud is not created.", model.imageHeight(), decimation);
				return cloud;
			}
			if(model.imageWidth() % decimation != 0)
			{
				UERROR("Decimation is not valid for current image size (model.imageWidth()=%d decimation=%d). The cloud is not created.", model.imageWidth(), decimation);
				return cloud;
			}

			// decimate from RGB image size, upsample depth if needed
			decimation = -1*decimation;

			int targetSize = model.imageHeight() / decimation;
			if(targetSize > imageDepthIn.rows)
			{
				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)
			{
				decimation = 1;
			}
			else
			{
				UASSERT(imageDepthIn.rows % targetSize == 0);
				decimation = imageDepthIn.rows / targetSize;
			}
		}
		else
		{
			if(imageDepthIn.rows % decimation != 0)
			{
				UERROR("Decimation is not valid for current image size (imageDepth.rows=%d decimation=%d). The cloud is not created.", imageDepthIn.rows, decimation);
				return cloud;
			}
			if(imageDepthIn.cols % decimation != 0)
			{
				UERROR("Decimation is not valid for current image size (imageDepth.cols=%d decimation=%d). The cloud is not created.", imageDepthIn.cols, decimation);
				return cloud;
			}
		}

		rgbToDepthFactorX = 1.0f/float((model.imageWidth() / imageDepth.cols));
		rgbToDepthFactorY = 1.0f/float((model.imageHeight() / imageDepth.rows));
	}
	else
	{
		decimation = abs(decimation);
		UASSERT_MSG(imageDepth.rows % decimation == 0, uFormat("rows=%d decimation=%d", imageDepth.rows, decimation).c_str());
		UASSERT_MSG(imageDepth.cols % decimation == 0, uFormat("cols=%d decimation=%d", imageDepth.cols, decimation).c_str());
	}

	//cloud.header = cameraInfo.header;
	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());
	}

	float depthFx = model.fx() * rgbToDepthFactorX;
	float depthFy = model.fy() * rgbToDepthFactorY;
	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) 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;
	for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
	{
		for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
		{
			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));

			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
			{
				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 = ptXYZ.x;
					pt.y = ptXYZ.y;
					pt.z = ptXYZ.z;
					if(validIndices)
					{
						validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
					}
				}
			}
		}
	}

	if(validIndices)
	{
		validIndices->resize(oi);
	}

	return cloud;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
		const cv::Mat & imageRgb,
		const cv::Mat & imageDepth,
		float cx, float cy,
		float fx, float fy,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices)
{
	CameraModel model(fx, fy, cx, cy);
	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>);
	if(decimation == 0)
	{
		decimation = 1;
	}
	UDEBUG("");
	UASSERT(model.isValidForProjection());
	UASSERT_MSG((model.imageHeight() == 0 && model.imageWidth() == 0) ||
			    (model.imageHeight() == imageRgb.rows && model.imageWidth() == imageRgb.cols),
				uFormat("model=%dx%d rgb=%dx%d", model.imageWidth(), model.imageHeight(), imageRgb.cols, imageRgb.rows).c_str());
	//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)
		{
			int oldDecimation = decimation;
			while(decimation <= -1)
			{
				if(imageRgb.rows % decimation == 0 && imageRgb.cols % decimation == 0)
				{
					break;
				}
				++decimation;
			}

			if(imageRgb.rows % oldDecimation != 0 || imageRgb.cols % oldDecimation != 0)
			{
				UWARN("Decimation (%d) is not valid for current image size (rgb=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageRgb.cols, imageRgb.rows, decimation);
			}
		}
	}
	else
	{
		if(imageDepthIn.rows % decimation != 0 || imageDepthIn.cols % decimation != 0)
		{
			int oldDecimation = decimation;
			while(decimation >= 1)
			{
				if(imageDepthIn.rows % decimation == 0 && imageDepthIn.cols % decimation == 0)
				{
					break;
				}
				--decimation;
			}

			if(imageDepthIn.rows % oldDecimation != 0 || imageDepthIn.cols % oldDecimation != 0)
			{
				UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDepthIn.cols, imageDepthIn.rows, decimation);
			}
		}
	}

	cv::Mat imageDepth = imageDepthIn;
	cv::Mat imageDepthConfidence = confidenceThr==0?cv::Mat():imageDepthConfidenceIn;
	if(decimation < 0)
	{
		UDEBUG("Decimation from RGB image (%d)", decimation);
		// decimate from RGB image size, upsample depth if needed
		decimation = -1*decimation;

		int targetSize = imageRgb.rows / decimation;
		if(targetSize > imageDepthIn.rows)
		{
			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)
		{
			decimation = 1;
		}
		else
		{
			UASSERT(imageDepthIn.rows % targetSize == 0);
			decimation = imageDepthIn.rows / targetSize;
		}
	}

	bool mono;
	if(imageRgb.channels() == 3) // BGR
	{
		mono = false;
	}
	else if(imageRgb.channels() == 1) // Mono
	{
		mono = true;
	}
	else
	{
		return cloud;
	}

	//cloud.header = cameraInfo.header;
	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());
	}

	float rgbToDepthFactorX = float(imageRgb.cols) / float(imageDepth.cols);
	float rgbToDepthFactorY = float(imageRgb.rows) / float(imageDepth.rows);
	float depthFx = model.fx() / rgbToDepthFactorX;
	float depthFy = model.fy() / rgbToDepthFactorY;
	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) 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;
	for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
	{
		for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
		{
			pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));

			int x = int(w*rgbToDepthFactorX);
			int y = int(h*rgbToDepthFactorY);
			UASSERT(x >=0 && x<imageRgb.cols && y >=0 && y<imageRgb.rows);
			if(!mono)
			{
				const unsigned char * bgr = imageRgb.ptr<unsigned char>(y,x);
				pt.b = bgr[0];
				pt.g = bgr[1];
				pt.r = bgr[2];
			}
			else
			{
				unsigned char v = imageRgb.at<unsigned char>(y,x);
				pt.b = v;
				pt.g = v;
				pt.r = v;
			}

			pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
			if(imageDepthConfidence.empty() || imageDepthConfidence.at<unsigned char>(h,w) >= confidenceThr)
			{
				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 = ptXYZ.x;
					pt.y = ptXYZ.y;
					pt.z = ptXYZ.z;
					if (validIndices)
					{
						validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
					}
					++oi;
				}
			}
		}
	}
	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,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices)
{
	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
	UASSERT(decimation >= 1);

	if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
	{
		int oldDecimation = decimation;
		while(decimation >= 1)
		{
			if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
			{
				break;
			}
			--decimation;
		}

		if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
		{
			UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
		}
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	//cloud.header = cameraInfo.header;
	cloud->height = imageDisparity.rows/decimation;
	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)
		{
			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
			{
				float disp = float(imageDisparity.at<short>(h,w))/16.0f;
				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
				if(pt.z >= minDepth && (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());
				}
			}
		}
	}
	else
	{
		for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
		{
			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
			{
				float disp = imageDisparity.at<float>(h,w);
				cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
				if(pt.z > minDepth && (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;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
		const cv::Mat & imageRgb,
		const cv::Mat & imageDisparity,
		const StereoCameraModel & model,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices)
{
	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
	UASSERT(imageRgb.rows == imageDisparity.rows &&
			imageRgb.cols == imageDisparity.cols &&
			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
	UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
	UASSERT(decimation >= 1);

	if(imageDisparity.rows % decimation != 0 || imageDisparity.cols % decimation != 0)
	{
		int oldDecimation = decimation;
		while(decimation >= 1)
		{
			if(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0)
			{
				break;
			}
			--decimation;
		}

		if(imageDisparity.rows % oldDecimation != 0 || imageDisparity.cols % oldDecimation != 0)
		{
			UWARN("Decimation (%d) is not valid for current image size (depth=%dx%d). Highest compatible decimation used=%d.", oldDecimation, imageDisparity.cols, imageDisparity.rows, decimation);
		}
	}

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	bool mono;
	if(imageRgb.channels() == 3) // BGR
	{
		mono = false;
	}
	else // Mono
	{
		mono = true;
	}

	//cloud.header = cameraInfo.header;
	cloud->height = imageRgb.rows/decimation;
	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)
		{
			pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
			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];
			}
			else
			{
				unsigned char v = imageRgb.at<unsigned char>(h,w);
				pt.b = v;
				pt.g = v;
				pt.r = v;
			}

			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);
			if(util3d::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (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;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
		const cv::Mat & imageLeft,
		const cv::Mat & imageRight,
		const StereoCameraModel & model,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices,
		const ParametersMap & parameters)
{
	UASSERT(!imageLeft.empty() && !imageRight.empty());
	UASSERT(imageRight.type() == CV_8UC1 || imageRight.type() == CV_8UC3);
	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
	UASSERT(imageLeft.rows == imageRight.rows &&
			imageLeft.cols == imageRight.cols);
	UASSERT(decimation >= 1.0f);

	cv::Mat leftColor = imageLeft;
	cv::Mat rightColor = imageRight;

	cv::Mat leftMono;
	if(leftColor.channels() == 3)
	{
		cv::cvtColor(leftColor, leftMono, cv::COLOR_BGR2GRAY);
	}
	else
	{
		leftMono = leftColor;
	}

	cv::Mat rightMono;
	if(rightColor.channels() == 3)
	{
		cv::cvtColor(rightColor, rightMono, cv::COLOR_BGR2GRAY);
	}
	else
	{
		rightMono = rightColor;
	}

	return cloudFromDisparityRGB(
			leftColor,
			util2d::disparityFromStereoImages(leftMono, rightMono, parameters),
			model,
			decimation,
			maxDepth,
			minDepth,
			validIndices);
}

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
		const SensorData & sensorData,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<pcl::IndicesPtr> * validIndices,
		const ParametersMap & stereoParameters,
		const std::vector<float> & roiRatios,
		unsigned char confidenceThr)
{
	if(decimation == 0)
	{
		decimation = 1;
	}

	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;

	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
	{
		//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)
		{
			clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
			if(validIndices)
			{
				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
			}
			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 ||
					roiRatios[1] > 0.0f ||
					roiRatios[2] > 0.0f ||
					roiRatios[3] > 0.0f))
				{
					cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
					cv::Rect roiRgb;
					if(model.imageWidth() && model.imageHeight())
					{
						roiRgb = util2d::computeRoi(model.imageSize(), roiRatios);
					}
					if(	roiDepth.width%decimation==0 &&
						roiDepth.height%decimation==0 &&
						(roiRgb.width != 0 ||
						   (roiRgb.width%decimation==0 &&
							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));
						}
						else
						{
							model = model.roi(roiDepth);
						}
					}
					else
					{
						UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
							  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
							  "by decimation parameter (%d). Ignoring ROI ratios...",
							  roiRatios[0],
							  roiRatios[1],
							  roiRatios[2],
							  roiRatios[3],
							  roiDepth.width,
							  roiDepth.height,
							  roiRgb.width,
							  roiRgb.height,
							  decimation);
					}
				}

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

				if(tmp->size())
				{
					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
					{
						tmp = util3d::transformPointCloud(tmp, model.localTransform());
					}
					clouds.back() = tmp;
				}
			}
			else
			{
				UERROR("Camera model %d is invalid", i);
			}
		}
	}
	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
	{
		//stereo
		UASSERT(sensorData.rightRaw().type() == CV_8UC1 || sensorData.rightRaw().type() == CV_8UC3);

		cv::Mat leftMono;
		if(sensorData.imageRaw().channels() == 3)
		{
			cv::cvtColor(sensorData.imageRaw(), leftMono, cv::COLOR_BGR2GRAY);
		}
		else
		{
			leftMono = sensorData.imageRaw();
		}

		cv::Mat rightMono;
		if(sensorData.rightRaw().channels() == 3)
		{
			cv::cvtColor(sensorData.rightRaw(), rightMono, cv::COLOR_BGR2GRAY);
		}
		else
		{
			rightMono = sensorData.rightRaw();
		}

		UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
		UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
		for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
		{
			clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
			if(validIndices)
			{
				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
			}
			if(sensorData.stereoCameraModels()[i].isValidForProjection())
			{
				cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
				cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
				StereoCameraModel model = sensorData.stereoCameraModels()[i];
				if( roiRatios.size() == 4 &&
					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
					 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
					 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
					 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
				{
					cv::Rect roi = util2d::computeRoi(left, roiRatios);
					if(	roi.width%decimation==0 &&
						roi.height%decimation==0)
					{
						left = cv::Mat(left, roi);
						right = cv::Mat(right, roi);
						model.roi(roi);
					}
					else
					{
						UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
							  "dimension (left=%dx%d) cannot be divided exactly "
							  "by decimation parameter (%d). Ignoring ROI ratios...",
							  roiRatios[0],
							  roiRatios[1],
							  roiRatios[2],
							  roiRatios[3],
							  roi.width,
							  roi.height,
							  decimation);
					}
				}

				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = cloudFromDisparity(
						util2d::disparityFromStereoImages(left, right, stereoParameters),
						model,
						decimation,
						maxDepth,
						minDepth,
						validIndices?validIndices->back().get():0);

				if(tmp->size())
				{
					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
					{
						tmp = util3d::transformPointCloud(tmp, model.localTransform());
					}
					clouds.back() = tmp;
				}
			}
			else
			{
				UERROR("Stereo camera model %d is invalid", i);
			}
		}
	}

	return clouds;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
		const SensorData & sensorData,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices,
		const ParametersMap & stereoParameters,
		const std::vector<float> & roiRatios,
		unsigned char confidenceThr)
{
	std::vector<pcl::IndicesPtr> validIndicesV;
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
		sensorData,
		decimation,
		maxDepth,
		minDepth,
		validIndices?&validIndicesV:0,
		stereoParameters,
		roiRatios,
		confidenceThr);

	if(validIndices)
	{
		UASSERT(validIndicesV.size() == clouds.size());
	}
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	if(clouds.size() == 1)
	{
		cloud = clouds[0];
		if(validIndices)
		{
			*validIndices = *validIndicesV[0];
		}
	}
	else
	{
		for(size_t i=0; i<clouds.size(); ++i)
		{
			*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
		}
		if(validIndices)
		{
			//generate indices for all points (they are all valid)
			validIndices->resize(cloud->size());
			for(size_t i=0; i<cloud->size(); ++i)
			{
				validIndices->at(i) = i;
			}
		}
	}
	return cloud;
}

std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
		const SensorData & sensorData,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<pcl::IndicesPtr> * validIndices,
		const ParametersMap & stereoParameters,
		const std::vector<float> & roiRatios,
		unsigned char confidenceThr)
{
	if(decimation == 0)
	{
		decimation = 1;
	}

	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;

	if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
	{
		//depth
		UDEBUG("");
		UASSERT(int((sensorData.imageRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.imageRaw().cols);
		UASSERT(int((sensorData.depthRaw().cols/sensorData.cameraModels().size())*sensorData.cameraModels().size()) == sensorData.depthRaw().cols);
		//UASSERT_MSG(sensorData.imageRaw().cols % sensorData.depthRaw().cols == 0, uFormat("rgb=%d depth=%d", sensorData.imageRaw().cols, sensorData.depthRaw().cols).c_str());
		//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)
		{
			clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
			if(validIndices)
			{
				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
			}
			if(sensorData.cameraModels()[i].isValidForProjection())
			{
				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) ||
					 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
					 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
					 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
				{
					cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
					cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
					if(	roiDepth.width%decimation==0 &&
						roiDepth.height%decimation==0 &&
						roiRgb.width%decimation==0 &&
						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);
					}
					else
					{
						UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
							  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
							  "by decimation parameter (%d). Ignoring ROI ratios...",
							  roiRatios[0],
							  roiRatios[1],
							  roiRatios[2],
							  roiRatios[3],
							  roiDepth.width,
							  roiDepth.height,
							  roiRgb.width,
							  roiRgb.height,
							  decimation);
					}
				}

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

				if(tmp->size())
				{
					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
					{
						tmp = util3d::transformPointCloud(tmp, model.localTransform());
					}
					clouds.back() = tmp;
				}
			}
			else
			{
				UERROR("Camera model %d is invalid", i);
			}
		}
	}
	else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
	{
		//stereo
		UDEBUG("");

		UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
		UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
		for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
		{
			clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
			if(validIndices)
			{
				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
			}
			if(sensorData.stereoCameraModels()[i].isValidForProjection())
			{
				cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows));
				cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
				StereoCameraModel model = sensorData.stereoCameraModels()[i];
				if( roiRatios.size() == 4 &&
					((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
					 (roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
					 (roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
					 (roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
				{
					cv::Rect roi = util2d::computeRoi(left, roiRatios);
					if(	roi.width%decimation==0 &&
						roi.height%decimation==0)
					{
						left = cv::Mat(left, roi);
						right = cv::Mat(right, roi);
						model.roi(roi);
					}
					else
					{
						UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
							  "dimension (left=%dx%d) cannot be divided exactly "
							  "by decimation parameter (%d). Ignoring ROI ratios...",
							  roiRatios[0],
							  roiRatios[1],
							  roiRatios[2],
							  roiRatios[3],
							  roi.width,
							  roi.height,
							  decimation);
					}
				}

				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = cloudFromStereoImages(
						left,
						right,
						model,
						decimation,
						maxDepth,
						minDepth,
						validIndices?validIndices->back().get():0,
						stereoParameters);

				if(tmp->size())
				{
					if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
					{
						tmp = util3d::transformPointCloud(tmp, model.localTransform());
					}
					clouds.back() = tmp;
				}
			}
			else
			{
				UERROR("Stereo camera model %d is invalid", i);
			}
		}
	}

	return clouds;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
		const SensorData & sensorData,
		int decimation,
		float maxDepth,
		float minDepth,
		std::vector<int> * validIndices,
		const ParametersMap & stereoParameters,
		const std::vector<float> & roiRatios,
		unsigned char confidenceThr)
{
	std::vector<pcl::IndicesPtr> validIndicesV;
	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
		sensorData,
		decimation,
		maxDepth,
		minDepth,
		validIndices?&validIndicesV:0,
		stereoParameters,
		roiRatios,
		confidenceThr);

	if(validIndices)
	{
		UASSERT(validIndicesV.size() == clouds.size());
	}
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

	if(clouds.size() == 1)
	{
		cloud = clouds[0];
		if(validIndices)
		{
			*validIndices = *validIndicesV[0];
		}
	}
	else
	{
		for(size_t i=0; i<clouds.size(); ++i)
		{
			*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
		}
		if(validIndices)
		{
			//generate indices for all points (they are all valid)
			validIndices->resize(cloud->size());
			for(size_t i=0; i<cloud->size(); ++i)
			{
				validIndices->at(i) = i;
			}
		}
	}
	return cloud;
}

pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
		const cv::Mat & depthImage,
		float fx,
		float fy,
		float cx,
		float cy,
		float maxDepth,
		float minDepth,
		const Transform & localTransform)
{
	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
	UASSERT(!localTransform.isNull());

	pcl::PointCloud<pcl::PointXYZ> scan;
	int middle = depthImage.rows/2;
	if(middle)
	{
		scan.resize(depthImage.cols);
		int oi = 0;
		for(int i=depthImage.cols-1; i>=0; --i)
		{
			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
			if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
			{
				if(!localTransform.isIdentity())
				{
					pt = util3d::transformPoint(pt, localTransform);
				}
				scan[oi++] = pt;
			}
		}
		scan.resize(oi);
	}
	return scan;
}

pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
		const cv::Mat & depthImages,
		const std::vector<CameraModel> & cameraModels,
		float maxDepth,
		float minDepth)
{
	pcl::PointCloud<pcl::PointXYZ> scan;
	UASSERT(!depthImages.empty() && !cameraModels.empty());
	UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
	int subImageWidth = depthImages.cols/cameraModels.size();
	for(int i=(int)cameraModels.size()-1; i>=0; --i)
	{
		UASSERT(cameraModels[i].isValidForProjection());
		UASSERT(cameraModels[i].imageWidth() == subImageWidth);
		UASSERT(subImageWidth*(i+1) <= depthImages.cols);
		cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));

		scan += laserScanFromDepthImage(
				depth,
				cameraModels[i].fx(),
				cameraModels[i].fy(),
				cameraModels[i].cx(),
				cameraModels[i].cy(),
				maxDepth,
				minDepth,
				cameraModels[i].localTransform());
	}
	return scan;
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi = 0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC3);
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || pcl::isFinite(cloud.at(index)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZ pt = pcl::transformPoint(cloud.at(index), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
				}
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC3);
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || pcl::isFinite(cloud.at(i)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
				}
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZ);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi=0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(6));
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
					uIsFinite(cloud.at(index).normal_x) &&
					uIsFinite(cloud.at(index).normal_y) &&
					uIsFinite(cloud.at(index).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointNormal pt = util3d::transformPoint(cloud.at(index), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[3] = pt.normal_x;
					ptr[4] = pt.normal_y;
					ptr[5] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
					ptr[3] = cloud.at(index).normal_x;
					ptr[4] = cloud.at(index).normal_y;
					ptr[5] = cloud.at(index).normal_z;
				}
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
					uIsFinite(cloud.at(i).normal_x) &&
					uIsFinite(cloud.at(i).normal_y) &&
					uIsFinite(cloud.at(i).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[3] = pt.normal_x;
					ptr[4] = pt.normal_y;
					ptr[5] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
					ptr[3] = cloud.at(i).normal_x;
					ptr[4] = cloud.at(i).normal_y;
					ptr[5] = cloud.at(i).normal_z;
				}
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
	UASSERT(cloud.size() == normals.size());
	cv::Mat laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(6));
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi =0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointNormal pt;
				pt.x = cloud.at(i).x;
				pt.y = cloud.at(i).y;
				pt.z = cloud.at(i).z;
				pt.normal_x = normals.at(i).normal_x;
				pt.normal_y = normals.at(i).normal_y;
				pt.normal_z = normals.at(i).normal_z;
				pt = util3d::transformPoint(pt, transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.z;
				ptr[3] = pt.normal_x;
				ptr[4] = pt.normal_y;
				ptr[5] = pt.normal_z;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = cloud.at(i).z;
				ptr[3] = normals.at(i).normal_x;
				ptr[4] = normals.at(i).normal_y;
				ptr[5] = normals.at(i).normal_z;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZNormal);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi=0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || pcl::isFinite(cloud.at(index)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(index), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
				}
				int * ptrInt = (int*)ptr;
				ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || pcl::isFinite(cloud.at(i)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
				}
				int * ptrInt = (int*)ptr;
				ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGB);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<rtabmap::PointXYZIRT> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	// Layout: [x, y, z, intensity, ring, time] (ring cast to float, values up to
	// ~16M are exactly representable so all realistic laser line counts fit).
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi = 0;
	const int total = indices.get() ? (int)indices->size() : (int)cloud.size();
	laserScan = cv::Mat(1, total, CV_32FC(6));
	for(int i=0; i<total; ++i)
	{
		int index = indices.get() ? indices->at(i) : i;
		const rtabmap::PointXYZIRT & src = cloud.at(index);
		if(filterNaNs && !pcl::isFinite(src))
		{
			continue;
		}
		float * ptr = laserScan.ptr<float>(0, oi++);
		if(!nullTransform)
		{
			pcl::PointXYZ pt(src.x, src.y, src.z);
			pt = pcl::transformPoint(pt, transform3f);
			ptr[0] = pt.x;
			ptr[1] = pt.y;
			ptr[2] = pt.z;
		}
		else
		{
			ptr[0] = src.x;
			ptr[1] = src.y;
			ptr[2] = src.z;
		}
		ptr[3] = src.intensity;
		ptr[4] = static_cast<float>(src.ring);
		ptr[5] = src.time;
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0, oi)), 0, 0.0f, LaserScan::kXYZIRT);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi=0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(4));
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || pcl::isFinite(cloud.at(index)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZI pt = pcl::transformPoint(cloud.at(index), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
				}
				ptr[3] = cloud.at(index).intensity;
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(4));
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || pcl::isFinite(cloud.at(i)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
				}
				ptr[3] = cloud.at(i).intensity;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZI);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
	UASSERT(cloud.size() == normals.size());
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi = 0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || pcl::isFinite(cloud.at(i)))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZRGBNormal pt;
				pt.x = cloud.at(i).x;
				pt.y = cloud.at(i).y;
				pt.z = cloud.at(i).z;
				pt.normal_x = normals.at(i).normal_x;
				pt.normal_y = normals.at(i).normal_y;
				pt.normal_z = normals.at(i).normal_z;
				pt = util3d::transformPoint(pt, transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.z;
				ptr[4] = pt.normal_x;
				ptr[5] = pt.normal_y;
				ptr[6] = pt.normal_z;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = cloud.at(i).z;
				ptr[4] = normals.at(i).normal_x;
				ptr[5] = normals.at(i).normal_y;
				ptr[6] = normals.at(i).normal_z;
			}
			int * ptrInt = (int*)ptr;
			ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi = 0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
					uIsFinite(cloud.at(index).normal_x) &&
					uIsFinite(cloud.at(index).normal_y) &&
					uIsFinite(cloud.at(index).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(index), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[4] = pt.normal_x;
					ptr[5] = pt.normal_y;
					ptr[6] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
					ptr[4] = cloud.at(index).normal_x;
					ptr[5] = cloud.at(index).normal_y;
					ptr[6] = cloud.at(index).normal_z;
				}
				int * ptrInt = (int*)ptr;
				ptrInt[3] = int(cloud.at(index).b) | (int(cloud.at(index).g) << 8) | (int(cloud.at(index).r) << 16);
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
					uIsFinite(cloud.at(i).normal_x) &&
					uIsFinite(cloud.at(i).normal_y) &&
					uIsFinite(cloud.at(i).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZRGBNormal pt = util3d::transformPoint(cloud.at(i), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[4] = pt.normal_x;
					ptr[5] = pt.normal_y;
					ptr[6] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
					ptr[4] = cloud.at(i).normal_x;
					ptr[5] = cloud.at(i).normal_y;
					ptr[6] = cloud.at(i).normal_z;
				}
				int * ptrInt = (int*)ptr;
				ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZRGBNormal);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
	UASSERT(cloud.size() == normals.size());
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZINormal pt;
				pt.x = cloud.at(i).x;
				pt.y = cloud.at(i).y;
				pt.z = cloud.at(i).z;
				pt.normal_x = normals.at(i).normal_x;
				pt.normal_y = normals.at(i).normal_y;
				pt.normal_z = normals.at(i).normal_z;
				pt = util3d::transformPoint(pt, transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.z;
				ptr[4] = pt.normal_x;
				ptr[5] = pt.normal_y;
				ptr[6] = pt.normal_z;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = cloud.at(i).z;
				ptr[4] = normals.at(i).normal_x;
				ptr[5] = normals.at(i).normal_y;
				ptr[6] = normals.at(i).normal_z;
			}
			ptr[3] = cloud.at(i).intensity;
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
}

LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{
	return laserScanFromPointCloud(cloud, pcl::IndicesPtr(), transform, filterNaNs);
}
LaserScan laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const pcl::IndicesPtr & indices, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan;
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi = 0;
	if(indices.get())
	{
		laserScan = cv::Mat(1, (int)indices->size(), CV_32FC(7));
		for(unsigned int i=0; i<indices->size(); ++i)
		{
			int index = indices->at(i);
			if(!filterNaNs || (pcl::isFinite(cloud.at(index)) &&
					uIsFinite(cloud.at(index).normal_x) &&
					uIsFinite(cloud.at(index).normal_y) &&
					uIsFinite(cloud.at(index).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(index), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[4] = pt.normal_x;
					ptr[5] = pt.normal_y;
					ptr[6] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(index).x;
					ptr[1] = cloud.at(index).y;
					ptr[2] = cloud.at(index).z;
					ptr[4] = cloud.at(index).normal_x;
					ptr[5] = cloud.at(index).normal_y;
					ptr[6] = cloud.at(index).normal_z;
				}
				ptr[3] = cloud.at(i).intensity;
			}
		}
	}
	else
	{
		laserScan = cv::Mat(1, (int)cloud.size(), CV_32FC(7));
		for(unsigned int i=0; i<cloud.size(); ++i)
		{
			if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
					uIsFinite(cloud.at(i).normal_x) &&
					uIsFinite(cloud.at(i).normal_y) &&
					uIsFinite(cloud.at(i).normal_z)))
			{
				float * ptr = laserScan.ptr<float>(0, oi++);
				if(!nullTransform)
				{
					pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
					ptr[0] = pt.x;
					ptr[1] = pt.y;
					ptr[2] = pt.z;
					ptr[4] = pt.normal_x;
					ptr[5] = pt.normal_y;
					ptr[6] = pt.normal_z;
				}
				else
				{
					ptr[0] = cloud.at(i).x;
					ptr[1] = cloud.at(i).y;
					ptr[2] = cloud.at(i).z;
					ptr[4] = cloud.at(i).normal_x;
					ptr[5] = cloud.at(i).normal_y;
					ptr[6] = cloud.at(i).normal_z;
				}
				ptr[3] = cloud.at(i).intensity;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYZINormal);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
	bool nullTransform = transform.isNull();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || pcl::isFinite(cloud.at(i)))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
			}
		}

	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXY);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
	bool nullTransform = transform.isNull();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || pcl::isFinite(cloud.at(i)))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZI pt = pcl::transformPoint(cloud.at(i), transform3f);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.intensity;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = cloud.at(i).intensity;
			}
		}

	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYI);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
	bool nullTransform = transform.isNull();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
				uIsFinite(cloud.at(i).normal_x) &&
				uIsFinite(cloud.at(i).normal_y) &&
				uIsFinite(cloud.at(i).normal_z)))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.normal_x;
				ptr[3] = pt.normal_y;
				ptr[4] = pt.normal_z;
			}
			else
			{
				const pcl::PointNormal & pt = cloud.at(i);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.normal_x;
				ptr[3] = pt.normal_y;
				ptr[4] = pt.normal_z;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
	UASSERT(cloud.size() == normals.size());
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointNormal pt;
				pt.x = cloud.at(i).x;
				pt.y = cloud.at(i).y;
				pt.z = cloud.at(i).z;
				pt.normal_x = normals.at(i).normal_x;
				pt.normal_y = normals.at(i).normal_y;
				pt.normal_z = normals.at(i).normal_z;
				pt = util3d::transformPoint(pt, transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.normal_x;
				ptr[3] = pt.normal_y;
				ptr[4] = pt.normal_z;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = normals.at(i).normal_x;
				ptr[3] = normals.at(i).normal_y;
				ptr[4] = normals.at(i).normal_z;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYNormal);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZINormal> & cloud, const Transform & transform, bool filterNaNs)
{
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
	bool nullTransform = transform.isNull();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) &&
				uIsFinite(cloud.at(i).normal_x) &&
				uIsFinite(cloud.at(i).normal_y) &&
				uIsFinite(cloud.at(i).normal_z)))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZINormal pt = util3d::transformPoint(cloud.at(i), transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.intensity;
				ptr[3] = pt.normal_x;
				ptr[4] = pt.normal_y;
				ptr[5] = pt.normal_z;
			}
			else
			{
				const pcl::PointXYZINormal & pt = cloud.at(i);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.intensity;
				ptr[3] = pt.normal_x;
				ptr[4] = pt.normal_y;
				ptr[5] = pt.normal_z;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
}

LaserScan laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZI> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform, bool filterNaNs)
{
	UASSERT(cloud.size() == normals.size());
	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
	bool nullTransform = transform.isNull() || transform.isIdentity();
	int oi=0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(!filterNaNs || (pcl::isFinite(cloud.at(i)) && pcl::isFinite(normals.at(i))))
		{
			float * ptr = laserScan.ptr<float>(0, oi++);
			if(!nullTransform)
			{
				pcl::PointXYZINormal pt;
				pt.x = cloud.at(i).x;
				pt.y = cloud.at(i).y;
				pt.z = cloud.at(i).z;
				pt.normal_x = normals.at(i).normal_x;
				pt.normal_y = normals.at(i).normal_y;
				pt.normal_z = normals.at(i).normal_z;
				pt = util3d::transformPoint(pt, transform);
				ptr[0] = pt.x;
				ptr[1] = pt.y;
				ptr[2] = pt.intensity;
				ptr[3] = pt.normal_x;
				ptr[4] = pt.normal_y;
				ptr[5] = pt.normal_z;
			}
			else
			{
				ptr[0] = cloud.at(i).x;
				ptr[1] = cloud.at(i).y;
				ptr[2] = cloud.at(i).intensity;
				ptr[3] = normals.at(i).normal_x;
				ptr[4] = normals.at(i).normal_y;
				ptr[5] = normals.at(i).normal_z;
			}
		}
	}
	if(oi == 0)
	{
		return LaserScan();
	}
	return LaserScan(laserScan(cv::Range::all(), cv::Range(0,oi)), 0, 0.0f, LaserScan::kXYINormal);
}

pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, const Transform & transform)
{
	pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
	if(laserScan.isEmpty())
	{
		return cloud;
	}

	if(laserScan.format() == LaserScan::kXY || laserScan.format() == LaserScan::kXYZ)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
	}
	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
	}
	else if(laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
	{
		// PCL has no point type with time (and ring): append them to the XYZI fields, with
		// the types laserScanFromPointCloud() reads back (time FLOAT32, ring UINT16).
		pcl::PCLPointCloud2 xyzi;
		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), xyzi);
		const bool hasRing = laserScan.format() == LaserScan::kXYZIRT;

		cloud->header = xyzi.header;
		cloud->height = xyzi.height;
		cloud->width = xyzi.width;
		cloud->is_bigendian = xyzi.is_bigendian;
		cloud->is_dense = xyzi.is_dense;
		cloud->fields = xyzi.fields;
		pcl::PCLPointField time;
		time.name = "time";
		time.offset = xyzi.point_step;
		time.datatype = pcl::PCLPointField::FLOAT32;
		time.count = 1;
		cloud->fields.push_back(time);
		cloud->point_step = xyzi.point_step + 4;
		pcl::PCLPointField ring;
		if(hasRing)
		{
			ring.name = "ring";
			ring.offset = cloud->point_step;
			ring.datatype = pcl::PCLPointField::UINT16;
			ring.count = 1;
			cloud->fields.push_back(ring);
			cloud->point_step += 4; // keep points 4-byte aligned
		}
		cloud->row_step = cloud->point_step * cloud->width;
		cloud->data.resize(size_t(cloud->row_step) * cloud->height, 0);

		const int cols = laserScan.data().cols;
		const size_t points = size_t(cloud->width) * cloud->height;
		for(size_t i=0; i<points; ++i)
		{
			unsigned char * dst = &cloud->data[i * cloud->point_step];
			memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step);
			const float * src = laserScan.data().ptr<float>(int(i) / cols, int(i) % cols);
			memcpy(dst + time.offset, src + laserScan.getTimeOffset(), sizeof(float));
			if(hasRing)
			{
				const std::uint16_t r = (std::uint16_t)src[laserScan.getRingOffset()];
				memcpy(dst + ring.offset, &r, sizeof(r));
			}
		}
	}
	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);
	}
	else if(laserScan.format() == LaserScan::kXYINormal || laserScan.format() == LaserScan::kXYZINormal)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloudINormal(laserScan, transform), *cloud);
	}
	else if(laserScan.format() == LaserScan::kXYZRGB)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloudRGB(laserScan, transform), *cloud);
	}
	else if(laserScan.format() == LaserScan::kXYZRGBNormal)
	{
		pcl::toPCLPointCloud2(*laserScanToPointCloudRGBNormal(laserScan, transform), *cloud);
	}
	else
	{
		UERROR("Unknown conversion from LaserScan format %d to PointCloud2.", laserScan.format());
	}
	return cloud;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = util3d::laserScanToPoint(laserScan, i);
		if(!nullTransform)
		{
			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
		}
	}
	return output;
}

pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
{
	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = laserScanToPointNormal(laserScan, i);
		if(!nullTransform)
		{
			output->at(i) = util3d::transformPoint(output->at(i), transform);
		}
	}
	return output;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
		if(!nullTransform)
		{
			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
		}
	}
	return output;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform,  float intensity)
{
	pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull() || transform.isIdentity();
	Eigen::Affine3f transform3f = transform.toEigen3f();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = util3d::laserScanToPointI(laserScan, i, intensity);
		if(!nullTransform)
		{
			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
		}
	}
	return output;
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
{
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull() || transform.isIdentity();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = util3d::laserScanToPointRGBNormal(laserScan, i, r, g, b);
		if(!nullTransform)
		{
			output->at(i) = util3d::transformPoint(output->at(i), transform);
		}
	}
	return output;
}

pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform,  float intensity)
{
	pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
	if(laserScan.isOrganized())
	{
		output->width = laserScan.data().cols;
		output->height = laserScan.data().rows;
		output->is_dense = false;
	}
	else
	{
		output->is_dense = true;
	}
	output->resize(laserScan.size());
	bool nullTransform = transform.isNull() || transform.isIdentity();
	for(int i=0; i<laserScan.size(); ++i)
	{
		output->at(i) = util3d::laserScanToPointINormal(laserScan, i, intensity);
		if(!nullTransform)
		{
			output->at(i) = util3d::transformPoint(output->at(i), transform);
		}
	}
	return output;
}

pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointXYZ output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}
	return output;
}

pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointNormal output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}
	if(laserScan.hasNormals())
	{
		int offset = laserScan.getNormalsOffset();
		output.normal_x = ptr[offset];
		output.normal_y = ptr[offset+1];
		output.normal_z = ptr[offset+2];
	}
	return output;
}

pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointXYZRGB output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}

	if(laserScan.hasRGB())
	{
		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
	}
	else if(laserScan.hasIntensity())
	{
		// package intensity float -> rgba
		int * ptrInt = (int*)ptr;
		int indexIntensity = laserScan.getIntensityOffset();
		output.r = (unsigned char)(ptrInt[indexIntensity] & 0xFF);
		output.g = (unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
		output.b = (unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
		output.a = (unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
	}
	else
	{
		output.r = r;
		output.g = g;
		output.b = b;
	}
	return output;
}

pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float intensity)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointXYZI output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}

	if(laserScan.hasIntensity())
	{
		int offset = laserScan.getIntensityOffset();
		output.intensity = ptr[offset];
	}
	else
	{
		output.intensity = intensity;
	}

	return output;
}

pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointXYZRGBNormal output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}

	if(laserScan.hasRGB())
	{
		LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
	}
	else if(laserScan.hasIntensity())
	{
		int * ptrInt = (int*)ptr;
		int indexIntensity = laserScan.getIntensityOffset();
		output.r = (unsigned char)(ptrInt[indexIntensity] & 0xFF);
		output.g = (unsigned char)((ptrInt[indexIntensity] >> 8) & 0xFF);
		output.b = (unsigned char)((ptrInt[indexIntensity] >> 16) & 0xFF);
		output.a = (unsigned char)((ptrInt[indexIntensity] >> 24) & 0xFF);
	}
	else
	{
		output.r = r;
		output.g = g;
		output.b = b;
	}

	if(laserScan.hasNormals())
	{
		int offset = laserScan.getNormalsOffset();
		output.normal_x = ptr[offset];
		output.normal_y = ptr[offset+1];
		output.normal_z = ptr[offset+2];
	}

	return output;
}

pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int index, float intensity)
{
	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
	pcl::PointXYZINormal output;
	int row = index / laserScan.data().cols;
	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
	output.x = ptr[0];
	output.y = ptr[1];
	if(!laserScan.is2d())
	{
		output.z = ptr[2];
	}

	if(laserScan.hasIntensity())
	{
		int offset = laserScan.getIntensityOffset();
		output.intensity = ptr[offset];
	}
	else
	{
		output.intensity = intensity;
	}

	if(laserScan.hasNormals())
	{
		int offset = laserScan.getNormalsOffset();
		output.normal_x = ptr[offset];
		output.normal_y = ptr[offset+1];
		output.normal_z = ptr[offset+2];
	}

	return output;
}

void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
{
	UASSERT(!laserScan.empty());
	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

	const float * ptr = laserScan.ptr<float>(0, 0);
	min.x = max.x = ptr[0];
	min.y = max.y = ptr[1];
	bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
	min.z = max.z = is3d?ptr[2]:0.0f;
	for(int i=1; i<laserScan.cols; ++i)
	{
		ptr = laserScan.ptr<float>(0, i);

		if(ptr[0] < min.x) min.x = ptr[0];
		else if(ptr[0] > max.x) max.x = ptr[0];

		if(ptr[1] < min.y) min.y = ptr[1];
		else if(ptr[1] > max.y) max.y = ptr[1];

		if(is3d)
		{
			if(ptr[2] < min.z) min.z = ptr[2];
			else if(ptr[2] > max.z) max.z = ptr[2];
		}
	}
}
void getMinMax3D(const cv::Mat & laserScan, pcl::PointXYZ & min, pcl::PointXYZ & max)
{
	cv::Point3f minCV, maxCV;
	getMinMax3D(laserScan, minCV, maxCV);
	min.x = minCV.x;
	min.y = minCV.y;
	min.z = minCV.z;
	max.x = maxCV.x;
	max.y = maxCV.y;
	max.z = maxCV.z;
}

// inspired from ROS image_geometry/src/stereo_camera_model.cpp
cv::Point3f projectDisparityTo3D(
		const cv::Point2f & pt,
		float disparity,
		const StereoCameraModel & model)
{
	if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
	{
		//Z = baseline * f / (d + cx1-cx0);
		float c = 0.0f;
		if(model.right().cx()>0.0f && model.left().cx()>0.0f)
		{
			c = model.right().cx() - model.left().cx();
		}
		float W = model.baseline()/(disparity + c);
		return cv::Point3f((pt.x - model.left().cx())*W,
			(pt.y - model.left().cy())*model.left().fx()/model.left().fy()*W, model.left().fx()*W);
	}
	float bad_point = std::numeric_limits<float>::quiet_NaN ();
	return cv::Point3f(bad_point, bad_point, bad_point);
}

cv::Point3f projectDisparityTo3D(
		const cv::Point2f & pt,
		const cv::Mat & disparity,
		const StereoCameraModel & model)
{
	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
	int u = int(pt.x+0.5f);
	int v = int(pt.y+0.5f);
	float bad_point = std::numeric_limits<float>::quiet_NaN ();
	if(uIsInBounds(u, 0, disparity.cols) &&
	   uIsInBounds(v, 0, disparity.rows))
	{
		float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
		return projectDisparityTo3D(pt, d, model);
	}
	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,
		const cv::Mat & laserScan,     // assuming laser scan points are already in /base_link coordinate
		const rtabmap::Transform & cameraTransform) // /base_link -> /camera_link
{
	UASSERT(!cameraTransform.isNull());
	UASSERT(!laserScan.empty());
	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
	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)
	{
		const float* ptr = laserScan.ptr<float>(0, i);

		// Get 3D from laser scan
		cv::Point3f ptScan;
		if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
		{
			// 2D scans
			ptScan.x = ptr[0];
			ptScan.y = ptr[1];
			ptScan.z = 0;
		}
		else // 3D scans
		{
			ptScan.x = ptr[0];
			ptScan.y = ptr[1];
			ptScan.z = ptr[2];
		}
		ptScan = util3d::transformPoint(ptScan, t);

		// re-project in camera frame
		float z = ptScan.z;

		bool set = false;
		if(z > 0.0f)
		{
			float invZ = 1.0f/z;
			float dx = (fx*ptScan.x)*invZ + cx;
			float dy = (fy*ptScan.y)*invZ + cy;
			int dx_low = dx;
			int dy_low = dy;
			int dx_high = dx + 0.5f;
			int dy_high = dy + 0.5f;

			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
			{
				float &zReg = registered.at<float>(dy_low, dx_low);
				if(zReg == 0 || z < zReg)
				{
					zReg = z;
				}
				set = true;
			}
			if((dx_low != dx_high || dy_low != dy_high) &&
				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
			{
				float &zReg = registered.at<float>(dy_high, dx_high);
				if(zReg == 0 || z < zReg)
				{
					zReg = z;
				}
				set = true;
			}
		}
		if(set)
		{
			count++;
		}
	}
	UDEBUG("Points in camera=%d/%d", count, laserScan.cols);

	return registered;
}

cv::Mat projectCloudToCamera(
		const cv::Size & imageSize,
		const cv::Mat & cameraMatrixK,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr laserScan,  // assuming points are already in /base_link coordinate
		const rtabmap::Transform & cameraTransform)           // /base_link -> /camera_link
{
	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;
		bool set = false;
		if(z > 0.0f)
		{
			float invZ = 1.0f/z;
			float dx = (fx*ptScan.x)*invZ + cx;
			float dy = (fy*ptScan.y)*invZ + cy;
			int dx_low = dx;
			int dy_low = dy;
			int dx_high = dx + 0.5f;
			int dy_high = dy + 0.5f;
			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
			{
				set = true;
				float &zReg = registered.at<float>(dy_low, dx_low);
				if(zReg == 0 || z < zReg)
				{
					zReg = z;
				}
			}
			if((dx_low != dx_high || dy_low != dy_high) &&
				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
			{
				set = true;
				float &zReg = registered.at<float>(dy_high, dx_high);
				if(zReg == 0 || z < zReg)
				{
					zReg = z;
				}
			}
		}
		if(set)
		{
			count++;
		}
	}
	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->size());

	return registered;
}

cv::Mat projectCloudToCamera(
		const cv::Size & imageSize,
		const cv::Mat & cameraMatrixK,
		const pcl::PCLPointCloud2::Ptr laserScan,  // assuming points are already in /base_link coordinate
		const rtabmap::Transform & cameraTransform)           // /base_link -> /camera_link
{
	UASSERT(!cameraTransform.isNull());
	UASSERT(!laserScan->data.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();

	pcl::MsgFieldMap field_map;
	pcl::createMapping<pcl::PointXYZ> (laserScan->fields, field_map);

	int count = 0;
	if(field_map.size() == 1)
	{
		for (uint32_t row = 0; row < (uint32_t)laserScan->height; ++row)
		{
			const uint8_t* row_data = &laserScan->data[row * laserScan->row_step];
			for (uint32_t col = 0; col < (uint32_t)laserScan->width; ++col)
			{
				const uint8_t* msg_data = row_data + col * laserScan->point_step;
				pcl::PointXYZ ptScan;
				memcpy (&ptScan, msg_data + field_map.front().serialized_offset, field_map.front().size);
				ptScan = util3d::transformPoint(ptScan, t);

				// re-project in camera frame
				float z = ptScan.z;
				bool set = false;
				if(z > 0.0f)
				{
					float invZ = 1.0f/z;
					float dx = (fx*ptScan.x)*invZ + cx;
					float dy = (fy*ptScan.y)*invZ + cy;
					int dx_low = dx;
					int dy_low = dy;
					int dx_high = dx + 0.5f;
					int dy_high = dy + 0.5f;
					if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
					{
						set = true;
						float &zReg = registered.at<float>(dy_low, dx_low);
						if(zReg == 0 || z < zReg)
						{
							zReg = z;
						}
					}
					if((dx_low != dx_high || dy_low != dy_high) &&
						uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
					{
						set = true;
						float &zReg = registered.at<float>(dy_high, dx_high);
						if(zReg == 0 || z < zReg)
						{
							zReg = z;
						}
					}
				}
				if(set)
				{
					count++;
				}
			}
		}
	}
	else
	{
		UERROR("field map pcl::pointXYZ not found!");
	}
	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.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;
				}
			}
		}
	}
}

cv::Mat filterFloor(const cv::Mat & depth, const std::vector<CameraModel> & cameraModels, float threshold, cv::Mat * depthBelow)
{
	cv::Mat output = depth.clone();
	if(depth.empty())
	{
		return output;
	}
	if(depthBelow)
	{
		*depthBelow = cv::Mat::zeros(output.size(), output.type());
	}

	UASSERT(!cameraModels.empty());
	UASSERT(cameraModels[0].isValidForReprojection());
	// Support camera model with different resolution than depth image
	float rgbToDepthFactorX = float(cameraModels[0].imageWidth()) / float(output.cols/cameraModels.size());
	float rgbToDepthFactorY = float(cameraModels[0].imageHeight()) / float(output.rows);
	int depthWidth = output.cols/cameraModels.size();
	UASSERT(depthWidth*(int)cameraModels.size() == output.cols);

	// for each camera
	for(size_t i=0; i<cameraModels.size(); ++i)
	{
		const CameraModel & cam = cameraModels[i];
		UASSERT(cam.isValidForReprojection());
		const Transform & localTransform = cam.localTransform();
		UASSERT(!localTransform.isNull());
		if(i>0)
		{
			// Make sure all models are the same resolution
			UASSERT(cam.imageWidth() == cameraModels[i-1].imageWidth());
			UASSERT(cam.imageHeight() == cameraModels[i-1].imageHeight());
		}

		float depthFx = cam.fx() / rgbToDepthFactorX;
		float depthFy = cam.fy() / rgbToDepthFactorY;
		float depthCx = cam.cx() / rgbToDepthFactorX;
		float depthCy = cam.cy() / rgbToDepthFactorY;

		cv::Mat subImage = output.colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));
		cv::Mat subImageBelow;
		if(depthBelow)
			subImageBelow = depthBelow->colRange(cv::Range(i*depthWidth, (i+1)*depthWidth));

		for(int y=0; y<subImage.rows; ++y)
		{
			if(subImage.type() == CV_16UC1)
			{
				unsigned short * ptr = (unsigned short *)subImage.row(y).ptr();
				unsigned short * ptrBelow = 0;
				if(depthBelow)
				{
					ptrBelow = (unsigned short *)subImageBelow.row(y).ptr();
				}
				for(int x=0; x<subImage.cols; ++x)
				{
					if(ptr[x] > 0)
					{
						float d = float(ptr[x])/1000.0f;
						cv::Point3f pt;
						pt.x = (x - depthCx) * d / depthFx;
						pt.y = (y - depthCy) * d / depthFy;
						pt.z = d;
						pt = util3d::transformPoint(pt, localTransform);
						if(pt.z < threshold)
						{
							if(ptrBelow)
							{
								ptrBelow[x] = ptr[x];
							}
							ptr[x] = 0;
						}
					}
				}
			}
			else // CV_32FC1
			{
				float * ptr = (float *)subImage.row(y).ptr();
				float * ptrBelow = 0;
				if(depthBelow)
				{
					ptrBelow = (float *)subImageBelow.row(y).ptr();
				}
				for(int x=0; x<subImage.cols; ++x)
				{
					if(ptr[x] > 0.0f)
					{
						float & d = ptr[x];
						cv::Point3f pt;
						pt.x = (x - depthCx) * d / depthFx;
						pt.y = (y - depthCy) * d / depthFy;
						pt.z = d;
						pt = util3d::transformPoint(pt, localTransform);
						if(pt.z < threshold)
						{
							if(ptrBelow)
							{
								ptrBelow[x] = ptr[x];
							}
							d = 0;
						}
					}
				}
			}
		}
	}
	return output;
}

class ProjectionInfo {
public:
	ProjectionInfo():
		nodeID(-1),
		cameraIndex(-1),
		distance(-1)
	{}
	int nodeID;
	int cameraIndex;
	pcl::PointXY uv;
	float distance;
};

class RegisteredPoints {
	public:
		class Point {
			public:
				Point(float distance_, int index_) : distance(distance_), index(index_) {}
				float distance;
				int index;
		};
		float minDistance;
		std::vector<Point> points;
};

/**
 * For each point, return pixel of the best camera (NodeID->CameraIndex)
 * looking at it based on the policy and parameters
 */
template<class PointT>
std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamerasImpl (
		const typename pcl::PointCloud<PointT> & cloud,
		const std::map<int, Transform> & cameraPoses,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		float maxDistance,
		float maxAngle,
		float maxDepthError,
		const std::vector<float> & roiRatios,
		const cv::Mat & projMask,
		bool distanceToCamPolicy,
		const ProgressState * state)
{
	UINFO("cloud=%d points", (int)cloud.size());
	UINFO("cameraPoses=%d", (int)cameraPoses.size());
	UINFO("cameraModels=%d", (int)cameraModels.size());
	UINFO("maxDistance=%f", maxDistance);
	UINFO("maxAngle=%f", maxAngle);
	UINFO("maxDepthError=%f", maxDepthError);
	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
	UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]).c_str():"");
	UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;

	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
		return pointToPixel;

	std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
	UINFO("%s", msg.c_str());
	if(state && !state->callback(msg))
	{
		//cancelled!
		UWARN("Projecting to cameras cancelled!");
		return pointToPixel;
	}

	std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
	int cameraProcessed = 0;
	bool wrongMaskFormatWarned = false;
	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
	{
		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
		if(iter!=cameraModels.end() && !iter->second.empty())
		{
			cv::Mat validProjMask;
			if(!projMask.empty())
			{
				if(projMask.type() != CV_8UC1)
				{
					if(!wrongMaskFormatWarned)
						UERROR("Wrong camera projection mask type %d, should be CV_8UC1", projMask.type());
					wrongMaskFormatWarned = true;
				}
				else if(projMask.cols == iter->second[0].imageWidth() * (int)iter->second.size() &&
				        projMask.rows == iter->second[0].imageHeight())
				{
					validProjMask = projMask;
				}
				else
				{
					UWARN("Camera projection mask (%dx%d) is not valid for current "
						   "camera model(s) (count=%ld, image size=%dx%d). It will be "
						   "ignored for node %d",
						   projMask.cols, projMask.rows,
						   iter->second.size(),
						   iter->second[0].imageWidth(),
						   iter->second[0].imageHeight(),
						   pter->first);
				}
			}

			for(size_t camIndex=0; camIndex<iter->second.size(); ++camIndex)
			{
				Transform cameraTransform = (pter->second * iter->second[camIndex].localTransform());
				UASSERT(!cameraTransform.isNull());
				cv::Mat cameraMatrixK = iter->second[camIndex].K();
				UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
				const cv::Size & imageSize = iter->second[camIndex].imageSize();

				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);

				// [rows][cols][depth, indexPt]
				std::vector<std::vector<RegisteredPoints> > registered(
					imageSize.height, std::vector<RegisteredPoints>(imageSize.width));
				Transform t = cameraTransform.inverse();

				cv::Rect roi(0,0,imageSize.width, imageSize.height);
				if(roiRatios.size()==4)
				{
					roi = util2d::computeRoi(imageSize, roiRatios);
				}

				int count = 0;
				for(size_t i=0; i<cloud.size(); ++i)
				{
					// Get 3D from laser scan
					PointT ptScan = cloud.at(i);
					ptScan = util3d::transformPoint(ptScan, t);

					// re-project in camera frame
					float z = ptScan.z;
					bool set = false;
					if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
					{
						float invZ = 1.0f/z;
						float u = (fx*ptScan.x)*invZ + cx;
						float v = (fy*ptScan.y)*invZ + cy;
						int x = u + 0.5f;
						int y = v + 0.5f;

						if(uIsInBounds(x, roi.x, roi.x+roi.width) && uIsInBounds(y, roi.y, roi.y+roi.height) &&
						   (validProjMask.empty() || validProjMask.at<unsigned char>(y, imageSize.width*camIndex+x) > 0)) {
							RegisteredPoints &zReg = registered[y][x];
							if(zReg.points.empty()) {
								zReg.minDistance = z;
								zReg.points.push_back(RegisteredPoints::Point(z, i));
								set = true;
							}
							else if(z < zReg.minDistance) {
								zReg.minDistance = z;
								if(maxDepthError<=0.0f) {
									// keeping only closest point, just update it
									zReg.points[0].distance = z;
									zReg.points[0].index = i;
								}
								else {
									// update the points attached to same pixel based on new closest distance
									std::vector<RegisteredPoints::Point> reOrderedPts;
									reOrderedPts.push_back(RegisteredPoints::Point(z, i));
									for(size_t p=0; p<zReg.points.size(); ++p) {
										if(zReg.points[p].distance - z < maxDepthError) {
											reOrderedPts.push_back(zReg.points[p]);
										}
									}
									zReg.points = reOrderedPts;
								}
								set = true;
							}
							else if(maxDepthError>=0.0f && z - zReg.minDistance < maxDepthError) {
								// The point is closer than current closest one to camera, 
								// but still under max depth difference, just append
								zReg.points.push_back(RegisteredPoints::Point(z, i));
								set = true;
							}
						}
					}
					if(set)
					{
						count++;
					}
				}
				if(count == 0)
				{
					registered.clear();
					UINFO("No points projected in camera %d/%d", pter->first, (int)camIndex);
				}
				else
				{
					UDEBUG("%d points projected in camera %d/%d", count, pter->first, (int)camIndex);
				}
				for(int u=0; u<imageSize.width; ++u)
				{
					for(int v=0; v<imageSize.height; ++v)
					{
						RegisteredPoints &zReg = registered[v][u];
						if(!zReg.points.empty())
						{
							ProjectionInfo info;
							info.nodeID = pter->first;
							info.cameraIndex = camIndex;
							info.uv.x = float(u)/float(imageSize.width);
							info.uv.y = float(v)/float(imageSize.height);
							const Transform & cam = cameraPoses.at(info.nodeID);
							for(size_t p=0; p<zReg.points.size(); ++p)
							{
								int ptIdx = zReg.points[p].index;
								const PointT & pt = cloud.at(ptIdx);
								Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
								Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
								float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
								float distanceToCam = zReg.points[p].distance;
								if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
									(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
								{
									float vx = info.uv.x-0.5f;
									float vy = info.uv.y-0.5f;

									float distanceToCenter = vx*vx+vy*vy;
									float distance = distanceToCenter;
									if(distanceToCamPolicy)
									{
										distance = distanceToCam;
									}

									info.distance = distance;

									if(invertedIndex[ptIdx].distance != -1.0f)
									{
										if(distance <= invertedIndex[ptIdx].distance)
										{
											invertedIndex[ptIdx] = info;
										}
									}
									else
									{
										invertedIndex[ptIdx] = info;
									}
								}
							}
						}
					}
				}
			}
		}

		msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
		UINFO("%s", msg.c_str());
		if(state && !state->callback(msg))
		{
			//cancelled!
			UWARN("Projecting to cameras cancelled!");
			return pointToPixel;
		}
		++cameraProcessed;
	}

	msg = uFormat("Select best camera for %d points...", (int)cloud.size());
	UINFO("%s", msg.c_str());
	if(state && !state->callback(msg))
	{
		//cancelled!
		UWARN("Projecting to cameras cancelled!");
		return pointToPixel;
	}

	pointToPixel.resize(invertedIndex.size());
	int colorized = 0;

	// For each point
	for(size_t i=0; i<invertedIndex.size(); ++i)
	{
		int nodeID = -1;
		int cameraIndex = -1;
		pcl::PointXY uv_coords;
		if(invertedIndex[i].distance > -1.0f)
		{
			nodeID = invertedIndex[i].nodeID;
			cameraIndex = invertedIndex[i].cameraIndex;
			uv_coords = invertedIndex[i].uv;
		}

		if(nodeID>-1 && cameraIndex> -1)
		{
			pointToPixel[i].first.first = nodeID;
			pointToPixel[i].first.second = cameraIndex;
			pointToPixel[i].second = uv_coords;
			++colorized;
		}
	}

	msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, (int)(colorized*100/cloud.size()));
	UINFO("%s", msg.c_str());
	if(state)
	{
		state->callback(msg);
	}

	return pointToPixel;
}

std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
		const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
		const std::map<int, Transform> & cameraPoses,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		float maxDistance,
		float maxAngle,
		float maxDepthError,
		const std::vector<float> & roiRatios,
		const cv::Mat & projMask,
		bool distanceToCamPolicy,
		const ProgressState * state)
{
	return projectCloudToCamerasImpl(cloud,
			cameraPoses,
			cameraModels,
			maxDistance,
			maxAngle,
			maxDepthError,
			roiRatios,
			projMask,
			distanceToCamPolicy,
			state);
}

std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
		const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud,
		const std::map<int, Transform> & cameraPoses,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		float maxDistance,
		float maxAngle,
		float maxDepthError,
		const std::vector<float> & roiRatios,
		const cv::Mat & projMask,
		bool distanceToCamPolicy,
		const ProgressState * state)
{
	return projectCloudToCamerasImpl(cloud,
			cameraPoses,
			cameraModels,
			maxDistance,
			maxAngle,
			maxDepthError,
			roiRatios,
			projMask,
			distanceToCamPolicy,
			state);
}

bool isFinite(const cv::Point3f & pt)
{
	return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
}

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>);
	for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
	{
		*cloud += *(*iter);
	}
	return cloud;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
{
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
	{
		*cloud+=*(*iter);
	}
	return cloud;
}

pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
{
	//compute total size
	unsigned int totalSize = 0;
	for(unsigned int i=0; i<indices.size(); ++i)
	{
		totalSize += (unsigned int)indices[i]->size();
	}
	pcl::IndicesPtr ind(new std::vector<int>(totalSize));
	unsigned int io = 0;
	for(unsigned int i=0; i<indices.size(); ++i)
	{
		for(unsigned int j=0; j<indices[i]->size(); ++j)
		{
			ind->at(io++) = indices[i]->at(j);
		}
	}
	return ind;
}

pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
{
	pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
	ind->resize(ind->size()+indicesB->size());
	unsigned int oi = (unsigned int)indicesA->size();
	for(unsigned int i=0; i<indicesB->size(); ++i)
	{
		ind->at(oi++) = indicesB->at(i);
	}
	return ind;
}

void savePCDWords(
		const std::string & fileName,
		const std::multimap<int, pcl::PointXYZ> & words,
		const Transform & transform)
{
	if(words.size())
	{
		pcl::PointCloud<pcl::PointXYZ> cloud;
		cloud.resize(words.size());
		int i=0;
		for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
		{
			cloud[i++] = transformPoint(iter->second, transform);
		}
		pcl::io::savePCDFile(fileName, cloud);
	}
}

void savePCDWords(
		const std::string & fileName,
		const std::multimap<int, cv::Point3f> & words,
		const Transform & transform)
{
	if(words.size())
	{
		pcl::PointCloud<pcl::PointXYZ> cloud;
		cloud.resize(words.size());
		int i=0;
		for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
		{
			cv::Point3f pt = transformPoint(iter->second, transform);
			cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
		}
		pcl::io::savePCDFile(fileName, cloud);
	}
}

cv::Mat loadBINScan(const std::string & fileName)
{
	cv::Mat output;
	long bytes = UFile::length(fileName);
	if(bytes)
	{
		int dim = 4;
		UASSERT(bytes % sizeof(float) == 0);
		size_t num = bytes/sizeof(float);
		UASSERT(num % dim == 0);
		output = cv::Mat(1, num/dim, CV_32FC(dim));

		// load point cloud
		FILE *stream;
		stream = fopen (fileName.c_str(),"rb");
		size_t actualReadNum = fread(output.data,sizeof(float),num,stream);
		UASSERT(num == actualReadNum);
		fclose(stream);
	}

	return output;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName)
{
	return laserScanToPointCloud(loadScan(fileName));
}

pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
	return loadBINCloud(fileName);
}

LaserScan loadScan(const std::string & path)
{
	std::string fileName = UFile::getName(path);
	if(UFile::getExtension(fileName).compare("bin") == 0)
	{
		return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
	}
	else
	{
		pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);

		if(UFile::getExtension(fileName).compare("pcd") == 0)
		{
			pcl::io::loadPCDFile(path, *cloud);
		}
		else // PLY
		{
			pcl::io::loadPLYFile(path, *cloud);
		}
		if(cloud->height > 1)
		{
			cloud->is_dense = false;
		}

		bool is2D = false;
		if(!cloud->data.empty())
		{
			// If all z values are zeros, we assume it is a 2D scan
			int zOffset = -1;
			for(unsigned int i=0; i<cloud->fields.size(); ++i)
			{
				if(cloud->fields[i].name.compare("z") == 0)
				{
					zOffset = cloud->fields[i].offset;
					break;
				}
			}
			if(zOffset>=0)
			{
				is2D = true;
				for (uint32_t row = 0; row < (uint32_t)cloud->height && is2D; ++row)
				{
					const uint8_t* row_data = &cloud->data[row * cloud->row_step];
					for (uint32_t col = 0; col < (uint32_t)cloud->width && is2D; ++col)
					{
						const uint8_t* msg_data = row_data + col * cloud->point_step;
						float z = *(float*)(msg_data + zOffset);
						is2D = z == 0.0f;
					}
				}
			}
		}

		return laserScanFromPointCloud(*cloud, true, is2D);
	}
	return LaserScan();
}

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)
	{
		cloud = util3d::loadBINCloud(path); // Assume KITTI velodyne format
	}
	else if(UFile::getExtension(fileName).compare("pcd") == 0)
	{
		pcl::io::loadPCDFile(path, *cloud);
	}
	else
	{
		pcl::io::loadPLYFile(path, *cloud);
	}
	int previousSize = (int)cloud->size();
	if(downsampleStep > 1 && cloud->size())
	{
		cloud = util3d::downsample(cloud, downsampleStep);
		UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size());
	}
	previousSize = (int)cloud->size();
	if(voxelSize > 0.0f && cloud->size())
	{
		cloud = util3d::voxelize(cloud, voxelSize);
		UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
	}
	if(transform.isIdentity())
	{
		return cloud;
	}
	return util3d::transformPointCloud(cloud, transform);
}

LaserScan deskew(
		const LaserScan & input,
		double inputStamp,
		const rtabmap::Transform & velocity)
{
	if(velocity.isNull())
	{
		UERROR("velocity should be valid!");
		return LaserScan();
	}

	if(!input.hasTime())
	{
		UERROR("input scan doesn't have a \"time\" channel! Supported formats: \"%s\", \"%s\".",
			LaserScan::formatName(LaserScan::kXYZIT).c_str(),
			LaserScan::formatName(LaserScan::kXYZIRT).c_str());
		return LaserScan();
	}

	if(input.empty())
	{
		UERROR("input scan is empty!");
		return LaserScan();
	}

	int offsetTime = input.getTimeOffset();

	// Get latest timestamp
	double firstStamp;
	double lastStamp;
	firstStamp = inputStamp + input.data().ptr<float>(0, 0)[offsetTime];
	lastStamp = inputStamp + input.data().ptr<float>(0, input.size()-1)[offsetTime];

	if(lastStamp <= firstStamp)
	{
		UERROR("First and last stamps in the scan are the same!");
		return LaserScan();
	}

	rtabmap::Transform firstPose;
	rtabmap::Transform lastPose;

	float vx,vy,vz, vroll,vpitch,vyaw;
	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);

	//  1- The pose of base frame in odom frame at first stamp
	//  2- The pose of base frame in odom frame at last stamp
	double dt1 = firstStamp - inputStamp;
	double dt2 = lastStamp - inputStamp;

	firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
	lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);

	if(firstPose.isNull())
	{
		UERROR("Could not get transform between stamps %f and %f!",
				firstStamp,
				inputStamp);
		return LaserScan();
	}
	if(lastPose.isNull())
	{
		UERROR("Could not get transform between stamps %f and %f!",
				lastStamp,
				inputStamp);
		return LaserScan();
	}

	double stamp;
	UTimer processingTime;
	double scanTime = lastStamp - firstStamp;
	// Preserve ring when input carries it (kXYZIRT): the geometric channel is
	// still meaningful after deskewing. Per-point time is zeroed because all
	// points share the same pose after correction.
	const bool preserveRing = input.hasRing();
	const int offsetRing = input.getRingOffset();
	const LaserScan::Format outputFormat = preserveRing ? LaserScan::kXYZIRT : LaserScan::kXYZI;
	const int outputChannels = preserveRing ? 6 : 4;
	cv::Mat output(1, input.size(), CV_32FC(outputChannels));
	int offsetIntensity = input.getIntensityOffset();
	bool isLocalTransformIdentity = input.localTransform().isIdentity();
	Transform localTransformInv = input.localTransform().inverse();

	bool timeOnColumns = input.data().cols > input.data().rows;
	int oi = 0;
	if(timeOnColumns)
	{
		// t1     t2    ...
		// ring1  ring1 ...
		// ring2  ring2 ...
		// ring3  ring4 ...
		// ring4  ring3 ...
		for(int u=0; u<input.data().cols; ++u)
		{
			const float * inputPtr = input.data().ptr<float>(0, u);
			stamp = inputStamp + inputPtr[offsetTime];
			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);

			for(int v=0; v<input.data().rows; ++v)
			{
				inputPtr = input.data().ptr<float>(v, u);
				pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
				if(pcl::isFinite(pt))
				{
					if(!isLocalTransformIdentity)
					{
						pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
					}
					pt = rtabmap::util3d::transformPoint(pt, transform);
					if(!isLocalTransformIdentity)
					{
						pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
					}
					float * dataPtr = output.ptr<float>(0, oi++);
					dataPtr[0] = pt.x;
					dataPtr[1] = pt.y;
					dataPtr[2] = pt.z;
					dataPtr[3] = inputPtr[offsetIntensity];
					if(preserveRing)
					{
						dataPtr[4] = inputPtr[offsetRing];
						dataPtr[5] = 0.0f;
					}
				}
			}
		}
	}
	else // time on rows
	{
		// t1     ring1 ring2 ring3 ring4
		// t2     ring1 ring2 ring3 ring4
		// t3     ring1 ring2 ring3 ring4
		// t4     ring1 ring2 ring3 ring4
		// ...    ...   ...   ...   ...
		for(int v=0; v<input.data().rows; ++v)
		{
			const float * inputPtr = input.data().ptr<float>(v, 0);
			stamp = inputStamp + inputPtr[offsetTime];
			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);

			for(int u=0; u<input.data().cols; ++u)
			{
				inputPtr = input.data().ptr<float>(v, u);
				pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
				if(pcl::isFinite(pt))
				{
					if(!isLocalTransformIdentity)
					{
						pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
					}
					pt = rtabmap::util3d::transformPoint(pt, transform);
					if(!isLocalTransformIdentity)
					{
						pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
					}
					float * dataPtr = output.ptr<float>(0, oi++);
					dataPtr[0] = pt.x;
					dataPtr[1] = pt.y;
					dataPtr[2] = pt.z;
					dataPtr[3] = inputPtr[offsetIntensity];
					if(preserveRing)
					{
						dataPtr[4] = inputPtr[offsetRing];
						dataPtr[5] = 0.0f;
					}
				}
			}
		}
	}
	output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
	return LaserScan(output, input.maxPoints(), input.rangeMax(), outputFormat, input.localTransform());
}


}

}