/*
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_surface.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UTimer.h"
#include <opencv2/core/core_c.h>
#include <opencv2/imgproc/types_c.h>
#include <pcl/search/kdtree.h>
#include <pcl/surface/gp3.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/surface/mls.h>
#include <pcl18/surface/texture_mapping.h>
#include <pcl/features/integral_image_normal.h>

#ifdef RTABMAP_ALICE_VISION
#include <aliceVision/sfmData/SfMData.hpp>
#include <aliceVision/sfmDataIO/sfmDataIO.hpp>
#include <aliceVision/mesh/Mesh.hpp>
#include <aliceVision/mesh/Texturing.hpp>
#include <aliceVision/camera/Pinhole.hpp>
#include <boost/algorithm/string.hpp>
using namespace aliceVision;
#endif

#ifndef DISABLE_VTK
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#endif

#if PCL_VERSION_COMPARE(>, 1, 11, 1)
#include <pcl/types.h>
#endif
#if PCL_VERSION_COMPARE(<, 1, 8, 0)
#include "pcl18/surface/organized_fast_mesh.h"
#else
#include <pcl/surface/organized_fast_mesh.h>
#include <pcl/surface/impl/marching_cubes.hpp>
#include <pcl/surface/impl/organized_fast_mesh.hpp>
#include <pcl/impl/instantiate.hpp>
#include <pcl/point_types.h>

// Instantiations of specific point types
PCL_INSTANTIATE(OrganizedFastMesh, (pcl::PointXYZRGBNormal))

#include <pcl/features/impl/normal_3d_omp.hpp>
#if PCL_VERSION_COMPARE(<=, 1, 8, 0)
#ifdef PCL_ONLY_CORE_POINT_TYPES
PCL_INSTANTIATE_PRODUCT(NormalEstimationOMP, ((pcl::PointXYZRGB))((pcl::Normal)))
#endif
#endif
#endif

namespace rtabmap
{

namespace util3d
{

void createPolygonIndexes(
		const std::vector<pcl::Vertices> & polygons,
		int cloudSize,
		std::vector<std::set<int> > & neighbors,
		std::vector<std::set<int> > & vertexToPolygons)
{
	vertexToPolygons = std::vector<std::set<int> >(cloudSize);
	neighbors = std::vector<std::set<int> >(polygons.size());

	for(unsigned int i=0; i<polygons.size(); ++i)
	{
		std::set<int> vertices(polygons[i].vertices.begin(), polygons[i].vertices.end());

		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
		{
			int v = polygons[i].vertices.at(j);
			for(std::set<int>::iterator iter=vertexToPolygons[v].begin(); iter!=vertexToPolygons[v].end(); ++iter)
			{
				int numSharedVertices = 0;
				for(unsigned int k=0; k<polygons.at(*iter).vertices.size() && numSharedVertices<2; ++k)
				{
					if(vertices.find(polygons.at(*iter).vertices.at(k)) != vertices.end())
					{
						++numSharedVertices;
					}
				}
				if(numSharedVertices >= 2)
				{
					neighbors[*iter].insert(i);
					neighbors[i].insert(*iter);
				}
			}
			vertexToPolygons[v].insert(i);
		}
	}
}

std::list<std::list<int> > clusterPolygons(
		const std::vector<std::set<int> > & neighborPolygons,
		int minClusterSize)
{
	std::set<int> polygonsChecked;

	std::list<std::list<int> > clusters;

	for(unsigned int i=0; i<neighborPolygons.size(); ++i)
	{
		if(polygonsChecked.find(i) == polygonsChecked.end())
		{
			std::list<int> currentCluster;
			currentCluster.push_back(i);
			polygonsChecked.insert(i);

			for(std::list<int>::iterator iter=currentCluster.begin(); iter!=currentCluster.end(); ++iter)
			{
				// get neighbor polygons
				std::set<int> neighbors = neighborPolygons[*iter];
				for(std::set<int>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
				{
					if(polygonsChecked.insert(*jter).second)
					{
						currentCluster.push_back(*jter);
					}
				}
			}
			if((int)currentCluster.size() > minClusterSize)
			{
				clusters.push_back(currentCluster);
			}
		}
	}
	return clusters;
}

std::vector<pcl::Vertices> organizedFastMesh(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		double angleTolerance,
		bool quad,
		int trianglePixelSize,
		const Eigen::Vector3f & viewpoint)
{
	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
	UASSERT(cloud->is_dense == false);
	UASSERT(cloud->width > 1 && cloud->height > 1);

	pcl::OrganizedFastMesh<pcl::PointXYZ> ofm;
	ofm.setTrianglePixelSize (trianglePixelSize);
	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZ>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZ>::TRIANGLE_RIGHT_CUT);
	ofm.setInputCloud (cloud);
	ofm.setAngleTolerance(angleTolerance);
	ofm.setViewpoint(viewpoint);

	std::vector<pcl::Vertices> vertices;
	ofm.reconstruct (vertices);

	if(quad)
	{
		//flip all polygons (right handed)
		std::vector<pcl::Vertices> output(vertices.size());
		for(unsigned int i=0; i<vertices.size(); ++i)
		{
			output[i].vertices.resize(4);
			output[i].vertices[0] = vertices[i].vertices[0];
			output[i].vertices[3] = vertices[i].vertices[1];
			output[i].vertices[2] = vertices[i].vertices[2];
			output[i].vertices[1] = vertices[i].vertices[3];
		}
		return output;
	}

	return vertices;
}
std::vector<pcl::Vertices> organizedFastMesh(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		double angleTolerance,
		bool quad,
		int trianglePixelSize,
		const Eigen::Vector3f & viewpoint)
{
	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
	UASSERT(cloud->is_dense == false);
	UASSERT(cloud->width > 1 && cloud->height > 1);

	pcl::OrganizedFastMesh<pcl::PointXYZRGB> ofm;
	ofm.setTrianglePixelSize (trianglePixelSize);
	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
	ofm.setInputCloud (cloud);
	ofm.setAngleTolerance(angleTolerance);
	ofm.setViewpoint(viewpoint);

	std::vector<pcl::Vertices> vertices;
	ofm.reconstruct (vertices);

	if(quad)
	{
		//flip all polygons (right handed)
		std::vector<pcl::Vertices> output(vertices.size());
		for(unsigned int i=0; i<vertices.size(); ++i)
		{
			output[i].vertices.resize(4);
			output[i].vertices[0] = vertices[i].vertices[0];
			output[i].vertices[3] = vertices[i].vertices[1];
			output[i].vertices[2] = vertices[i].vertices[2];
			output[i].vertices[1] = vertices[i].vertices[3];
		}
		return output;
	}

	return vertices;
}
std::vector<pcl::Vertices> organizedFastMesh(
		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
		double angleTolerance,
		bool quad,
		int trianglePixelSize,
		const Eigen::Vector3f & viewpoint)
{
	UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
	UASSERT(cloud->is_dense == false);
	UASSERT(cloud->width > 1 && cloud->height > 1);

	pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal> ofm;
	ofm.setTrianglePixelSize (trianglePixelSize);
	ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
	ofm.setInputCloud (cloud);
	ofm.setAngleTolerance(angleTolerance);
	ofm.setViewpoint(viewpoint);

	std::vector<pcl::Vertices> vertices;
	ofm.reconstruct (vertices);

	if(quad)
	{
		//flip all polygons (right handed)
		std::vector<pcl::Vertices> output(vertices.size());
		for(unsigned int i=0; i<vertices.size(); ++i)
		{
			output[i].vertices.resize(4);
			output[i].vertices[0] = vertices[i].vertices[0];
			output[i].vertices[3] = vertices[i].vertices[1];
			output[i].vertices[2] = vertices[i].vertices[2];
			output[i].vertices[1] = vertices[i].vertices[3];
		}
		return output;
	}

	return vertices;
}

void appendMesh(
		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudA,
		std::vector<pcl::Vertices> & polygonsA,
		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloudB,
		const std::vector<pcl::Vertices> & polygonsB)
{
	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());

	int sizeA = (int)cloudA.size();
	cloudA += cloudB;

	int sizePolygonsA = (int)polygonsA.size();
	polygonsA.resize(sizePolygonsA+polygonsB.size());

	for(unsigned int i=0; i<polygonsB.size(); ++i)
	{
		pcl::Vertices vertices = polygonsB[i];
		for(unsigned int j=0; j<vertices.vertices.size(); ++j)
		{
			vertices.vertices[j] += sizeA;
		}
		polygonsA[i+sizePolygonsA] = vertices;
	}
}

void appendMesh(
		pcl::PointCloud<pcl::PointXYZRGB> & cloudA,
		std::vector<pcl::Vertices> & polygonsA,
		const pcl::PointCloud<pcl::PointXYZRGB> & cloudB,
		const std::vector<pcl::Vertices> & polygonsB)
{
	UDEBUG("cloudA=%d polygonsA=%d cloudB=%d polygonsB=%d", (int)cloudA.size(), (int)polygonsA.size(), (int)cloudB.size(), (int)polygonsB.size());
	UASSERT(!cloudA.isOrganized() && !cloudB.isOrganized());

	int sizeA = (int)cloudA.size();
	cloudA += cloudB;

	int sizePolygonsA = (int)polygonsA.size();
	polygonsA.resize(sizePolygonsA+polygonsB.size());

	for(unsigned int i=0; i<polygonsB.size(); ++i)
	{
		pcl::Vertices vertices = polygonsB[i];
		for(unsigned int j=0; j<vertices.vertices.size(); ++j)
		{
			vertices.vertices[j] += sizeA;
		}
		polygonsA[i+sizePolygonsA] = vertices;
	}
}

std::vector<int> filterNotUsedVerticesFromMesh(
		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
		const std::vector<pcl::Vertices> & polygons,
		pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
		std::vector<pcl::Vertices> & outputPolygons)
{
	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
	std::map<int, int> addedVertices; //<oldIndex, newIndex>
	std::vector<int> output; //<oldIndex>
	output.resize(cloud.size());
	outputCloud.resize(cloud.size());
	outputCloud.is_dense = true;
	outputPolygons.resize(polygons.size());
	int oi = 0;
	for(unsigned int i=0; i<polygons.size(); ++i)
	{
		pcl::Vertices & v = outputPolygons[i];
		v.vertices.resize(polygons[i].vertices.size());
		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
		{
			std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
			if(iter == addedVertices.end())
			{
				outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
				addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
				output[oi] = polygons[i].vertices[j];
				v.vertices[j] = oi++;
			}
			else
			{
				v.vertices[j] = iter->second;
			}
		}
	}
	outputCloud.resize(oi);
	output.resize(oi);

	return output;
}

std::vector<int> filterNotUsedVerticesFromMesh(
		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
		const std::vector<pcl::Vertices> & polygons,
		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
		std::vector<pcl::Vertices> & outputPolygons)
{
	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
	std::map<int, int> addedVertices; //<oldIndex, newIndex>
	std::vector<int> output; //<oldIndex>
	output.resize(cloud.size());
	outputCloud.resize(cloud.size());
	outputCloud.is_dense = true;
	outputPolygons.resize(polygons.size());
	int oi = 0;
	for(unsigned int i=0; i<polygons.size(); ++i)
	{
		pcl::Vertices & v = outputPolygons[i];
		v.vertices.resize(polygons[i].vertices.size());
		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
		{
			std::map<int, int>::iterator iter = addedVertices.find(polygons[i].vertices[j]);
			if(iter == addedVertices.end())
			{
				outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
				addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
				output[oi] = polygons[i].vertices[j];
				v.vertices[j] = oi++;
			}
			else
			{
				v.vertices[j] = iter->second;
			}
		}
	}
	outputCloud.resize(oi);
	output.resize(oi);

	return output;
}

std::vector<int> filterNaNPointsFromMesh(
		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
		const std::vector<pcl::Vertices> & polygons,
		pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
		std::vector<pcl::Vertices> & outputPolygons)
{
	UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
	std::map<int, int> addedVertices; //<oldIndex, newIndex>
	std::vector<int> output; //<oldIndex>
	output.resize(cloud.size());
	outputCloud.resize(cloud.size());
	outputCloud.is_dense = true;
	std::vector<int> organizedToDense(cloud.size(), -1);

	int oi = 0;
	for(unsigned int i=0; i<cloud.size(); ++i)
	{
		if(pcl::isFinite(cloud.at(i)))
		{
			outputCloud.at(oi) = cloud.at(i);
			output[oi] = i;
			organizedToDense[i] = oi;
			++oi;
 		}
	}
	outputCloud.resize(oi);
	output.resize(oi);

	// remap polygons to dense cloud
	outputPolygons = polygons;
	for(unsigned int i=0; i<outputPolygons.size(); ++i)
	{
		pcl::Vertices & v = outputPolygons[i];
		for(unsigned int j=0; j<v.vertices.size(); ++j)
		{
			UASSERT(organizedToDense[v.vertices[j]] >= 0);
			v.vertices[j] = organizedToDense[v.vertices[j]];
		}
	}

	return output;
}

std::vector<pcl::Vertices> filterCloseVerticesFromMesh(
		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud,
		const std::vector<pcl::Vertices> & polygons,
		float radius,
		float angle, // FIXME angle not used
		bool keepLatestInRadius)
{
	UDEBUG("size=%d polygons=%d radius=%f angle=%f keepLatest=%d",
			(int)cloud->size(), (int)polygons.size(), radius, angle, keepLatestInRadius?1:0);
	std::vector<pcl::Vertices> outputPolygons;
	pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>::Ptr kdtree(new pcl::KdTreeFLANN<pcl::PointXYZRGBNormal>);
	kdtree->setInputCloud(cloud);

	std::map<int, int> verticesDone;
	outputPolygons = polygons;
	for(unsigned int i=0; i<outputPolygons.size(); ++i)
	{
		pcl::Vertices & polygon = outputPolygons[i];
		for(unsigned int j=0; j<polygon.vertices.size(); ++j)
		{
			std::map<int, int>::iterator iter = verticesDone.find(polygon.vertices[j]);
			if(iter != verticesDone.end())
			{
				polygon.vertices[j] = iter->second;
			}
			else
			{
				std::vector<int> kIndices;
				std::vector<float> kDistances;
				kdtree->radiusSearch(polygon.vertices[j], radius, kIndices, kDistances);
				if(kIndices.size())
				{
					int reference = -1;
					for(unsigned int z=0; z<kIndices.size(); ++z)
					{
						if(reference == -1)
						{
							reference = kIndices[z];
						}
						else if(keepLatestInRadius)
						{
							if(kIndices[z] < reference)
							{
								reference = kIndices[z];
							}
						}
						else
						{
							if(kIndices[z] > reference)
							{
								reference = kIndices[z];
							}
						}
					}
					if(reference >= 0)
					{
						for(unsigned int z=0; z<kIndices.size(); ++z)
						{
							verticesDone.insert(std::make_pair(kIndices[j], reference));
						}
						polygon.vertices[j] = reference;
					}
				}
				else
				{
					verticesDone.insert(std::make_pair(polygon.vertices[j], polygon.vertices[j]));
				}
			}
		}
	}
	return outputPolygons;
}

std::vector<pcl::Vertices> filterInvalidPolygons(const std::vector<pcl::Vertices> & polygons)
{
	std::vector<pcl::Vertices> output(polygons.size());
	int oi=0;
	for(unsigned int i=0; i<polygons.size(); ++i)
	{
		bool valid = true;
		for(unsigned int j=0; j<polygons[i].vertices.size(); ++j)
		{
			if(polygons[i].vertices[j] == polygons[i].vertices[(j+1)%polygons[i].vertices.size()])
			{
				valid = false;
				break;
			}
		}
		if(valid)
		{
			output[oi++] = polygons[i];
		}
	}
	output.resize(oi);
	return output;
}

pcl::PolygonMesh::Ptr createMesh(
		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
		float gp3SearchRadius,
		float gp3Mu,
		int gp3MaximumNearestNeighbors,
		float gp3MaximumSurfaceAngle,
		float gp3MinimumAngle,
		float gp3MaximumAngle,
		bool gp3NormalConsistency)
{
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);

	// Create search tree*
	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
	tree2->setInputCloud (cloudWithNormalsNoNaN);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
	pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);

	// Set the maximum distance between connected points (maximum edge length)
	gp3.setSearchRadius (gp3SearchRadius);

	// Set typical values for the parameters
	gp3.setMu (gp3Mu);
	gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
	gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
	gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
	gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
	gp3.setNormalConsistency(gp3NormalConsistency);
	gp3.setConsistentVertexOrdering(gp3NormalConsistency);

	// Get result
	gp3.setInputCloud (cloudWithNormalsNoNaN);
	gp3.setSearchMethod (tree2);
	gp3.reconstruct (*mesh);

	//UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
	//mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);

	return mesh;
}

pcl::texture_mapping::CameraVector createTextureCameras(
		const std::map<int, Transform> & poses,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		const std::map<int, cv::Mat> & cameraDepths,
		const std::vector<float> & roiRatios)
{
	UASSERT(roiRatios.empty() || roiRatios.size() == 4);
	pcl::texture_mapping::CameraVector cameras;

	for(std::map<int, Transform>::const_iterator poseIter=poses.begin(); poseIter!=poses.end(); ++poseIter)
	{
		std::map<int, std::vector<CameraModel> >::const_iterator modelIter=cameraModels.find(poseIter->first);

		if(modelIter!=cameraModels.end())
		{
			std::map<int, cv::Mat>::const_iterator depthIter = cameraDepths.find(poseIter->first);

			// for each sub camera
			for(unsigned int i=0; i<modelIter->second.size(); ++i)
			{
				pcl::TextureMapping<pcl::PointXYZ>::Camera cam;
				// should be in camera frame
				UASSERT(!modelIter->second[i].localTransform().isNull() && !poseIter->second.isNull());
				Transform t = poseIter->second*modelIter->second[i].localTransform();

				cam.pose = t.toEigen3f();

				if(modelIter->second[i].imageHeight() <=0 || modelIter->second[i].imageWidth() <=0)
				{
					UERROR("Should have camera models with width/height set to create texture cameras!");
					return pcl::texture_mapping::CameraVector();
				}

				UASSERT(modelIter->second[i].fx()>0 && modelIter->second[i].imageHeight()>0 && modelIter->second[i].imageWidth()>0);
				cam.focal_length_w=modelIter->second[i].fx();
				cam.focal_length_h=modelIter->second[i].fy();
				cam.center_w=modelIter->second[i].cx();
				cam.center_h=modelIter->second[i].cy();
				cam.height=modelIter->second[i].imageHeight();
				cam.width=modelIter->second[i].imageWidth();
				if(modelIter->second.size() == 1)
				{
					cam.texture_file = uFormat("%d", poseIter->first); // camera index
				}
				else
				{
					cam.texture_file = uFormat("%d_%d", poseIter->first, (int)i); // camera index, sub camera model index
				}
				if(!roiRatios.empty())
				{
					cam.roi.resize(4);
					cam.roi[0] = cam.width * roiRatios[0]; // left -> x
					cam.roi[1] = cam.height * roiRatios[2]; // top -> y
					cam.roi[2] = cam.width * (1.0 - roiRatios[1]) - cam.roi[0]; // right -> width
					cam.roi[3] = cam.height * (1.0 - roiRatios[3]) - cam.roi[1]; // bottom -> height
				}

				if(depthIter != cameraDepths.end() && !depthIter->second.empty())
				{
					UASSERT(depthIter->second.type() == CV_32FC1 || depthIter->second.type() == CV_16UC1);
					UASSERT(depthIter->second.cols % modelIter->second.size() == 0);
					int subWidth = depthIter->second.cols/(modelIter->second.size());
					cam.depth = cv::Mat(depthIter->second, cv::Range(0, depthIter->second.rows), cv::Range(subWidth*i, subWidth*(i+1)));
				}

				UDEBUG("%f", cam.focal_length);
				UDEBUG("%f", cam.height);
				UDEBUG("%f", cam.width);
				UDEBUG("cam.pose=%s", t.prettyPrint().c_str());

				cameras.push_back(cam);
			}
		}
	}
	return cameras;
}

pcl::TextureMesh::Ptr createTextureMesh(
		const pcl::PolygonMesh::Ptr & mesh,
		const std::map<int, Transform> & poses,
		const std::map<int, CameraModel> & cameraModels,
		const std::map<int, cv::Mat> & cameraDepths,
		float maxDistance,
		float maxDepthError,
		float maxAngle,
		int minClusterSize,
		const std::vector<float> & roiRatios,
		const ProgressState * state,
		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
		bool distanceToCamPolicy)
{
	std::map<int, std::vector<CameraModel> > cameraSubModels;
	for(std::map<int, CameraModel>::const_iterator iter=cameraModels.begin(); iter!=cameraModels.end(); ++iter)
	{
		std::vector<CameraModel> models;
		models.push_back(iter->second);
		cameraSubModels.insert(std::make_pair(iter->first, models));
	}

	return createTextureMesh(
			mesh,
			poses,
			cameraSubModels,
			cameraDepths,
			maxDistance,
			maxDepthError,
			maxAngle,
			minClusterSize,
			roiRatios,
			state,
			vertexToPixels,
			distanceToCamPolicy);
}

pcl::TextureMesh::Ptr createTextureMesh(
		const pcl::PolygonMesh::Ptr & mesh,
		const std::map<int, Transform> & poses,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		const std::map<int, cv::Mat> & cameraDepths,
		float maxDistance,
		float maxDepthError,
		float maxAngle,
		int minClusterSize,
		const std::vector<float> & roiRatios,
		const ProgressState * state,
		std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
		bool distanceToCamPolicy)
{
	UASSERT(mesh->polygons.size());
	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
	textureMesh->cloud = mesh->cloud;
	textureMesh->tex_polygons.push_back(mesh->polygons);

	// Original from pcl/gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp:
	// Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)

	// Create the texturemesh object that will contain our UV-mapped mesh

	// create cameras
	pcl::texture_mapping::CameraVector cameras = createTextureCameras(
			poses,
			cameraModels,
			cameraDepths,
			roiRatios);

	// Create materials for each texture (and one extra for occluded faces)
	textureMesh->tex_materials.resize (cameras.size () + 1);
	for(unsigned int i = 0 ; i <= cameras.size() ; ++i)
	{
		pcl::TexMaterial mesh_material;
		mesh_material.tex_Ka.r = 0.2f;
		mesh_material.tex_Ka.g = 0.2f;
		mesh_material.tex_Ka.b = 0.2f;

		mesh_material.tex_Kd.r = 0.8f;
		mesh_material.tex_Kd.g = 0.8f;
		mesh_material.tex_Kd.b = 0.8f;

		mesh_material.tex_Ks.r = 1.0f;
		mesh_material.tex_Ks.g = 1.0f;
		mesh_material.tex_Ks.b = 1.0f;

		mesh_material.tex_d = 1.0f;
		mesh_material.tex_Ns = 75.0f;
		mesh_material.tex_illum = 2;

		std::stringstream tex_name;
		tex_name << "material_" << i;
		tex_name >> mesh_material.tex_name;

		if(i < cameras.size ())
		{
			mesh_material.tex_file = cameras[i].texture_file;
		}
		else
		{
			mesh_material.tex_file = "occluded";
		}

		textureMesh->tex_materials[i] = mesh_material;
	}

	// Texture by projection
	pcl::TextureMapping<pcl::PointXYZ> tm; // TextureMapping object that will perform the sort
	tm.setMaxDistance(maxDistance);
	tm.setMaxAngle(maxAngle);
	tm.setMaxDepthError(maxDepthError);
	tm.setMinClusterSize(minClusterSize);
	if(tm.textureMeshwithMultipleCameras2(*textureMesh, cameras, state, vertexToPixels, distanceToCamPolicy))
	{
		// compute normals for the mesh if not already here
		bool hasNormals = false;
		bool hasColors = false;
		for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
		{
			if(textureMesh->cloud.fields[i].name.compare("normal_x") == 0)
			{
				hasNormals = true;
			}
			else if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
			{
				hasColors = true;
			}
		}
		if(!hasNormals)
		{
			// use polygons
			if(hasColors)
			{
				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
				pcl::fromPCLPointCloud2(mesh->cloud, *cloud);

				for(unsigned int i=0; i<mesh->polygons.size(); ++i)
				{
					pcl::Vertices & v = mesh->polygons[i];
					UASSERT(v.vertices.size()>2);
					Eigen::Vector3f v0(
							cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
							cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
							cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
					int last = v.vertices.size()-1;
					Eigen::Vector3f v1(
							cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
							cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
							cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
					Eigen::Vector3f normal = v0.cross(v1);
					normal.normalize();
					// flat normal (per face)
					for(unsigned int j=0; j<v.vertices.size(); ++j)
					{
						cloud->at(v.vertices[j]).normal_x = normal[0];
						cloud->at(v.vertices[j]).normal_y = normal[1];
						cloud->at(v.vertices[j]).normal_z = normal[2];
					}
				}
				pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
			}
			else
			{
				pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
				pcl::fromPCLPointCloud2(mesh->cloud, *cloud);

				for(unsigned int i=0; i<mesh->polygons.size(); ++i)
				{
					pcl::Vertices & v = mesh->polygons[i];
					UASSERT(v.vertices.size()>2);
					Eigen::Vector3f v0(
							cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
							cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
							cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
					int last = v.vertices.size()-1;
					Eigen::Vector3f v1(
							cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
							cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
							cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
					Eigen::Vector3f normal = v0.cross(v1);
					normal.normalize();
					// flat normal (per face)
					for(unsigned int j=0; j<v.vertices.size(); ++j)
					{
						cloud->at(v.vertices[j]).normal_x = normal[0];
						cloud->at(v.vertices[j]).normal_y = normal[1];
						cloud->at(v.vertices[j]).normal_z = normal[2];
					}
				}
				pcl::toPCLPointCloud2 (*cloud, textureMesh->cloud);
			}
		}
	}
	return textureMesh;
}

void cleanTextureMesh(
		pcl::TextureMesh & textureMesh,
		int minClusterSize)
{
	UDEBUG("minClusterSize=%d", minClusterSize);
	// Remove occluded polygons (polygons with no texture)
	if(textureMesh.tex_coordinates.size())
	{
		// assume last texture is the occluded texture
		textureMesh.tex_coordinates.pop_back();
		textureMesh.tex_polygons.pop_back();
		textureMesh.tex_materials.pop_back();

		if(minClusterSize!=0)
		{
			// concatenate all polygons
			unsigned int totalSize = 0;
			for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
			{
				totalSize+=textureMesh.tex_polygons[t].size();
			}
			std::vector<pcl::Vertices> allPolygons(totalSize);
			int oi=0;
			for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
			{
				for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
				{
					allPolygons[oi++] =  textureMesh.tex_polygons[t][i];
				}
			}

			// filter polygons
			std::vector<std::set<int> > neighbors;
			std::vector<std::set<int> > vertexToPolygons;
			util3d::createPolygonIndexes(allPolygons,
					(int)textureMesh.cloud.data.size()/textureMesh.cloud.point_step,
					neighbors,
					vertexToPolygons);

			std::list<std::list<int> > clusters = util3d::clusterPolygons(
					neighbors,
					minClusterSize<0?0:minClusterSize);

			std::set<int> validPolygons;
			if(minClusterSize < 0)
			{
				// only keep the biggest cluster
				std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
				unsigned int biggestClusterSize = 0;
				for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
				{
					if(iter->size() > biggestClusterSize)
					{
						biggestClusterIndex = iter;
						biggestClusterSize = iter->size();
					}
				}
				if(biggestClusterIndex != clusters.end())
				{
					for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
					{
						validPolygons.insert(*jter);
					}
				}
			}
			else
			{
				for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
				{
					for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
					{
						validPolygons.insert(*jter);
					}
				}
			}

			if(validPolygons.size() == 0)
			{
				UWARN("All %d polygons filtered after polygon cluster filtering. Cluster minimum size is %d.",totalSize, minClusterSize);
			}

			// for each texture
			unsigned int allPolygonsIndex = 0;
			for(unsigned int t=0; t<textureMesh.tex_polygons.size(); ++t)
			{
				std::vector<pcl::Vertices> filteredPolygons(textureMesh.tex_polygons[t].size());
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
				std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > filteredCoordinates(textureMesh.tex_coordinates[t].size());
#else
				std::vector<Eigen::Vector2f> filteredCoordinates(textureMesh.tex_coordinates[t].size());
#endif

				if(textureMesh.tex_polygons[t].size())
				{
					UASSERT_MSG(allPolygonsIndex < allPolygons.size(), uFormat("%d vs %d", (int)allPolygonsIndex, (int)allPolygons.size()).c_str());

					// make index polygon to coordinate
					std::vector<unsigned int> polygonToCoord(textureMesh.tex_polygons[t].size());
					unsigned int totalCoord = 0;
					for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
					{
						polygonToCoord[i] = totalCoord;
						totalCoord+=textureMesh.tex_polygons[t][i].vertices.size();
					}
					UASSERT_MSG(totalCoord == textureMesh.tex_coordinates[t].size(), uFormat("%d vs %d", totalCoord, (int)textureMesh.tex_coordinates[t].size()).c_str());

					int oi=0;
					int ci=0;
					for(unsigned int i=0; i<textureMesh.tex_polygons[t].size(); ++i)
					{
						if(validPolygons.find(allPolygonsIndex) != validPolygons.end())
						{
							filteredPolygons[oi] = textureMesh.tex_polygons[t].at(i);
							for(unsigned int j=0; j<filteredPolygons[oi].vertices.size(); ++j)
							{
								UASSERT(polygonToCoord[i] < textureMesh.tex_coordinates[t].size());
								filteredCoordinates[ci] = textureMesh.tex_coordinates[t][polygonToCoord[i]+j];
								++ci;
							}
							++oi;
						}
						++allPolygonsIndex;
					}
					filteredPolygons.resize(oi);
					filteredCoordinates.resize(ci);
					textureMesh.tex_polygons[t] = filteredPolygons;
					textureMesh.tex_coordinates[t] = filteredCoordinates;
				}
			}
		}
	}
}

pcl::TextureMesh::Ptr concatenateTextureMeshes(const std::list<pcl::TextureMesh::Ptr> & meshes)
{
	pcl::TextureMesh::Ptr output(new pcl::TextureMesh);
	std::map<std::string, int> addedMaterials; //<file, index>
	for(std::list<pcl::TextureMesh::Ptr>::const_iterator iter = meshes.begin(); iter!=meshes.end(); ++iter)
	{
		if((*iter)->cloud.point_step &&
		   (*iter)->cloud.data.size()/(*iter)->cloud.point_step &&
		   (*iter)->tex_polygons.size() &&
		   (*iter)->tex_coordinates.size())
		{
			// append point cloud
			int polygonStep = output->cloud.height * output->cloud.width;
			pcl::PCLPointCloud2 tmp;
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
			pcl::concatenate(output->cloud, iter->get()->cloud, tmp);
#else
			pcl::concatenatePointCloud(output->cloud, iter->get()->cloud, tmp);
#endif
			output->cloud = tmp;

			UASSERT((*iter)->tex_polygons.size() == (*iter)->tex_coordinates.size() &&
					(*iter)->tex_polygons.size() == (*iter)->tex_materials.size());

			int materialCount = (*iter)->tex_polygons.size();
			for(int i=0; i<materialCount; ++i)
			{
				std::map<std::string, int>::iterator jter = addedMaterials.find((*iter)->tex_materials[i].tex_file);
				int index;
				if(jter != addedMaterials.end())
				{
					index = jter->second;
				}
				else
				{
					addedMaterials.insert(std::make_pair((*iter)->tex_materials[i].tex_file, output->tex_materials.size()));
					index = output->tex_materials.size();
					output->tex_materials.push_back((*iter)->tex_materials[i]);
					output->tex_materials.back().tex_name = uFormat("material_%d", index);
					output->tex_polygons.resize(output->tex_polygons.size() + 1);
					output->tex_coordinates.resize(output->tex_coordinates.size() + 1);
				}

				// update and append polygon indices
				int oi = output->tex_polygons[index].size();
				output->tex_polygons[index].resize(output->tex_polygons[index].size() + (*iter)->tex_polygons[i].size());
				for(unsigned int j=0; j<(*iter)->tex_polygons[i].size(); ++j)
				{
					pcl::Vertices polygon = (*iter)->tex_polygons[i][j];
					for(unsigned int k=0; k<polygon.vertices.size(); ++k)
					{
						polygon.vertices[k] += polygonStep;
					}
					output->tex_polygons[index][oi+j] = polygon;
				}

				// append uv coordinates
				oi = output->tex_coordinates[index].size();
				output->tex_coordinates[index].resize(output->tex_coordinates[index].size() + (*iter)->tex_coordinates[i].size());
				for(unsigned int j=0; j<(*iter)->tex_coordinates[i].size(); ++j)
				{
					output->tex_coordinates[index][oi+j] = (*iter)->tex_coordinates[i][j];
				}
			}
		}
	}
	return output;
}

int gcd(int a, int b) {
    return b == 0 ? a : gcd(b, a % b);
}

void concatenateTextureMaterials(pcl::TextureMesh & mesh, const cv::Size & imageSize, int textureSize, int maxTextures, float & scale, std::vector<bool> * materialsKept)
{
	UASSERT(textureSize>0 && imageSize.width>0 && imageSize.height>0);
	if(maxTextures < 1)
	{
		maxTextures = 1;
	}
	int materials = 0;
	for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
	{
		if(mesh.tex_polygons.size())
		{
			++materials;
		}
	}
	if(materials)
	{
		int w = imageSize.width; // 640
		int h = imageSize.height; // 480
		int g = gcd(w,h); // 160
		int a = w/g; // 4=640/160
		int b = h/g; // 3=480/160
		UDEBUG("w=%d h=%d g=%d a=%d b=%d", w, h, g, a, b);
		int colCount = 0;
		int rowCount = 0;
		float factor = 0.1f;
		float epsilon = 0.001f;
		scale = 1.0f;
		while((colCount*rowCount)*maxTextures < materials || (factor == 0.1f || scale > 1.0f))
		{
			// first run try scale = 1 (no scaling)
			if(factor!=0.1f)
			{
				scale = float(textureSize)/float(w*b*factor);
			}
			colCount = float(textureSize)/(scale*float(w));
			rowCount = float(textureSize)/(scale*float(h));
			factor+=epsilon; // search the maximum perfect fit
		}
		int outputTextures = (materials / (colCount*rowCount)) + (materials % (colCount*rowCount) > 0?1:0);
		UDEBUG("materials=%d col=%d row=%d output textures=%d factor=%f scale=%f", materials, colCount, rowCount, outputTextures, factor-epsilon, scale);

		UASSERT(mesh.tex_coordinates.size() == mesh.tex_materials.size() && mesh.tex_polygons.size() == mesh.tex_materials.size());

		// prepare size
		std::vector<int> totalPolygons(outputTextures, 0);
		std::vector<int> totalCoordinates(outputTextures, 0);
		int count = 0;
		for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
		{
			if(mesh.tex_polygons[i].size())
			{
				int indexMaterial = count / (colCount*rowCount);
				UASSERT(indexMaterial < outputTextures);

				totalPolygons[indexMaterial]+=mesh.tex_polygons[i].size();
				totalCoordinates[indexMaterial]+=mesh.tex_coordinates[i].size();

				++count;
			}
		}

		pcl::TextureMesh outputMesh;

		int pi = 0;
		int ci = 0;
		int ti=0;
		float scaledHeight = float(int(scale*float(h)))/float(textureSize);
		float scaledWidth = float(int(scale*float(w)))/float(textureSize);
		float lowerBorderSize = 1.0f - scaledHeight*float(rowCount);
		UDEBUG("scaledWidth=%f scaledHeight=%f lowerBorderSize=%f", scaledWidth, scaledHeight, lowerBorderSize);
		if(materialsKept)
		{
			materialsKept->resize(mesh.tex_materials.size(), false);
		}
		for(unsigned int t=0; t<mesh.tex_materials.size(); ++t)
		{
			if(mesh.tex_polygons[t].size())
			{
				int indexMaterial = ti / (colCount*rowCount);
				UASSERT(indexMaterial < outputTextures);
                                if((int)outputMesh.tex_polygons.size() <= indexMaterial)
				{
					std::vector<pcl::Vertices> newPolygons(totalPolygons[indexMaterial]);
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
					std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
#else
					std::vector<Eigen::Vector2f> newCoordinates(totalCoordinates[indexMaterial]); // UV coordinates
#endif
					outputMesh.tex_polygons.push_back(newPolygons);
					outputMesh.tex_coordinates.push_back(newCoordinates);

					pi=0;
					ci=0;
				}

				int row = (ti/colCount) % rowCount;
				int col = ti%colCount;
				float offsetU = scaledWidth * float(col);
				float offsetV = scaledHeight * float((rowCount - 1) - row) + lowerBorderSize;
				// Texture coords have lower-left origin

				for(unsigned int i=0; i<mesh.tex_polygons[t].size(); ++i)
				{
					UASSERT(pi < (int)outputMesh.tex_polygons[indexMaterial].size());
					outputMesh.tex_polygons[indexMaterial][pi++] = mesh.tex_polygons[t].at(i);
				}

				for(unsigned int i=0; i<mesh.tex_coordinates[t].size(); ++i)
				{
					const Eigen::Vector2f & v = mesh.tex_coordinates[t].at(i);
					if(v[0] >= 0 && v[1] >=0)
					{
						outputMesh.tex_coordinates[indexMaterial][ci][0] = v[0]*scaledWidth + offsetU;
						outputMesh.tex_coordinates[indexMaterial][ci][1] = v[1]*scaledHeight + offsetV;
					}
					else
					{
						outputMesh.tex_coordinates[indexMaterial][ci] = v;
					}
					++ci;
				}
				++ti;
				if(materialsKept)
				{
					materialsKept->at(t) = true;
				}
			}
		}
		pcl::TexMaterial m = mesh.tex_materials.front();
		mesh.tex_materials.clear();
		for(int i=0; i<outputTextures; ++i)
		{
			m.tex_file = "texture";
			m.tex_name = "material";
			if(outputTextures > 1)
			{
				m.tex_file += uNumber2Str(i);
				m.tex_name += uNumber2Str(i);
			}

			mesh.tex_materials.push_back(m);
		}
		mesh.tex_coordinates = outputMesh.tex_coordinates;
		mesh.tex_polygons = outputMesh.tex_polygons;
	}
}

std::vector<std::vector<RTABMAP_PCL_INDEX> > convertPolygonsFromPCL(const std::vector<pcl::Vertices> & polygons)
{
	std::vector<std::vector<RTABMAP_PCL_INDEX> > polygonsOut(polygons.size());
	for(unsigned int p=0; p<polygons.size(); ++p)
	{
		polygonsOut[p] = polygons[p].vertices;
	}
	return polygonsOut;
}
std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > convertPolygonsFromPCL(const std::vector<std::vector<pcl::Vertices> > & tex_polygons)
{
	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygonsOut(tex_polygons.size());
	for(unsigned int t=0; t<tex_polygons.size(); ++t)
	{
		polygonsOut[t].resize(tex_polygons[t].size());
		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
		{
			polygonsOut[t][p] = tex_polygons[t][p].vertices;
		}
	}
	return polygonsOut;
}
std::vector<pcl::Vertices> convertPolygonsToPCL(const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
{
	std::vector<pcl::Vertices> polygonsOut(polygons.size());
	for(unsigned int p=0; p<polygons.size(); ++p)
	{
		polygonsOut[p].vertices = polygons[p];
	}
	return polygonsOut;
}
std::vector<std::vector<pcl::Vertices> > convertPolygonsToPCL(const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & tex_polygons)
{
	std::vector<std::vector<pcl::Vertices> > polygonsOut(tex_polygons.size());
	for(unsigned int t=0; t<tex_polygons.size(); ++t)
	{
		polygonsOut[t].resize(tex_polygons[t].size());
		for(unsigned int p=0; p<tex_polygons[t].size(); ++p)
		{
			polygonsOut[t][p].vertices = tex_polygons[t][p];
		}
	}
	return polygonsOut;
}

pcl::TextureMesh::Ptr assembleTextureMesh(
		const cv::Mat & cloudMat,
		const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
		const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
#else
		const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
#endif
		cv::Mat & textures,
		bool mergeTextures)
{
	pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);

	if(cloudMat.channels() <= 3)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
	}
	else if(cloudMat.channels() == 4)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
	}
	else if(cloudMat.channels() == 6)
	{
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
	}
	else if(cloudMat.channels() == 7)
	{
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
	}

	if(textureMesh->cloud.data.size() && polygons.size())
	{
		textureMesh->tex_polygons.resize(polygons.size());
		for(unsigned int t=0; t<polygons.size(); ++t)
		{
			textureMesh->tex_polygons[t].resize(polygons[t].size());
			for(unsigned int p=0; p<polygons[t].size(); ++p)
			{
				textureMesh->tex_polygons[t][p].vertices = polygons[t][p];
			}
		}

		if(!texCoords.empty() && !textures.empty())
		{
			textureMesh->tex_coordinates = texCoords;

			textureMesh->tex_materials.resize (textureMesh->tex_coordinates.size());
			for(unsigned int i = 0 ; i < textureMesh->tex_coordinates.size() ; ++i)
			{
				pcl::TexMaterial mesh_material;
				mesh_material.tex_Ka.r = 0.2f;
				mesh_material.tex_Ka.g = 0.2f;
				mesh_material.tex_Ka.b = 0.2f;

				mesh_material.tex_Kd.r = 0.8f;
				mesh_material.tex_Kd.g = 0.8f;
				mesh_material.tex_Kd.b = 0.8f;

				mesh_material.tex_Ks.r = 1.0f;
				mesh_material.tex_Ks.g = 1.0f;
				mesh_material.tex_Ks.b = 1.0f;

				mesh_material.tex_d = 1.0f;
				mesh_material.tex_Ns = 75.0f;
				mesh_material.tex_illum = 2;

				std::stringstream tex_name;
				tex_name << "material_" << i;
				tex_name >> mesh_material.tex_name;

				mesh_material.tex_file = uFormat("%d", i);

				textureMesh->tex_materials[i] = mesh_material;
			}

			if(mergeTextures && textures.cols/textures.rows > 1)
			{
				UASSERT(textures.cols % textures.rows == 0 && textures.cols/textures.rows == (int)textureMesh->tex_coordinates.size());
				std::vector<bool> materialsKept;
				float scale = 0.0f;
				cv::Size imageSize(textures.rows, textures.rows);
				int imageType = textures.type();
				rtabmap::util3d::concatenateTextureMaterials(*textureMesh, imageSize, textures.rows, 1, scale, &materialsKept);
				if(scale && textureMesh->tex_materials.size() == 1)
				{
					int cols = float(textures.rows)/(scale*imageSize.width);
					int rows = float(textures.rows)/(scale*imageSize.height);

					cv::Mat mergedTextures = cv::Mat(textures.rows, textures.rows, imageType, cv::Scalar::all(255));

					// make a blank texture
					cv::Size resizedImageSize(int(imageSize.width*scale), int(imageSize.height*scale));
					int oi=0;
					for(int i=0; i<(int)materialsKept.size(); ++i)
					{
						if(materialsKept.at(i))
						{
							int u = oi%cols * resizedImageSize.width;
							int v = ((oi/cols) % rows ) * resizedImageSize.height;
							UASSERT(u < textures.rows-resizedImageSize.width);
							UASSERT(v < textures.rows-resizedImageSize.height);

							cv::Mat resizedImage;
							cv::resize(textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)), resizedImage, resizedImageSize, 0.0f, 0.0f, cv::INTER_AREA);

							UASSERT(resizedImage.type() == mergedTextures.type());
							resizedImage.copyTo(mergedTextures(cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));

							++oi;
						}
					}
					textures = mergedTextures;
				}
			}
		}
	}
	return textureMesh;
}

pcl::PolygonMesh::Ptr assemblePolygonMesh(
		const cv::Mat & cloudMat,
		const std::vector<std::vector<RTABMAP_PCL_INDEX> > & polygons)
{
	pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);

	if(cloudMat.channels() <= 3)
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
	}
	else if(cloudMat.channels() == 4)
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
	}
	else if(cloudMat.channels() == 6)
	{
		pcl::PointCloud<pcl::PointNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudNormal(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
	}
	else if(cloudMat.channels() == 7)
	{
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudRGBNormal(LaserScan::backwardCompatibility(cloudMat));
		pcl::toPCLPointCloud2(*cloud, polygonMesh->cloud);
	}

	if(polygonMesh->cloud.data.size() && polygons.size())
	{
		polygonMesh->polygons.resize(polygons.size());
		for(unsigned int p=0; p<polygons.size(); ++p)
		{
			polygonMesh->polygons[p].vertices = polygons[p];
		}
	}
	return polygonMesh;
}

double sqr(uchar v)
{
	return double(v)*double(v);
}

cv::Mat mergeTextures(
		pcl::TextureMesh & mesh,
		const std::map<int, cv::Mat> & images,
		const std::map<int, CameraModel> & calibrations,
		const Memory * memory,
		const DBDriver * dbDriver,
		int textureSize,
		int textureCount,
		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
		bool gainCompensation,
		float gainBeta,
		bool gainRGB,
		bool blending,
		int blendingDecimation,
		int brightnessContrastRatioLow,
		int brightnessContrastRatioHigh,
		bool exposureFusion,
		const ProgressState * state,
		unsigned char blankValue,
		bool clearVertexColorUnderTexture,
		std::map<int, std::map<int, cv::Vec4d> > * gains,
		std::map<int, std::map<int, cv::Mat> > * blendingGains,
		std::pair<float, float> * contrastValues)
{
	std::map<int, std::vector<CameraModel> > calibVectors;
	for(std::map<int, CameraModel>::const_iterator iter=calibrations.begin(); iter!=calibrations.end(); ++iter)
	{
		std::vector<CameraModel> m;
		m.push_back(iter->second);
		calibVectors.insert(std::make_pair(iter->first, m));
	}
	return mergeTextures(mesh,
			images,
			calibVectors,
			memory,
			dbDriver,
			textureSize,
			textureCount,
			vertexToPixels,
			gainCompensation,
			gainBeta,
			gainRGB,
			blending,
			blendingDecimation,
			brightnessContrastRatioLow,
			brightnessContrastRatioHigh,
			exposureFusion,
			state,
			blankValue,
			clearVertexColorUnderTexture,
			gains,
			blendingGains,
			contrastValues);
}
cv::Mat mergeTextures(
		pcl::TextureMesh & mesh,
		const std::map<int, cv::Mat> & images,
		const std::map<int, std::vector<CameraModel> > & calibrations,
		const Memory * memory,
		const DBDriver * dbDriver,
		int textureSize,
		int textureCount,
		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
		bool gainCompensation,
		float gainBeta,
		bool gainRGB,
		bool blending,
		int blendingDecimation,
		int brightnessContrastRatioLow,
		int brightnessContrastRatioHigh,
		bool exposureFusion,
		const ProgressState * state,
		unsigned char blankValue,
		bool clearVertexColorUnderTexture,
		std::map<int, std::map<int, cv::Vec4d> > * gainsOut,
		std::map<int, std::map<int, cv::Mat> > * blendingGainsOut,
		std::pair<float, float> * contrastValuesOut)
{
	//get texture size, if disabled use default 1024
	UASSERT(textureSize%256 == 0);
	UDEBUG("textureSize = %d", textureSize);
	cv::Mat globalTextures;
	if(!mesh.tex_materials.empty())
	{
		std::vector<std::pair<int, int> > textures(mesh.tex_materials.size(), std::pair<int, int>(-1,0));
		cv::Size imageSize;
		const int imageType=CV_8UC3;

		UDEBUG("");
		for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
		{
			std::list<std::string> texFileSplit = uSplit(mesh.tex_materials[i].tex_file, '_');
			if(!mesh.tex_materials[i].tex_file.empty() &&
				mesh.tex_polygons[i].size() &&
			   uIsInteger(texFileSplit.front(), false))
			{
				textures[i].first = uStr2Int(texFileSplit.front());
				if(texFileSplit.size() == 2 &&
				   uIsInteger(texFileSplit.back(), false)	)
				{
					textures[i].second = uStr2Int(texFileSplit.back());
				}

				int textureId = textures[i].first;
				if(imageSize.width == 0 || imageSize.height == 0)
				{
					if(images.find(textureId) != images.end() &&
						!images.find(textureId)->second.empty() &&
						calibrations.find(textureId) != calibrations.end())
					{
						const std::vector<CameraModel> & models = calibrations.find(textureId)->second;
						UASSERT(models.size()>=1);
						if(	models[0].imageHeight()>0 &&
							models[0].imageWidth()>0)
						{
							imageSize = models[0].imageSize();
						}
						else if(images.find(textureId)!=images.end())
						{
							// backward compatibility for image size not set in CameraModel
							cv::Mat image = images.find(textureId)->second;
							if(image.rows == 1 && image.type() == CV_8UC1)
							{
								image = uncompressImage(image);
							}
							UASSERT(!image.empty());
							imageSize = image.size();
							if(models.size()>1)
							{
								imageSize.width/=models.size();
							}
						}
					}
					else if(memory)
					{
						SensorData data = memory->getNodeData(textureId, true, false, false, false);
						const std::vector<CameraModel> & models = data.cameraModels();
						const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
						if(models.size()>=1 &&
							models[0].imageHeight()>0 &&
							models[0].imageWidth()>0)
						{
							imageSize = models[0].imageSize();
						}
						else if(stereoModels.size()>=1 &&
								stereoModels[0].left().imageHeight() > 0 &&
								stereoModels[0].left().imageWidth() > 0)
						{
							imageSize = stereoModels[0].left().imageSize();
						}
						else // backward compatibility for image size not set in CameraModel
						{
							cv::Mat image;
							data.uncompressDataConst(&image, 0);
							UASSERT(!image.empty());
							imageSize = image.size();
							if(data.cameraModels().size()>1)
							{
								imageSize.width/=data.cameraModels().size();
							}
						}
					}
					else if(dbDriver)
					{
						std::vector<CameraModel> models;
						std::vector<StereoCameraModel> stereoModels;
						dbDriver->getCalibration(textureId, models, stereoModels);
						if(models.size()>=1 &&
							models[0].imageHeight()>0 &&
							models[0].imageWidth()>0)
						{
							imageSize = models[0].imageSize();
						}
						else if(stereoModels.size()>=1 &&
								stereoModels[0].left().imageHeight() > 0 &&
								stereoModels[0].left().imageWidth() > 0)
						{
							imageSize = stereoModels[0].left().imageSize();
						}
						else // backward compatibility for image size not set in CameraModel
						{
							SensorData data;
							dbDriver->getNodeData(textureId, data, true, false, false, false);
							cv::Mat image;
							data.uncompressDataConst(&image, 0);
							UASSERT(!image.empty());
							imageSize = image.size();
							if(data.cameraModels().size()>1)
							{
								imageSize.width/=data.cameraModels().size();
							}
						}
					}
				}
			}
			else if(mesh.tex_polygons[i].size() && mesh.tex_materials[i].tex_file.compare("occluded")!=0)
			{
				UWARN("Failed parsing texture file name: %s", mesh.tex_materials[i].tex_file.c_str());
			}
		}
		UDEBUG("textures=%d imageSize=%dx%d", (int)textures.size(), imageSize.height, imageSize.width);
		if(textures.size() && imageSize.height>0 && imageSize.width>0)
		{
			float scale = 0.0f;
			UDEBUG("");
			std::vector<bool> materialsKept;
			util3d::concatenateTextureMaterials(mesh, imageSize, textureSize, textureCount, scale, &materialsKept);
			if(scale && mesh.tex_materials.size())
			{
				int materials = (int)mesh.tex_materials.size();
				int cols = float(textureSize)/(scale*imageSize.width);
				int rows = float(textureSize)/(scale*imageSize.height);

				globalTextures = cv::Mat(textureSize, materials*textureSize, imageType, cv::Scalar::all(blankValue));
				cv::Mat globalTextureMasks = cv::Mat(textureSize, materials*textureSize, CV_8UC1, cv::Scalar::all(0));

				// used for multi camera texturing, to avoid reloading same texture for sub cameras
				cv::Mat previousImage;
				int previousTextureId = 0;
				std::vector<CameraModel> previousCameraModels;

				// make a blank texture
				cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(blankValue));
				cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255));
				int oi=0;
				std::vector<cv::Point2i> imageOrigin(textures.size());
				std::vector<int> newCamIndex(textures.size(), -1);
				for(int t=0; t<(int)textures.size(); ++t)
				{
					if(materialsKept.at(t))
					{
						int indexMaterial = oi / (cols*rows);
						UASSERT(indexMaterial < materials);

						newCamIndex[t] = oi;
						int u = oi%cols * emptyImage.cols;
						int v = ((oi/cols) % rows ) * emptyImage.rows;
						UASSERT_MSG(u < textureSize-emptyImage.cols, uFormat("u=%d textureSize=%d emptyImage.cols=%d", u, textureSize, emptyImage.cols).c_str());
						UASSERT_MSG(v < textureSize-emptyImage.rows, uFormat("v=%d textureSize=%d emptyImage.rows=%d", v, textureSize, emptyImage.rows).c_str());
						imageOrigin[t].x = u;
						imageOrigin[t].y = v;
						if(textures[t].first>=0)
						{
							cv::Mat image;
							std::vector<CameraModel> models;

							if(textures[t].first == previousTextureId)
							{
								image = previousImage;
								models = previousCameraModels;
							}
							else
							{
								if(images.find(textures[t].first) != images.end() &&
									!images.find(textures[t].first)->second.empty() &&
									calibrations.find(textures[t].first) != calibrations.end())
								{
									image = images.find(textures[t].first)->second;
									if(image.rows == 1 && image.type() == CV_8UC1)
									{
										image = uncompressImage(image);
									}
									models = calibrations.find(textures[t].first)->second;
								}
								else if(memory)
								{
									SensorData data = memory->getNodeData(textures[t].first, true, false, false, false);
									models = data.cameraModels();
									if(models.empty() && !data.stereoCameraModels().empty())
									{
										for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
										{
											models.push_back(data.stereoCameraModels()[i].left());
										}
									}
									data.uncompressDataConst(&image, 0);
								}
								else if(dbDriver)
								{
									SensorData data;
									dbDriver->getNodeData(textures[t].first, data, true, false, false, false);
									data.uncompressDataConst(&image, 0);
									std::vector<StereoCameraModel> stereoModels;
									dbDriver->getCalibration(textures[t].first, models, stereoModels);
									if(models.empty() && !stereoModels.empty())
									{
										for(size_t i=0; i<stereoModels.size(); ++i)
										{
											models.push_back(stereoModels[i].left());
										}
									}
								}

								previousImage = image;
								previousCameraModels = models;
								previousTextureId = textures[t].first;
							}

							UASSERT(!image.empty());

							if(textures[t].second>=0)
							{
								UASSERT(textures[t].second < (int)models.size());
								int width = image.cols/models.size();
								image = image.colRange(width*textures[t].second, width*(textures[t].second+1));
							}

							cv::Mat resizedImage;
							cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
							UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
							if(resizedImage.type() == CV_8UC1)
							{
								cv::Mat resizedImageColor;
								cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
								resizedImage = resizedImageColor;
							}
							UASSERT(resizedImage.type() == globalTextures.type());
							resizedImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, resizedImage.cols, resizedImage.rows)));
							emptyImageMask.copyTo(globalTextureMasks(cv::Rect(u+indexMaterial*globalTextureMasks.rows, v, resizedImage.cols, resizedImage.rows)));
						}
						else
						{
							emptyImage.copyTo(globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows)));
						}
						++oi;
					}

					if(state)
					{
						if(state->isCanceled())
						{
							return cv::Mat();
						}
						state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
					}
				}

				UTimer timer;
				if(vertexToPixels.size())
				{
					//UWARN("Saving original.png", globalTexture);
					//cv::imwrite("original.png", globalTexture);

					if(gainCompensation)
					{
						/**
						 * Original code from OpenCV: GainCompensator
						 */

						const int num_images = static_cast<int>(oi);
						cv::Mat_<int> N(num_images, num_images); N.setTo(0);
						cv::Mat_<double> I(num_images, num_images); I.setTo(0);

						cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
						cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
						cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);

						// Adjust UV coordinates to globalTexture
						for(unsigned int p=0; p<vertexToPixels.size(); ++p)
						{
							for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
							{
								if(materialsKept.at(iter->first))
								{
									N(newCamIndex[iter->first], newCamIndex[iter->first]) +=1;

									std::map<int, pcl::PointXY>::const_iterator jter=iter;
									++jter;
									int k = 1;
									for(; jter!=vertexToPixels[p].end(); ++jter, ++k)
									{
										if(materialsKept.at(jter->first))
										{
											int i = newCamIndex[iter->first];
											int j = newCamIndex[jter->first];

											N(i, j) += 1;
											N(j, i) += 1;

											int indexMaterial = i / (cols*rows);

											// uv in globalTexture
											int ui = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
											int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
											int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
											int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
											cv::Vec3b * pt1 = globalTextures.ptr<cv::Vec3b>(vi,ui+indexMaterial*globalTextures.rows);
											cv::Vec3b * pt2 = globalTextures.ptr<cv::Vec3b>(vj,uj+indexMaterial*globalTextures.rows);

											I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
											I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));

											IR(i, j) += static_cast<double>(pt1->val[2]);
											IR(j, i) += static_cast<double>(pt2->val[2]);
											IG(i, j) += static_cast<double>(pt1->val[1]);
											IG(j, i) += static_cast<double>(pt2->val[1]);
											IB(i, j) += static_cast<double>(pt1->val[0]);
											IB(j, i) += static_cast<double>(pt2->val[0]);
										}
									}
								}
							}
						}

						for(int i=0; i<num_images; ++i)
						{
							for(int j=i; j<num_images; ++j)
							{
								if(i == j)
								{
									if(N(i,j) == 0)
									{
										N(i,j) = 1;
									}
								}
								else if(N(i, j))
								{
									I(i, j) /= N(i, j);
									I(j, i) /= N(j, i);

									IR(i, j) /= N(i, j);
									IR(j, i) /= N(j, i);
									IG(i, j) /= N(i, j);
									IG(j, i) /= N(j, i);
									IB(i, j) /= N(i, j);
									IB(j, i) /= N(j, i);
								}
							}
						}

						cv::Mat_<double> A(num_images, num_images); A.setTo(0);
						cv::Mat_<double> b(num_images, 1); b.setTo(0);
						cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
						cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
						cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
						double alpha = 0.01;
						double beta = gainBeta;
						for (int i = 0; i < num_images; ++i)
						{
							for (int j = 0; j < num_images; ++j)
							{
								b(i, 0) += beta * N(i, j);
								A(i, i) += beta * N(i, j);
								AR(i, i) += beta * N(i, j);
								AG(i, i) += beta * N(i, j);
								AB(i, i) += beta * N(i, j);
								if (j == i) continue;
								A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
								A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);

								AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
								AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);

								AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
								AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);

								AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
								AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
							}
						}

						cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
						cv::solve(A, b, gainsGray);

						cv::solve(AR, b, gainsR);
						cv::solve(AG, b, gainsG);
						cv::solve(AB, b, gainsB);

						cv::Mat_<double> gains(gainsGray.rows, 4);
						gainsGray.copyTo(gains.col(0));
						gainsR.copyTo(gains.col(1));
						gainsG.copyTo(gains.col(2));
						gainsB.copyTo(gains.col(3));

						for(int t=0; t<(int)textures.size(); ++t)
						{
							//break;
							if(materialsKept.at(t))
							{
								int u = imageOrigin[t].x;
								int v = imageOrigin[t].y;

								UDEBUG("Gain cam%d = %f", newCamIndex[t], gainsGray(newCamIndex[t], 0));

								int indexMaterial = newCamIndex[t] / (cols*rows);
								cv::Mat roi = globalTextures(cv::Rect(u+indexMaterial*globalTextures.rows, v, emptyImage.cols, emptyImage.rows));

								std::vector<cv::Mat> channels;
								cv::split(roi, channels);

								// assuming BGR
								cv::multiply(channels[0], gains(newCamIndex[t], gainRGB?3:0), channels[0]);
								cv::multiply(channels[1], gains(newCamIndex[t], gainRGB?2:0), channels[1]);
								cv::multiply(channels[2], gains(newCamIndex[t], gainRGB?1:0), channels[2]);

								cv::merge(channels, roi);

								if(gainsOut)
								{
									cv::Vec4d g(
										gains(newCamIndex[t], 0),
										gains(newCamIndex[t], 1),
										gains(newCamIndex[t], 2),
										gains(newCamIndex[t], 3));
									if(gainsOut->find(textures[t].first) == gainsOut->end())
									{
										std::map<int,cv::Vec4d> value;
										value.insert(std::make_pair(textures[t].second, g));
										gainsOut->insert(std::make_pair(textures[t].first, value));
									}
									else
									{
										gainsOut->at(textures[t].first).insert(std::make_pair(textures[t].second,  g));
									}
								}
							}
						}
						//UWARN("Saving gain.png", globalTexture);
						//cv::imwrite("gain.png", globalTexture);
						if(state) state->callback(uFormat("Gain compensation %fs", timer.ticks()));
					}

					if(blending)
					{
						// blending BGR
						int decimation = 1;
						if(blendingDecimation <= 0)
						{
							// determinate decimation to apply
							std::vector<float> edgeLengths;
							if(mesh.tex_coordinates.size() && mesh.tex_coordinates[0].size())
							{
								UASSERT(mesh.tex_polygons.size() && mesh.tex_polygons[0].size() && mesh.tex_polygons[0][0].vertices.size());
								int polygonSize = mesh.tex_polygons[0][0].vertices.size();
								UDEBUG("polygon size=%d", polygonSize);

								for(unsigned int k=0; k<mesh.tex_coordinates.size(); ++k)
								{
									for(unsigned int i=0; i<mesh.tex_coordinates[k].size(); i+=polygonSize)
									{
										for(int j=0; j<polygonSize; ++j)
										{
											const Eigen::Vector2f & uc1 = mesh.tex_coordinates[k][i + j];
											const Eigen::Vector2f & uc2 = mesh.tex_coordinates[k][i + (j+1)%polygonSize];
											Eigen::Vector2f edge = (uc1-uc2)*textureSize;
											edgeLengths.push_back(fabs(edge[0]));
											edgeLengths.push_back(fabs(edge[1]));
										}
									}
								}
								float edgeLength = 0.0f;
								if(edgeLengths.size())
								{
									std::sort(edgeLengths.begin(), edgeLengths.end());
									float m = uMean(edgeLengths.data(), edgeLengths.size());
									float stddev = std::sqrt(uVariance(edgeLengths.data(), edgeLengths.size(), m));
									edgeLength = m+stddev;
									decimation = 1 << 6;
									for(int i=1; i<=6; ++i)
									{
										if(float(1 << i) >= edgeLength)
										{
											decimation = 1 << i;
											break;
										}
									}
								}

								UDEBUG("edge length=%f decimation=%d", edgeLength, decimation);
							}
						}
						else
						{
							if(blendingDecimation > 1)
							{
								UASSERT(textureSize % blendingDecimation == 0);
							}
							decimation = blendingDecimation;
							UDEBUG("decimation=%d", decimation);
						}

						std::vector<cv::Mat> blendGains(materials);
						for(int i=0; i<materials;++i)
						{
							blendGains[i] = cv::Mat(globalTextures.rows/decimation, globalTextures.rows/decimation, CV_32FC3, cv::Scalar::all(1.0f));
						}

						for(unsigned int p=0; p<vertexToPixels.size(); ++p)
						{
							if(vertexToPixels[p].size() > 1)
							{
								std::vector<float> gainsB(vertexToPixels[p].size());
								std::vector<float> gainsG(vertexToPixels[p].size());
								std::vector<float> gainsR(vertexToPixels[p].size());
								float sumWeight = 0.0f;
								int k=0;
								for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
								{
									if(materialsKept.at(iter->first))
									{
										int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
										int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
										float x = iter->second.x - 0.5f;
										float y = iter->second.y - 0.5f;
										float weight = 0.7f - sqrt(x*x+y*y);
										if(weight<0.0f)
										{
											weight = 0.0f;
										}
										int indexMaterial = newCamIndex[iter->first] / (cols*rows);
										cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
										gainsB[k] = static_cast<double>(pt->val[0]) * weight;
										gainsG[k] = static_cast<double>(pt->val[1]) * weight;
										gainsR[k] = static_cast<double>(pt->val[2]) * weight;
										sumWeight += weight;
										++k;
									}
								}
								gainsB.resize(k);
								gainsG.resize(k);
								gainsR.resize(k);

								if(sumWeight > 0)
								{
									float targetColor[3];
									targetColor[0] = uSum(gainsB.data(), gainsB.size()) / sumWeight;
									targetColor[1] = uSum(gainsG.data(), gainsG.size()) / sumWeight;
									targetColor[2] = uSum(gainsR.data(), gainsR.size()) / sumWeight;
									for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
									{
										if(materialsKept.at(iter->first))
										{
											int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
											int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
											int indexMaterial = newCamIndex[iter->first] / (cols*rows);
											cv::Vec3b * pt = globalTextures.ptr<cv::Vec3b>(v,u+indexMaterial*globalTextures.rows);
											float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
											float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
											float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
											cv::Vec3f * ptr = blendGains[indexMaterial].ptr<cv::Vec3f>(v/decimation, u/decimation);
											ptr->val[0] = (gB>1.3f)?1.3f:(gB<0.7f)?0.7f:gB;
											ptr->val[1] = (gG>1.3f)?1.3f:(gG<0.7f)?0.7f:gG;
											ptr->val[2] = (gR>1.3f)?1.3f:(gR<0.7f)?0.7f:gR;
										}
									}
								}
							}
						}

						if(blendingGainsOut)
						{
							for(int t=0; t<(int)textures.size(); ++t)
							{
								//break;
								if(materialsKept.at(t))
								{
									int u = imageOrigin[t].x/decimation;
									int v = imageOrigin[t].y/decimation;

									int indexMaterial = newCamIndex[t] / (cols*rows);
									cv::Mat roi = blendGains[indexMaterial](cv::Rect(u, v, emptyImage.cols/decimation, emptyImage.rows/decimation));
									if(blendingGainsOut->find(textures[t].first) == blendingGainsOut->end())
									{
										std::map<int,cv::Mat> value;
										value.insert(std::make_pair(textures[t].second, roi.clone()));
										blendingGainsOut->insert(std::make_pair(textures[t].first, value));
									}
									else
									{
										blendingGainsOut->at(textures[t].first).insert(std::make_pair(textures[t].second, roi.clone()));
									}
								}
							}
						}

						for(int i=0; i<materials; ++i)
						{
							/*std::vector<cv::Mat> channels;
							cv::split(blendGains, channels);
							cv::Mat img;
							channels[0].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendSmallB.png", img);
							channels[1].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendSmallG.png", img);
							channels[2].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendSmallR.png", img);*/

							cv::Mat globalTexturesROI = globalTextures(cv::Range::all(), cv::Range(i*globalTextures.rows, (i+1)*globalTextures.rows));
							cv::Mat dst;
							cv::blur(blendGains[i], dst, cv::Size(3,3));
							cv::resize(dst, blendGains[i], globalTexturesROI.size(), 0, 0, cv::INTER_LINEAR);

							/*cv::split(blendGains, channels);
							channels[0].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendFullB.png", img);
							channels[1].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendFullG.png", img);
							channels[2].convertTo(img,CV_8U,128.0,0);
							cv::imwrite("blendFullR.png", img);*/

							cv::multiply(globalTexturesROI, blendGains[i], globalTexturesROI, 1.0, CV_8UC3);

							//UWARN("Saving blending.png", globalTexture);
							//cv::imwrite("blending.png", globalTexture);
						}

						if(state) state->callback(uFormat("Blending (decimation=%d) %fs", decimation, timer.ticks()));
					}
				}

				if(brightnessContrastRatioLow > 0 || brightnessContrastRatioHigh > 0)
				{
					if(exposureFusion)
					{
						std::vector<cv::Mat> images;
						images.push_back(globalTextures);
						if (brightnessContrastRatioLow > 0)
						{
							images.push_back(util2d::brightnessAndContrastAuto(
									globalTextures,
									globalTextureMasks,
									(float)brightnessContrastRatioLow,
									0.0f));
						}
						if (brightnessContrastRatioHigh > 0)
						{
							images.push_back(util2d::brightnessAndContrastAuto(
									globalTextures,
									globalTextureMasks,
									0.0f,
									(float)brightnessContrastRatioHigh));
						}

						globalTextures = util2d::exposureFusion(images);
					}
					else
					{
						float alpha, beta;
						globalTextures = util2d::brightnessAndContrastAuto(
								globalTextures,
								globalTextureMasks,
								(float)brightnessContrastRatioLow,
								(float)brightnessContrastRatioHigh,
								&alpha,
								&beta);
						if(contrastValuesOut)
						{
							contrastValuesOut->first = alpha;
							contrastValuesOut->second = beta;
						}
					}
					if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
				}
			}
			
			// Cloud color
			if(clearVertexColorUnderTexture)
			{
				int colorOffset = 0;
				for(unsigned int i=0; i<mesh.cloud.fields.size(); ++i)
				{
					if(mesh.cloud.fields[i].name.compare("rgb") == 0)
					{
						colorOffset = mesh.cloud.fields[i].offset;
						break;
					}
				}
				if(colorOffset>0)
				{
					pcl::IndicesPtr notTexturedVertexIndices(new std::vector<int>);
					UASSERT(mesh.tex_coordinates.size() == mesh.tex_polygons.size());
					for(size_t t=0; t<mesh.tex_polygons.size(); ++t)
					{
						int pixelIndex = 0;
						for(size_t p=0; p<mesh.tex_polygons[t].size(); ++p)
						{
							// only clear polygon color if all 3 vertices have valid tex coordinates.
							bool valid = true;
							for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size() && valid; ++v)
							{
								UASSERT(pixelIndex+v < mesh.tex_coordinates[t].size());
								const Eigen::Vector2f & uv = mesh.tex_coordinates[t][pixelIndex+v];
								if(uv[0] == -1 || uv[1] == -1)
								{
									valid = false;
								}
							}
							if(!valid)
							{
								for(size_t v=0; v<mesh.tex_polygons[t][p].vertices.size(); ++v)
								{
									int vertex = mesh.tex_polygons[t][p].vertices[v];
									notTexturedVertexIndices->push_back(vertex);
								}
							}
							pixelIndex+=mesh.tex_polygons[t][p].vertices.size();
						}
					}
					// Set up the full indices set
					pcl::IndicesPtr full_indices(new std::vector<int>(mesh.cloud.width* mesh.cloud.height));
					for (size_t fii = 0; fii < full_indices->size(); ++fii)  // fii = full indices iterator
						full_indices->at(fii) = fii;

					// Set up the sorted input indices
					std::sort (notTexturedVertexIndices->begin (), notTexturedVertexIndices->end ());

					// Store the difference in indices
					pcl::IndicesPtr texturedVertexIndices(new std::vector<int>());
					std::set_difference (full_indices->begin (), full_indices->end (), notTexturedVertexIndices->begin (), notTexturedVertexIndices->end (), std::inserter (*texturedVertexIndices, texturedVertexIndices->begin ()));

					for(size_t i=0; i<texturedVertexIndices->size(); ++i)
					{
						std::uint32_t white = 0xffffff;
						UASSERT(texturedVertexIndices->at(i) * mesh.cloud.point_step + colorOffset < mesh.cloud.data.size());
						memcpy(&mesh.cloud.data.data()[texturedVertexIndices->at(i) * mesh.cloud.point_step + colorOffset], reinterpret_cast<float*>(&white), sizeof(float));
					}
				}
			}
		}
	}
	UDEBUG("globalTextures=%d", globalTextures.cols?globalTextures.cols / globalTextures.rows:0);
	return globalTextures;
}

void fixTextureMeshForVisualization(pcl::TextureMesh & textureMesh)
{
	// VTK issue:
	//  tex_coordinates should be linked to points, not
	//  polygon vertices. Points linked to multiple different TCoords (different textures) should
	//  be duplicated.
	for (unsigned int t = 0; t < textureMesh.tex_coordinates.size(); ++t)
	{
		if(textureMesh.tex_polygons[t].size())
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::fromPCLPointCloud2(textureMesh.cloud, *originalCloud);

			// make a cloud with as many points than polygon vertices
			unsigned int nPoints = textureMesh.tex_coordinates[t].size();
			UASSERT(nPoints == textureMesh.tex_polygons[t].size()*textureMesh.tex_polygons[t][0].vertices.size()); // assuming polygon size is constant!

			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>);
			newCloud->resize(nPoints);

			unsigned int oi = 0;
			for (unsigned int i = 0; i < textureMesh.tex_polygons[t].size(); ++i)
			{
				pcl::Vertices & vertices = textureMesh.tex_polygons[t][i];

				for(unsigned int j=0; j<vertices.vertices.size(); ++j)
				{
					UASSERT(oi < newCloud->size());
					UASSERT_MSG((size_t)vertices.vertices[j] < originalCloud->size(), uFormat("%d vs %d", vertices.vertices[j], (int)originalCloud->size()).c_str());
					newCloud->at(oi) = originalCloud->at(vertices.vertices[j]);
					vertices.vertices[j] = oi; // new vertex index
					++oi;
				}
			}
			pcl::toPCLPointCloud2(*newCloud, textureMesh.cloud);
		}
	}
}

bool multiBandTexturing(
		const std::string & outputOBJPath,
		const pcl::PCLPointCloud2 & cloud,
		const std::vector<pcl::Vertices> & polygons,
		const std::map<int, Transform> & cameraPoses,
		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels, // required output of util3d::createTextureMesh()
		const std::map<int, cv::Mat> & images,        // raw or compressed, can be empty if memory or dbDriver should be used
		const std::map<int, std::vector<CameraModel> > & cameraModels, // Should match images
		const Memory * memory,                    // Should be set if images are not set
		const DBDriver * dbDriver,                // Should be set if images and memory are not set
		int textureSize,
		const std::string & textureFormat,
		const std::map<int, std::map<int, cv::Vec4d> > & gains,       // optional output of util3d::mergeTextures()
		const std::map<int, std::map<int, cv::Mat> > & blendingGains, // optional output of util3d::mergeTextures()
		const std::pair<float, float> & contrastValues,               // optional output of util3d::mergeTextures()
		bool gainRGB)
{
	return multiBandTexturing(
			outputOBJPath,
			cloud,
			polygons,
			cameraPoses,
			vertexToPixels,
			images,
			cameraModels,
			memory,
			dbDriver,
			textureSize,
			2,
			"1 5 10 0",
			textureFormat,
			gains,
			blendingGains,
			contrastValues,
			gainRGB);
}

bool multiBandTexturing(
		const std::string & outputOBJPath,
		const pcl::PCLPointCloud2 & cloud,
		const std::vector<pcl::Vertices> & polygons,
		const std::map<int, Transform> & cameraPoses,
		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels,
		const std::map<int, cv::Mat> & images,
		const std::map<int, std::vector<CameraModel> > & cameraModels,
		const Memory * memory,
		const DBDriver * dbDriver,
		unsigned int textureSize,
		unsigned int textureDownScale,
		const std::string & nbContrib,
		const std::string & textureFormat,
		const std::map<int, std::map<int, cv::Vec4d> > & gains,
		const std::map<int, std::map<int, cv::Mat> > & blendingGains,
		const std::pair<float, float> & contrastValues,
		bool gainRGB,
		unsigned int unwrapMethod,
		bool fillHoles,
		unsigned int padding,
		double bestScoreThreshold,
		double angleHardThreshold,
		bool forceVisibleByAllVertices)
{

#ifdef RTABMAP_ALICE_VISION
	if(ULogger::level() == ULogger::kDebug)
	{
		system::Logger::get()->setLogLevel(system::EVerboseLevel::Trace);
	}
	else if(ULogger::level() == ULogger::kInfo)
	{
		system::Logger::get()->setLogLevel(system::EVerboseLevel::Info);
	}
	else if(ULogger::level() == ULogger::kWarning)
	{
		system::Logger::get()->setLogLevel(system::EVerboseLevel::Warning);
	}
	else
	{
		system::Logger::get()->setLogLevel(system::EVerboseLevel::Error);
	}

	sfmData::SfMData sfmData;
	pcl::PointCloud<pcl::PointXYZRGB> cloud2;
	pcl::fromPCLPointCloud2(cloud, cloud2);
	UASSERT(vertexToPixels.size() == cloud2.size());
	UINFO("Input mesh: %d points %d polygons", (int)cloud2.size(), (int)polygons.size());
	mesh::Texturing texturing;
#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
	texturing.mesh = new mesh::Mesh();
	texturing.mesh->pts.resize(cloud2.size());
	texturing.mesh->pointsVisibilities.resize(cloud2.size());
#else
	texturing.me = new mesh::Mesh();
	texturing.me->pts = new StaticVector<Point3d>(cloud2.size());
	texturing.pointsVisibilities = new mesh::PointsVisibility();
	texturing.pointsVisibilities->reserve(cloud2.size());
#endif
	texturing.texParams.textureSide = textureSize;
	texturing.texParams.downscale = textureDownScale;
	std::vector<int> multiBandNbContrib;
	std::list<std::string> values = uSplit(nbContrib, ' ');
	for(std::list<std::string>::iterator iter=values.begin(); iter!=values.end(); ++iter)
	{
		multiBandNbContrib.push_back(uStr2Int(*iter));
	}
	if(multiBandNbContrib.size() != 4)
	{
		UERROR("multiband: Wrong number of nb of contribution (vaue=\"%s\", should be 4), using default values instead.", nbContrib.c_str());
	}
	else
	{
		texturing.texParams.multiBandNbContrib = multiBandNbContrib;
	}
	texturing.texParams.padding = padding;
	texturing.texParams.fillHoles = fillHoles;
	texturing.texParams.bestScoreThreshold = bestScoreThreshold;
	texturing.texParams.angleHardThreshold = angleHardThreshold;
	texturing.texParams.forceVisibleByAllVertices = forceVisibleByAllVertices;
	texturing.texParams.visibilityRemappingMethod = mesh::EVisibilityRemappingMethod::Pull;


	for(size_t i=0;i<cloud2.size();++i)
	{
		pcl::PointXYZRGB pt = cloud2.at(i);
#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
		texturing.mesh->pointsVisibilities[i].reserve(vertexToPixels[i].size());
		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
		{
			texturing.mesh->pointsVisibilities[i].push_back(iter->first);
		}
		texturing.mesh->pts[i] = Point3d(pt.x, pt.y, pt.z);
#else
		mesh::PointVisibility* pointVisibility = new mesh::PointVisibility();
		pointVisibility->reserve(vertexToPixels[i].size());
		for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[i].begin(); iter!=vertexToPixels[i].end();++iter)
		{
			pointVisibility->push_back(iter->first);
		}
		texturing.pointsVisibilities->push_back(pointVisibility);
		(*texturing.me->pts)[i] = Point3d(pt.x, pt.y, pt.z);
#endif
	}

#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
	texturing.mesh->tris.resize(polygons.size());
	texturing.mesh->trisMtlIds().resize(polygons.size());
#else
	texturing.me->tris = new StaticVector<mesh::Mesh::triangle>(polygons.size());
#endif
	for(size_t i=0;i<polygons.size();++i)
	{
		UASSERT(polygons[i].vertices.size() == 3);
#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
		texturing.mesh->trisMtlIds()[i] = -1;
		texturing.mesh->tris[i] = mesh::Mesh::triangle(
#else
		(*texturing.me->tris)[i] = mesh::Mesh::triangle(
#endif
				polygons[i].vertices[0],
				polygons[i].vertices[1],
				polygons[i].vertices[2]);
	}
	UTimer timer;
	std::string outputDirectory = UDirectory::getDir(outputOBJPath);
	std::string tmpImageDirectory = outputDirectory+"/rtabmap_tmp_textures";
	UDirectory::removeDir(tmpImageDirectory);
	UDirectory::makeDir(tmpImageDirectory);
	UINFO("Temporary saving images from %ld nodes in directory \"%s\"...", cameraPoses.size(), tmpImageDirectory.c_str());
	int viewId = 0;
	for(std::map<int, Transform>::const_iterator iter = cameraPoses.lower_bound(1); iter!=cameraPoses.end(); ++iter)
	{
		int camId = iter->first;
		cv::Mat image;
		std::vector<CameraModel> models;

		if( images.find(camId) != images.end() &&
			!images.find(camId)->second.empty() &&
			cameraModels.find(camId) != cameraModels.end())
		{
			image = images.find(camId)->second;
			models = cameraModels.find(camId)->second;
		}
		else if(memory)
		{
			SensorData data = memory->getNodeData(camId, true, false, false, false);
			models = data.cameraModels();
			if(models.empty() && data.stereoCameraModels().size())
			{
				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
				{
					models.push_back(data.stereoCameraModels()[i].left());
				}
			}
			if(data.imageRaw().empty())
			{
				image = data.imageCompressed();
			}
			else
			{
				image = data.imageRaw();
			}

			if(models.empty() || image.empty())
			{
				Transform odomPose;
				int mapId;
				int weight=0;
				std::string label;
				double stamp;
				Transform gt;
				std::vector<float> vel;
				GPS gps;
				EnvSensors envs;
				memory->getNodeInfo(camId, odomPose, mapId, weight, label, stamp, gt, vel, gps, envs, true);
				if(weight == -1) // just ignore intermediate nodes if their data is not set
					continue;
			}
		}
		else if(dbDriver)
		{
			std::vector<StereoCameraModel> stereoModels;
			dbDriver->getCalibration(camId, models, stereoModels);
			if(models.empty() && stereoModels.size())
			{
				for(size_t i=0; i<stereoModels.size(); ++i)
				{
					models.push_back(stereoModels[i].left());
				}
			}

			SensorData data;
			dbDriver->getNodeData(camId, data, true, false, false, false);
			if(data.imageRaw().empty())
			{
				image = data.imageCompressed();
			}
			else
			{
				image = data.imageRaw();
			}

			if(models.empty() || image.empty())
			{
				int weight=0;
				dbDriver->getWeight(camId, weight);
				if(weight == -1) // just ignore intermediate nodes if their data is not set
					continue;
			}
		}
		if(models.empty())
		{
			UERROR("No camera models found for camera %d. Aborting multiband texturing...", iter->first);
			return false;
		}
		if(image.empty())
		{
			UERROR("No image found for camera %d. Aborting multiband texturing...", iter->first);
			return false;
		}

		if(image.rows == 1 && image.type() == CV_8UC1)
		{
			image = uncompressImage(image);
		}
		else
		{
			image = image.clone();
		}

		for(size_t i=0; i<models.size(); ++i)
		{
			const CameraModel & model = models.at(i);
			cv::Size imageSize = model.imageSize();
			if(imageSize.height == 0)
			{
				// backward compatibility
				imageSize.height = image.rows;
				imageSize.width = image.cols;
			}

			UASSERT(image.cols % imageSize.width == 0);
			cv::Mat imageRoi = image.colRange(i*imageSize.width, (i+1)*imageSize.width);

			if(gains.find(camId) != gains.end() &&
			   gains.at(camId).find(i) != gains.at(camId).end())
			{
				const cv::Vec4d & g = gains.at(camId).at(i);
				if(imageRoi.channels() == 1)
				{
					cv::multiply(imageRoi, g.val[0], imageRoi);
				}
				else
				{
					std::vector<cv::Mat> channels;
					cv::split(imageRoi, channels);

					// assuming BGR
					cv::multiply(channels[0], g.val[gainRGB?3:0], channels[0]);
					cv::multiply(channels[1], g.val[gainRGB?2:0], channels[1]);
					cv::multiply(channels[2], g.val[gainRGB?1:0], channels[2]);

					cv::Mat output;
					cv::merge(channels, output);
					imageRoi = output;
				}
			}

			if(blendingGains.find(camId) != blendingGains.end() &&
			   blendingGains.at(camId).find(i) != blendingGains.at(camId).end())
			{
				// Should be color for blending options
				if(imageRoi.channels() == 1)
				{
					cv::Mat imageRoiColor;
					cv::cvtColor(imageRoi, imageRoiColor, CV_GRAY2BGR);
					imageRoi = imageRoiColor;
				}

				cv::Mat g = blendingGains.at(camId).at(i);
				cv::Mat dst;
				cv::blur(g, dst, cv::Size(3,3));
				cv::Mat gResized;
				cv::resize(dst, gResized, imageRoi.size(), 0, 0, cv::INTER_LINEAR);
				cv::Mat output;
				cv::multiply(imageRoi, gResized, output, 1.0, CV_8UC3);
				imageRoi = output;
			}

			Transform t = (iter->second * model.localTransform()).inverse();
			Eigen::Matrix<double, 3, 4> m = t.toEigen3d().matrix().block<3,4>(0, 0);
			sfmData::CameraPose pose(geometry::Pose3(m), true);
			sfmData.setAbsolutePose((IndexT)viewId, pose);

			UDEBUG("%d %d %f %f %f %f", imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx(), model.cy());
			std::shared_ptr<camera::IntrinsicBase> camPtr = std::make_shared<camera::Pinhole>(
#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
					//https://github.com/alicevision/AliceVision/commit/9fab5c79a1c65595fe5c5001267e1c5212bc93f0#diff-b0c0a3c30de50be8e4ed283dfe4c8ae4a9bc861aa9a83bd8bfda8182e9d67c08
					// [all] the camera principal point is now defined as an offset relative to the image center
					imageSize.width, imageSize.height, model.fx(), model.fy(), model.cx() - double(imageSize.width) * 0.5, model.cy() - double(imageSize.height) * 0.5);
#else
					imageSize.width, imageSize.height, model.fx(), model.cx(), model.cy());
#endif
			sfmData.intrinsics.insert(std::make_pair((IndexT)viewId, camPtr));

			std::string imagePath = tmpImageDirectory+uFormat("/%d.jpg", viewId);
			cv::imwrite(imagePath, imageRoi);
			std::shared_ptr<sfmData::View> viewPtr = std::make_shared<sfmData::View>(
					imagePath,
					(IndexT)viewId,
					(IndexT)viewId,
					(IndexT)viewId,
					imageSize.width,
					imageSize.height);
			sfmData.views.insert(std::make_pair((IndexT)viewId, viewPtr));
			++viewId;
		}
		UDEBUG("camId=%d", camId);
	}
	UINFO("Temporary saving images in directory \"%s\"... done (%d images of %d nodes). %fs", tmpImageDirectory.c_str(), viewId, (int)cameraPoses.size(), timer.ticks());

	mvsUtils::MultiViewParams mp(sfmData);

	UINFO("Unwrapping (method=%d=%s)...", unwrapMethod, mesh::EUnwrapMethod_enumToString((mesh::EUnwrapMethod)unwrapMethod).c_str());
	texturing.unwrap(mp, (mesh::EUnwrapMethod)unwrapMethod);
	UINFO("Unwrapping done. %fs", timer.ticks());

	// save final obj file
	std::string baseName = uSplit(UFile::getName(outputOBJPath), '.').front();
#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=4)
	texturing.saveAs(outputDirectory, baseName, aliceVision::mesh::EFileType::OBJ, imageIO::EImageFileType::PNG);
#else
	texturing.saveAsOBJ(outputDirectory, baseName);
#endif
	UINFO("Saved %s. %fs", outputOBJPath.c_str(), timer.ticks());

	// generate textures
	UINFO("Generating textures...");
	texturing.generateTextures(mp, outputDirectory);
	UINFO("Generating textures done. %fs", timer.ticks());

	UINFO("Cleanup temporary directory \"%s\"...", tmpImageDirectory.c_str());
	UDirectory dir(tmpImageDirectory);
	std::string fp = dir.getNextFilePath();
	while(!fp.empty())
	{
		UFile::erase(fp);
		fp = dir.getNextFilePath();
	}
	UDirectory::removeDir(tmpImageDirectory);
	UINFO("Cleanup temporary directory \"%s\"... done.", tmpImageDirectory.c_str());

	UINFO("Rename/convert textures...");
	dir.setPath(outputDirectory, "png");
	std::map<std::string, std::string> texNames; // <old, new>
	std::string outputFormat = textureFormat;
	if(outputFormat.front() == '.')
	{
		outputFormat = outputFormat.substr(1, std::string::npos);
	}
	for(std::list<std::string>::const_iterator iter=dir.getFileNames().begin(); iter!=dir.getFileNames().end(); ++iter)
	{
		// Textures are called "texture_1001.png", "texture_1002.png", ...
		if(uStrContains(*iter, "texture_10"))
		{
			cv::Mat img = cv::imread(outputDirectory+"/"+*iter);
			if(contrastValues.first != 0.0f || contrastValues.second != 0.0f)
			{
				UASSERT(img.channels() == 3);
				// Re-use same contrast values with all images
				UINFO("Apply contrast values %f %f", contrastValues.first, contrastValues.second);
				img.convertTo(img, -1, contrastValues.first, contrastValues.second);
			}
			std::string newName = *iter;
			boost::replace_all(newName, "png", outputFormat);
			boost::replace_all(newName, "texture", baseName);
			texNames.insert(std::make_pair(*iter, newName));
			cv::imwrite(outputDirectory+"/"+newName, img);
			UFile::erase(outputDirectory+"/"+*iter);
		}
	}
	std::ifstream fi(outputDirectory+"/"+baseName+".mtl");
	std::string mtlStr((std::istreambuf_iterator<char>(fi)),
	                 std::istreambuf_iterator<char>());
	fi.close();
	UFile::erase(outputDirectory+"/"+baseName);
	for(std::map<std::string, std::string>::iterator iter=texNames.begin(); iter!=texNames.end(); ++iter)
	{
		boost::replace_all(mtlStr, iter->first, iter->second);
	}
	std::ofstream fo(outputDirectory+"/"+baseName+".mtl");
	fo.write(mtlStr.c_str(), mtlStr.size());
	fo.close();
	UINFO("Rename/convert textures... done. %fs", timer.ticks());

#if RTABMAP_ALICE_VISION_MAJOR > 2 || (RTABMAP_ALICE_VISION_MAJOR==2 && RTABMAP_ALICE_VISION_MINOR>=3)
	UINFO("Cleanup sfmdata...");
	sfmData.clear();
	UINFO("Cleanup sfmdata... done. %fs", timer.ticks());
#endif

	return true;
#else
	UERROR("Cannot unwrap texture mesh. RTAB-Map is not built with Alice Vision support! Returning false.");
	return false;
#endif
}

LaserScan computeNormals(
		const LaserScan & laserScan,
		int searchK,
		float searchRadius)
{
	if(laserScan.isEmpty())
	{
		return laserScan;
	}

	pcl::PointCloud<pcl::Normal>::Ptr normals;
	// convert to compatible PCL format and filter it
	if(laserScan.hasRGB())
	{
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = laserScanToPointCloudRGB(laserScan);
		if(cloud->size())
		{
			UASSERT(!laserScan.is2d());
			pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
			return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
		}
	}
	else if(laserScan.hasIntensity())
	{
		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = laserScanToPointCloudI(laserScan);
		if(cloud->size())
		{
			if(laserScan.is2d())
			{
				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
				if(laserScan.angleIncrement() > 0.0f)
				{
					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
				}
				else
				{
					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
				}

			}
			else
			{
				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
			}
		}
	}
	else
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = laserScanToPointCloud(laserScan);
		if(cloud->size())
		{
			if(laserScan.is2d())
			{
				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
				if(laserScan.angleIncrement() > 0.0f)
				{
					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.rangeMin(), laserScan.rangeMax(), laserScan.angleMin(), laserScan.angleMax(), laserScan.angleIncrement(), laserScan.localTransform());
				}
				else
				{
					return LaserScan(laserScan2dFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
				}
			}
			else
			{
				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, searchK, searchRadius);
				return LaserScan(laserScanFromPointCloud(*cloud, *normals), laserScan.maxPoints(), laserScan.rangeMax(), laserScan.localTransform());
			}
		}
	}
	return LaserScan();
}

template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeNormalsImpl(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
	if(indices->size())
	{
		tree->setInputCloud(cloud, indices);
	}
	else
	{
		tree->setInputCloud (cloud);
	}

	// Normal estimation*
#ifdef PCL_OMP
	pcl::NormalEstimationOMP<PointT, pcl::Normal> n;
#else
	pcl::NormalEstimation<PointT, pcl::Normal> n;
#endif
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	n.setInputCloud (cloud);
	// Commented: Keep the output normals size the same as the input cloud
	//if(indices->size())
	//{
	//	n.setIndices(indices);
	//}
	n.setSearchMethod (tree);
	n.setKSearch (searchK);
	n.setRadiusSearch (searchRadius);
	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
	n.compute (*normals);

	return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	pcl::IndicesPtr indices(new std::vector<int>);
	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	pcl::IndicesPtr indices(new std::vector<int>);
	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	pcl::IndicesPtr indices(new std::vector<int>);
	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeNormalsImpl<pcl::PointXYZ>(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeNormalsImpl<pcl::PointXYZRGB>(cloud, indices, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeNormalsImpl<pcl::PointXYZI>(cloud, indices, searchK, searchRadius, viewPoint);
}

template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2DImpl(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	UASSERT(searchK>0 || searchRadius>0.0f);
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
	tree->setInputCloud (cloud);

	normals->resize(cloud->size());

	float bad_point = std::numeric_limits<float>::quiet_NaN ();

	// assuming that points are ordered
	for(unsigned int i=0; i<cloud->size(); ++i)
	{
		const PointT & pt = cloud->at(i);
		std::vector<Eigen::Vector3f> neighborNormals;
		Eigen::Vector3f direction;
		direction[0] = viewPoint[0] - pt.x;
		direction[1] = viewPoint[1] - pt.y;
		direction[2] = viewPoint[2] - pt.z;

		std::vector<int> k_indices;
		std::vector<float> k_sqr_distances;
		if(searchRadius>0.0f)
		{
			tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
		}
		else
		{
			tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
		}

		for(unsigned int j=0; j<k_indices.size(); ++j)
		{
			if(k_indices.at(j) != (int)i)
			{
				const PointT & pt2 = cloud->at(k_indices.at(j));
				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
				Eigen::Vector3f up = v.cross(direction);
				Eigen::Vector3f n = up.cross(v);
				n.normalize();
				neighborNormals.push_back(n);
			}
		}

		if(neighborNormals.empty())
		{
			normals->at(i).normal_x = bad_point;
			normals->at(i).normal_y = bad_point;
			normals->at(i).normal_z = bad_point;
		}
		else
		{
			Eigen::Vector3f meanNormal(0,0,0);
			for(unsigned int j=0; j<neighborNormals.size(); ++j)
			{
				meanNormal+=neighborNormals[j];
			}
			meanNormal /= (float)neighborNormals.size();
			meanNormal.normalize();
			normals->at(i).normal_x = meanNormal[0];
			normals->at(i).normal_y = meanNormal[1];
			normals->at(i).normal_z = meanNormal[2];
		}
	}

	return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
}

template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2DImpl(
		const typename pcl::PointCloud<PointT>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	UASSERT(searchK>0);
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);

	normals->resize(cloud->size());
	searchRadius *= searchRadius; // squared distance

	float bad_point = std::numeric_limits<float>::quiet_NaN ();

	// assuming that points are ordered
	for(int i=0; i<(int)cloud->size(); ++i)
	{
		int li = i-searchK;
		if(li<0)
		{
			li=0;
		}
		int hi = i+searchK;
		if(hi>=(int)cloud->size())
		{
			hi=(int)cloud->size()-1;
		}

		// get points before not too far
		const PointT & pt = cloud->at(i);
		std::vector<Eigen::Vector3f> neighborNormals;
		Eigen::Vector3f direction;
		direction[0] = viewPoint[0] - cloud->at(i).x;
		direction[1] = viewPoint[1] - cloud->at(i).y;
		direction[2] = viewPoint[2] - cloud->at(i).z;
		for(int j=i-1; j>=li; --j)
		{
			const PointT & pt2 = cloud->at(j);
			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
			{
				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
				Eigen::Vector3f up = v.cross(direction);
				Eigen::Vector3f n = up.cross(v);
				n.normalize();
				neighborNormals.push_back(n);
			}
			else
			{
				break;
			}
		}
		for(int j=i+1; j<=hi; ++j)
		{
			const PointT & pt2 = cloud->at(j);
			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
			{
				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
				Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
				Eigen::Vector3f n = up.cross(v);
				n.normalize();
				neighborNormals.push_back(n);
			}
			else
			{
				break;
			}
		}

		if(neighborNormals.empty())
		{
			normals->at(i).normal_x = bad_point;
			normals->at(i).normal_y = bad_point;
			normals->at(i).normal_z = bad_point;
		}
		else
		{
			Eigen::Vector3f meanNormal(0,0,0);
			for(unsigned int j=0; j<neighborNormals.size(); ++j)
			{
				meanNormal+=neighborNormals[j];
			}
			meanNormal /= (float)neighborNormals.size();
			meanNormal.normalize();
			normals->at(i).normal_x = meanNormal[0];
			normals->at(i).normal_y = meanNormal[1];
			normals->at(i).normal_z = meanNormal[2];
		}
	}

	return normals;
}

pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeFastOrganizedNormals2DImpl<pcl::PointXYZ>(cloud, searchK, searchRadius, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
		int searchK,
		float searchRadius,
		const Eigen::Vector3f & viewPoint)
{
	return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
}

pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		float maxDepthChangeFactor,
		float normalSmoothingSize,
		const Eigen::Vector3f & viewPoint)
{
	pcl::IndicesPtr indices(new std::vector<int>);
	return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float maxDepthChangeFactor,
		float normalSmoothingSize,
		const Eigen::Vector3f & viewPoint)
{
	UASSERT(cloud->isOrganized());

	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
	if(indices->size())
	{
		tree->setInputCloud(cloud, indices);
	}
	else
	{
		tree->setInputCloud (cloud);
	}

	// Normal estimation
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
	ne.setNormalSmoothingSize(normalSmoothingSize);
	ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
	ne.setInputCloud(cloud);
	// Commented: Keep the output normals size the same as the input cloud
	//if(indices->size())
	//{
	//	ne.setIndices(indices);
	//}
	ne.setSearchMethod(tree);
	ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
	ne.compute(*normals);

	return normals;
}

float computeNormalsComplexity(
		const LaserScan & scan,
		const Transform & t,
		cv::Mat * pcaEigenVectors,
		cv::Mat * pcaEigenValues)
{
	if(!scan.isEmpty() && (scan.hasNormals()))
	{
		 //Construct a buffer used by the pca analysis
		int sz = static_cast<int>(scan.size()*2);
		bool is2d = scan.is2d();
		cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
		int oi = 0;
		int nOffset = scan.getNormalsOffset();
		bool doTransform = false;
		Transform tn;
		if(!t.isIdentity() || !scan.localTransform().isIdentity())
		{
			tn = (t*scan.localTransform()).rotation();
			doTransform = true;
		}
		for (int i = 0; i < scan.size(); ++i)
		{
			const float * ptrScan = scan.data().ptr<float>(0, i);

			if(is2d)
			{
				if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]))
				{
					cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], 0);
					if(doTransform)
					{
						n = util3d::transformPoint(n, tn);
					}
					float * ptr = data_normals.ptr<float>(oi++, 0);
					ptr[0] = n.x;
					ptr[1] = n.y;
				}
			}
			else
			{
				if(uIsFinite(ptrScan[nOffset]) && uIsFinite(ptrScan[nOffset+1]) && uIsFinite(ptrScan[nOffset+2]))
				{
					cv::Point3f n(ptrScan[nOffset], ptrScan[nOffset+1], ptrScan[nOffset+2]);
					if(doTransform)
					{
						n = util3d::transformPoint(n, tn);
					}
					float * ptr = data_normals.ptr<float>(oi++, 0);
					ptr[0] = n.x;
					ptr[1] = n.y;
					ptr[2] = n.z;
				}
			}
		}
		if(oi>1)
		{
			cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);

			if(pcaEigenVectors)
			{
				*pcaEigenVectors = pca_analysis.eigenvectors;
			}
			if(pcaEigenValues)
			{
				*pcaEigenValues = pca_analysis.eigenvalues;
			}
			UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
			// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
			return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
		}
	}
	else if(!scan.isEmpty())
	{
		UERROR("Scan doesn't have normals!");
	}
	return 0.0f;
}

float computeNormalsComplexity(
		const pcl::PointCloud<pcl::PointNormal> & cloud,
		const Transform & t,
		bool is2d,
		cv::Mat * pcaEigenVectors,
		cv::Mat * pcaEigenValues)
{
	 //Construct a buffer used by the pca analysis
	int sz = static_cast<int>(cloud.size()*2);
	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
	int oi = 0;
	bool doTransform = false;
	Transform tn;
	if(!t.isIdentity() && !t.isNull())
	{
		tn = t.rotation();
		doTransform = true;
	}
	for (unsigned int i = 0; i < cloud.size(); ++i)
	{
		const pcl::PointNormal & pt = cloud.at(i);
		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
		if(doTransform)
		{
			n = util3d::transformPoint(n, tn);
		}
		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
		{
			float * ptr = data_normals.ptr<float>(oi++, 0);
			ptr[0] = n.x;
			ptr[1] = n.y;
			if(!is2d)
			{
				ptr[2] = n.z;
			}
		}
	}
	if(oi>1)
	{
		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);

		if(pcaEigenVectors)
		{
			*pcaEigenVectors = pca_analysis.eigenvectors;
		}
		if(pcaEigenValues)
		{
			*pcaEigenValues = pca_analysis.eigenvalues;
		}

		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
	}
	return 0.0f;
}

float computeNormalsComplexity(
		const pcl::PointCloud<pcl::Normal> & normals,
		const Transform & t,
		bool is2d,
		cv::Mat * pcaEigenVectors,
		cv::Mat * pcaEigenValues)
{
	 //Construct a buffer used by the pca analysis
	int sz = static_cast<int>(normals.size()*2);
	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
	int oi = 0;
	bool doTransform = false;
	Transform tn;
	if(!t.isIdentity())
	{
		tn = t.rotation();
		doTransform = true;
	}
	for (unsigned int i = 0; i < normals.size(); ++i)
	{
		const pcl::Normal & pt = normals.at(i);
		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
		if(doTransform)
		{
			n = util3d::transformPoint(n, tn);
		}
		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
		{
			float * ptr = data_normals.ptr<float>(oi++, 0);
			ptr[0] = n.x;
			ptr[1] = n.y;
			if(!is2d)
			{
				ptr[2] = n.z;
			}
		}
	}
	if(oi>1)
	{
		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);

		if(pcaEigenVectors)
		{
			*pcaEigenVectors = pca_analysis.eigenvectors;
		}
		if(pcaEigenValues)
		{
			*pcaEigenValues = pca_analysis.eigenvalues;
		}

		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
	}
	return 0.0f;
}

float computeNormalsComplexity(
		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
		const Transform & t,
		bool is2d,
		cv::Mat * pcaEigenVectors,
		cv::Mat * pcaEigenValues)
{
	 //Construct a buffer used by the pca analysis
	int sz = static_cast<int>(cloud.size()*2);
	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
	int oi = 0;
	bool doTransform = false;
	Transform tn;
	if(!t.isIdentity())
	{
		tn = t.rotation();
		doTransform = true;
	}
	for (unsigned int i = 0; i < cloud.size(); ++i)
	{
		const pcl::PointXYZINormal & pt = cloud.at(i);
		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
		if(doTransform)
		{
			n = util3d::transformPoint(n, tn);
		}
		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
		{
			float * ptr = data_normals.ptr<float>(oi++, 0);
			ptr[0] = n.x;
			ptr[1] = n.y;
			if(!is2d)
			{
				ptr[2] = n.z;
			}
		}
	}
	if(oi>1)
	{
		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);

		if(pcaEigenVectors)
		{
			*pcaEigenVectors = pca_analysis.eigenvectors;
		}
		if(pcaEigenValues)
		{
			*pcaEigenValues = pca_analysis.eigenvalues;
		}

		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
	}
	return 0.0f;
}

float computeNormalsComplexity(
		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
		const Transform & t,
		bool is2d,
		cv::Mat * pcaEigenVectors,
		cv::Mat * pcaEigenValues)
{
	 //Construct a buffer used by the pca analysis
	int sz = static_cast<int>(cloud.size()*2);
	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
	int oi = 0;
	bool doTransform = false;
	Transform tn;
	if(!t.isIdentity())
	{
		tn = t.rotation();
		doTransform = true;
	}
	for (unsigned int i = 0; i < cloud.size(); ++i)
	{
		const pcl::PointXYZRGBNormal & pt = cloud.at(i);
		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
		if(doTransform)
		{
			n = util3d::transformPoint(n, tn);
		}
		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
		{
			float * ptr = data_normals.ptr<float>(oi++, 0);
			ptr[0] = n.x;
			ptr[1] = n.y;
			if(!is2d)
			{
				ptr[2] = n.z;
			}
		}
	}
	if(oi>1)
	{
		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);

		if(pcaEigenVectors)
		{
			*pcaEigenVectors = pca_analysis.eigenvectors;
		}
		if(pcaEigenValues)
		{
			*pcaEigenValues = pca_analysis.eigenvalues;
		}

		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
	}
	return 0.0f;
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		float searchRadius,
		int polygonialOrder,
		int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
		float upsamplingRadius,      // SAMPLE_LOCAL_PLANE
		float upsamplingStep,        // SAMPLE_LOCAL_PLANE
		int pointDensity,             // RANDOM_UNIFORM_DENSITY
		float dilationVoxelSize,     // VOXEL_GRID_DILATION
		int dilationIterations)       // VOXEL_GRID_DILATION
{
	pcl::IndicesPtr indices(new std::vector<int>);
	return mls(cloud,
			indices,
			searchRadius,
			polygonialOrder,
			upsamplingMethod,
			upsamplingRadius,
			upsamplingStep,
			pointDensity,
			dilationVoxelSize,
			dilationIterations);
}

pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
		const pcl::IndicesPtr & indices,
		float searchRadius,
		int polygonialOrder,
		int upsamplingMethod, // NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION
		float upsamplingRadius,      // SAMPLE_LOCAL_PLANE
		float upsamplingStep,        // SAMPLE_LOCAL_PLANE
		int pointDensity,             // RANDOM_UNIFORM_DENSITY
		float dilationVoxelSize,     // VOXEL_GRID_DILATION
		int dilationIterations)       // VOXEL_GRID_DILATION
{
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
	if(indices->size())
	{
		tree->setInputCloud (cloud, indices);
	}
	else
	{
		tree->setInputCloud (cloud);
	}

	// Init object (second point type is for the normals)
	pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;

	// Set parameters
	mls.setComputeNormals (true);
	if(polygonialOrder > 0)
	{
#if PCL_VERSION_COMPARE(<, 1, 10, 0)
		mls.setPolynomialFit (true);
#endif
		mls.setPolynomialOrder(polygonialOrder);
	}
	else
	{
#if PCL_VERSION_COMPARE(<, 1, 10, 0)
		mls.setPolynomialFit (false);
#else
		mls.setPolynomialOrder(1);
#endif
	}
	UASSERT(upsamplingMethod >= mls.NONE &&
			upsamplingMethod <= mls.VOXEL_GRID_DILATION);
	mls.setUpsamplingMethod((pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::UpsamplingMethod)upsamplingMethod);
	mls.setSearchRadius(searchRadius);
	mls.setUpsamplingRadius(upsamplingRadius);
	mls.setUpsamplingStepSize(upsamplingStep);
	mls.setPointDensity(pointDensity);
	mls.setDilationVoxelSize(dilationVoxelSize);
	mls.setDilationIterations(dilationIterations);

	// Reconstruct
	mls.setInputCloud (cloud);
	if(indices->size())
	{
		mls.setIndices(indices);
	}
	mls.setSearchMethod (tree);
	mls.process (*cloud_with_normals);

	// It seems that returned normals are not normalized!? FIXME: Is it a bug only in PCL 1.7.1?
	for(unsigned int i=0; i<cloud_with_normals->size(); ++i)
	{
		Eigen::Vector3f normal(cloud_with_normals->at(i).normal_x, cloud_with_normals->at(i).normal_y, cloud_with_normals->at(i).normal_z);
		normal.normalize();
		cloud_with_normals->at(i).normal_x = normal[0];
		cloud_with_normals->at(i).normal_y = normal[1];
		cloud_with_normals->at(i).normal_z = normal[2];
	}

	return cloud_with_normals;
}

LaserScan adjustNormalsToViewPoint(
		const LaserScan & scan,
		const Eigen::Vector3f & viewpoint,
		bool forceGroundNormalsUp)
{
	return adjustNormalsToViewPoint(scan, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
LaserScan adjustNormalsToViewPoint(
		const LaserScan & scan,
		const Eigen::Vector3f & viewpoint,
		float groundNormalsUp)
{
	if(scan.size() && !scan.is2d() && scan.hasNormals())
	{
		int nx = scan.getNormalsOffset();
		int ny = nx+1;
		int nz = ny+1;
		cv::Mat output = scan.data().clone();
		#pragma omp parallel for
		for(int j=0; j<scan.data().rows; ++j)
		{
			for(int i=0; i<scan.data().cols; ++i)
			{
				float * ptr = output.ptr<float>(j, i);
				if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
				{
					Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
					Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);

					float result = v.dot(n);
					if(result < 0
					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
					{
						//reverse normal
						ptr[nx] *= -1.0f;
						ptr[ny] *= -1.0f;
						ptr[nz] *= -1.0f;
					}
				}
			}
		}
		if(scan.angleIncrement() > 0.0f)
		{
			return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement(), scan.localTransform());
		}
		else
		{
			return LaserScan(output, scan.maxPoints(), scan.rangeMax(), scan.format(), scan.localTransform());
		}
	}
	return scan;
}

template<typename PointNormalT>
void adjustNormalsToViewPointImpl(
		typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		float groundNormalsUp)
{
	#pragma omp parallel for
	for(int i=0; i<(int)cloud->size(); ++i)
	{
		pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
		if(pcl::isFinite(normal))
		{
			Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
			Eigen::Vector3f n(normal.x, normal.y, normal.z);

			float result = v.dot(n);
			if(result < 0
				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[2])) // some far velodyne rays on road can have normals toward ground
			{
				//reverse normal
				cloud->points[i].normal_x *= -1.0f;
				cloud->points[i].normal_y *= -1.0f;
				cloud->points[i].normal_z *= -1.0f;
			}
		}
	}
}

void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		bool forceGroundNormalsUp)
{
	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		float groundNormalsUp)
{
	adjustNormalsToViewPointImpl<pcl::PointNormal>(cloud, viewpoint, groundNormalsUp);
}

void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		bool forceGroundNormalsUp)
{
	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		float groundNormalsUp)
{
	adjustNormalsToViewPointImpl<pcl::PointXYZRGBNormal>(cloud, viewpoint, groundNormalsUp);
}

void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		bool forceGroundNormalsUp)
{
	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
}
void adjustNormalsToViewPoint(
		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
		const Eigen::Vector3f & viewpoint,
		float groundNormalsUp)
{
	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
}

template<typename PointT>
void adjustNormalsToViewPointsImpl(
		const std::map<int, Transform> & poses,
		const std::vector<int> & cameraIndices,
		typename pcl::PointCloud<PointT>::Ptr & cloud,
		float groundNormalsUp)
{
	if(poses.size() && cloud->size() == cameraIndices.size() && cloud->size())
	{
		#pragma omp parallel for
		for(int i=0; i<(int)cloud->size(); ++i)
		{
			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
			if(pcl::isFinite(normal))
			{
				const Transform & p = poses.at(cameraIndices[i]);
				pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
				Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();

				Eigen::Vector3f n(normal.x, normal.y, normal.z);

				float result = v.dot(n);
				if(result < 0 ||
					(groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
				{
					//reverse normal
					cloud->points[i].normal_x *= -1.0f;
					cloud->points[i].normal_y *= -1.0f;
					cloud->points[i].normal_z *= -1.0f;
				}
			}
		}
	}
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const std::vector<int> & cameraIndices,
		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, cameraIndices, cloud, groundNormalsUp);
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const std::vector<int> & cameraIndices,
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, cameraIndices, cloud, groundNormalsUp);
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const std::vector<int> & cameraIndices,
		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, cameraIndices, cloud, groundNormalsUp);
}

template<typename PointT>
void adjustNormalsToViewPointsImpl(
		const std::map<int, Transform> & poses,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
		const std::vector<int> & rawCameraIndices,
		typename pcl::PointCloud<PointT>::Ptr & cloud,
		float groundNormalsUp)
{
	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
	{
		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
		rawTree->setInputCloud (rawCloud);

		#pragma omp parallel for
		for(int i=0; i<(int)cloud->size(); ++i)
		{
			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
			if(pcl::isFinite(normal))
			{
				std::vector<int> indices;
				std::vector<float> dist;
				rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
				UASSERT(indices.size() == 1);
				if(indices.size() && indices[0]>=0)
				{
					const Transform & p = poses.at(rawCameraIndices[indices[0]]);
					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();

					Eigen::Vector3f n(normal.x, normal.y, normal.z);

					float result = v.dot(n);
					if(result < 0 ||
					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
					{
						//reverse normal
						cloud->points[i].normal_x *= -1.0f;
						cloud->points[i].normal_y *= -1.0f;
						cloud->points[i].normal_z *= -1.0f;
					}
				}
				else
				{
					UWARN("Not found camera viewpoint for point %d", i);
				}
			}
		}
	}
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
		const std::vector<int> & rawCameraIndices,
		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
		const std::vector<int> & rawCameraIndices,
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & poses,
		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
		const std::vector<int> & rawCameraIndices,
		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
		float groundNormalsUp)
{
	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
}

void adjustNormalsToViewPoints(
		const std::map<int, Transform> & viewpoints,
		const LaserScan & rawScan,
		const std::vector<int> & viewpointIds,
		LaserScan & scan,
		float groundNormalsUp)
{
	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
	if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
	{
		pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
		rawTree->setInputCloud (rawCloud);
		#pragma omp parallel for
		for(int i=0; i<scan.size(); ++i)
		{
			pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);
			pcl::PointXYZ normal(point.normal_x, point.normal_y, point.normal_z);
			if(pcl::isFinite(normal))
			{
				std::vector<int> indices;
				std::vector<float> dist;
				rawTree->nearestKSearch(pcl::PointXYZ(point.x, point.y, point.z), 1, indices, dist);
				if(indices.size() && indices[0]>=0)
				{
					UASSERT_MSG(indices[0]<(int)viewpointIds.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)viewpointIds.size()).c_str());
					UASSERT(uContains(viewpoints, viewpointIds[indices[0]]));
					Transform p = viewpoints.at(viewpointIds[indices[0]]);
					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
					Eigen::Vector3f v = viewpoint.getVector3fMap() - point.getVector3fMap();

					Eigen::Vector3f n(normal.x, normal.y, normal.z);

					float result = v.dot(n);
					if(result < 0 ||
					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && point.z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground))
					{
						//reverse normal
						scan.field(i, scan.getNormalsOffset()) *= -1.0f;
						scan.field(i, scan.getNormalsOffset()+1) *= -1.0f;
						scan.field(i, scan.getNormalsOffset()+2) *= -1.0f;
					}
				}
				else
				{
					UWARN("Not found camera viewpoint for point %d!?", i);
				}
			}
		}
	}
}

pcl::PolygonMesh::Ptr meshDecimation(const pcl::PolygonMesh::Ptr & mesh, float factor)
{
	pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
#ifndef DISABLE_VTK
	pcl::MeshQuadricDecimationVTK mqd;
	mqd.setTargetReductionFactor(factor);
	mqd.setInputMesh(mesh);
	mqd.process (*output);
#else
	UWARN("RTAB-Map is not built with VTK module so mesh decimation cannot be used!");
	*output = *mesh;
#endif
	return output;
}

bool intersectRayTriangle(
		const Eigen::Vector3f & p,
		const Eigen::Vector3f & dir,
		const Eigen::Vector3f & v0,
		const Eigen::Vector3f & v1,
		const Eigen::Vector3f & v2,
		float & distance,
		Eigen::Vector3f & normal)
{
    // get triangle edge cv::Vec3fs and plane normal
    const Eigen::Vector3f u = v1-v0;
    const Eigen::Vector3f v = v2-v0;
    normal = u.cross(v);              // cross product
    if (normal == Eigen::Vector3f(0,0,0))   // triangle is degenerate
        return false;                 // do not deal with this case

    const float denomimator = normal.dot(dir);
    if (fabs(denomimator) < 10e-9)    // ray is  parallel to triangle plane
       return false;

    // get intersect of ray with triangle plane
    distance = normal.dot(v0 - p) / denomimator;
    if (distance < 0.0)                      // ray goes away from triangle
        return false;

    // is I inside T?
    float    uu, uv, vv, wu, wv, D;
    uu = u.dot(u);
    uv = u.dot(v);
    vv = v.dot(v);
    const Eigen::Vector3f w = p + dir * distance - v0;
    wu = w.dot(u);
    wv = w.dot(v);
    D = uv * uv - uu * vv;

    // get and test parametric coords
    float s, t;
    s = (uv * wv - vv * wu) / D;
    if (s < 0.0 || s > 1.0)         // I is outside T
        return false;
    t = (uv * wu - uu * wv) / D;
    if (t < 0.0 || (s + t) > 1.0)   // I is outside T
        return false;

    return true;                    // I is in T
}

/**
 * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
 * with added color on each vertex if provided
 */
int saveOBJFile(
	const std::string &file_name,
	const pcl::TextureMesh &tex_mesh,
	unsigned precision)
{
  if (tex_mesh.cloud.data.empty ())
  {
    UERROR ("Input point cloud has no data!\n");
    return (-1);
  }
  // Open file
  std::ofstream fs;
  fs.precision (precision);
  fs.open (file_name.c_str ());

  // Define material file
  std::string mtl_file_name = file_name.substr (0, file_name.find_last_of ('.')) + ".mtl";
  // Strip path for "mtllib" command
  std::string mtl_file_name_nopath = mtl_file_name;
  mtl_file_name_nopath.erase (0, mtl_file_name.find_last_of ('/') + 1);

  /* Write 3D information */
  // number of points
  unsigned nr_points  = tex_mesh.cloud.width * tex_mesh.cloud.height;
  unsigned point_size = static_cast<unsigned> (tex_mesh.cloud.data.size () / nr_points);

  // mesh size
  unsigned nr_meshes = static_cast<unsigned> (tex_mesh.tex_polygons.size ());
  // number of faces for header
  unsigned nr_faces = 0;
  for (unsigned m = 0; m < nr_meshes; ++m)
    nr_faces += static_cast<unsigned> (tex_mesh.tex_polygons[m].size ());

  // Write the header information
  fs << "####" << '\n';
  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
  fs << "# Vertices: " << nr_points << '\n';
  fs << "# Faces: " <<nr_faces << '\n';
  fs << "# Material information:" << '\n';
  fs << "mtllib " << mtl_file_name_nopath << '\n';
  fs << "####" << '\n';

  // Write vertex coordinates
  fs << "# Vertices" << '\n';
  for (unsigned i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    // "v" just be written one
    bool v_written = false;
    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
    {
      // adding vertex
      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
          tex_mesh.cloud.fields[d].name == "x" ||
          tex_mesh.cloud.fields[d].name == "y" ||
          tex_mesh.cloud.fields[d].name == "z"))
      {
        if (!v_written)
        {
           // write vertices beginning with v
          fs << "v ";
          v_written = true;
        }
        float value;
        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
        fs << value;
        if (++xyz == 3)
          continue;
        fs << " ";
      }
	  else if(tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
          	  tex_mesh.cloud.fields[d].name == "rgb")
	  {
		std::uint32_t rgb = *reinterpret_cast<const int*>(&tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset]);
		std::uint8_t r = (rgb >> 16) & 0x0000ff;
		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
		std::uint8_t b = (rgb)       & 0x0000ff;
		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
		break;
	  }
    }
    if (xyz != 3)
    {
      UERROR ("Input point cloud has no XYZ data!\n");
      return (-2);
    }
    fs << '\n';
  }
  fs << "# "<< nr_points <<" vertices" << '\n';

  // Write vertex normals
  for (unsigned i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    // "vn" just be written one
    bool v_written = false;
    for (std::size_t d = 0; d < tex_mesh.cloud.fields.size (); ++d)
    {
      // adding vertex
      if ((tex_mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
          tex_mesh.cloud.fields[d].name == "normal_x" ||
          tex_mesh.cloud.fields[d].name == "normal_y" ||
          tex_mesh.cloud.fields[d].name == "normal_z"))
      {
    	  if (!v_written)
    	  {
          // write vertices beginning with vn
          fs << "vn ";
          v_written = true;
    	  }
        float value;
        memcpy (&value, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[d].offset], sizeof (float));
        fs << value;
        if (++xyz == 3)
          break;
        fs << " ";
      }
    }
    if (xyz != 3)
    {
      UERROR ("Input point cloud has no normals!\n");
      return (-2);
    }
    fs << '\n';
  }
  // Write vertex texture with "vt" (adding latter)

  for (unsigned m = 0; m < nr_meshes; ++m)
  {
    fs << "# " << tex_mesh.tex_coordinates[m].size() << " vertex textures in submesh " << m <<  '\n';
    for (const auto &coordinate : tex_mesh.tex_coordinates[m])
    {
      fs << "vt ";
      fs <<  coordinate[0] << " " << coordinate[1] << '\n';
    }
  }

  unsigned f_idx = 0;

  // int idx_vt =0;
  for (unsigned m = 0; m < nr_meshes; ++m)
  {
    if (m > 0) f_idx += static_cast<unsigned> (tex_mesh.tex_polygons[m-1].size ());

    fs << "# The material will be used for mesh " << m << '\n';
    fs << "usemtl " <<  tex_mesh.tex_materials[m].tex_name << '\n';
    fs << "# Faces" << '\n';

    for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size(); ++i)
    {
      // Write faces with "f"
      fs << "f";
      // There's one UV per vertex per face, i.e., the same vertex can have
      // different UV depending on the face.
      for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        std::uint32_t idx = tex_mesh.tex_polygons[m][i].vertices[j] + 1;
        fs << " " << idx
           << "/" << tex_mesh.tex_polygons[m][i].vertices.size () * (i+f_idx) +j+1
           << "/" << idx; // vertex index in obj file format starting with 1
      }
      fs << '\n';
    }
    fs << "# "<< tex_mesh.tex_polygons[m].size() << " faces in mesh " << m << '\n';
  }
  fs << "# End of File" << std::flush;

  // Close obj file
  fs.close ();

  /* Write material definition for OBJ file*/
  // Open file

  std::ofstream m_fs;
  m_fs.precision (precision);
  m_fs.open (mtl_file_name.c_str ());

  // default
  m_fs << "#" << '\n';
  m_fs << "# Wavefront material file" << '\n';
  m_fs << "#" << '\n';
  for(unsigned m = 0; m < nr_meshes; ++m)
  {
    m_fs << "newmtl " << tex_mesh.tex_materials[m].tex_name << '\n';
    m_fs << "Ka "<< tex_mesh.tex_materials[m].tex_Ka.r << " " << tex_mesh.tex_materials[m].tex_Ka.g << " " << tex_mesh.tex_materials[m].tex_Ka.b << '\n'; // defines the ambient color of the material to be (r,g,b).
    m_fs << "Kd "<< tex_mesh.tex_materials[m].tex_Kd.r << " " << tex_mesh.tex_materials[m].tex_Kd.g << " " << tex_mesh.tex_materials[m].tex_Kd.b << '\n'; // defines the diffuse color of the material to be (r,g,b).
    m_fs << "Ks "<< tex_mesh.tex_materials[m].tex_Ks.r << " " << tex_mesh.tex_materials[m].tex_Ks.g << " " << tex_mesh.tex_materials[m].tex_Ks.b << '\n'; // defines the specular color of the material to be (r,g,b). This color shows up in highlights.
    m_fs << "d " << tex_mesh.tex_materials[m].tex_d << '\n'; // defines the transparency of the material to be alpha.
    m_fs << "Ns "<< tex_mesh.tex_materials[m].tex_Ns  << '\n'; // defines the shininess of the material to be s.
    m_fs << "illum "<< tex_mesh.tex_materials[m].tex_illum << '\n'; // denotes the illumination model used by the material.
                                            // illum = 1 indicates a flat material with no specular highlights, so the value of Ks is not used.
                                            // illum = 2 denotes the presence of specular highlights, and so a specification for Ks is required.
    m_fs << "map_Kd " << tex_mesh.tex_materials[m].tex_file << '\n';
    m_fs << "###" << '\n';
  }
  m_fs.close ();
  return (0);
}

/**
 * This is a modified copy of https://github.com/PointCloudLibrary/pcl/blob/pcl-1.12.1/io/src/obj_io.cpp
 * with added color on each vertex if provided
 */
int saveOBJFile(
	const std::string &file_name,
	const pcl::PolygonMesh &mesh,
	unsigned precision)
{
  if (mesh.cloud.data.empty ())
  {
    UERROR ("Input point cloud has no data!\n");
    return (-1);
  }
  // Open file
  std::ofstream fs;
  fs.precision (precision);
  fs.open (file_name.c_str ());

  /* Write 3D information */
  // number of points
  int nr_points  = mesh.cloud.width * mesh.cloud.height;
  // point size
  unsigned point_size = static_cast<unsigned> (mesh.cloud.data.size () / nr_points);
  // number of faces for header
  unsigned nr_faces = static_cast<unsigned> (mesh.polygons.size ());
  // Do we have vertices normals?
  int normal_index = getFieldIndex (mesh.cloud, "normal_x");

  // Write the header information
  fs << "####" << '\n';
  fs << "# OBJ dataFile simple version. File name: " << file_name << '\n';
  fs << "# Vertices: " << nr_points << '\n';
  if (normal_index != -1)
    fs << "# Vertices normals : " << nr_points << '\n';
  fs << "# Faces: " <<nr_faces << '\n';
  fs << "####" << '\n';

  // Write vertex coordinates
  fs << "# List of Vertices, with (x,y,z) coordinates, w is optional." << '\n';
  for (int i = 0; i < nr_points; ++i)
  {
    int xyz = 0;
    for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
    {
      // adding vertex
      if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
          mesh.cloud.fields[d].name == "x" ||
          mesh.cloud.fields[d].name == "y" ||
          mesh.cloud.fields[d].name == "z"))
      {
        if (mesh.cloud.fields[d].name == "x")
           // write vertices beginning with v
          fs << "v ";

        float value;
        memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
        fs << value;
        if (++xyz == 3)
          continue;
        fs << " ";
      }
	  else if(mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32 &&
          	  mesh.cloud.fields[d].name == "rgb")
	  {
		std::uint32_t rgb = *reinterpret_cast<const int*>(&mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset]);
		std::uint8_t r = (rgb >> 16) & 0x0000ff;
		std::uint8_t g = (rgb >> 8)  & 0x0000ff;
		std::uint8_t b = (rgb)       & 0x0000ff;
		fs << " " << float(r)/255.0f << " " << float(g)/255.0f << " " << float(b)/255.0f;
		break;
	  }
    }
    if (xyz != 3)
    {
      UERROR ("Input point cloud has no XYZ data!\n");
      return (-2);
    }
    fs << '\n';
  }

  fs << "# "<< nr_points <<" vertices" << '\n';

  if(normal_index != -1)
  {
    fs << "# Normals in (x,y,z) form; normals might not be unit." <<  '\n';
    // Write vertex normals
    for (int i = 0; i < nr_points; ++i)
    {
      int nxyz = 0;
      for (std::size_t d = 0; d < mesh.cloud.fields.size (); ++d)
      {
        // adding vertex
        if ((mesh.cloud.fields[d].datatype == pcl::PCLPointField::FLOAT32) && (
              mesh.cloud.fields[d].name == "normal_x" ||
              mesh.cloud.fields[d].name == "normal_y" ||
              mesh.cloud.fields[d].name == "normal_z"))
        {
          if (mesh.cloud.fields[d].name == "normal_x")
            // write vertices beginning with vn
            fs << "vn ";

          float value;
          memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset], sizeof (float));
          fs << value;
          if (++nxyz == 3)
            break;
          fs << " ";
        }
      }
      if (nxyz != 3)
      {
        UERROR ("Input point cloud has no normals!\n");
        return (-2);
      }
      fs << '\n';
    }

    fs << "# "<< nr_points <<" vertices normals" << '\n';
  }

  fs << "# Face Definitions" << '\n';
  // Write down faces
  if(normal_index == -1)
  {
    for(unsigned i = 0; i < nr_faces; i++)
    {
      fs << "f ";      
      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
        fs << mesh.polygons[i].vertices[j] + 1 << " ";
      fs << mesh.polygons[i].vertices.back() + 1 << '\n';
    }
  }
  else
  {
    for(unsigned i = 0; i < nr_faces; i++)
    {
      fs << "f ";
      for (std::size_t j = 0; j < mesh.polygons[i].vertices.size () - 1; ++j)
        fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << " ";
      fs << mesh.polygons[i].vertices.back() + 1 << "//" << mesh.polygons[i].vertices.back() + 1 << '\n';
    }
  }
  fs << "# End of File" << std::endl;

  // Close obj file
  fs.close ();
  return 0;
}

}

}